From 4261fe9d683a645cd82bba76c5953df68e1c4695 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 6 Apr 2023 19:41:45 +0200 Subject: [PATCH] Initial integration tests for URDF importer --- Gems/ROS2/Code/CMakeLists.txt | 27 + Gems/ROS2/Code/Tests/UrdfParserTest.cpp | 918 +++++++++--------- Gems/ROS2/Code/Tests/urdfImporterTest.cpp | 130 +++ ...porter_physics_tests_supported_files.cmake | 11 + 4 files changed, 627 insertions(+), 459 deletions(-) create mode 100644 Gems/ROS2/Code/Tests/urdfImporterTest.cpp create mode 100644 Gems/ROS2/Code/urdf_importer_physics_tests_supported_files.cmake diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt index ef05eb429..5a5f0c3a3 100644 --- a/Gems/ROS2/Code/CMakeLists.txt +++ b/Gems/ROS2/Code/CMakeLists.txt @@ -195,6 +195,33 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) ly_add_googletest( NAME Gem::${gem_name}.Editor.Tests ) + + # integration test for URDF importer + ly_add_target( + NAME ROS2.URDF.importer.Tests ${PAL_TRAIT_TEST_TARGET_TYPE} + NAMESPACE Gem + FILES_CMAKE + urdf_importer_physics_tests_supported_files.cmake + INCLUDE_DIRECTORIES + PRIVATE + Source + Tests + BUILD_DEPENDENCIES + PRIVATE + AZ::AzTestShared + AZ::AzToolsFramework + Legacy::CryCommon + Legacy::EditorCommon + Legacy::Editor.Headers + AZ::AzManipulatorTestFramework.Static + Gem::ROS2.Editor.Static + RUNTIME_DEPENDENCIES + Gem::PhysX.Editor + ) + + ly_add_googletest( + NAME Gem::ROS2.URDF.importer.Tests + ) endif() endif() endif() diff --git a/Gems/ROS2/Code/Tests/UrdfParserTest.cpp b/Gems/ROS2/Code/Tests/UrdfParserTest.cpp index 48de6a90a..a3b04f836 100644 --- a/Gems/ROS2/Code/Tests/UrdfParserTest.cpp +++ b/Gems/ROS2/Code/Tests/UrdfParserTest.cpp @@ -1,459 +1,459 @@ -/* - * 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 UrdfParserTest : public LeakDetectionFixture - { - public: - AZStd::string GetXacroParams() - { - return "\n" - " \n" - ""; - } - AZStd::string GetUrdfWithOneLink() - { - return "" - " \n" - " \n" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - ""; - } - - AZStd::string GetUrdfWithTwoLinksAndJoint() - { - return " " - " \n" - " \n" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - ""; - } - AZStd::string GetURDFWithTranforms() - { - return "\n" - "\n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - ""; - } - - AZStd::string GetURDFWithWheel( - const AZStd::string& wheel_name, - const AZStd::string& wheel_joint_type, - bool wheel_has_visual = true, - bool wheel_has_collider = true) - { - // clang-format off - return "\n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - +(wheel_has_visual?"\n" - " \n" - " \n" - " \n" - " \n" - " \n":"")+ - +(wheel_has_collider?"\n" - " \n" - " \n" - " \n" - " \n" - " \n":"")+ - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - " \n" - ""; - // clang-format on - } - }; - - TEST_F(UrdfParserTest, ParseUrdfWithOneLink) - { - - const auto xmlStr = GetUrdfWithOneLink(); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - - EXPECT_EQ(urdf->getName(), "test_one_link"); - - std::vector links; - urdf->getLinks(links); - EXPECT_EQ(links.size(), 1U); - - const auto link1 = urdf->getLink("link1"); - - ASSERT_TRUE(link1); - EXPECT_EQ(link1->inertial->mass, 1.0); - EXPECT_EQ(link1->inertial->ixx, 1.0); - EXPECT_EQ(link1->inertial->ixy, 0.0); - EXPECT_EQ(link1->inertial->ixz, 0.0); - EXPECT_EQ(link1->inertial->iyy, 1.0); - EXPECT_EQ(link1->inertial->iyz, 0.0); - EXPECT_EQ(link1->inertial->izz, 1.0); - - EXPECT_EQ(link1->visual->geometry->type, 1); - const auto visualBox = std::dynamic_pointer_cast(link1->visual->geometry); - EXPECT_EQ(visualBox->dim.x, 1.0); - EXPECT_EQ(visualBox->dim.y, 2.0); - EXPECT_EQ(visualBox->dim.z, 1.0); - - EXPECT_EQ(link1->collision->geometry->type, 1); - const auto collisionBox = std::dynamic_pointer_cast(link1->visual->geometry); - EXPECT_EQ(collisionBox->dim.x, 1.0); - EXPECT_EQ(collisionBox->dim.y, 2.0); - EXPECT_EQ(collisionBox->dim.z, 1.0); - } - - TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndJoint) - { - - const auto xmlStr = GetUrdfWithTwoLinksAndJoint(); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - - EXPECT_EQ(urdf->getName(), "test_two_links_one_joint"); - - std::vector links; - urdf->getLinks(links); - EXPECT_EQ(links.size(), 2U); - - const auto link1 = urdf->getLink("link1"); - ASSERT_TRUE(link1); - - const auto link2 = urdf->getLink("link2"); - ASSERT_TRUE(link2); - - const auto joint12 = urdf->getJoint("joint12"); - ASSERT_TRUE(joint12); - - EXPECT_EQ(joint12->parent_link_name, "link1"); - EXPECT_EQ(joint12->child_link_name, "link2"); - - EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.x, 1.0); - EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.y, 0.5); - EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.z, 0.0); - - double roll, pitch, yaw; - joint12->parent_to_joint_origin_transform.rotation.getRPY(roll, pitch, yaw); - EXPECT_DOUBLE_EQ(roll, 0.0); - EXPECT_DOUBLE_EQ(pitch, 0.0); - EXPECT_DOUBLE_EQ(yaw, 0.0); - - EXPECT_EQ(joint12->dynamics->damping, 10.0); - EXPECT_EQ(joint12->dynamics->friction, 5.0); - - EXPECT_EQ(joint12->limits->lower, 10.0); - EXPECT_EQ(joint12->limits->upper, 20.0); - EXPECT_EQ(joint12->limits->effort, 90.0); - EXPECT_EQ(joint12->limits->velocity, 10.0); - } - - TEST_F(UrdfParserTest, WheelHeuristicNameValid) - { - const AZStd::string wheel_name("wheel_left_link"); - const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous"); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - auto wheel_candidate = urdf->getLink(wheel_name.c_str()); - ASSERT_TRUE(wheel_candidate); - EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), true); - } - - TEST_F(UrdfParserTest, WheelHeuristicNameNotValid1) - { - const AZStd::string wheel_name("wheel_left_joint"); - const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous"); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - auto wheel_candidate = urdf->getLink(wheel_name.c_str()); - ASSERT_TRUE(wheel_candidate); - EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); - } - - TEST_F(UrdfParserTest, WheelHeuristicJointNotValid) - { - const AZStd::string wheel_name("wheel_left_link"); - const auto xmlStr = GetURDFWithWheel(wheel_name, "fixed"); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - auto wheel_candidate = urdf->getLink(wheel_name.c_str()); - ASSERT_TRUE(wheel_candidate); - EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); - } - - TEST_F(UrdfParserTest, WheelHeuristicJointVisualNotValid) - { - const AZStd::string wheel_name("wheel_left_link"); - const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous", false, true); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - auto wheel_candidate = urdf->getLink(wheel_name.c_str()); - ASSERT_TRUE(wheel_candidate); - EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); - } - - TEST_F(UrdfParserTest, WheelHeuristicJointColliderNotValid) - { - const AZStd::string wheel_name("wheel_left_link"); - const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous", true, false); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - auto wheel_candidate = urdf->getLink(wheel_name.c_str()); - ASSERT_TRUE(wheel_candidate); - EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); - } - - TEST_F(UrdfParserTest, TestLinkListing) - { - const auto xmlStr = GetURDFWithTranforms(); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - auto links = ROS2::Utils::GetAllLinks(urdf->getRoot()->child_links); - EXPECT_EQ(links.size(), 3); - ASSERT_TRUE(links.contains("link1")); - ASSERT_TRUE(links.contains("link2")); - ASSERT_TRUE(links.contains("link3")); - EXPECT_EQ(links.at("link1")->name, "link1"); - EXPECT_EQ(links.at("link2")->name, "link2"); - EXPECT_EQ(links.at("link3")->name, "link3"); - } - - TEST_F(UrdfParserTest, TestJointLink) - { - const auto xmlStr = GetURDFWithTranforms(); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - auto joints = ROS2::Utils::GetAllJoints(urdf->getRoot()->child_links); - EXPECT_EQ(joints.size(), 3); - } - - TEST_F(UrdfParserTest, TestTransforms) - { - const auto xmlStr = GetURDFWithTranforms(); - const auto urdf = ROS2::UrdfParser::Parse(xmlStr); - const auto links = ROS2::Utils::GetAllLinks(urdf->getRoot()->child_links); - const auto link1_ptr = links.at("link1"); - const auto link2_ptr = links.at("link2"); - const auto link3_ptr = links.at("link3"); - - // values exported from Blender - const AZ::Vector3 expected_translation_link1{ 0.0, 0.0, 0.0 }; - const AZ::Vector3 expected_translation_link2{ -1.2000000476837158, 2.0784599781036377, 0.0 }; - const AZ::Vector3 expected_translation_link3{ -2.4000000953674316, 0.0, 0.0 }; - - const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::GetWorldTransformURDF(link1_ptr); - EXPECT_NEAR(expected_translation_link1.GetX(), transform_from_urdf_link1.GetTranslation().GetX(), 1e-5); - EXPECT_NEAR(expected_translation_link1.GetY(), transform_from_urdf_link1.GetTranslation().GetY(), 1e-5); - EXPECT_NEAR(expected_translation_link1.GetZ(), transform_from_urdf_link1.GetTranslation().GetZ(), 1e-5); - - const AZ::Transform transform_from_urdf_link2 = ROS2::Utils::GetWorldTransformURDF(link2_ptr); - EXPECT_NEAR(expected_translation_link2.GetX(), transform_from_urdf_link2.GetTranslation().GetX(), 1e-5); - EXPECT_NEAR(expected_translation_link2.GetY(), transform_from_urdf_link2.GetTranslation().GetY(), 1e-5); - EXPECT_NEAR(expected_translation_link2.GetZ(), transform_from_urdf_link2.GetTranslation().GetZ(), 1e-5); - - const AZ::Transform transform_from_urdf_link3 = ROS2::Utils::GetWorldTransformURDF(link3_ptr); - EXPECT_NEAR(expected_translation_link3.GetX(), transform_from_urdf_link3.GetTranslation().GetX(), 1e-5); - EXPECT_NEAR(expected_translation_link3.GetY(), transform_from_urdf_link3.GetTranslation().GetY(), 1e-5); - EXPECT_NEAR(expected_translation_link3.GetZ(), transform_from_urdf_link3.GetTranslation().GetZ(), 1e-5); - } - - TEST_F(UrdfParserTest, TestPathResolvementGlobal) - { - AZStd::string dae = "file:///home/foo/ros_ws/install/foo_robot/meshes/bar.dae"; - AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf"; - auto result = ROS2::Utils::ResolveURDFPath( - dae, - urdf, - [](const AZStd::string& p) -> bool - { - return false; - }); - EXPECT_EQ(result, "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae"); - } - - TEST_F(UrdfParserTest, TestPathResolvementRelative) - { - AZStd::string dae = "meshes/bar.dae"; - AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf"; - auto result = ROS2::Utils::ResolveURDFPath( - dae, - urdf, - [](const AZStd::string& p) -> bool - { - return false; - }); - EXPECT_EQ(result, "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae"); - } - - TEST_F(UrdfParserTest, TestPathResolvementRelativePackage) - { - AZStd::string dae = "package://meshes/bar.dae"; - AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/description/foo_robot.urdf"; - AZStd::string xml = "/home/foo/ros_ws/install/foo_robot/package.xml"; - auto mockFileSystem = [&](const AZStd::string& p) -> bool - { - return (p == xml); - }; - auto result = ROS2::Utils::ResolveURDFPath(dae, urdf, mockFileSystem); - EXPECT_EQ(result, "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae"); - } - - TEST_F(UrdfParserTest, XacroParseArgs) - { - AZStd::string xacroParams = GetXacroParams(); - ROS2::Utils::xacro::Params params = ROS2::Utils::xacro::GetParameterFromXacroData(xacroParams); - EXPECT_EQ(params.size(), 1); - ASSERT_TRUE(params.contains("laser_enabled")); - EXPECT_EQ(params["laser_enabled"], "false"); - } - -} // namespace UnitTest +///* +// * 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 UrdfParserTest : public LeakDetectionFixture +// { +// public: +// AZStd::string GetXacroParams() +// { +// return "\n" +// " \n" +// ""; +// } +// AZStd::string GetUrdfWithOneLink() +// { +// return "" +// " \n" +// " \n" +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// ""; +// } +// +// AZStd::string GetUrdfWithTwoLinksAndJoint() +// { +// return " " +// " \n" +// " \n" +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// " " +// ""; +// } +// AZStd::string GetURDFWithTranforms() +// { +// return "\n" +// "\n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// ""; +// } +// +// AZStd::string GetURDFWithWheel( +// const AZStd::string& wheel_name, +// const AZStd::string& wheel_joint_type, +// bool wheel_has_visual = true, +// bool wheel_has_collider = true) +// { +// // clang-format off +// return "\n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// +(wheel_has_visual?"\n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n":"")+ +// +(wheel_has_collider?"\n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n":"")+ +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// " \n" +// ""; +// // clang-format on +// } +// }; +// +// TEST_F(UrdfParserTest, ParseUrdfWithOneLink) +// { +// +// const auto xmlStr = GetUrdfWithOneLink(); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// +// EXPECT_EQ(urdf->getName(), "test_one_link"); +// +// std::vector links; +// urdf->getLinks(links); +// EXPECT_EQ(links.size(), 1U); +// +// const auto link1 = urdf->getLink("link1"); +// +// ASSERT_TRUE(link1); +// EXPECT_EQ(link1->inertial->mass, 1.0); +// EXPECT_EQ(link1->inertial->ixx, 1.0); +// EXPECT_EQ(link1->inertial->ixy, 0.0); +// EXPECT_EQ(link1->inertial->ixz, 0.0); +// EXPECT_EQ(link1->inertial->iyy, 1.0); +// EXPECT_EQ(link1->inertial->iyz, 0.0); +// EXPECT_EQ(link1->inertial->izz, 1.0); +// +// EXPECT_EQ(link1->visual->geometry->type, 1); +// const auto visualBox = std::dynamic_pointer_cast(link1->visual->geometry); +// EXPECT_EQ(visualBox->dim.x, 1.0); +// EXPECT_EQ(visualBox->dim.y, 2.0); +// EXPECT_EQ(visualBox->dim.z, 1.0); +// +// EXPECT_EQ(link1->collision->geometry->type, 1); +// const auto collisionBox = std::dynamic_pointer_cast(link1->visual->geometry); +// EXPECT_EQ(collisionBox->dim.x, 1.0); +// EXPECT_EQ(collisionBox->dim.y, 2.0); +// EXPECT_EQ(collisionBox->dim.z, 1.0); +// } +// +// TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndJoint) +// { +// +// const auto xmlStr = GetUrdfWithTwoLinksAndJoint(); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// +// EXPECT_EQ(urdf->getName(), "test_two_links_one_joint"); +// +// std::vector links; +// urdf->getLinks(links); +// EXPECT_EQ(links.size(), 2U); +// +// const auto link1 = urdf->getLink("link1"); +// ASSERT_TRUE(link1); +// +// const auto link2 = urdf->getLink("link2"); +// ASSERT_TRUE(link2); +// +// const auto joint12 = urdf->getJoint("joint12"); +// ASSERT_TRUE(joint12); +// +// EXPECT_EQ(joint12->parent_link_name, "link1"); +// EXPECT_EQ(joint12->child_link_name, "link2"); +// +// EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.x, 1.0); +// EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.y, 0.5); +// EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.z, 0.0); +// +// double roll, pitch, yaw; +// joint12->parent_to_joint_origin_transform.rotation.getRPY(roll, pitch, yaw); +// EXPECT_DOUBLE_EQ(roll, 0.0); +// EXPECT_DOUBLE_EQ(pitch, 0.0); +// EXPECT_DOUBLE_EQ(yaw, 0.0); +// +// EXPECT_EQ(joint12->dynamics->damping, 10.0); +// EXPECT_EQ(joint12->dynamics->friction, 5.0); +// +// EXPECT_EQ(joint12->limits->lower, 10.0); +// EXPECT_EQ(joint12->limits->upper, 20.0); +// EXPECT_EQ(joint12->limits->effort, 90.0); +// EXPECT_EQ(joint12->limits->velocity, 10.0); +// } +// +// TEST_F(UrdfParserTest, WheelHeuristicNameValid) +// { +// const AZStd::string wheel_name("wheel_left_link"); +// const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous"); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// auto wheel_candidate = urdf->getLink(wheel_name.c_str()); +// ASSERT_TRUE(wheel_candidate); +// EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), true); +// } +// +// TEST_F(UrdfParserTest, WheelHeuristicNameNotValid1) +// { +// const AZStd::string wheel_name("wheel_left_joint"); +// const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous"); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// auto wheel_candidate = urdf->getLink(wheel_name.c_str()); +// ASSERT_TRUE(wheel_candidate); +// EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); +// } +// +// TEST_F(UrdfParserTest, WheelHeuristicJointNotValid) +// { +// const AZStd::string wheel_name("wheel_left_link"); +// const auto xmlStr = GetURDFWithWheel(wheel_name, "fixed"); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// auto wheel_candidate = urdf->getLink(wheel_name.c_str()); +// ASSERT_TRUE(wheel_candidate); +// EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); +// } +// +// TEST_F(UrdfParserTest, WheelHeuristicJointVisualNotValid) +// { +// const AZStd::string wheel_name("wheel_left_link"); +// const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous", false, true); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// auto wheel_candidate = urdf->getLink(wheel_name.c_str()); +// ASSERT_TRUE(wheel_candidate); +// EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); +// } +// +// TEST_F(UrdfParserTest, WheelHeuristicJointColliderNotValid) +// { +// const AZStd::string wheel_name("wheel_left_link"); +// const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous", true, false); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// auto wheel_candidate = urdf->getLink(wheel_name.c_str()); +// ASSERT_TRUE(wheel_candidate); +// EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); +// } +// +// TEST_F(UrdfParserTest, TestLinkListing) +// { +// const auto xmlStr = GetURDFWithTranforms(); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// auto links = ROS2::Utils::GetAllLinks(urdf->getRoot()->child_links); +// EXPECT_EQ(links.size(), 3); +// ASSERT_TRUE(links.contains("link1")); +// ASSERT_TRUE(links.contains("link2")); +// ASSERT_TRUE(links.contains("link3")); +// EXPECT_EQ(links.at("link1")->name, "link1"); +// EXPECT_EQ(links.at("link2")->name, "link2"); +// EXPECT_EQ(links.at("link3")->name, "link3"); +// } +// +// TEST_F(UrdfParserTest, TestJointLink) +// { +// const auto xmlStr = GetURDFWithTranforms(); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// auto joints = ROS2::Utils::GetAllJoints(urdf->getRoot()->child_links); +// EXPECT_EQ(joints.size(), 3); +// } +// +// TEST_F(UrdfParserTest, TestTransforms) +// { +// const auto xmlStr = GetURDFWithTranforms(); +// const auto urdf = ROS2::UrdfParser::Parse(xmlStr); +// const auto links = ROS2::Utils::GetAllLinks(urdf->getRoot()->child_links); +// const auto link1_ptr = links.at("link1"); +// const auto link2_ptr = links.at("link2"); +// const auto link3_ptr = links.at("link3"); +// +// // values exported from Blender +// const AZ::Vector3 expected_translation_link1{ 0.0, 0.0, 0.0 }; +// const AZ::Vector3 expected_translation_link2{ -1.2000000476837158, 2.0784599781036377, 0.0 }; +// const AZ::Vector3 expected_translation_link3{ -2.4000000953674316, 0.0, 0.0 }; +// +// const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::GetWorldTransformURDF(link1_ptr); +// EXPECT_NEAR(expected_translation_link1.GetX(), transform_from_urdf_link1.GetTranslation().GetX(), 1e-5); +// EXPECT_NEAR(expected_translation_link1.GetY(), transform_from_urdf_link1.GetTranslation().GetY(), 1e-5); +// EXPECT_NEAR(expected_translation_link1.GetZ(), transform_from_urdf_link1.GetTranslation().GetZ(), 1e-5); +// +// const AZ::Transform transform_from_urdf_link2 = ROS2::Utils::GetWorldTransformURDF(link2_ptr); +// EXPECT_NEAR(expected_translation_link2.GetX(), transform_from_urdf_link2.GetTranslation().GetX(), 1e-5); +// EXPECT_NEAR(expected_translation_link2.GetY(), transform_from_urdf_link2.GetTranslation().GetY(), 1e-5); +// EXPECT_NEAR(expected_translation_link2.GetZ(), transform_from_urdf_link2.GetTranslation().GetZ(), 1e-5); +// +// const AZ::Transform transform_from_urdf_link3 = ROS2::Utils::GetWorldTransformURDF(link3_ptr); +// EXPECT_NEAR(expected_translation_link3.GetX(), transform_from_urdf_link3.GetTranslation().GetX(), 1e-5); +// EXPECT_NEAR(expected_translation_link3.GetY(), transform_from_urdf_link3.GetTranslation().GetY(), 1e-5); +// EXPECT_NEAR(expected_translation_link3.GetZ(), transform_from_urdf_link3.GetTranslation().GetZ(), 1e-5); +// } +// +// TEST_F(UrdfParserTest, TestPathResolvementGlobal) +// { +// AZStd::string dae = "file:///home/foo/ros_ws/install/foo_robot/meshes/bar.dae"; +// AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf"; +// auto result = ROS2::Utils::ResolveURDFPath( +// dae, +// urdf, +// [](const AZStd::string& p) -> bool +// { +// return false; +// }); +// EXPECT_EQ(result, "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae"); +// } +// +// TEST_F(UrdfParserTest, TestPathResolvementRelative) +// { +// AZStd::string dae = "meshes/bar.dae"; +// AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf"; +// auto result = ROS2::Utils::ResolveURDFPath( +// dae, +// urdf, +// [](const AZStd::string& p) -> bool +// { +// return false; +// }); +// EXPECT_EQ(result, "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae"); +// } +// +// TEST_F(UrdfParserTest, TestPathResolvementRelativePackage) +// { +// AZStd::string dae = "package://meshes/bar.dae"; +// AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/description/foo_robot.urdf"; +// AZStd::string xml = "/home/foo/ros_ws/install/foo_robot/package.xml"; +// auto mockFileSystem = [&](const AZStd::string& p) -> bool +// { +// return (p == xml); +// }; +// auto result = ROS2::Utils::ResolveURDFPath(dae, urdf, mockFileSystem); +// EXPECT_EQ(result, "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae"); +// } +// +// TEST_F(UrdfParserTest, XacroParseArgs) +// { +// AZStd::string xacroParams = GetXacroParams(); +// ROS2::Utils::xacro::Params params = ROS2::Utils::xacro::GetParameterFromXacroData(xacroParams); +// EXPECT_EQ(params.size(), 1); +// ASSERT_TRUE(params.contains("laser_enabled")); +// EXPECT_EQ(params["laser_enabled"], "false"); +// } +// +//} // namespace UnitTest diff --git a/Gems/ROS2/Code/Tests/urdfImporterTest.cpp b/Gems/ROS2/Code/Tests/urdfImporterTest.cpp new file mode 100644 index 000000000..a4ebc246c --- /dev/null +++ b/Gems/ROS2/Code/Tests/urdfImporterTest.cpp @@ -0,0 +1,130 @@ +/* +* 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 "Components/EditorWhiteBoxColliderComponent.h" +//#include "Components/WhiteBoxColliderComponent.h" +//#include "WhiteBox/EditorWhiteBoxComponentBus.h" +//#include "WhiteBox/WhiteBoxToolApi.h" +//#include "WhiteBoxTestFixtures.h" +//#include "WhiteBoxTestUtil.h" + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "RobotImporter/URDF/InertialsMaker.h" +#include "RobotImporter/URDF/CollidersMaker.h" + + +namespace UnitTest +{ + class URDFImporterPhysicsTestEnvironment : public AZ::Test::GemTestEnvironment + { + // AZ::Test::GemTestEnvironment overrides ... + void AddGemsAndComponents() override; + AZ::ComponentApplication* CreateApplicationInstance() override; + void PostSystemEntityActivate() override; + + public: + URDFImporterPhysicsTestEnvironment() = default; + ~URDFImporterPhysicsTestEnvironment() override = default; + }; + + void URDFImporterPhysicsTestEnvironment::AddGemsAndComponents() + { + AddDynamicModulePaths({"PhysX.Editor.Gem"}); + AddComponentDescriptors( + {}); + } + + AZ::ComponentApplication* URDFImporterPhysicsTestEnvironment::CreateApplicationInstance() + { + // Using ToolsTestApplication to have AzFramework and AzToolsFramework components. + return aznew UnitTest::ToolsTestApplication("UrdfImpoterPhysics"); + } + + void URDFImporterPhysicsTestEnvironment::PostSystemEntityActivate() + { + AZ::UserSettingsComponentRequestBus::Broadcast(&AZ::UserSettingsComponentRequests::DisableSaveOnFinalize); + } + + class UrdfImportPhysicsFixture : public ::testing::Test + { + }; + + TEST_F(UrdfImportPhysicsFixture, URDFImporterIntertiaMaker) + { + + AZ::Entity entity; + entity.CreateComponent(); + entity.Init(); + + urdf::InertialSharedPtr inertial = std::make_shared(); + + ROS2::InertialsMaker inertialsMaker; + inertialsMaker.AddInertial(inertial, entity.GetId()); + entity.Activate(); + + EXPECT_EQ(entity.GetState(), AZ::Entity::State::Active); + + } + + TEST_F(UrdfImportPhysicsFixture, URDFImporterCollisionSphere) + { + + AZ::Entity entity; + entity.CreateComponent(); + entity.Init(); + + urdf::CollisionSharedPtr collision = std::make_shared(); + urdf::SphereSharedPtr sphere = std::make_shared(); + sphere->radius = 2.0f; + collision->geometry = sphere; + + urdf::LinkSharedPtr link = std::make_shared(); + link->collision = collision; + link->inertial = std::make_shared(); + + AZStd::shared_ptr mappings = AZStd::make_shared(); + ROS2::CollidersMaker collidersMaker (mappings); + ROS2::InertialsMaker inertialsMaker; + + inertialsMaker.AddInertial(link->inertial, entity.GetId()); + collidersMaker.AddColliders(link, entity.GetId()); + + entity.Activate(); + + EXPECT_EQ(entity.GetState(), AZ::Entity::State::Active); + } + + +} // namespace UnitTest + +// required to support running integration tests with Qt and PhysX +AZTEST_EXPORT int AZ_UNIT_TEST_HOOK_NAME(int argc, char** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + AzQtComponents::PrepareQtPaths(); + QApplication app(argc, argv); + AZ::Test::printUnusedParametersWarning(argc, argv); + AZ::Test::addTestEnvironments({new UnitTest::URDFImporterPhysicsTestEnvironment()}); + int result = RUN_ALL_TESTS(); + return result; +} + +IMPLEMENT_TEST_EXECUTABLE_MAIN(); diff --git a/Gems/ROS2/Code/urdf_importer_physics_tests_supported_files.cmake b/Gems/ROS2/Code/urdf_importer_physics_tests_supported_files.cmake new file mode 100644 index 000000000..a9986e81a --- /dev/null +++ b/Gems/ROS2/Code/urdf_importer_physics_tests_supported_files.cmake @@ -0,0 +1,11 @@ +# +# 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 +# +# + +set(FILES + Tests/urdfImporterTest.cpp + )