From 0419530a1f9bad7c14880c44284722deaabd6cf7 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Mon, 7 Aug 2023 13:58:34 +0200 Subject: [PATCH 01/15] add GNSS sensor SDF importer hook Signed-off-by: Jan Hanca --- .../Source/GNSS/ROS2GNSSSensorComponent.cpp | 11 ++----- .../Source/GNSS/ROS2GNSSSensorComponent.h | 6 ++++ .../SDFormat/ROS2SensorHooks.cpp | 31 ++++++++++++++++++- .../RobotImporter/SDFormat/ROS2SensorHooks.h | 1 + 4 files changed, 40 insertions(+), 9 deletions(-) diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index 65f889c14..fea32b1d1 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -21,11 +21,6 @@ namespace ROS2 { - namespace Internal - { - const char* kGNSSMsgType = "sensor_msgs::msg::NavSatFix"; - } - void ROS2GNSSSensorComponent::Reflect(AZ::ReflectContext* context) { GNSSSensorConfiguration::Reflect(context); @@ -54,10 +49,10 @@ namespace ROS2 ROS2GNSSSensorComponent::ROS2GNSSSensorComponent() { TopicConfiguration pc; - pc.m_type = Internal::kGNSSMsgType; + pc.m_type = GNSSConstants::GNSSMessageType; pc.m_topic = "gnss"; m_sensorConfiguration.m_frequency = 10; - m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(Internal::kGNSSMsgType, pc)); + m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(AZStd::string(GNSSConstants::GNSSDataConfig), pc)); } ROS2GNSSSensorComponent::ROS2GNSSSensorComponent( @@ -73,7 +68,7 @@ namespace ROS2 auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for GNSS sensor"); - const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kGNSSMsgType]; + const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[GNSSConstants::GNSSDataConfig]; const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_gnssPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h index d13b6e0bd..dbfaa10c7 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h @@ -17,6 +17,12 @@ namespace ROS2 { + namespace GNSSConstants + { + inline constexpr char GNSSMessageType[] = "sensor_msgs::msg::NavSatFix"; + inline constexpr char GNSSDataConfig[] = "GNSS Data"; + } // namespace GNSSConstants + //! Global Navigation Satellite Systems (GNSS) sensor component class //! It provides NavSatFix data of sensor's position in GNSS frame which is defined by GNSS origin offset //! Offset is provided as latitude [deg], longitude [deg], altitude [m] of o3de global frame diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp index d3a2e06ea..990461126 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -85,7 +86,35 @@ namespace ROS2::SDFormat sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); } - if (entity.CreateComponent(sensorConfiguration, cameraConfiguration)) + if (entity.CreateComponent() && + entity.CreateComponent(sensorConfiguration, cameraConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component")); + } + }; + + return importerHook; + } + + 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 + { + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + Internal::AddTopicConfiguration(sensorConfiguration, "gnss", GNSSConstants::GNSSMessageType, GNSSConstants::GNSSDataConfig); + + if (entity.CreateComponent(sensorConfiguration, GNSSSensorConfiguration())) { return AZ::Success(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h index 32d2db0d8..d7f0beef5 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h @@ -15,5 +15,6 @@ namespace ROS2::SDFormat namespace ROS2SensorHooks { SensorImporterHook ROS2CameraSensor(); + SensorImporterHook ROS2GNSSSensor(); } // namespace ROS2SensorHooks } // namespace ROS2::SDFormat From d58c85ad98fefb017708abfe31069cb01191794b Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 8 Aug 2023 12:06:57 +0200 Subject: [PATCH 02/15] add Imu sensor SDF importer hook Signed-off-by: Jan Hanca --- .../Source/Imu/ROS2ImuSensorComponent.cpp | 9 +- .../Code/Source/Imu/ROS2ImuSensorComponent.h | 6 ++ .../SDFormat/ROS2SensorHooks.cpp | 90 ++++++++++++++++++- .../RobotImporter/SDFormat/ROS2SensorHooks.h | 1 + 4 files changed, 98 insertions(+), 8 deletions(-) diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp index 4e4714897..cfebd6244 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -24,11 +24,6 @@ namespace ROS2 { - namespace Internal - { - const char* kImuMsgType = "sensor_msgs::msg::Imu"; - } - void ROS2ImuSensorComponent::Reflect(AZ::ReflectContext* context) { ImuSensorConfiguration::Reflect(context); @@ -56,7 +51,7 @@ namespace ROS2 ROS2ImuSensorComponent::ROS2ImuSensorComponent() { - const AZStd::string type = Internal::kImuMsgType; + const AZStd::string type = ImuConstants::ImuMessageType; TopicConfiguration pc; pc.m_type = type; pc.m_topic = "imu"; @@ -146,7 +141,7 @@ namespace ROS2 auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for IMU sensor"); m_imuMsg.header.frame_id = GetFrameID().c_str(); - const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImuMsgType]; + const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[ImuConstants::ImuMessageType]; const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_imuPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h index 9ff8d761b..233e2169f 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h @@ -21,6 +21,12 @@ namespace ROS2 { + namespace ImuConstants + { + inline constexpr char ImuMessageType[] = "sensor_msgs::msg::Imu"; + inline constexpr char ImuDataConfig[] = "Imu Data"; + } // namespace ImuConstants + //! An IMU (Inertial Measurement Unit) sensor Component. //! IMUs typically include gyroscopes, accelerometers and magnetometers. This component encapsulates data //! acquisition and its publishing to ROS2 ecosystem. IMU Component requires ROS2FrameComponent. diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp index 990461126..49cd52047 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp @@ -11,8 +11,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -50,6 +52,10 @@ 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(); @@ -110,6 +116,11 @@ namespace ROS2::SDFormat 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(); Internal::AddTopicConfiguration(sensorConfiguration, "gnss", GNSSConstants::GNSSMessageType, GNSSConstants::GNSSDataConfig); @@ -120,7 +131,84 @@ namespace ROS2::SDFormat } else { - return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component")); + return AZ::Failure(AZStd::string("Failed to create ROS2 GNSS Sensor component")); + } + }; + + return importerHook; + } + + 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(); + Internal::AddTopicConfiguration(sensorConfiguration, "imu", ImuConstants::ImuMessageType, ImuConstants::ImuDataConfig); + + if (entity.CreateComponent() && entity.CreateComponent() && + entity.CreateComponent(sensorConfiguration, imuConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 Imu Sensor component")); } }; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h index d7f0beef5..bea6a3a13 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h @@ -16,5 +16,6 @@ namespace ROS2::SDFormat { SensorImporterHook ROS2CameraSensor(); SensorImporterHook ROS2GNSSSensor(); + SensorImporterHook ROS2ImuSensor(); } // namespace ROS2SensorHooks } // namespace ROS2::SDFormat From ab9ae526eca84611bda7ad6350b2c68359876efd Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 8 Aug 2023 12:49:33 +0200 Subject: [PATCH 03/15] Imu sensor SDF importer hook tests Signed-off-by: Jan Hanca --- Gems/ROS2/Code/Tests/SdfParserTest.cpp | 261 ++++++++++++++++++------- 1 file changed, 192 insertions(+), 69 deletions(-) diff --git a/Gems/ROS2/Code/Tests/SdfParserTest.cpp b/Gems/ROS2/Code/Tests/SdfParserTest.cpp index 42451ebcf..97057e82c 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,44 @@ 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& importerHook = 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, importerHook.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, 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)); + } + { + 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 From c0ab8a9de0a0415f3d9498c02bea4af6eab7c696 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 8 Aug 2023 14:52:33 +0200 Subject: [PATCH 04/15] add Lidar sensor SDF importer hook Signed-off-by: Jan Hanca --- .../Code/Source/Lidar/LidarTemplateUtils.cpp | 50 +++++++------- .../SDFormat/ROS2SensorHooks.cpp | 67 +++++++++++++++++++ .../RobotImporter/SDFormat/ROS2SensorHooks.h | 1 + Gems/ROS2/Code/Tests/SdfParserTest.cpp | 29 +++++--- 4 files changed, 113 insertions(+), 34 deletions(-) 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/RobotImporter/SDFormat/ROS2SensorHooks.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp index 49cd52047..42e194254 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -215,4 +216,70 @@ namespace ROS2::SDFormat return importerHook; } + 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"; + Internal::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(); + + if (entity.CreateComponent() && + entity.CreateComponent(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 bea6a3a13..f5ffcbc3f 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h @@ -17,5 +17,6 @@ namespace ROS2::SDFormat SensorImporterHook ROS2CameraSensor(); SensorImporterHook ROS2GNSSSensor(); SensorImporterHook ROS2ImuSensor(); + SensorImporterHook ROS2LidarSensor(); } // namespace ROS2SensorHooks } // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Tests/SdfParserTest.cpp b/Gems/ROS2/Code/Tests/SdfParserTest.cpp index 97057e82c..c90e2b516 100644 --- a/Gems/ROS2/Code/Tests/SdfParserTest.cpp +++ b/Gems/ROS2/Code/Tests/SdfParserTest.cpp @@ -326,27 +326,38 @@ namespace UnitTest 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& cameraImporterHook = ROS2::SDFormat::ROS2SensorHooks::ROS2CameraSensor(); const auto& unsupportedCameraParams = - ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, importerHook.m_supportedSensorParams); + 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)); + 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, importerHook.m_pluginNames)); + EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames)); plug.SetFilename("~/dev/libgazebo_ros_imu.so"); - EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames)); + EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames)); plug.SetFilename("libgazebo_ros_camera"); - EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames)); + EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.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)); + 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(); From ac0f3cf45c9549d21eeca013f8b89c15fade8c92 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 8 Aug 2023 15:50:14 +0200 Subject: [PATCH 05/15] fix sensor types mappings Signed-off-by: Jan Hanca --- .../ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp | 11 ++++++++--- Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h | 6 ------ Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp | 9 +++++++-- Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h | 6 ------ .../Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp | 6 ++++-- 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index fea32b1d1..65f889c14 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -21,6 +21,11 @@ namespace ROS2 { + namespace Internal + { + const char* kGNSSMsgType = "sensor_msgs::msg::NavSatFix"; + } + void ROS2GNSSSensorComponent::Reflect(AZ::ReflectContext* context) { GNSSSensorConfiguration::Reflect(context); @@ -49,10 +54,10 @@ namespace ROS2 ROS2GNSSSensorComponent::ROS2GNSSSensorComponent() { TopicConfiguration pc; - pc.m_type = GNSSConstants::GNSSMessageType; + pc.m_type = Internal::kGNSSMsgType; pc.m_topic = "gnss"; m_sensorConfiguration.m_frequency = 10; - m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(AZStd::string(GNSSConstants::GNSSDataConfig), pc)); + m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(Internal::kGNSSMsgType, pc)); } ROS2GNSSSensorComponent::ROS2GNSSSensorComponent( @@ -68,7 +73,7 @@ namespace ROS2 auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for GNSS sensor"); - const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[GNSSConstants::GNSSDataConfig]; + const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kGNSSMsgType]; const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_gnssPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h index dbfaa10c7..d13b6e0bd 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h @@ -17,12 +17,6 @@ namespace ROS2 { - namespace GNSSConstants - { - inline constexpr char GNSSMessageType[] = "sensor_msgs::msg::NavSatFix"; - inline constexpr char GNSSDataConfig[] = "GNSS Data"; - } // namespace GNSSConstants - //! Global Navigation Satellite Systems (GNSS) sensor component class //! It provides NavSatFix data of sensor's position in GNSS frame which is defined by GNSS origin offset //! Offset is provided as latitude [deg], longitude [deg], altitude [m] of o3de global frame diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp index cfebd6244..4e4714897 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -24,6 +24,11 @@ namespace ROS2 { + namespace Internal + { + const char* kImuMsgType = "sensor_msgs::msg::Imu"; + } + void ROS2ImuSensorComponent::Reflect(AZ::ReflectContext* context) { ImuSensorConfiguration::Reflect(context); @@ -51,7 +56,7 @@ namespace ROS2 ROS2ImuSensorComponent::ROS2ImuSensorComponent() { - const AZStd::string type = ImuConstants::ImuMessageType; + const AZStd::string type = Internal::kImuMsgType; TopicConfiguration pc; pc.m_type = type; pc.m_topic = "imu"; @@ -141,7 +146,7 @@ namespace ROS2 auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for IMU sensor"); m_imuMsg.header.frame_id = GetFrameID().c_str(); - const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[ImuConstants::ImuMessageType]; + 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()); diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h index 233e2169f..9ff8d761b 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h @@ -21,12 +21,6 @@ namespace ROS2 { - namespace ImuConstants - { - inline constexpr char ImuMessageType[] = "sensor_msgs::msg::Imu"; - inline constexpr char ImuDataConfig[] = "Imu Data"; - } // namespace ImuConstants - //! An IMU (Inertial Measurement Unit) sensor Component. //! IMUs typically include gyroscopes, accelerometers and magnetometers. This component encapsulates data //! acquisition and its publishing to ROS2 ecosystem. IMU Component requires ROS2FrameComponent. diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp index 42e194254..821979c30 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp @@ -124,7 +124,8 @@ namespace ROS2::SDFormat SensorConfiguration sensorConfiguration; sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); - Internal::AddTopicConfiguration(sensorConfiguration, "gnss", GNSSConstants::GNSSMessageType, GNSSConstants::GNSSDataConfig); + const AZStd::string messageType = "sensor_msgs::msg::NavSatFix"; + Internal::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType); if (entity.CreateComponent(sensorConfiguration, GNSSSensorConfiguration())) { @@ -200,7 +201,8 @@ namespace ROS2::SDFormat SensorConfiguration sensorConfiguration; sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); - Internal::AddTopicConfiguration(sensorConfiguration, "imu", ImuConstants::ImuMessageType, ImuConstants::ImuDataConfig); + const AZStd::string messageType = "sensor_msgs::msg::Imu"; + Internal::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); if (entity.CreateComponent() && entity.CreateComponent() && entity.CreateComponent(sensorConfiguration, imuConfiguration)) From 4bf2791be79ee65c8cc8fa0f658907a0e61526e0 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Wed, 9 Aug 2023 11:48:45 +0200 Subject: [PATCH 06/15] code review: split into files Signed-off-by: Jan Hanca --- .../SDFormat/Hooks/ROS2CameraSensorHook.cpp | 90 +++++++ .../SDFormat/Hooks/ROS2GNSSSensorHook.cpp | 50 ++++ .../SDFormat/Hooks/ROS2ImuSensorHook.cpp | 98 +++++++ .../SDFormat/Hooks/ROS2LidarSensorHook.cpp | 84 ++++++ .../SDFormat/ROS2SensorHooks.cpp | 253 +----------------- .../RobotImporter/SDFormat/ROS2SensorHooks.h | 24 ++ Gems/ROS2/Code/ros2_editor_files.cmake | 4 + 7 files changed, 353 insertions(+), 250 deletions(-) create mode 100644 Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp 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 diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp new file mode 100644 index 000000000..2edb3342f --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -0,0 +1,90 @@ +/* + * 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::ROS2CameraSensor() + { + SensorImporterHook importerHook; + importerHook.m_sensorTypes = + AZStd::unordered_set{ sdf::SensorType::CAMERA, sdf::SensorType::DEPTH_CAMERA, sdf::SensorType::RGBD_CAMERA }; + importerHook.m_supportedSensorParams = + AZStd::unordered_set{ ">update_rate", ">camera>horizontal_fov", ">camera>image>width", + ">camera>image>height", ">camera>clip>near", ">camera>clip>far" }; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_camera.so", + "libgazebo_ros_depth_camera.so", + "libgazebo_ros_openni_kinect.so" }; + importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, + 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 (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) + { + cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->NearClip()); + cameraConfiguration.m_farClipDistance = static_cast(cameraSensor->FarClip()); + } + else + { + cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->DepthNearClip()); + cameraConfiguration.m_farClipDistance = static_cast(cameraSensor->DepthFarClip()); + } + + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) + { + Utils::AddTopicConfiguration( + sensorConfiguration, "camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig); + Utils::AddTopicConfiguration( + sensorConfiguration, "color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig); + } + if (sdfSensor.Type() != sdf::SensorType::CAMERA) + { + Utils::AddTopicConfiguration( + sensorConfiguration, "camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig); + Utils::AddTopicConfiguration( + sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); + } + + if (entity.CreateComponent() && + entity.CreateComponent(sensorConfiguration, cameraConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component")); + } + }; + + 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..f7338786e --- /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 + +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 (entity.CreateComponent(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..9c95d621d --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -0,0 +1,98 @@ +/* + * 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::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); + + if (entity.CreateComponent() && entity.CreateComponent() && + entity.CreateComponent(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..09dfd96ef --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -0,0 +1,84 @@ +/* + * 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::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(); + + if (entity.CreateComponent() && + entity.CreateComponent(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.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp index 821979c30..9f6ff7137 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp @@ -22,9 +22,9 @@ namespace ROS2::SDFormat { - namespace Internal + namespace ROS2SensorHooks { - void AddTopicConfiguration( + void Utils::AddTopicConfiguration( SensorConfiguration& sensorConfig, const AZStd::string& topic, const AZStd::string& messageType, @@ -35,253 +35,6 @@ namespace ROS2::SDFormat config.m_type = messageType; sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config)); } - } // namespace Internal - - SensorImporterHook ROS2SensorHooks::ROS2CameraSensor() - { - SensorImporterHook importerHook; - importerHook.m_sensorTypes = - AZStd::unordered_set{ sdf::SensorType::CAMERA, sdf::SensorType::DEPTH_CAMERA, sdf::SensorType::RGBD_CAMERA }; - importerHook.m_supportedSensorParams = - AZStd::unordered_set{ ">update_rate", ">camera>horizontal_fov", ">camera>image>width", - ">camera>image>height", ">camera>clip>near", ">camera>clip>far" }; - importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_camera.so", - "libgazebo_ros_depth_camera.so", - "libgazebo_ros_openni_kinect.so" }; - importerHook.m_supportedPluginParams = AZStd::unordered_set{}; - importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, - 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 (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) - { - cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->NearClip()); - cameraConfiguration.m_farClipDistance = static_cast(cameraSensor->FarClip()); - } - else - { - cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->DepthNearClip()); - cameraConfiguration.m_farClipDistance = static_cast(cameraSensor->DepthFarClip()); - } - - SensorConfiguration sensorConfiguration; - sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); - if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) - { - Internal::AddTopicConfiguration( - sensorConfiguration, "camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig); - Internal::AddTopicConfiguration( - sensorConfiguration, "color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig); - } - if (sdfSensor.Type() != sdf::SensorType::CAMERA) - { - Internal::AddTopicConfiguration( - sensorConfiguration, "camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig); - Internal::AddTopicConfiguration( - sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); - } - - if (entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, cameraConfiguration)) - { - return AZ::Success(); - } - else - { - return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component")); - } - }; - - return importerHook; - } - - 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"; - Internal::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType); - - if (entity.CreateComponent(sensorConfiguration, GNSSSensorConfiguration())) - { - return AZ::Success(); - } - else - { - return AZ::Failure(AZStd::string("Failed to create ROS2 GNSS Sensor component")); - } - }; - - return importerHook; - } - - 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"; - Internal::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); - - if (entity.CreateComponent() && entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, imuConfiguration)) - { - return AZ::Success(); - } - else - { - return AZ::Failure(AZStd::string("Failed to create ROS2 Imu Sensor component")); - } - }; - - return importerHook; - } - - 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"; - Internal::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(); - - if (entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, lidarConfiguration)) - { - return AZ::Success(); - } - else - { - return AZ::Failure(AZStd::string("Failed to create ROS2 Lidar Sensor component")); - } - }; - - return importerHook; - } + } // namespace ROS2SensorHooks } // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h index f5ffcbc3f..37772c4c5 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h @@ -8,15 +8,39 @@ #pragma once +#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); + } // namespace Utils + + //! 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/ros2_editor_files.cmake b/Gems/ROS2/Code/ros2_editor_files.cmake index dcdfde185..a9ed00672 100644 --- a/Gems/ROS2/Code/ros2_editor_files.cmake +++ b/Gems/ROS2/Code/ros2_editor_files.cmake @@ -28,6 +28,10 @@ set(FILES Source/RobotImporter/RobotImporterWidget.h Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h + 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/ROS2SensorHooks.cpp Source/RobotImporter/SDFormat/ROS2SensorHooks.h Source/RobotImporter/URDF/ArticulationsMaker.cpp From 460fbe5634f56d3cc7c10df38b2e360ef7138c76 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Mon, 21 Aug 2023 18:21:39 +0200 Subject: [PATCH 07/15] add get/set to conf in ROS2 Sensors; refactor hooks Signed-off-by: Jan Hanca --- .../ROS2CameraSensorEditorComponent.cpp | 23 ++++++++++- .../Camera/ROS2CameraSensorEditorComponent.h | 5 +++ .../Source/GNSS/ROS2GNSSSensorComponent.cpp | 24 ++++++++++- .../Source/GNSS/ROS2GNSSSensorComponent.h | 5 +++ .../Source/Imu/ROS2ImuSensorComponent.cpp | 40 ++++++++++++++----- .../Code/Source/Imu/ROS2ImuSensorComponent.h | 7 +++- .../Lidar/ROS2Lidar2DSensorComponent.cpp | 23 ++++++++++- .../Source/Lidar/ROS2Lidar2DSensorComponent.h | 5 +++ .../Source/Lidar/ROS2LidarSensorComponent.cpp | 23 ++++++++++- .../Source/Lidar/ROS2LidarSensorComponent.h | 5 +++ .../ROS2RobotControlComponent.cpp | 2 +- .../RobotControl/ROS2RobotControlComponent.h | 2 +- .../SDFormat/Hooks/ROS2CameraSensorHook.cpp | 22 ++++++---- .../SDFormat/Hooks/ROS2GNSSSensorHook.cpp | 11 +++-- .../SDFormat/Hooks/ROS2ImuSensorHook.cpp | 19 ++++++--- .../SDFormat/Hooks/ROS2LidarSensorHook.cpp | 17 ++++++-- .../SDFormat/ROS2SensorHooks.cpp | 14 +------ 17 files changed, 194 insertions(+), 53 deletions(-) diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp index dde3a7dfb..aa1b21521 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp @@ -30,8 +30,8 @@ namespace ROS2 ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent( const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration) - : m_sensorConfiguration(sensorConfiguration) - , m_cameraSensorConfiguration(cameraConfiguration) + : m_sensorConfiguration(AZStd::move(sensorConfiguration)) + , m_cameraSensorConfiguration(AZStd::move(cameraConfiguration)) { } @@ -121,6 +121,25 @@ namespace ROS2 return m_cameraSensorConfiguration.m_verticalFieldOfViewDeg; }; + const SensorConfiguration& ROS2CameraSensorEditorComponent::GetSensorConfiguration() const + { + return m_sensorConfiguration; + } + + void ROS2CameraSensorEditorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + + const CameraSensorConfiguration& ROS2CameraSensorEditorComponent::GetCameraSensorConfiguration() const + { + return m_cameraSensorConfiguration; + } + void ROS2CameraSensorEditorComponent::SetCameraSensorConfiguration(const CameraSensorConfiguration& cameraSensorConfiguration) + { + m_cameraSensorConfiguration = cameraSensorConfiguration; + } + void ROS2CameraSensorEditorComponent::DisplayEntityViewport( [[maybe_unused]] const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) { diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h index 787338502..f85ecf34e 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h @@ -51,6 +51,11 @@ namespace ROS2 int GetHeight() const override; float GetVerticalFOV() const override; + const SensorConfiguration& GetSensorConfiguration() const; + void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); + const CameraSensorConfiguration& GetCameraSensorConfiguration() const; + void SetCameraSensorConfiguration(const CameraSensorConfiguration& cameraSensorConfiguration); + private: // EntityDebugDisplayEventBus::Handler overrides void DisplayEntityViewport(const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) override; diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index 65f889c14..e79a7eac4 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -62,9 +62,9 @@ namespace ROS2 ROS2GNSSSensorComponent::ROS2GNSSSensorComponent( const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration) - : m_gnssConfiguration(gnssConfiguration) + : m_gnssConfiguration(AZStd::move(gnssConfiguration)) { - m_sensorConfiguration = sensorConfiguration; + m_sensorConfiguration = AZStd::move(sensorConfiguration); } void ROS2GNSSSensorComponent::Activate() @@ -86,6 +86,26 @@ namespace ROS2 m_gnssPublisher.reset(); } + const SensorConfiguration& ROS2GNSSSensorComponent::GetSensorConfiguration() const + { + return m_sensorConfiguration; + } + + void ROS2GNSSSensorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + + const GNSSSensorConfiguration& ROS2GNSSSensorComponent::GetGNSSSensorConfiguration() const + { + return m_gnssConfiguration; + } + + void ROS2GNSSSensorComponent::SetGNSSSensorConfiguration(const GNSSSensorConfiguration& gnssConfiguration) + { + m_gnssConfiguration = gnssConfiguration; + } + void ROS2GNSSSensorComponent::FrequencyTick() { const AZ::Vector3 currentPosition = GetCurrentPose().GetTranslation(); diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h index d13b6e0bd..38f7a9297 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h @@ -35,6 +35,11 @@ namespace ROS2 void Deactivate() override; ////////////////////////////////////////////////////////////////////////// + const SensorConfiguration& GetSensorConfiguration() const; + void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); + const GNSSSensorConfiguration& GetGNSSSensorConfiguration() const; + void SetGNSSSensorConfiguration(const GNSSSensorConfiguration& gnssConfiguration); + private: GNSSSensorConfiguration m_gnssConfiguration; diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp index 4e4714897..9d1716e4b 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -36,7 +36,7 @@ namespace ROS2 if (AZ::SerializeContext* serialize = azrtti_cast(context)) { serialize->Class()->Version(2)->Field( - "imuSensorConfiguration", &ROS2ImuSensorComponent::m_imuConfiguration); + "imuSensorConfiguration", &ROS2ImuSensorComponent::m_imuSensorConfiguration); if (AZ::EditContext* ec = serialize->GetEditContext()) { @@ -46,7 +46,7 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2ImuSensorComponent::m_imuConfiguration, + &ROS2ImuSensorComponent::m_imuSensorConfiguration, "Imu sensor configuration", "Imu sensor configuration") ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); @@ -66,9 +66,9 @@ namespace ROS2 ROS2ImuSensorComponent::ROS2ImuSensorComponent( const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration) - : m_imuConfiguration(imuConfiguration) + : m_imuSensorConfiguration(AZStd::move(imuConfiguration)) { - m_sensorConfiguration = sensorConfiguration; + m_sensorConfiguration = AZStd::move(sensorConfiguration); } void ROS2ImuSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) @@ -104,7 +104,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_imuConfiguration.m_filterSize) + if (m_filterAcceleration.size() > m_imuSensorConfiguration.m_filterSize) { m_filterAcceleration.pop_front(); m_filterAngularVelocity.pop_front(); @@ -122,7 +122,7 @@ namespace ROS2 m_previousLinearVelocity = linearVelocityFilter; m_acceleration = -acc + angularRateFiltered.Cross(linearVelocityFilter); - if (m_imuConfiguration.m_includeGravity) + if (m_imuSensorConfiguration.m_includeGravity) { m_acceleration += inv.TransformVector(gravity); } @@ -131,7 +131,7 @@ namespace ROS2 m_imuMsg.angular_velocity = ROS2Conversions::ToROS2Vector3(angularRateFiltered); m_imuMsg.angular_velocity_covariance = ROS2Conversions::ToROS2Covariance(m_angularVelocityCovariance); - if (m_imuConfiguration.m_absoluteRotation) + if (m_imuSensorConfiguration.m_absoluteRotation) { m_imuMsg.orientation = ROS2Conversions::ToROS2Quaternion(rigidbody->GetTransform().GetRotation()); m_imuMsg.orientation_covariance = ROS2Conversions::ToROS2Covariance(m_orientationCovariance); @@ -150,9 +150,9 @@ namespace ROS2 const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_imuPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_linearAccelerationVariance); - m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_angularVelocityVariance); - m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_orientationVariance); + m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuSensorConfiguration.m_linearAccelerationVariance); + m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuSensorConfiguration.m_angularVelocityVariance); + m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuSensorConfiguration.m_orientationVariance); ROS2SensorComponent::Activate(); } @@ -173,4 +173,24 @@ namespace ROS2 return covarianceMatrix; } + const SensorConfiguration& ROS2ImuSensorComponent::GetSensorConfiguration() const + { + return m_sensorConfiguration; + } + + void ROS2ImuSensorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + + const ImuSensorConfiguration& ROS2ImuSensorComponent::GetImuSensorConfiguration() const + { + return m_imuSensorConfiguration; + } + + void ROS2ImuSensorComponent::SetImuSensorConfiguration(const ImuSensorConfiguration& imuSensorConfiguration) + { + m_imuSensorConfiguration = imuSensorConfiguration; + } + } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h index 9ff8d761b..f0e42d71d 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h @@ -41,6 +41,11 @@ namespace ROS2 void Deactivate() override; ////////////////////////////////////////////////////////////////////////// + const SensorConfiguration& GetSensorConfiguration() const; + void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); + const ImuSensorConfiguration& GetImuSensorConfiguration() const; + void SetImuSensorConfiguration(const ImuSensorConfiguration& imuSensorConfiguration); + private: std::shared_ptr> m_imuPublisher; sensor_msgs::msg::Imu m_imuMsg; @@ -50,7 +55,7 @@ namespace ROS2 AZStd::deque m_filterAcceleration; AZStd::deque m_filterAngularVelocity; - ImuSensorConfiguration m_imuConfiguration; + ImuSensorConfiguration m_imuSensorConfiguration; AZ::Matrix3x3 m_orientationCovariance = AZ::Matrix3x3::CreateZero(); AZ::Matrix3x3 m_angularVelocityCovariance = AZ::Matrix3x3::CreateZero(); diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index 06b75be59..23831b67d 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -126,9 +126,9 @@ namespace ROS2 ROS2Lidar2DSensorComponent::ROS2Lidar2DSensorComponent( const SensorConfiguration& sensorConfiguration, const Lidar2DSensorConfiguration& lidarConfiguration) - : m_lidarConfiguration(lidarConfiguration) + : m_lidarConfiguration(AZStd::move(lidarConfiguration)) { - m_sensorConfiguration = sensorConfiguration; + m_sensorConfiguration = AZStd::move(sensorConfiguration); } void ROS2Lidar2DSensorComponent::Visualize() @@ -221,4 +221,23 @@ namespace ROS2 message.ranges.assign(m_lastScanResults.m_ranges.begin(), m_lastScanResults.m_ranges.end()); m_laserScanPublisher->publish(message); } + + const SensorConfiguration& ROS2Lidar2DSensorComponent::GetSensorConfiguration() const + { + return m_sensorConfiguration; + } + void ROS2Lidar2DSensorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + + const Lidar2DSensorConfiguration& ROS2Lidar2DSensorComponent::GetLidarSensorConfiguration() const + { + return m_lidarConfiguration; + } + + void ROS2Lidar2DSensorComponent::SetLidarSensorConfiguration(const Lidar2DSensorConfiguration& lidarSensorConfiguration) + { + m_lidarConfiguration = lidarSensorConfiguration; + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h index 97b954201..e4c919215 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h @@ -38,6 +38,11 @@ namespace ROS2 void Deactivate() override; ////////////////////////////////////////////////////////////////////////// + const SensorConfiguration& GetSensorConfiguration() const; + void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); + const Lidar2DSensorConfiguration& GetLidarSensorConfiguration() const; + void SetLidarSensorConfiguration(const Lidar2DSensorConfiguration& lidarSensorConfiguration); + private: ////////////////////////////////////////////////////////////////////////// // ROS2SensorComponent overrides diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index ca04e4309..b99e60dbc 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -134,9 +134,9 @@ namespace ROS2 ROS2LidarSensorComponent::ROS2LidarSensorComponent( const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration) - : m_lidarConfiguration(lidarConfiguration) + : m_lidarConfiguration(AZStd::move(lidarConfiguration)) { - m_sensorConfiguration = sensorConfiguration; + m_sensorConfiguration = AZStd::move(sensorConfiguration); } void ROS2LidarSensorComponent::Visualize() @@ -270,4 +270,23 @@ namespace ROS2 memcpy(message.data.data(), m_lastScanResults.m_points.data(), sizeInBytes); m_pointCloudPublisher->publish(message); } + + const SensorConfiguration& ROS2LidarSensorComponent::GetSensorConfiguration() const + { + return m_sensorConfiguration; + } + void ROS2LidarSensorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + + const LidarSensorConfiguration& ROS2LidarSensorComponent::GetLidarSensorConfiguration() const + { + return m_lidarConfiguration; + } + + void ROS2LidarSensorComponent::SetLidarSensorConfiguration(const LidarSensorConfiguration& lidarSensorConfiguration) + { + m_lidarConfiguration = lidarSensorConfiguration; + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h index f441667e2..69d5bf413 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h @@ -38,6 +38,11 @@ namespace ROS2 void Deactivate() override; ////////////////////////////////////////////////////////////////////////// + const SensorConfiguration& GetSensorConfiguration() const; + void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); + const LidarSensorConfiguration& GetLidarSensorConfiguration() const; + void SetLidarSensorConfiguration(const LidarSensorConfiguration& lidarSensorConfiguration); + private: ////////////////////////////////////////////////////////////////////////// // ROS2SensorComponent overrides 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/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index 2edb3342f..fe40084e4 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -16,7 +17,6 @@ namespace ROS2::SDFormat { - SensorImporterHook ROS2SensorHooks::ROS2CameraSensor() { SensorImporterHook importerHook; @@ -59,32 +59,40 @@ namespace ROS2::SDFormat SensorConfiguration sensorConfiguration; sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) - { + { // COLOR_CAMERA and RGBD_CAMERA Utils::AddTopicConfiguration( sensorConfiguration, "camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig); Utils::AddTopicConfiguration( sensorConfiguration, "color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig); } if (sdfSensor.Type() != sdf::SensorType::CAMERA) - { + { // DEPTH_CAMERA and RGBD_CAMERA Utils::AddTopicConfiguration( sensorConfiguration, "camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig); Utils::AddTopicConfiguration( sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); } - if (entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, cameraConfiguration)) + const auto& entityId = entity.GetId(); + if (!ROS2::Utils::CreateComponent(entityId, ROS2FrameComponent::TYPEINFO_Uuid())) { - return AZ::Success(); + return AZ::Failure(AZStd::string("Failed to create ROS2FrameComponent required for ROS2 Camera Sensor component")); + } + const auto componentId = ROS2::Utils::CreateComponent(entityId, ROS2CameraSensorEditorComponent::TYPEINFO_Uuid()); + if (componentId) + { + auto* component = ROS2::Utils::GetGameOrEditorComponent(&entity); + component->SetSensorConfiguration(sensorConfiguration); + component->SetCameraSensorConfiguration(cameraConfiguration); } else { return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component")); } + + return AZ::Success(); }; 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 index f7338786e..34fefc430 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp @@ -7,6 +7,7 @@ */ #include +#include #include #include @@ -34,17 +35,21 @@ namespace ROS2::SDFormat const AZStd::string messageType = "sensor_msgs::msg::NavSatFix"; Utils::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType); - if (entity.CreateComponent(sensorConfiguration, GNSSSensorConfiguration())) + const auto& entityId = entity.GetId(); + const auto componentId = ROS2::Utils::CreateComponent(entityId, ROS2GNSSSensorComponent::TYPEINFO_Uuid()); + if (componentId) { - return AZ::Success(); + auto* component = ROS2::Utils::GetGameOrEditorComponent(&entity); + component->SetSensorConfiguration(sensorConfiguration); } else { return AZ::Failure(AZStd::string("Failed to create ROS2 GNSS Sensor component")); } + + return AZ::Success(); }; 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 index 9c95d621d..d66f46334 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -16,7 +17,6 @@ namespace ROS2::SDFormat { - SensorImporterHook ROS2SensorHooks::ROS2ImuSensor() { SensorImporterHook importerHook; @@ -81,18 +81,27 @@ namespace ROS2::SDFormat const AZStd::string messageType = "sensor_msgs::msg::Imu"; Utils::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); - if (entity.CreateComponent() && entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, imuConfiguration)) + const auto& entityId = entity.GetId(); + if (!ROS2::Utils::CreateComponent(entityId, PhysX::EditorStaticRigidBodyComponent::TYPEINFO_Uuid()) || + !ROS2::Utils::CreateComponent(entityId, ROS2FrameComponent::TYPEINFO_Uuid())) + { + return AZ::Failure(AZStd::string("Failed to create components required for ROS2 Imu Sensor component")); + } + const auto componentId = ROS2::Utils::CreateComponent(entityId, ROS2ImuSensorComponent::TYPEINFO_Uuid()); + if (componentId) { - return AZ::Success(); + auto* component = ROS2::Utils::GetGameOrEditorComponent(&entity); + component->SetSensorConfiguration(sensorConfiguration); + component->SetImuSensorConfiguration(imuConfiguration); } else { return AZ::Failure(AZStd::string("Failed to create ROS2 Imu Sensor component")); } + + return AZ::Success(); }; 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 index 09dfd96ef..ee3770912 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -67,18 +68,26 @@ namespace ROS2::SDFormat lidarConfiguration.m_lidarParameters.m_minRange = lidarSensor->RangeMin(); lidarConfiguration.m_lidarParameters.m_maxRange = lidarSensor->RangeMax(); - if (entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, lidarConfiguration)) + const auto& entityId = entity.GetId(); + if (!ROS2::Utils::CreateComponent(entityId, ROS2FrameComponent::TYPEINFO_Uuid())) { - return AZ::Success(); + return AZ::Failure(AZStd::string("Failed to create ROS2FrameComponent required for ROS2 Lidar Sensor component")); + } + const auto componentId = ROS2::Utils::CreateComponent(entityId, ROS2LidarSensorComponent::TYPEINFO_Uuid()); + if (componentId) + { + auto* component = ROS2::Utils::GetGameOrEditorComponent(&entity); + component->SetSensorConfiguration(sensorConfiguration); + component->SetLidarSensorConfiguration(lidarConfiguration); } else { return AZ::Failure(AZStd::string("Failed to create ROS2 Lidar Sensor component")); } + + return AZ::Success(); }; return importerHook; } - } // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp index 9f6ff7137..39faba79b 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp @@ -7,18 +7,7 @@ */ #include "ROS2SensorHooks.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include namespace ROS2::SDFormat { @@ -36,5 +25,4 @@ namespace ROS2::SDFormat sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config)); } } // namespace ROS2SensorHooks - } // namespace ROS2::SDFormat From 665d12da7c864099d7d19c82bb2b50b3a1d41f92 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 22 Aug 2023 11:23:37 +0200 Subject: [PATCH 08/15] revert AZStd::move addition Signed-off-by: Jan Hanca --- .../Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp | 4 ++-- Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp | 4 ++-- Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp | 4 ++-- Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp | 4 ++-- Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp index aa1b21521..d90937558 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp @@ -30,8 +30,8 @@ namespace ROS2 ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent( const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration) - : m_sensorConfiguration(AZStd::move(sensorConfiguration)) - , m_cameraSensorConfiguration(AZStd::move(cameraConfiguration)) + : m_sensorConfiguration(sensorConfiguration) + , m_cameraSensorConfiguration(cameraConfiguration) { } diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index e79a7eac4..e050d39ba 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -62,9 +62,9 @@ namespace ROS2 ROS2GNSSSensorComponent::ROS2GNSSSensorComponent( const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration) - : m_gnssConfiguration(AZStd::move(gnssConfiguration)) + : m_gnssConfiguration(gnssConfiguration) { - m_sensorConfiguration = AZStd::move(sensorConfiguration); + m_sensorConfiguration = sensorConfiguration; } void ROS2GNSSSensorComponent::Activate() diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp index 9d1716e4b..713e4be6c 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -66,9 +66,9 @@ namespace ROS2 ROS2ImuSensorComponent::ROS2ImuSensorComponent( const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration) - : m_imuSensorConfiguration(AZStd::move(imuConfiguration)) + : m_imuSensorConfiguration(imuConfiguration) { - m_sensorConfiguration = AZStd::move(sensorConfiguration); + m_sensorConfiguration = sensorConfiguration; } void ROS2ImuSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index 23831b67d..c58ad5975 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -126,9 +126,9 @@ namespace ROS2 ROS2Lidar2DSensorComponent::ROS2Lidar2DSensorComponent( const SensorConfiguration& sensorConfiguration, const Lidar2DSensorConfiguration& lidarConfiguration) - : m_lidarConfiguration(AZStd::move(lidarConfiguration)) + : m_lidarConfiguration(lidarConfiguration) { - m_sensorConfiguration = AZStd::move(sensorConfiguration); + m_sensorConfiguration = sensorConfiguration; } void ROS2Lidar2DSensorComponent::Visualize() diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index b99e60dbc..7a2595ea8 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -134,9 +134,9 @@ namespace ROS2 ROS2LidarSensorComponent::ROS2LidarSensorComponent( const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration) - : m_lidarConfiguration(AZStd::move(lidarConfiguration)) + : m_lidarConfiguration(lidarConfiguration) { - m_sensorConfiguration = AZStd::move(sensorConfiguration); + m_sensorConfiguration = sensorConfiguration; } void ROS2LidarSensorComponent::Visualize() From 2fec1ee640b8a6ddeaea2d862dda755c04bb5b0d Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 22 Aug 2023 11:49:52 +0200 Subject: [PATCH 09/15] add ROS2SensorHooksUtils Signed-off-by: Jan Hanca --- .../SDFormat/Hooks/ROS2CameraSensorHook.cpp | 1 + .../SDFormat/Hooks/ROS2GNSSSensorHook.cpp | 1 + .../SDFormat/Hooks/ROS2ImuSensorHook.cpp | 1 + .../SDFormat/Hooks/ROS2LidarSensorHook.cpp | 1 + .../RobotImporter/SDFormat/ROS2SensorHooks.h | 16 ---------- ...nsorHooks.cpp => ROS2SensorHooksUtils.cpp} | 2 +- .../SDFormat/ROS2SensorHooksUtils.h | 32 +++++++++++++++++++ Gems/ROS2/Code/ros2_editor_files.cmake | 3 +- 8 files changed, 39 insertions(+), 18 deletions(-) rename Gems/ROS2/Code/Source/RobotImporter/SDFormat/{ROS2SensorHooks.cpp => ROS2SensorHooksUtils.cpp} (96%) create mode 100644 Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index fe40084e4..05f33edc8 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp index 34fefc430..9ce070005 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp index d66f46334..000d9f0d5 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp index ee3770912..f826b5e20 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h index 37772c4c5..2ba4d2636 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h @@ -8,28 +8,12 @@ #pragma once -#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); - } // namespace Utils - //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type camera, depth or rgbd into O3DE //! ROS2CameraSensorComponent SensorImporterHook ROS2CameraSensor(); diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp similarity index 96% rename from Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp rename to Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp index 39faba79b..004a5755a 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp @@ -6,7 +6,7 @@ * */ -#include "ROS2SensorHooks.h" +#include "ROS2SensorHooksUtils.h" #include 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..9adc2176e --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h @@ -0,0 +1,32 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include +#include + +namespace ROS2::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); + } // namespace Utils + } // namespace ROS2SensorHooks +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/ros2_editor_files.cmake b/Gems/ROS2/Code/ros2_editor_files.cmake index a9ed00672..2314f02fb 100644 --- a/Gems/ROS2/Code/ros2_editor_files.cmake +++ b/Gems/ROS2/Code/ros2_editor_files.cmake @@ -32,7 +32,8 @@ set(FILES Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp - Source/RobotImporter/SDFormat/ROS2SensorHooks.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 From 21b836aed81b4048e891fe6df77d8fff9311d0d5 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Wed, 23 Aug 2023 12:54:11 +0200 Subject: [PATCH 10/15] remove get/set methods; revert component create method Signed-off-by: Jan Hanca --- .../ROS2CameraSensorEditorComponent.cpp | 19 ---------- .../Camera/ROS2CameraSensorEditorComponent.h | 5 --- .../Source/GNSS/ROS2GNSSSensorComponent.cpp | 20 ---------- .../Source/GNSS/ROS2GNSSSensorComponent.h | 5 --- .../Source/Imu/ROS2ImuSensorComponent.cpp | 38 +++++-------------- .../Code/Source/Imu/ROS2ImuSensorComponent.h | 7 +--- .../Lidar/ROS2Lidar2DSensorComponent.cpp | 19 ---------- .../Source/Lidar/ROS2Lidar2DSensorComponent.h | 5 --- .../Source/Lidar/ROS2LidarSensorComponent.cpp | 19 ---------- .../Source/Lidar/ROS2LidarSensorComponent.h | 5 --- .../SDFormat/Hooks/ROS2CameraSensorHook.cpp | 16 ++------ .../SDFormat/Hooks/ROS2GNSSSensorHook.cpp | 10 +---- .../SDFormat/Hooks/ROS2ImuSensorHook.cpp | 17 ++------- .../SDFormat/Hooks/ROS2LidarSensorHook.cpp | 16 ++------ 14 files changed, 21 insertions(+), 180 deletions(-) diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp index d90937558..dde3a7dfb 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp @@ -121,25 +121,6 @@ namespace ROS2 return m_cameraSensorConfiguration.m_verticalFieldOfViewDeg; }; - const SensorConfiguration& ROS2CameraSensorEditorComponent::GetSensorConfiguration() const - { - return m_sensorConfiguration; - } - - void ROS2CameraSensorEditorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) - { - m_sensorConfiguration = sensorConfiguration; - } - - const CameraSensorConfiguration& ROS2CameraSensorEditorComponent::GetCameraSensorConfiguration() const - { - return m_cameraSensorConfiguration; - } - void ROS2CameraSensorEditorComponent::SetCameraSensorConfiguration(const CameraSensorConfiguration& cameraSensorConfiguration) - { - m_cameraSensorConfiguration = cameraSensorConfiguration; - } - void ROS2CameraSensorEditorComponent::DisplayEntityViewport( [[maybe_unused]] const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) { diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h index f85ecf34e..787338502 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h @@ -51,11 +51,6 @@ namespace ROS2 int GetHeight() const override; float GetVerticalFOV() const override; - const SensorConfiguration& GetSensorConfiguration() const; - void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); - const CameraSensorConfiguration& GetCameraSensorConfiguration() const; - void SetCameraSensorConfiguration(const CameraSensorConfiguration& cameraSensorConfiguration); - private: // EntityDebugDisplayEventBus::Handler overrides void DisplayEntityViewport(const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) override; diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index e050d39ba..65f889c14 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -86,26 +86,6 @@ namespace ROS2 m_gnssPublisher.reset(); } - const SensorConfiguration& ROS2GNSSSensorComponent::GetSensorConfiguration() const - { - return m_sensorConfiguration; - } - - void ROS2GNSSSensorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) - { - m_sensorConfiguration = sensorConfiguration; - } - - const GNSSSensorConfiguration& ROS2GNSSSensorComponent::GetGNSSSensorConfiguration() const - { - return m_gnssConfiguration; - } - - void ROS2GNSSSensorComponent::SetGNSSSensorConfiguration(const GNSSSensorConfiguration& gnssConfiguration) - { - m_gnssConfiguration = gnssConfiguration; - } - void ROS2GNSSSensorComponent::FrequencyTick() { const AZ::Vector3 currentPosition = GetCurrentPose().GetTranslation(); diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h index 38f7a9297..d13b6e0bd 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h @@ -35,11 +35,6 @@ namespace ROS2 void Deactivate() override; ////////////////////////////////////////////////////////////////////////// - const SensorConfiguration& GetSensorConfiguration() const; - void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); - const GNSSSensorConfiguration& GetGNSSSensorConfiguration() const; - void SetGNSSSensorConfiguration(const GNSSSensorConfiguration& gnssConfiguration); - private: GNSSSensorConfiguration m_gnssConfiguration; diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp index 713e4be6c..4e4714897 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -36,7 +36,7 @@ namespace ROS2 if (AZ::SerializeContext* serialize = azrtti_cast(context)) { serialize->Class()->Version(2)->Field( - "imuSensorConfiguration", &ROS2ImuSensorComponent::m_imuSensorConfiguration); + "imuSensorConfiguration", &ROS2ImuSensorComponent::m_imuConfiguration); if (AZ::EditContext* ec = serialize->GetEditContext()) { @@ -46,7 +46,7 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2ImuSensorComponent::m_imuSensorConfiguration, + &ROS2ImuSensorComponent::m_imuConfiguration, "Imu sensor configuration", "Imu sensor configuration") ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); @@ -66,7 +66,7 @@ namespace ROS2 ROS2ImuSensorComponent::ROS2ImuSensorComponent( const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration) - : m_imuSensorConfiguration(imuConfiguration) + : m_imuConfiguration(imuConfiguration) { m_sensorConfiguration = sensorConfiguration; } @@ -104,7 +104,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_imuSensorConfiguration.m_filterSize) + if (m_filterAcceleration.size() > m_imuConfiguration.m_filterSize) { m_filterAcceleration.pop_front(); m_filterAngularVelocity.pop_front(); @@ -122,7 +122,7 @@ namespace ROS2 m_previousLinearVelocity = linearVelocityFilter; m_acceleration = -acc + angularRateFiltered.Cross(linearVelocityFilter); - if (m_imuSensorConfiguration.m_includeGravity) + if (m_imuConfiguration.m_includeGravity) { m_acceleration += inv.TransformVector(gravity); } @@ -131,7 +131,7 @@ namespace ROS2 m_imuMsg.angular_velocity = ROS2Conversions::ToROS2Vector3(angularRateFiltered); m_imuMsg.angular_velocity_covariance = ROS2Conversions::ToROS2Covariance(m_angularVelocityCovariance); - if (m_imuSensorConfiguration.m_absoluteRotation) + if (m_imuConfiguration.m_absoluteRotation) { m_imuMsg.orientation = ROS2Conversions::ToROS2Quaternion(rigidbody->GetTransform().GetRotation()); m_imuMsg.orientation_covariance = ROS2Conversions::ToROS2Covariance(m_orientationCovariance); @@ -150,9 +150,9 @@ namespace ROS2 const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_imuPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuSensorConfiguration.m_linearAccelerationVariance); - m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuSensorConfiguration.m_angularVelocityVariance); - m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuSensorConfiguration.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(); } @@ -173,24 +173,4 @@ namespace ROS2 return covarianceMatrix; } - const SensorConfiguration& ROS2ImuSensorComponent::GetSensorConfiguration() const - { - return m_sensorConfiguration; - } - - void ROS2ImuSensorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) - { - m_sensorConfiguration = sensorConfiguration; - } - - const ImuSensorConfiguration& ROS2ImuSensorComponent::GetImuSensorConfiguration() const - { - return m_imuSensorConfiguration; - } - - void ROS2ImuSensorComponent::SetImuSensorConfiguration(const ImuSensorConfiguration& imuSensorConfiguration) - { - m_imuSensorConfiguration = imuSensorConfiguration; - } - } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h index f0e42d71d..9ff8d761b 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h @@ -41,11 +41,6 @@ namespace ROS2 void Deactivate() override; ////////////////////////////////////////////////////////////////////////// - const SensorConfiguration& GetSensorConfiguration() const; - void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); - const ImuSensorConfiguration& GetImuSensorConfiguration() const; - void SetImuSensorConfiguration(const ImuSensorConfiguration& imuSensorConfiguration); - private: std::shared_ptr> m_imuPublisher; sensor_msgs::msg::Imu m_imuMsg; @@ -55,7 +50,7 @@ namespace ROS2 AZStd::deque m_filterAcceleration; AZStd::deque m_filterAngularVelocity; - ImuSensorConfiguration m_imuSensorConfiguration; + 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/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index c58ad5975..06b75be59 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -221,23 +221,4 @@ namespace ROS2 message.ranges.assign(m_lastScanResults.m_ranges.begin(), m_lastScanResults.m_ranges.end()); m_laserScanPublisher->publish(message); } - - const SensorConfiguration& ROS2Lidar2DSensorComponent::GetSensorConfiguration() const - { - return m_sensorConfiguration; - } - void ROS2Lidar2DSensorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) - { - m_sensorConfiguration = sensorConfiguration; - } - - const Lidar2DSensorConfiguration& ROS2Lidar2DSensorComponent::GetLidarSensorConfiguration() const - { - return m_lidarConfiguration; - } - - void ROS2Lidar2DSensorComponent::SetLidarSensorConfiguration(const Lidar2DSensorConfiguration& lidarSensorConfiguration) - { - m_lidarConfiguration = lidarSensorConfiguration; - } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h index e4c919215..97b954201 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h @@ -38,11 +38,6 @@ namespace ROS2 void Deactivate() override; ////////////////////////////////////////////////////////////////////////// - const SensorConfiguration& GetSensorConfiguration() const; - void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); - const Lidar2DSensorConfiguration& GetLidarSensorConfiguration() const; - void SetLidarSensorConfiguration(const Lidar2DSensorConfiguration& lidarSensorConfiguration); - private: ////////////////////////////////////////////////////////////////////////// // ROS2SensorComponent overrides diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index 7a2595ea8..ca04e4309 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -270,23 +270,4 @@ namespace ROS2 memcpy(message.data.data(), m_lastScanResults.m_points.data(), sizeInBytes); m_pointCloudPublisher->publish(message); } - - const SensorConfiguration& ROS2LidarSensorComponent::GetSensorConfiguration() const - { - return m_sensorConfiguration; - } - void ROS2LidarSensorComponent::SetSensorConfiguration(const SensorConfiguration& sensorConfiguration) - { - m_sensorConfiguration = sensorConfiguration; - } - - const LidarSensorConfiguration& ROS2LidarSensorComponent::GetLidarSensorConfiguration() const - { - return m_lidarConfiguration; - } - - void ROS2LidarSensorComponent::SetLidarSensorConfiguration(const LidarSensorConfiguration& lidarSensorConfiguration) - { - m_lidarConfiguration = lidarSensorConfiguration; - } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h index 69d5bf413..f441667e2 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h @@ -38,11 +38,6 @@ namespace ROS2 void Deactivate() override; ////////////////////////////////////////////////////////////////////////// - const SensorConfiguration& GetSensorConfiguration() const; - void SetSensorConfiguration(const SensorConfiguration& sensorConfiguration); - const LidarSensorConfiguration& GetLidarSensorConfiguration() const; - void SetLidarSensorConfiguration(const LidarSensorConfiguration& lidarSensorConfiguration); - private: ////////////////////////////////////////////////////////////////////////// // ROS2SensorComponent overrides diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index 05f33edc8..2cdd00fee 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include @@ -74,24 +73,15 @@ namespace ROS2::SDFormat sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); } - const auto& entityId = entity.GetId(); - if (!ROS2::Utils::CreateComponent(entityId, ROS2FrameComponent::TYPEINFO_Uuid())) + if (entity.CreateComponent() && + entity.CreateComponent(sensorConfiguration, cameraConfiguration)) { - return AZ::Failure(AZStd::string("Failed to create ROS2FrameComponent required for ROS2 Camera Sensor component")); - } - const auto componentId = ROS2::Utils::CreateComponent(entityId, ROS2CameraSensorEditorComponent::TYPEINFO_Uuid()); - if (componentId) - { - auto* component = ROS2::Utils::GetGameOrEditorComponent(&entity); - component->SetSensorConfiguration(sensorConfiguration); - component->SetCameraSensorConfiguration(cameraConfiguration); + return AZ::Success(); } else { return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component")); } - - return AZ::Success(); }; return importerHook; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp index 9ce070005..adca9ebc2 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp @@ -7,7 +7,6 @@ */ #include -#include #include #include @@ -36,19 +35,14 @@ namespace ROS2::SDFormat const AZStd::string messageType = "sensor_msgs::msg::NavSatFix"; Utils::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType); - const auto& entityId = entity.GetId(); - const auto componentId = ROS2::Utils::CreateComponent(entityId, ROS2GNSSSensorComponent::TYPEINFO_Uuid()); - if (componentId) + if (entity.CreateComponent(sensorConfiguration, GNSSSensorConfiguration())) { - auto* component = ROS2::Utils::GetGameOrEditorComponent(&entity); - component->SetSensorConfiguration(sensorConfiguration); + return AZ::Success(); } else { return AZ::Failure(AZStd::string("Failed to create ROS2 GNSS Sensor component")); } - - return AZ::Success(); }; return importerHook; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp index 000d9f0d5..0a7b41ced 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include #include @@ -82,25 +81,15 @@ namespace ROS2::SDFormat const AZStd::string messageType = "sensor_msgs::msg::Imu"; Utils::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); - const auto& entityId = entity.GetId(); - if (!ROS2::Utils::CreateComponent(entityId, PhysX::EditorStaticRigidBodyComponent::TYPEINFO_Uuid()) || - !ROS2::Utils::CreateComponent(entityId, ROS2FrameComponent::TYPEINFO_Uuid())) + if (entity.CreateComponent() && entity.CreateComponent() && + entity.CreateComponent(sensorConfiguration, imuConfiguration)) { - return AZ::Failure(AZStd::string("Failed to create components required for ROS2 Imu Sensor component")); - } - const auto componentId = ROS2::Utils::CreateComponent(entityId, ROS2ImuSensorComponent::TYPEINFO_Uuid()); - if (componentId) - { - auto* component = ROS2::Utils::GetGameOrEditorComponent(&entity); - component->SetSensorConfiguration(sensorConfiguration); - component->SetImuSensorConfiguration(imuConfiguration); + return AZ::Success(); } else { return AZ::Failure(AZStd::string("Failed to create ROS2 Imu Sensor component")); } - - return AZ::Success(); }; return importerHook; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp index f826b5e20..ebc0637ca 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include @@ -69,24 +68,15 @@ namespace ROS2::SDFormat lidarConfiguration.m_lidarParameters.m_minRange = lidarSensor->RangeMin(); lidarConfiguration.m_lidarParameters.m_maxRange = lidarSensor->RangeMax(); - const auto& entityId = entity.GetId(); - if (!ROS2::Utils::CreateComponent(entityId, ROS2FrameComponent::TYPEINFO_Uuid())) + if (entity.CreateComponent() && + entity.CreateComponent(sensorConfiguration, lidarConfiguration)) { - return AZ::Failure(AZStd::string("Failed to create ROS2FrameComponent required for ROS2 Lidar Sensor component")); - } - const auto componentId = ROS2::Utils::CreateComponent(entityId, ROS2LidarSensorComponent::TYPEINFO_Uuid()); - if (componentId) - { - auto* component = ROS2::Utils::GetGameOrEditorComponent(&entity); - component->SetSensorConfiguration(sensorConfiguration); - component->SetLidarSensorConfiguration(lidarConfiguration); + return AZ::Success(); } else { return AZ::Failure(AZStd::string("Failed to create ROS2 Lidar Sensor component")); } - - return AZ::Success(); }; return importerHook; From 080bfdf6c099a5bbfbc149fd1612ea6f8e0f3914 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Wed, 23 Aug 2023 16:20:17 +0200 Subject: [PATCH 11/15] add custom CreateComponent method to use with hooks Signed-off-by: Jan Hanca --- .../SDFormat/Hooks/ROS2CameraSensorHook.cpp | 4 +- .../SDFormat/Hooks/ROS2GNSSSensorHook.cpp | 2 +- .../SDFormat/Hooks/ROS2ImuSensorHook.cpp | 5 ++- .../SDFormat/Hooks/ROS2LidarSensorHook.cpp | 4 +- .../SDFormat/ROS2SensorHooksUtils.h | 40 +++++++++++++++++++ 5 files changed, 48 insertions(+), 7 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index 2cdd00fee..bbe5fc619 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -73,8 +73,8 @@ namespace ROS2::SDFormat sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); } - if (entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, cameraConfiguration)) + if (Utils::CreateComponent(entity) && + Utils::CreateComponent(entity, sensorConfiguration, cameraConfiguration)) { return AZ::Success(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp index adca9ebc2..0a8485f1a 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp @@ -35,7 +35,7 @@ namespace ROS2::SDFormat const AZStd::string messageType = "sensor_msgs::msg::NavSatFix"; Utils::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType); - if (entity.CreateComponent(sensorConfiguration, GNSSSensorConfiguration())) + if (Utils::CreateComponent(entity, sensorConfiguration, GNSSSensorConfiguration())) { return AZ::Success(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp index 0a7b41ced..4835ee018 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -81,8 +81,9 @@ namespace ROS2::SDFormat const AZStd::string messageType = "sensor_msgs::msg::Imu"; Utils::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); - if (entity.CreateComponent() && entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, imuConfiguration)) + if (Utils::CreateComponent(entity) && + Utils::CreateComponent(entity) && + Utils::CreateComponent(entity, sensorConfiguration, imuConfiguration)) { return AZ::Success(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp index ebc0637ca..d4353d5cc 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -68,8 +68,8 @@ namespace ROS2::SDFormat lidarConfiguration.m_lidarParameters.m_minRange = lidarSensor->RangeMin(); lidarConfiguration.m_lidarParameters.m_maxRange = lidarSensor->RangeMax(); - if (entity.CreateComponent() && - entity.CreateComponent(sensorConfiguration, lidarConfiguration)) + if (Utils::CreateComponent(entity) && + Utils::CreateComponent(entity, sensorConfiguration, lidarConfiguration)) { return AZ::Success(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h index 9adc2176e..368d7fdfd 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h @@ -8,7 +8,13 @@ #pragma once +#include +#include +#include +#include #include +#include +#include #include namespace ROS2::SDFormat @@ -27,6 +33,40 @@ namespace ROS2::SDFormat 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) + { + // 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.AddComponent(component)) + { + delete component; + component = nullptr; + } + } + return component; + } } // namespace Utils } // namespace ROS2SensorHooks } // namespace ROS2::SDFormat From 0a85c2e11b29df6e31a8f1ebed06cb484b513f06 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 29 Aug 2023 15:03:57 +0200 Subject: [PATCH 12/15] do not create duplicate components; check for required components Signed-off-by: Jan Hanca --- .../SDFormat/Hooks/ROS2CameraSensorHook.cpp | 7 +++++-- .../RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp | 9 ++++++--- .../RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp | 7 +++++-- .../Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h | 8 +++++++- 4 files changed, 23 insertions(+), 8 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index bbe5fc619..3d400adae 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -73,8 +73,11 @@ namespace ROS2::SDFormat sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); } - if (Utils::CreateComponent(entity) && - Utils::CreateComponent(entity, sensorConfiguration, cameraConfiguration)) + // Create required components + Utils::CreateComponent(entity); + + // Create Camera component + if (Utils::CreateComponent(entity, sensorConfiguration, cameraConfiguration)) { return AZ::Success(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp index 4835ee018..aa135ab2d 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -81,9 +81,12 @@ namespace ROS2::SDFormat const AZStd::string messageType = "sensor_msgs::msg::Imu"; Utils::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); - if (Utils::CreateComponent(entity) && - Utils::CreateComponent(entity) && - Utils::CreateComponent(entity, sensorConfiguration, imuConfiguration)) + // Create required components + Utils::CreateComponent(entity); + Utils::CreateComponent(entity); + + // Create Imu component + if (Utils::CreateComponent(entity, sensorConfiguration, imuConfiguration)) { return AZ::Success(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp index d4353d5cc..fdd09cc64 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -68,8 +68,11 @@ namespace ROS2::SDFormat lidarConfiguration.m_lidarParameters.m_minRange = lidarSensor->RangeMin(); lidarConfiguration.m_lidarParameters.m_maxRange = lidarSensor->RangeMax(); - if (Utils::CreateComponent(entity) && - Utils::CreateComponent(entity, sensorConfiguration, lidarConfiguration)) + // Create required components + Utils::CreateComponent(entity); + + // Create Lidar component + if (Utils::CreateComponent(entity, sensorConfiguration, lidarConfiguration)) { return AZ::Success(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h index 368d7fdfd..a11aa414c 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h @@ -42,6 +42,12 @@ namespace ROS2::SDFormat 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; @@ -59,7 +65,7 @@ namespace ROS2::SDFormat if (component) { - if (!entity.AddComponent(component)) + if (!entity.IsComponentReadyToAdd(component) || !entity.AddComponent(component)) { delete component; component = nullptr; From a47a65b47189159c05ebb89507af5ff981397a1e Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 29 Aug 2023 16:01:31 +0200 Subject: [PATCH 13/15] fix camera field of view Signed-off-by: Jan Hanca --- .../RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index 3d400adae..25c00b2a3 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -44,7 +44,7 @@ namespace ROS2::SDFormat cameraConfiguration.m_width = cameraSensor->ImageWidth(); cameraConfiguration.m_height = cameraSensor->ImageHeight(); cameraConfiguration.m_verticalFieldOfViewDeg = - cameraSensor->HorizontalFov().Degree() * (cameraConfiguration.m_height / cameraConfiguration.m_width); + cameraSensor->HorizontalFov().Degree() * cameraConfiguration.m_height / cameraConfiguration.m_width; if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) { cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->NearClip()); From f641d5f4c17502c63756344775eb312bcff0f63e Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Wed, 30 Aug 2023 17:01:45 +0200 Subject: [PATCH 14/15] review comment: possible division by zero Signed-off-by: Jan Hanca --- .../RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index 25c00b2a3..b7fb4e186 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -43,8 +43,11 @@ namespace ROS2::SDFormat 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) + { + cameraConfiguration.m_verticalFieldOfViewDeg = + cameraSensor->HorizontalFov().Degree() * cameraConfiguration.m_height / cameraConfiguration.m_width; + } if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) { cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->NearClip()); From 0a903289ef08d79880c59dd08967073a6f10acf6 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Thu, 31 Aug 2023 16:25:42 +0200 Subject: [PATCH 15/15] redo aspect ratio calculation Signed-off-by: Jan Hanca --- .../RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index b7fb4e186..04f9c7929 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -45,8 +45,9 @@ namespace ROS2::SDFormat cameraConfiguration.m_height = cameraSensor->ImageHeight(); if (cameraConfiguration.m_width != 0) { + double aspectRatio = static_cast(cameraConfiguration.m_height) / cameraConfiguration.m_width; cameraConfiguration.m_verticalFieldOfViewDeg = - cameraSensor->HorizontalFov().Degree() * cameraConfiguration.m_height / cameraConfiguration.m_width; + 2.0 * AZStd::atan(AZStd::tan(cameraSensor->HorizontalFov().Radian() / 2.0) * aspectRatio); } if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) {