diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt index ec997fb71..fb6a4300e 100644 --- a/Gems/ROS2/Code/CMakeLists.txt +++ b/Gems/ROS2/Code/CMakeLists.txt @@ -165,8 +165,8 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS) ) # By default, we will specify that the above target ROS2 would be used by - # Tool and Builder type targets when this gem is enabled. If you don't want it - # active in Tools or Builders by default, delete one of both of the following lines: + # Tool and Builder type targets when this gem is enabled. If you don't want it + # active in Tools or Builders by default, delete one or both of the following lines: ly_create_alias(NAME ${gem_name}.Tools NAMESPACE Gem TARGETS Gem::${gem_name}.Editor) ly_create_alias(NAME ${gem_name}.Builders NAMESPACE Gem TARGETS Gem::${gem_name}.Editor) endif() @@ -218,6 +218,7 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) PRIVATE AZ::AzTest Gem::${gem_name}.Editor + 3rdParty::sdformat ) # Add ROS2.Editor.Tests to googletest diff --git a/Gems/ROS2/Code/Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h b/Gems/ROS2/Code/Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h new file mode 100644 index 000000000..06de97f7d --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h @@ -0,0 +1,59 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace sdf +{ + inline namespace v13 + { + enum class SensorType; + class Sensor; + } // namespace v13 +} // namespace sdf + +namespace ROS2::SDFormat +{ + //! Hook used in ROS2 Gem Robot Importer to create ROS2 sensor components based on SDFormat model description. + //! It implements the parameters mapping between the SDFormat sensor and the particular O3DE component. + //! It should be registered as a serialization attribute in the component's reflection to make the it visible in the SDFormat model + //! parser. See the sensor element at http://sdformat.org/spec?ver=1.10&elem=sensor for more details on SDFormat. + struct SensorImporterHook + { + AZ_TYPE_INFO(SensorImporterHook, "{23f189e3-8da4-4498-9b89-1ef6c900940a}"); + + //! Types of sensors in SDFormat description that can be mapped to the particular O3DE component. + //! Multiple SDFormat sensors can map to one O3DE component. + AZStd::unordered_set m_sensorTypes; + + //! Names of the sensor's parameters in SDFormat description that are supported by the particular O3DE component. + AZStd::unordered_set m_supportedSensorParams; + + //! Names of plugins associated with the sensor in SDFormat description that are supported by the particular O3DE component. + //! Multiple SDFormat plugins can map to one O3DE component. + AZStd::unordered_set m_pluginNames; + + //! Names of the plugin's parameters associated with the sensor in SDFormat description that are supported + //! by the particular O3DE component. + AZStd::unordered_set m_supportedPluginParams; + + //! Registered function that is invoked when a sensor of the specified type is processed by the SDFormat Importer. + //! @param AZ::Entity& a reference to the Entity in which one or more O3DE component is created by the importer + //! @param sdf::Sensor& a reference to the sensor data in SDFormat, which is used to configure O3DE component + using ErrorString = AZStd::string; + using ConvertSensorOutcome = AZ::Outcome; + using ConvertSDFSensorCallback = AZStd::function; + ConvertSDFSensorCallback m_sdfSensorToComponentCallback; + }; +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp index 5796f12b9..edee40e15 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp @@ -28,6 +28,13 @@ namespace ROS2 MakeTopicConfigurationPair("depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig)); } + ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent( + const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration) + : m_sensorConfiguration(sensorConfiguration) + , m_cameraSensorConfiguration(cameraConfiguration) + { + } + void ROS2CameraSensorEditorComponent::Reflect(AZ::ReflectContext* context) { CameraSensorConfiguration::Reflect(context); diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h index d8f6a73b5..c9e4696f5 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h @@ -22,7 +22,7 @@ namespace ROS2 { //! ROS2 Camera Editor sensor component class //! Allows turning an entity into a camera sensor in Editor - //! Component draws camera frustrum in the Editor + //! Component draws camera frustum in the Editor class ROS2CameraSensorEditorComponent : public AzToolsFramework::Components::EditorComponentBase , public CameraCalibrationRequestBus::Handler @@ -30,6 +30,8 @@ namespace ROS2 { public: ROS2CameraSensorEditorComponent(); + ROS2CameraSensorEditorComponent( + const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration); ~ROS2CameraSensorEditorComponent() override = default; AZ_EDITOR_COMPONENT(ROS2CameraSensorEditorComponent, "{3C2A86B2-AD58-4BF1-A5EF-71E0F94A2B42}"); static void Reflect(AZ::ReflectContext* context); @@ -58,4 +60,4 @@ namespace ROS2 SensorConfiguration m_sensorConfiguration; CameraSensorConfiguration m_cameraSensorConfiguration; }; -} // namespace ROS2 \ No newline at end of file +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp new file mode 100644 index 000000000..d3a2e06ea --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include "ROS2SensorHooks.h" + +#include +#include +#include +#include + +#include +#include + +namespace ROS2::SDFormat +{ + namespace Internal + { + void AddTopicConfiguration( + SensorConfiguration& sensorConfig, + const AZStd::string& topic, + const AZStd::string& messageType, + const AZStd::string& configName) + { + TopicConfiguration config; + config.m_topic = topic; + config.m_type = messageType; + sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config)); + } + } // namespace Internal + + SensorImporterHook ROS2SensorHooks::ROS2CameraSensor() + { + SensorImporterHook importerHook; + 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(); + + 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(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/ROS2SensorHooks.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h new file mode 100644 index 000000000..32d2db0d8 --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h @@ -0,0 +1,19 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include + +namespace ROS2::SDFormat +{ + namespace ROS2SensorHooks + { + SensorImporterHook ROS2CameraSensor(); + } // namespace ROS2SensorHooks +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp index 6629f8fb0..a711d199c 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp @@ -15,6 +15,7 @@ #include #include #include +#include namespace ROS2 { @@ -218,9 +219,8 @@ namespace ROS2 return filenames; } - AZStd::optional GetResolvedPath(const AZ::IO::Path &packagePath, - const AZ::IO::Path &unresolvedPath, - const AZStd::function& fileExists) + AZStd::optional GetResolvedPath( + const AZ::IO::Path& packagePath, const AZ::IO::Path& unresolvedPath, const AZStd::function& fileExists) { AZ::IO::Path packageXmlCandite = packagePath / "package.xml"; if (fileExists(packageXmlCandite.String())) @@ -228,7 +228,7 @@ namespace ROS2 AZ::IO::Path resolvedPath = packagePath / unresolvedPath; if (fileExists(resolvedPath.String())) { - return AZStd::optional{resolvedPath}; + return AZStd::optional{ resolvedPath }; } } return AZStd::optional{}; @@ -241,7 +241,7 @@ namespace ROS2 { return subpath; } - for (AZ::IO::Path::iterator pathIt = begin;pathIt!=end;pathIt++) + for (AZ::IO::Path::iterator pathIt = begin; pathIt != end; pathIt++) { subpath /= *pathIt; } @@ -271,7 +271,7 @@ namespace ROS2 if (package.ends_with(packageNameCandidate)) { auto pathIt = unresolvedProperPath.begin(); - AZStd::advance(pathIt,1); + AZStd::advance(pathIt, 1); if (pathIt != unresolvedProperPath.end()) { AZ::IO::Path unresolvedPathStripped = GetPathFromSubPath(pathIt, unresolvedProperPath.end()); @@ -295,7 +295,7 @@ namespace ROS2 auto it = --urdfProperPath.end(); for (; it != urdfProperPath.begin(); it--) { - const auto packagePath = GetPathFromSubPath(urdfProperPath.begin(),it); + const auto packagePath = GetPathFromSubPath(urdfProperPath.begin(), it); std::cout << "packagePath : " << packagePath.String().c_str() << std::endl; const auto resolvedPath = GetResolvedPath(packagePath, unresolvedPath, fileExists); if (resolvedPath.has_value()) @@ -324,4 +324,55 @@ namespace ROS2 return resolvedPath.String(); } + namespace Utils::SDFormat + { + AZStd::string GetPluginFilename(const sdf::Plugin& plugin) + { + const AZ::IO::Path path{ plugin.Filename().c_str() }; + return path.Filename().String(); + } + + AZStd::vector GetUnsupportedParams( + const sdf::ElementPtr& rootElement, const AZStd::unordered_set& supportedParams) + { + AZStd::vector unsupportedParams; + + AZStd::function elementVisitor = + [&](const sdf::ElementPtr& elementPtr, const std::string& prefix) -> void + { + auto childPtr = elementPtr->GetFirstElement(); + + AZStd::string prefixAz(prefix.c_str(), prefix.size()); + if (!childPtr && !prefixAz.empty() && !supportedParams.contains(prefixAz)) + { + unsupportedParams.push_back(prefixAz); + } + + while (childPtr) + { + if (childPtr->GetName() == "plugin") + { + break; + } + + std::string currentName = prefix; + currentName.append(">"); + currentName.append(childPtr->GetName()); + + elementVisitor(childPtr, currentName); + childPtr = childPtr->GetNextElement(); + } + }; + + elementVisitor(rootElement, ""); + + return unsupportedParams; + } + + bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set& supportedPlugins) + { + return supportedPlugins.contains(GetPluginFilename(plugin)); + } + } // namespace Utils::SDFormat + } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h index 3f8dbc517..708676864 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h @@ -11,10 +11,14 @@ #include #include #include +#include +#include #include #include #include +#include + namespace ROS2 { namespace @@ -77,5 +81,27 @@ namespace ROS2 //! @returns false if a timeout or error occurs, otherwise true bool WaitForAssetsToProcess(const AZStd::unordered_map& sourceAssetsPaths); + namespace SDFormat + { + //! Retrieve plugin's filename. The filepath is converted into the filename if necessary. + //! @param plugin plugin in the parsed SDFormat data + //! @returns filename (including extension) without path + AZStd::string GetPluginFilename(const sdf::Plugin& plugin); + + //! Retrieve all parameters that were defined for an element in XML data that are not supported in O3DE. + //! Allows to store the list of unsupported parameters in metadata and logs. It is typically used with sensors and plugins. + //! @param rootElement pointer to a root Element in parsed XML data that will be a subject to heuristics + //! @param supportedParams set of predefined parameters that are supported + //! @returns list of unsupported parameters defined for given element + AZStd::vector GetUnsupportedParams( + const sdf::ElementPtr& rootElement, const AZStd::unordered_set& supportedParams); + + //! Check if plugin is supported by using it's filename. The filepath is converted into the filename if necessary. + //! @param plugin plugin in the parsed SDFormat data + //! @param supportedPlugins set of predefined plugins that are supported + //! @returns true if plugin is supported + bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set& supportedPlugins); + } // namespace SDFormat + } // namespace Utils } // namespace ROS2 \ No newline at end of file diff --git a/Gems/ROS2/Code/Tests/SdfParserTest.cpp b/Gems/ROS2/Code/Tests/SdfParserTest.cpp new file mode 100644 index 000000000..42451ebcf --- /dev/null +++ b/Gems/ROS2/Code/Tests/SdfParserTest.cpp @@ -0,0 +1,241 @@ +/* + * 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 UnitTest +{ + + class SdfParserTest : public LeakDetectionFixture + { + public: + AZStd::shared_ptr Parse(const std::string& xmlString) + { + sdf::SDFPtr sdfElement(new sdf::SDF()); + sdf::init(sdfElement); + sdf::Errors readErrors; + const bool success = sdf::readString(xmlString, sdfElement, readErrors); + if (!success) + { + return nullptr; + } + + auto sdfDom = AZStd::make_shared(); + sdf::Errors parseErrors = sdfDom->Load(sdfElement); + return sdfDom; + } + + std::string GetSdfWithTwoSensors() + { + return R"( + + + + + 0 0 0 0 0 0 + + 2.0 + + 640 + 480 + + + 0.01 + 1000 + + + 10 + + custom_camera + + + + + + 1 + 1 + 20.0 + 0 0 0 0 0 0 + + + + 640 + 1.0 + -2.0 + 2.5 + + + + 0.02 + 10 + 0.01 + + + + + + + )"; + } + }; + + 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"); + } + + TEST_F(SdfParserTest, RobotImporterUtils) + { + AZStd::unordered_set supportedPlugins{ "libgazebo_ros_camera.so", "libgazebo_ros_openni_kinect.so" }; + sdf::Plugin plug; + + plug.SetName("test_camera"); + plug.SetFilename("libgazebo_ros_camera.so"); + EXPECT_EQ("libgazebo_ros_camera.so", ROS2::Utils::SDFormat::GetPluginFilename(plug)); + EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, supportedPlugins)); + plug.SetFilename("/usr/lib/libgazebo_ros_camera.so"); + EXPECT_EQ("libgazebo_ros_camera.so", ROS2::Utils::SDFormat::GetPluginFilename(plug)); + EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, supportedPlugins)); + plug.SetFilename("~/dev/libgazebo_ros_camera.so"); + EXPECT_EQ("libgazebo_ros_camera.so", ROS2::Utils::SDFormat::GetPluginFilename(plug)); + EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, supportedPlugins)); + plug.SetFilename("fun.so"); + EXPECT_EQ("fun.so", ROS2::Utils::SDFormat::GetPluginFilename(plug)); + EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, supportedPlugins)); + plug.SetFilename("fun"); + EXPECT_EQ("fun", ROS2::Utils::SDFormat::GetPluginFilename(plug)); + EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, supportedPlugins)); + + AZStd::unordered_set cameraSupportedParams{ + ">update_rate", ">camera>horizontal_fov", ">camera>image>width", ">camera>image>height" + }; + + 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 sdf::ElementPtr laserElement = sdfModel->LinkByName("link2")->SensorByIndex(0U)->Element(); + + { + const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraSupportedParams); + EXPECT_EQ(unsupportedCameraParams.size(), 3U); + EXPECT_EQ(unsupportedCameraParams[0U], ">pose"); + EXPECT_EQ(unsupportedCameraParams[1U], ">camera>clip>near"); + EXPECT_EQ(unsupportedCameraParams[2U], ">camera>clip>far"); + } + + cameraSupportedParams.emplace(">pose"); + { + const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraSupportedParams); + EXPECT_EQ(unsupportedCameraParams.size(), 2U); + EXPECT_EQ(unsupportedCameraParams[0U], ">camera>clip>near"); + EXPECT_EQ(unsupportedCameraParams[1U], ">camera>clip>far"); + } + + cameraSupportedParams.emplace(">camera>clip>near"); + cameraSupportedParams.emplace(">camera>clip>far"); + { + const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraSupportedParams); + EXPECT_EQ(unsupportedCameraParams.size(), 0U); + } + + const AZStd::unordered_set laserSupportedParams{ ">pose", + ">update_rate", + ">ray>scan>horizontal>samples", + ">ray>scan>horizontal>resolution", + ">ray>scan>horizontal>min_angle", + ">ray>scan>horizontal>max_angle", + ">ray>range>min", + ">ray>range>max", + ">ray>range>resolution", + ">always_on", + ">visualize" }; + const auto& unsupportedLaserParams = ROS2::Utils::SDFormat::GetUnsupportedParams(laserElement, laserSupportedParams); + EXPECT_EQ(unsupportedLaserParams.size(), 0U); + } + + 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& 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)); + } +} // namespace UnitTest diff --git a/Gems/ROS2/Code/ros2_editor_files.cmake b/Gems/ROS2/Code/ros2_editor_files.cmake index 787260840..a11ea655a 100644 --- a/Gems/ROS2/Code/ros2_editor_files.cmake +++ b/Gems/ROS2/Code/ros2_editor_files.cmake @@ -28,6 +28,8 @@ set(FILES Source/RobotImporter/RobotImporterWidget.h Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h + Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp + Source/RobotImporter/SDFormat/ROS2SensorHooks.h Source/RobotImporter/URDF/ArticulationsMaker.cpp Source/RobotImporter/URDF/ArticulationsMaker.h Source/RobotImporter/URDF/CollidersMaker.cpp diff --git a/Gems/ROS2/Code/ros2_editor_tests_files.cmake b/Gems/ROS2/Code/ros2_editor_tests_files.cmake index 0b3fb6437..fadcf6973 100644 --- a/Gems/ROS2/Code/ros2_editor_tests_files.cmake +++ b/Gems/ROS2/Code/ros2_editor_tests_files.cmake @@ -5,5 +5,6 @@ set(FILES Tests/ROS2EditorTest.cpp + Tests/SdfParserTest.cpp Tests/UrdfParserTest.cpp ) diff --git a/Gems/ROS2/Code/ros2_header_files.cmake b/Gems/ROS2/Code/ros2_header_files.cmake index d55b1cf0d..a590221cf 100644 --- a/Gems/ROS2/Code/ros2_header_files.cmake +++ b/Gems/ROS2/Code/ros2_header_files.cmake @@ -28,6 +28,7 @@ set(FILES Include/ROS2/ProximitySensor/ProximitySensorNotificationBusHandler.h Include/ROS2/RobotControl/ControlConfiguration.h Include/ROS2/RobotControl/ControlSubscriptionHandler.h + Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h Include/ROS2/Lidar/LidarRaycasterBus.h Include/ROS2/Lidar/LidarSystemBus.h Include/ROS2/Lidar/LidarRegistrarBus.h