From 8ef0023f79b58c81e8c57edcfe73efbcc104c894 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 8 Feb 2023 13:13:29 -0600 Subject: [PATCH 1/7] Garden test cleanup This cleans up physics tests in a few ways: Consolidate TestUtilities.hh and test/Utils.hh Move test headers to test/include/test Move mock headers to test/include/mock Create an INTERFACE library for test/mock headers that sets common compiler definitions as well as common libraries to link against Remove all instances of hardcoded world names or resource names and instead use constants. Signed-off-by: Michael Carroll --- CMakeLists.txt | 5 +- dartsim/CMakeLists.txt | 20 ++----- dartsim/src/AddedMassFeatures_TEST.cc | 3 +- dartsim/src/Base_TEST.cc | 5 +- dartsim/src/EntityManagement_TEST.cc | 11 ++-- dartsim/src/SDFFeatures_TEST.cc | 27 +++++---- dartsim/src/WorldFeatures_TEST.cc | 7 +-- dartsim/src/Worlds.hh | 51 +++++++++++++++++ src/CMakeLists.txt | 5 +- src/Cloneable_TEST.cc | 2 +- src/CompositeData_TEST.cc | 2 +- src/FindFeatures_TEST.cc | 4 +- src/SpecifyData_TEST.cc | 2 +- src/TestUtilities.hh | 53 ----------------- test/CMakeLists.txt | 11 ++++ test/benchmark/CMakeLists.txt | 3 +- test/benchmark/ExpectData.cc | 2 +- test/common_test/CMakeLists.txt | 15 ++--- test/common_test/Worlds.hh | 54 ++++++++++++++++++ test/common_test/added_mass.cc | 8 ++- test/common_test/addexternalforcetorque.cc | 7 ++- test/common_test/basic_test.cc | 2 +- test/common_test/collisions.cc | 8 +-- test/common_test/construct_empty_world.cc | 2 +- test/common_test/detachable_joint.cc | 7 ++- test/common_test/free_joint_features.cc | 10 ++-- test/common_test/joint_features.cc | 36 ++++++------ .../joint_transmitted_wrench_features.cc | 7 ++- test/common_test/kinematic_features.cc | 9 +-- test/common_test/link_features.cc | 11 ++-- test/common_test/shape_features.cc | 11 ++-- test/common_test/simulation_features.cc | 26 ++++----- test/common_test/world_features.cc | 13 +++-- test/{ => include/mock}/MockCenterOfMass.hh | 0 test/{ => include/mock}/MockCreateEntities.hh | 0 test/{ => include/mock}/MockDoublePendulum.hh | 0 test/{ => include/mock}/MockFeatures.hh | 0 test/{ => include/mock}/MockFrameSemantics.hh | 0 test/{ => include/mock}/MockGetByName.hh | 0 test/{ => include/mock}/MockJoints.hh | 0 test/{ => include/mock}/MockSetName.hh | 0 test/{ => include/test}/PhysicsPluginsList.hh | 0 test/include/test/Resources.hh | 38 ++++++++++++ .../include/test}/TestDataTypes.hh | 0 .../test}/TestLibLoader.hh | 3 + test/{ => include/test}/Utils.hh | 28 +++++++++ test/integration/CMakeLists.txt | 7 +-- test/integration/CanReadWrite.cc | 2 +- test/integration/DoublePendulum.cc | 2 +- test/integration/FeatureSystem.cc | 2 +- test/integration/FrameSemantics.hh | 4 +- test/integration/JointTypes.hh | 4 +- test/integration/RequestFeatures.cc | 6 +- test/performance/CMakeLists.txt | 4 +- test/performance/ExpectData.cc | 2 +- test/plugins/CMakeLists.txt | 3 +- test/plugins/DARTDoublePendulum.cc | 6 +- test/plugins/MockEntities.cc | 2 +- test/plugins/MockJoints.cc | 2 +- test/plugins/frames.cc | 2 +- {resources => test/resources}/chassis.dae | 0 .../resources}/heightmap_bowl.png | Bin {resources => test/resources}/rrbot.xml | 0 {resources => test/resources}/volcano.tif | Bin tpe/plugin/CMakeLists.txt | 13 ++--- tpe/plugin/src/SDFFeatures_TEST.cc | 5 +- tpe/plugin/src/SimulationFeatures_TEST.cc | 19 +++--- tpe/plugin/src/Worlds.hh | 40 +++++++++++++ 68 files changed, 405 insertions(+), 228 deletions(-) create mode 100644 dartsim/src/Worlds.hh delete mode 100644 src/TestUtilities.hh create mode 100644 test/common_test/Worlds.hh rename test/{ => include/mock}/MockCenterOfMass.hh (100%) rename test/{ => include/mock}/MockCreateEntities.hh (100%) rename test/{ => include/mock}/MockDoublePendulum.hh (100%) rename test/{ => include/mock}/MockFeatures.hh (100%) rename test/{ => include/mock}/MockFrameSemantics.hh (100%) rename test/{ => include/mock}/MockGetByName.hh (100%) rename test/{ => include/mock}/MockJoints.hh (100%) rename test/{ => include/mock}/MockSetName.hh (100%) rename test/{ => include/test}/PhysicsPluginsList.hh (100%) create mode 100644 test/include/test/Resources.hh rename {src/utils => test/include/test}/TestDataTypes.hh (100%) rename test/{common_test => include/test}/TestLibLoader.hh (98%) rename test/{ => include/test}/Utils.hh (91%) rename {resources => test/resources}/chassis.dae (100%) rename {resources => test/resources}/heightmap_bowl.png (100%) rename {resources => test/resources}/rrbot.xml (100%) rename {resources => test/resources}/volcano.tif (100%) create mode 100644 tpe/plugin/src/Worlds.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index 01ffa1b22..d55743e38 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,6 +42,9 @@ gz_find_package(gz-common5 REQUIRED_BY heightmap mesh dartsim tpe tpelib bullet) set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) +# This is only used for test support +gz_find_package(gz-common5 COMPONENTS testing) + #-------------------------------------- # Find gz-math gz_find_package(gz-math7 REQUIRED COMPONENTS eigen3) @@ -90,8 +93,6 @@ gz_find_package(GzBullet message(STATUS "-------------------------------------------\n") -set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources") - # Plugin install dirs set(GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR ${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/engine-plugins diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index 35b416bb3..f7f8a0973 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -84,22 +84,21 @@ gz_build_tests( LIB_DEPS ${features} ${dartsim_plugin} - gz-plugin${GZ_PLUGIN_VER}::loader - gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-physics-test gz-common${GZ_COMMON_VER}::geospatial ${PROJECT_LIBRARY_TARGET_NAME}-sdf ${PROJECT_LIBRARY_TARGET_NAME}-heightmap ${PROJECT_LIBRARY_TARGET_NAME}-mesh TEST_LIST tests ENVIRONMENT - GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/src +) foreach(test ${tests}) - target_compile_definitions(${test} PRIVATE - "dartsim_plugin_LIB=\"$\"" - "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"") + "dartsim_plugin_LIB=\"$\"") if (DART_HAS_CONTACT_SURFACE_HEADER) target_compile_definitions(${test} PRIVATE DART_HAS_CONTACT_SURFACE) @@ -108,10 +107,3 @@ foreach(test ${tests}) # Helps when we want to build a single test after making changes to dartsim_plugin add_dependencies(${test} ${dartsim_plugin}) endforeach() - -foreach(test UNIT_FindFeatures_TEST UNIT_RequestFeatures_TEST) - if(TARGET ${test}) - target_compile_definitions(${test} PRIVATE - "dartsim_plugin_LIB=\"$\"") - endif() -endforeach() diff --git a/dartsim/src/AddedMassFeatures_TEST.cc b/dartsim/src/AddedMassFeatures_TEST.cc index 1eec689c5..da476827f 100644 --- a/dartsim/src/AddedMassFeatures_TEST.cc +++ b/dartsim/src/AddedMassFeatures_TEST.cc @@ -39,6 +39,7 @@ #include #include +#include "Worlds.hh" struct TestFeatureList : gz::physics::FeatureList< gz::physics::GetEntities, @@ -121,7 +122,7 @@ TEST(AddedMassFeatures, AddedMass) 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - auto world = LoadWorld(TEST_WORLD_DIR"/added_mass.sdf"); + const auto world = LoadWorld(dartsim::worlds::kAddedMassSdf); ASSERT_NE(nullptr, world); auto dartWorld = world->GetDartsimWorld(); diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index 1d8a37352..c0b5be3c5 100644 --- a/dartsim/src/Base_TEST.cc +++ b/dartsim/src/Base_TEST.cc @@ -38,8 +38,9 @@ #include "EntityManagementFeatures.hh" #include "SDFFeatures.hh" -using namespace gz::physics; +#include "test/Resources.hh" +using namespace gz::physics; TEST(BaseClass, RemoveModel) @@ -166,7 +167,7 @@ TEST(BaseClass, SdfConstructionBookkeeping) ::sdf::Root root; - auto errors = root.Load(GZ_PHYSICS_RESOURCE_DIR "/rrbot.xml"); + auto errors = root.Load(gz::physics::test::resources::kRrbotXml); ASSERT_TRUE(errors.empty()); const ::sdf::Model *sdfModel = root.Model(); ASSERT_NE(nullptr, sdfModel); diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 07e056d4f..52a3e9b4a 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -35,6 +35,8 @@ #include "KinematicsFeatures.hh" #include "ShapeFeatures.hh" +#include "test/Resources.hh" + using namespace gz; struct TestFeatureList : physics::FeatureList< @@ -172,8 +174,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto meshLink = model->ConstructEmptyLink("mesh_link"); meshLink->AttachFixedJoint(child, "fixed"); - const std::string meshFilename = common::joinPaths( - GZ_PHYSICS_RESOURCE_DIR, "chassis.dae"); + const std::string meshFilename = gz::physics::test::resources::kChassisDae; auto &meshManager = *common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); @@ -212,8 +213,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto heightmapLink = model->ConstructEmptyLink("heightmap_link"); heightmapLink->AttachFixedJoint(child, "heightmap_joint"); - auto heightmapFilename = common::joinPaths( - GZ_PHYSICS_RESOURCE_DIR, "heightmap_bowl.png"); + auto heightmapFilename = gz::physics::test::resources::kHeightmapBowlPng; common::ImageHeightmap data; EXPECT_EQ(0, data.Load(heightmapFilename)); @@ -239,8 +239,7 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto demLink = model->ConstructEmptyLink("dem_link"); demLink->AttachFixedJoint(child, "dem_joint"); - auto demFilename = common::joinPaths( - GZ_PHYSICS_RESOURCE_DIR, "volcano.tif"); + auto demFilename = gz::physics::test::resources::kVolcanoTif; common::Dem dem; EXPECT_EQ(0, dem.Load(demFilename)); diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index b1c7218b5..83e6095cf 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -53,6 +53,7 @@ #include #include +#include "dartsim/src/Worlds.hh" using namespace gz; @@ -313,7 +314,7 @@ INSTANTIATE_TEST_SUITE_P(LoadWorld, SDFFeatures_TEST, // Test that the dartsim plugin loaded all the relevant information correctly. TEST_P(SDFFeatures_TEST, CheckDartsimData) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -421,7 +422,7 @@ TEST_P(SDFFeatures_TEST, CheckDartsimData) // Test that joint limits are working by running the simulation TEST_P(SDFFeatures_TEST, CheckJointLimitEnforcement) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -580,8 +581,7 @@ TEST_P(SDFFeatures_TEST, WorldIsParentOrChild) // TEST_P(SDFFeatures_TEST, WorldWithNestedModel) { - WorldPtr world = - this->LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kWorldWithNestedModelSdf); ASSERT_NE(nullptr, world); EXPECT_EQ(2u, world->GetModelCount()); @@ -646,7 +646,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) { WorldPtr world = this->LoadWorld( - TEST_WORLD_DIR "/world_with_nested_model_joint_to_world.sdf"); + dartsim::worlds::kWorldWithNestedModelJointToWorldSdf); ASSERT_NE(nullptr, world); EXPECT_EQ(1u, world->GetModelCount()); @@ -690,7 +690,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) // Test that joint type falls back to fixed if the type is not supported TEST_P(SDFFeatures_TEST, FallbackToFixedJoint) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -717,7 +717,7 @@ TEST_P(SDFFeatures_TEST, FallbackToFixedJoint) TEST_P(SDFFeatures_TEST, JointsAcrossNestedModels) { WorldPtr world = this->LoadWorld( - TEST_WORLD_DIR "/joint_across_nested_models.sdf"); + dartsim::worlds::kJointAcrossNestedModelsSdf); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -775,7 +775,7 @@ INSTANTIATE_TEST_SUITE_P(LoadWorld, SDFFeatures_FrameSemantics, ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, LinkRelativeTo) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kModelFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "link_relative_to"; @@ -807,7 +807,7 @@ TEST_P(SDFFeatures_FrameSemantics, LinkRelativeTo) ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, CollisionRelativeTo) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kModelFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "collision_relative_to"; @@ -844,7 +844,7 @@ TEST_P(SDFFeatures_FrameSemantics, CollisionRelativeTo) ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kModelFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "explicit_frames_with_links"; @@ -891,7 +891,7 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kModelFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "explicit_frames_with_collisions"; @@ -928,7 +928,7 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) { - WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/world_frames.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kWorldFramesSdf); ASSERT_NE(nullptr, world); const std::string modelName = "M"; @@ -938,7 +938,6 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); - ASSERT_NE(nullptr, skeleton); ASSERT_EQ(1u, skeleton->getNumBodyNodes()); @@ -965,7 +964,7 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) ///////////////////////////////////////////////// TEST_P(SDFFeatures_TEST, Shapes) { - auto world = this->LoadWorld(TEST_WORLD_DIR"/shapes.sdf"); + WorldPtr world = this->LoadWorld(dartsim::worlds::kShapesSdf); ASSERT_NE(nullptr, world); auto dartWorld = world->GetDartsimWorld(); diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index 459460ba2..9f6fa75ba 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -32,6 +32,7 @@ #include #include "test/Utils.hh" +#include "Worlds.hh" struct TestFeatureList : gz::physics::FeatureList< gz::physics::CollisionDetector, @@ -82,8 +83,7 @@ TestWorldPtr LoadWorld( ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, CollisionDetector) { - auto world = LoadWorld(this->engine, - gz::common::joinPaths(TEST_WORLD_DIR, "empty.sdf")); + const auto world = LoadWorld(this->engine, dartsim::worlds::kEmptySdf); EXPECT_EQ("ode", world->GetCollisionDetector()); world->SetCollisionDetector("banana"); @@ -105,8 +105,7 @@ TEST_F(WorldFeaturesFixture, CollisionDetector) ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, Solver) { - auto world = LoadWorld(this->engine, - gz::common::joinPaths(TEST_WORLD_DIR, "empty.sdf")); + const auto world = LoadWorld(this->engine, dartsim::worlds::kEmptySdf); EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver()); diff --git a/dartsim/src/Worlds.hh b/dartsim/src/Worlds.hh new file mode 100644 index 000000000..201d437ad --- /dev/null +++ b/dartsim/src/Worlds.hh @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef DARTSIM_WORLDS_HH_ +#define DARTSIM_WORLDS_HH_ + +#include + +#include + +// \brief retrieve a filename from the dartsim/worlds directory +// \param[in] _world filename to retrieve +// \return full path to the request world +inline std::string DartsimTestWorld(const std::string &_world) +{ + return gz::common::testing::SourceFile("dartsim", "worlds", _world); +} + +namespace dartsim::worlds +{ +const auto kAddedMassSdf = DartsimTestWorld("added_mass.sdf"); +const auto kEmptySdf = DartsimTestWorld("empty.sdf"); +const auto kFallingWorld = DartsimTestWorld("falling.world"); +const auto kJointAcrossNestedModelsSdf = + DartsimTestWorld("joint_across_nested_models.sdf"); +const auto kModelFramesSdf = DartsimTestWorld("model_frames.sdf"); +const auto kShapesSdf = DartsimTestWorld("shapes.sdf"); +const auto kShapesBitmaskWorld = DartsimTestWorld("shapes_bitmask.sdf"); +const auto kTestWorld = DartsimTestWorld("test.world"); +const auto kWorldFramesSdf = DartsimTestWorld("world_frames.sdf"); +const auto kWorldWithNestedModelSdf = + DartsimTestWorld("world_with_nested_model.sdf"); +const auto kWorldWithNestedModelJointToWorldSdf = + DartsimTestWorld("world_with_nested_model_joint_to_world.sdf"); + +} // namespace dartsim::worlds +#endif // DARTSIM_WORLDS_HH_ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3f57a1355..8e00aed09 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -25,8 +25,11 @@ endif() gz_build_tests( TYPE UNIT SOURCES ${gtest_sources} + LIB_DEPS + gz-physics-test ENVIRONMENT - GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} +) if(TARGET UNIT_FindFeatures_TEST) target_link_libraries(UNIT_FindFeatures_TEST diff --git a/src/Cloneable_TEST.cc b/src/Cloneable_TEST.cc index 08f8bf7c5..db5766197 100644 --- a/src/Cloneable_TEST.cc +++ b/src/Cloneable_TEST.cc @@ -18,7 +18,7 @@ #include #include "gz/physics/Cloneable.hh" -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" using namespace gz; using physics::Cloneable; diff --git a/src/CompositeData_TEST.cc b/src/CompositeData_TEST.cc index 4e76776d0..3a9e1a2b2 100644 --- a/src/CompositeData_TEST.cc +++ b/src/CompositeData_TEST.cc @@ -18,7 +18,7 @@ #include #include "gz/physics/CompositeData.hh" -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" using namespace gz; diff --git a/src/FindFeatures_TEST.cc b/src/FindFeatures_TEST.cc index 165e3f42f..4309807ab 100644 --- a/src/FindFeatures_TEST.cc +++ b/src/FindFeatures_TEST.cc @@ -24,7 +24,7 @@ #include -#include "TestUtilities.hh" +#include "test/Utils.hh" using namespace gz; @@ -59,7 +59,7 @@ TEST(FindFeatures_TEST, ForwardStep) TEST(FindFeatures_TEST, Unimplemented) { using TestFeatures = - physics::FeatureList; + physics::FeatureList; plugin::Loader loader; PrimeTheLoader(loader); diff --git a/src/SpecifyData_TEST.cc b/src/SpecifyData_TEST.cc index 4b46f28c4..2018f2018 100644 --- a/src/SpecifyData_TEST.cc +++ b/src/SpecifyData_TEST.cc @@ -19,7 +19,7 @@ #define GZ_UNITTEST_EXPECTDATA_ACCESS -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" #include "gz/physics/SpecifyData.hh" #include "gz/math/Vector3.hh" diff --git a/src/TestUtilities.hh b/src/TestUtilities.hh deleted file mode 100644 index 75c0c7cad..000000000 --- a/src/TestUtilities.hh +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright (C) 2020 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef SRC_GZ_PHYSICS_TESTUTILITIES_HH_ -#define SRC_GZ_PHYSICS_TESTUTILITIES_HH_ - -#include - -#include -#include - -#include - -using namespace gz; -namespace test -{ - class UnimplementedFeature : public virtual physics::Feature - { - public: template - class Implementation : public virtual Feature::Implementation - { - public: virtual void someUnimplementedVirtualFunction() = 0; - - public: ~Implementation() override; - }; - }; -} - -inline void PrimeTheLoader(plugin::Loader &_loader) -{ - for (const std::string &library - : physics::test::g_PhysicsPluginLibraries) - { - if (!library.empty()) - _loader.LoadLib(library); - } -} - -#endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 5e1e3e0ec..47dbfe30f 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -19,6 +19,17 @@ ExternalProject_Add( "-DCMAKE_INSTALL_PREFIX=${FAKE_INSTALL_PREFIX}" ) +add_library(gz-physics-test INTERFACE) +target_include_directories(gz-physics-test INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_compile_definitions(gz-physics-test INTERFACE + "TESTING_PROJECT_SOURCE_DIR=\"${PROJECT_SOURCE_DIR}\"" +) +target_link_libraries(gz-physics-test INTERFACE + gz-plugin${GZ_PLUGIN_VER}::loader + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-common${GZ_COMMON_VER}::testing +) + add_subdirectory(gtest_vendor) add_subdirectory(benchmark) add_subdirectory(common_test) diff --git a/test/benchmark/CMakeLists.txt b/test/benchmark/CMakeLists.txt index 292a4f003..c9e800fde 100644 --- a/test/benchmark/CMakeLists.txt +++ b/test/benchmark/CMakeLists.txt @@ -4,5 +4,4 @@ set(tests ExpectData.cc ) -gz_add_benchmarks(SOURCES ${tests} - INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/src) +gz_add_benchmarks(SOURCES ${tests} LIB_DEPS gz-physics-test) diff --git a/test/benchmark/ExpectData.cc b/test/benchmark/ExpectData.cc index 1a8277276..08c30bc20 100644 --- a/test/benchmark/ExpectData.cc +++ b/test/benchmark/ExpectData.cc @@ -1,6 +1,6 @@ #include -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" std::size_t gNumTests = 100000; diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 2e38a0c25..1ec78d74e 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -1,5 +1,4 @@ set(TEST_TYPE "COMMON_TEST") -include_directories(${BULLET_INCLUDE_DIRS}) set(tests added_mass @@ -45,7 +44,6 @@ function(configure_common_test PHYSICS_ENGINE_NAME test_name) endif() endfunction() -set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources") foreach(test ${tests}) set(test_executable "${TEST_TYPE}_${test}") @@ -53,8 +51,7 @@ foreach(test ${tests}) target_link_libraries(${test_executable} PUBLIC - gz-plugin${GZ_PLUGIN_VER}::loader - gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-physics-test ${PROJECT_LIBRARY_TARGET_NAME} ${PROJECT_LIBRARY_TARGET_NAME}-sdf ${PROJECT_LIBRARY_TARGET_NAME}-mesh @@ -62,15 +59,15 @@ foreach(test ${tests}) gtest_main ) - target_compile_definitions(${test_executable} PRIVATE - "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"" - ) + target_include_directories(${test_executable} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) install(TARGETS ${test_executable} DESTINATION ${TEST_INSTALL_DIR}) - configure_common_test("bullet" ${test_executable}) configure_common_test("bullet-featherstone" ${test_executable}) configure_common_test("dartsim" ${test_executable}) configure_common_test("tpe" ${test_executable}) endforeach() + +if(TARGET COMMON_TEST_joint_transmitted_wrench_features) + target_include_directories(COMMON_TEST_joint_transmitted_wrench_features PRIVATE ${BULLET_INCLUDE_DIRS}) +endif() diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh new file mode 100644 index 000000000..d57aa32d3 --- /dev/null +++ b/test/common_test/Worlds.hh @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef COMMON_TEST_WORLDS_WORLDS_HH_ +#define COMMON_TEST_WORLDS_WORLDS_HH_ + +#include + +#include + +inline std::string CommonTestWorld(const std::string &_world) +{ + return gz::common::testing::TestFile("common_test", "worlds", _world); +} + +namespace common_test::worlds +{ +const auto kContactSdf = CommonTestWorld("contact.sdf"); +const auto kDetachableJointWorld = CommonTestWorld("detachable_joint.world"); +const auto kEmptySdf = CommonTestWorld("empty.sdf"); +const auto kFallingWorld = CommonTestWorld("falling.world"); +const auto kFallingAddedMassWorld = CommonTestWorld("falling_added_mass.world"); +const auto kGroundSdf = CommonTestWorld("ground.sdf"); +const auto kJointAcrossModelsSdf = CommonTestWorld("joint_across_models.sdf"); +const auto kJointConstraintSdf = CommonTestWorld("joint_constraint.sdf"); +const auto kMultipleCollisionsSdf = CommonTestWorld("multiple_collisions.sdf"); +const auto kPendulumJointWrenchSdf = + CommonTestWorld("pendulum_joint_wrench.sdf"); +const auto kShapesWorld = CommonTestWorld("shapes.world"); +const auto kShapesBitmaskWorld = CommonTestWorld("shapes_bitmask.sdf"); +const auto kSlipComplianceSdf = CommonTestWorld("slip_compliance.sdf"); +const auto kSphereSdf = CommonTestWorld("sphere.sdf"); +const auto kStringPendulumSdf = CommonTestWorld("string_pendulum.sdf"); +const auto kTestWorld = CommonTestWorld("test.world"); +const auto kWorldJointTestSdf = CommonTestWorld("world_joint_test.sdf"); +const auto kWorldUnsortedLinksSdf = CommonTestWorld("world_unsorted_links.sdf"); +const auto kWorldWithNestedModelSdf = + CommonTestWorld("world_with_nested_model.sdf"); +} // namespace common_test::worlds +#endif // COMMON_TEST_WORLDS_WORLDS_HH_ diff --git a/test/common_test/added_mass.cc b/test/common_test/added_mass.cc index f46468bab..c3d5cb902 100644 --- a/test/common_test/added_mass.cc +++ b/test/common_test/added_mass.cc @@ -17,10 +17,12 @@ #include #include + #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -87,7 +89,7 @@ TEST_F(AddedMassFeaturesTest, Gravity) sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "falling_added_mass.world")); + common_test::worlds::kFallingAddedMassWorld); EXPECT_TRUE(errors.empty()) << errors; const sdf::World *sdfWorld = root.WorldByIndex(0); EXPECT_NE(nullptr, sdfWorld); diff --git a/test/common_test/addexternalforcetorque.cc b/test/common_test/addexternalforcetorque.cc index 93409f345..21794458e 100644 --- a/test/common_test/addexternalforcetorque.cc +++ b/test/common_test/addexternalforcetorque.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -108,7 +109,7 @@ TYPED_TEST(LinkFeaturesTest, JointSetCommand) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "sphere.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kSphereSdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"sphere"}; diff --git a/test/common_test/basic_test.cc b/test/common_test/basic_test.cc index a3b91d87b..96af1eeb5 100644 --- a/test/common_test/basic_test.cc +++ b/test/common_test/basic_test.cc @@ -19,7 +19,7 @@ #include #include -#include "TestLibLoader.hh" +#include "test/TestLibLoader.hh" #include #include diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 48189718c..4b987ab50 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -19,7 +19,8 @@ #include #include -#include "TestLibLoader.hh" +#include "test/Resources.hh" +#include "test/TestLibLoader.hh" #include #include @@ -35,8 +36,6 @@ #include -// #include - template class CollisionTest: public testing::Test, public gz::physics::TestLibLoader @@ -100,8 +99,7 @@ TYPED_TEST(CollisionTest, MeshAndPlane) auto model = world->ConstructEmptyModel("mesh"); auto link = model->ConstructEmptyLink("link"); - const std::string meshFilename = gz::common::joinPaths( - GZ_PHYSICS_RESOURCE_DIR, "chassis.dae"); + const std::string meshFilename = gz::physics::test::resources::kChassisDae; auto &meshManager = *gz::common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); diff --git a/test/common_test/construct_empty_world.cc b/test/common_test/construct_empty_world.cc index a2974aaba..f454b9ee4 100644 --- a/test/common_test/construct_empty_world.cc +++ b/test/common_test/construct_empty_world.cc @@ -19,7 +19,7 @@ #include #include -#include "TestLibLoader.hh" +#include "test/TestLibLoader.hh" #include #include diff --git a/test/common_test/detachable_joint.cc b/test/common_test/detachable_joint.cc index ab06e6bb9..fad71648d 100644 --- a/test/common_test/detachable_joint.cc +++ b/test/common_test/detachable_joint.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include "gz/physics/FrameSemantics.hh" #include @@ -111,7 +112,7 @@ TYPED_TEST(DetachableJointTest, CorrectAttachmentPoints) sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "detachable_joint.world")); + common_test::worlds::kDetachableJointWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 9292fe66a..d7bd4903c 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -18,7 +18,8 @@ #include -#include "TestLibLoader.hh" +#include "test/TestLibLoader.hh" +#include "Worlds.hh" #include #include @@ -36,7 +37,6 @@ #include "gz/physics/sdf/ConstructModel.hh" #include "gz/physics/sdf/ConstructNestedModel.hh" #include "gz/physics/sdf/ConstructWorld.hh" -// #include "test/Utils.hh" struct TestFeatureList : gz::physics::FeatureList< gz::physics::GetEngineInfo, @@ -112,7 +112,8 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors &errors = root.Load(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + const sdf::Errors &errors = root.Load( + common_test::worlds::kWorldWithNestedModelSdf); EXPECT_EQ(0u, errors.size()) << errors; EXPECT_EQ(1u, root.WorldCount()); @@ -163,7 +164,8 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors &errors = root.Load(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + const sdf::Errors &errors = root.Load( + common_test::worlds::kWorldWithNestedModelSdf); EXPECT_EQ(0u, errors.size()) << errors; EXPECT_EQ(1u, root.WorldCount()); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 2ceb48b83..5f0a695b6 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include "gz/physics/FrameSemantics.hh" #include @@ -115,7 +116,7 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; @@ -225,7 +226,7 @@ TYPED_TEST(JointFeaturesPositionLimitsTest, JointSetPositionLimitsWithForceContr ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -331,7 +332,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWi ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -418,7 +419,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetEffortLimitsWith ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -495,7 +496,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -603,7 +604,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, DISABLED_JointSetPositio ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"simple_joint_test"}; @@ -655,7 +656,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWi ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -726,7 +727,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetEffortLimitsWith ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -786,7 +787,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -861,7 +862,7 @@ TYPED_TEST(JointFeaturesDetachTest, JointDetach) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; @@ -1022,7 +1023,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "joint_across_models.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsSdf); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1166,7 +1167,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachMultiple) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "joint_constraint.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kJointConstraintSdf); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1315,7 +1316,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, LinkCountsInJointAttachDetach) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "joint_across_models.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kJointAcrossModelsSdf); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1412,7 +1413,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachSpawnedModel) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "ground.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kGroundSdf); ASSERT_TRUE(errors.empty()) << errors.front(); world = engine->ConstructWorld(*root.WorldByIndex(0)); @@ -1510,8 +1511,7 @@ class WorldModelTest : public JointFeaturesTest gz::physics::RequestEngine3d::From(plugin); sdf::Root root; - const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "world_joint_test.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kWorldJointTestSdf); EXPECT_TRUE(errors.empty()) << errors; if (errors.empty()) { diff --git a/test/common_test/joint_transmitted_wrench_features.cc b/test/common_test/joint_transmitted_wrench_features.cc index 8d1563b2d..afa0d55fa 100644 --- a/test/common_test/joint_transmitted_wrench_features.cc +++ b/test/common_test/joint_transmitted_wrench_features.cc @@ -25,8 +25,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include "gz/physics/FrameSemantics.hh" #include @@ -103,7 +104,7 @@ class JointTransmittedWrenchFixture : ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "pendulum_joint_wrench.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kPendulumJointWrenchSdf); ASSERT_TRUE(errors.empty()) << errors.front(); this->world = engine->ConstructWorld(*root.WorldByIndex(0)); diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index c479800a7..effa773dc 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -19,8 +19,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -94,8 +95,8 @@ TYPED_TEST(KinematicFeaturesTest, JointFrameSemantics) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = - root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "string_pendulum.sdf")); + const sdf::Errors errors = root.Load( + common_test::worlds::kStringPendulumSdf); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = engine->ConstructWorld(*root.WorldByIndex(0)); diff --git a/test/common_test/link_features.cc b/test/common_test/link_features.cc index 8b36b6424..adcb54817 100644 --- a/test/common_test/link_features.cc +++ b/test/common_test/link_features.cc @@ -24,8 +24,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -105,7 +106,7 @@ TYPED_TEST(LinkFeaturesTest, JointSetCommand) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "empty.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kEmptySdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; @@ -303,7 +304,7 @@ TYPED_TEST(LinkFeaturesTest, AxisAlignedBoundingBox) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kTestWorld); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; @@ -354,7 +355,7 @@ TYPED_TEST(LinkFeaturesTest, ModelAxisAlignedBoundingBox) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "contact.sdf")); + const sdf::Errors errors = root.Load(common_test::worlds::kContactSdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; diff --git a/test/common_test/shape_features.cc b/test/common_test/shape_features.cc index c2de4cf96..596c1f717 100644 --- a/test/common_test/shape_features.cc +++ b/test/common_test/shape_features.cc @@ -19,8 +19,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" // Features #include @@ -103,7 +104,8 @@ TYPED_TEST(ShapeFeaturesTest, PrimarySlipCompliance) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "slip_compliance.sdf")); + const sdf::Errors errors = root.Load( + common_test::worlds::kSlipComplianceSdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"box"}; @@ -175,7 +177,8 @@ TYPED_TEST(ShapeFeaturesTest, SecondarySlipCompliance) ASSERT_NE(nullptr, engine); sdf::Root root; - const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "slip_compliance.sdf")); + const sdf::Errors errors = root.Load( + common_test::worlds::kSlipComplianceSdf); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"box"}; diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 44eb69456..661a0222a 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -25,8 +25,9 @@ #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -208,7 +209,7 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); auto checkedOutput = StepWorld(world, true, 1).first; EXPECT_TRUE(checkedOutput); @@ -245,7 +246,7 @@ TYPED_TEST(SimulationFeaturesStepTest, StepWorld) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); auto checkedOutput = StepWorld(world, true, 1000).first; EXPECT_TRUE(checkedOutput); } @@ -278,10 +279,9 @@ TYPED_TEST(SimulationFeaturesFallingTest, Falling) // See https://github.com/gazebosim/gz-physics/issues/483 CHECK_UNSUPPORTED_ENGINE(name, "bullet") #endif - auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + common_test::worlds::kFallingWorld); auto [checkedOutput, output] = StepWorld(world, true, 1000); @@ -344,7 +344,7 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures) #endif auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); std::cerr << "world model count " << world->GetModelCount() << '\n'; // test ShapeFeatures auto sphere = world->GetModel("sphere"); @@ -523,7 +523,7 @@ TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "sphere.sdf")); + common_test::worlds::kSphereSdf); // model free group test auto model = world->GetModel("sphere"); @@ -611,7 +611,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeBoundingBox) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + common_test::worlds::kFallingWorld); auto sphere = world->GetModel("sphere"); auto sphereCollision = sphere->GetLink(0)->GetShape(0); auto ground = world->GetModel("box"); @@ -660,7 +660,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes_bitmask.sdf")); + common_test::worlds::kShapesBitmaskWorld); auto baseBox = world->GetModel("box_base"); auto filteredBox = world->GetModel("box_filtered"); @@ -705,7 +705,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts) LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); auto sphere = world->GetModel("sphere"); auto sphereFreeGroup = sphere->FindFreeGroup(); @@ -917,7 +917,7 @@ TYPED_TEST(SimulationFeaturesTestFeaturesContactPropertiesCallback, ContactPrope LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "contact.sdf")); + common_test::worlds::kContactSdf); auto sphere = world->GetModel("sphere"); auto groundPlane = world->GetModel("ground_plane"); @@ -1147,7 +1147,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, MultipleCollisions) auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "multiple_collisions.sdf")); + common_test::worlds::kMultipleCollisionsSdf); // model free group test auto model = world->GetModel("box"); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 6c08ddb98..ad28f8bbb 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -19,8 +19,9 @@ #include #include -#include "TestLibLoader.hh" -#include "../Utils.hh" +#include "test/TestLibLoader.hh" +#include "test/Utils.hh" +#include "Worlds.hh" #include #include @@ -93,8 +94,7 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures) std::string::npos); sdf::Root root; - const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "falling.world")); + const sdf::Errors errors = root.Load(common_test::worlds::kFallingWorld); EXPECT_TRUE(errors.empty()) << errors; const sdf::World *sdfWorld = root.WorldByIndex(0); EXPECT_NE(nullptr, sdfWorld); @@ -206,7 +206,8 @@ TEST_F(WorldFeaturesTestConstructModel, ConstructModelUnsortedLinks) sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "world_unsorted_links.sdf")); + common_test::worlds::kWorldUnsortedLinksSdf); + EXPECT_TRUE(errors.empty()) << errors; const sdf::World *sdfWorld = root.WorldByIndex(0); ASSERT_NE(nullptr, sdfWorld); @@ -245,7 +246,7 @@ class WorldModelTest : public WorldFeaturesTest sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "world_joint_test.sdf")); + common_test::worlds::kWorldJointTestSdf); EXPECT_TRUE(errors.empty()) << errors; if (errors.empty()) { diff --git a/test/MockCenterOfMass.hh b/test/include/mock/MockCenterOfMass.hh similarity index 100% rename from test/MockCenterOfMass.hh rename to test/include/mock/MockCenterOfMass.hh diff --git a/test/MockCreateEntities.hh b/test/include/mock/MockCreateEntities.hh similarity index 100% rename from test/MockCreateEntities.hh rename to test/include/mock/MockCreateEntities.hh diff --git a/test/MockDoublePendulum.hh b/test/include/mock/MockDoublePendulum.hh similarity index 100% rename from test/MockDoublePendulum.hh rename to test/include/mock/MockDoublePendulum.hh diff --git a/test/MockFeatures.hh b/test/include/mock/MockFeatures.hh similarity index 100% rename from test/MockFeatures.hh rename to test/include/mock/MockFeatures.hh diff --git a/test/MockFrameSemantics.hh b/test/include/mock/MockFrameSemantics.hh similarity index 100% rename from test/MockFrameSemantics.hh rename to test/include/mock/MockFrameSemantics.hh diff --git a/test/MockGetByName.hh b/test/include/mock/MockGetByName.hh similarity index 100% rename from test/MockGetByName.hh rename to test/include/mock/MockGetByName.hh diff --git a/test/MockJoints.hh b/test/include/mock/MockJoints.hh similarity index 100% rename from test/MockJoints.hh rename to test/include/mock/MockJoints.hh diff --git a/test/MockSetName.hh b/test/include/mock/MockSetName.hh similarity index 100% rename from test/MockSetName.hh rename to test/include/mock/MockSetName.hh diff --git a/test/PhysicsPluginsList.hh b/test/include/test/PhysicsPluginsList.hh similarity index 100% rename from test/PhysicsPluginsList.hh rename to test/include/test/PhysicsPluginsList.hh diff --git a/test/include/test/Resources.hh b/test/include/test/Resources.hh new file mode 100644 index 000000000..feab2acb9 --- /dev/null +++ b/test/include/test/Resources.hh @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef TEST_RESOURCES_HH_ +#define TEST_RESOURCES_HH_ + +#include + +#include + +inline std::string TestResource(const std::string &_filename) +{ + return gz::common::testing::TestFile("resources", _filename); +} + +namespace gz::physics::test::resources +{ +const auto kChassisDae = TestResource("chassis.dae"); +const auto kHeightmapBowlPng = TestResource("heightmap_bowl.png"); +const auto kRrbotXml = TestResource("rrbot.xml"); +const auto kVolcanoTif = TestResource("volcano.tif"); +} // namespace gz::physics::test + +#endif // TEST_RESOURCES_HH_ diff --git a/src/utils/TestDataTypes.hh b/test/include/test/TestDataTypes.hh similarity index 100% rename from src/utils/TestDataTypes.hh rename to test/include/test/TestDataTypes.hh diff --git a/test/common_test/TestLibLoader.hh b/test/include/test/TestLibLoader.hh similarity index 98% rename from test/common_test/TestLibLoader.hh rename to test/include/test/TestLibLoader.hh index 5d5910eea..a7f27abd3 100644 --- a/test/common_test/TestLibLoader.hh +++ b/test/include/test/TestLibLoader.hh @@ -24,6 +24,9 @@ #include #include +#include + +#include namespace gz { diff --git a/test/Utils.hh b/test/include/test/Utils.hh similarity index 91% rename from test/Utils.hh rename to test/include/test/Utils.hh index 5755c6618..81d865ef2 100644 --- a/test/Utils.hh +++ b/test/include/test/Utils.hh @@ -23,9 +23,37 @@ #include #include #include +#include + +#include + +#include + +///////////////////////////////////////////////// +inline void PrimeTheLoader(gz::plugin::Loader &_loader) +{ + for (const std::string &library + : gz::physics::test::g_PhysicsPluginLibraries) + { + if (!library.empty()) + _loader.LoadLib(library); + } +} namespace gz::physics::test { +///////////////////////////////////////////////// +class UnimplementedFeature : public virtual gz::physics::Feature +{ + public: template + class Implementation : public virtual gz::physics::Feature::Implementation + { + public: virtual void someUnimplementedVirtualFunction() = 0; + + public: ~Implementation() override; + }; +}; + ///////////////////////////////////////////////// template VectorType RandomVector(const double range) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 6cf8c9eb6..8c0b32183 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -11,12 +11,11 @@ gz_build_tests( SOURCES ${tests} LIB_DEPS Eigen3::Eigen - gz-plugin${GZ_PLUGIN_VER}::loader - INCLUDE_DIRS - ${PROJECT_SOURCE_DIR}/src + gz-physics-test TEST_LIST list ENVIRONMENT - GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + TEST_LIST list) if (BUILD_TESTING) foreach(test ${list}) diff --git a/test/integration/CanReadWrite.cc b/test/integration/CanReadWrite.cc index bc9583ef6..1385c08e6 100644 --- a/test/integration/CanReadWrite.cc +++ b/test/integration/CanReadWrite.cc @@ -23,7 +23,7 @@ #include "gz/physics/CanReadData.hh" #include "gz/physics/CanWriteData.hh" -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" using gz::physics::CanReadRequiredData; using gz::physics::CanReadExpectedData; diff --git a/test/integration/DoublePendulum.cc b/test/integration/DoublePendulum.cc index 329d1e5e2..482e2e137 100644 --- a/test/integration/DoublePendulum.cc +++ b/test/integration/DoublePendulum.cc @@ -26,7 +26,7 @@ #include #include -#include "../MockDoublePendulum.hh" +#include "mock/MockDoublePendulum.hh" using namespace gz::physics; diff --git a/test/integration/FeatureSystem.cc b/test/integration/FeatureSystem.cc index 48fd3b453..46ee7a74f 100644 --- a/test/integration/FeatureSystem.cc +++ b/test/integration/FeatureSystem.cc @@ -20,7 +20,7 @@ #include #include -#include "../MockFeatures.hh" +#include "mock/MockFeatures.hh" using namespace gz::physics; diff --git a/test/integration/FrameSemantics.hh b/test/integration/FrameSemantics.hh index 57de63556..093776189 100644 --- a/test/integration/FrameSemantics.hh +++ b/test/integration/FrameSemantics.hh @@ -27,8 +27,8 @@ #include #include -#include "../Utils.hh" -#include "../MockFrameSemantics.hh" +#include "test/Utils.hh" +#include "mock/MockFrameSemantics.hh" using gz::physics::FrameData; using gz::physics::FrameID; diff --git a/test/integration/JointTypes.hh b/test/integration/JointTypes.hh index d658604c9..dc4f730f1 100644 --- a/test/integration/JointTypes.hh +++ b/test/integration/JointTypes.hh @@ -29,8 +29,8 @@ #include #include -#include "../MockJoints.hh" -#include "../Utils.hh" +#include "mock/MockJoints.hh" +#include "test/Utils.hh" using namespace gz::physics; using namespace gz::physics::test; diff --git a/test/integration/RequestFeatures.cc b/test/integration/RequestFeatures.cc index 7e0512cf0..81f70c5fc 100644 --- a/test/integration/RequestFeatures.cc +++ b/test/integration/RequestFeatures.cc @@ -29,8 +29,8 @@ #include -#include "../../src/TestUtilities.hh" -#include "../MockFeatures.hh" +#include "test/Utils.hh" +#include "mock/MockFeatures.hh" TEST(RequestFeatures_TEST, Casting) { @@ -48,7 +48,7 @@ TEST(RequestFeatures_TEST, Casting) mock::MockCenterOfMass>; using UnavailableFeatures = - gz::physics::FeatureList; + gz::physics::FeatureList; // Get a list of all plugins who satisfy the features that are needed for // testing diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index 6a1627d67..dcb1ae5f7 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -10,8 +10,8 @@ endif() gz_build_tests( TYPE PERFORMANCE SOURCES ${tests} - INCLUDE_DIRS - ${PROJECT_SOURCE_DIR}/src ENVIRONMENT GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + LIB_DEPS + gz-physics-test ) diff --git a/test/performance/ExpectData.cc b/test/performance/ExpectData.cc index 1c1842343..41ff92eca 100644 --- a/test/performance/ExpectData.cc +++ b/test/performance/ExpectData.cc @@ -21,7 +21,7 @@ #include #include -#include "utils/TestDataTypes.hh" +#include "test/TestDataTypes.hh" std::size_t gNumTests = 100000; diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt index 6d76ac351..92c038744 100644 --- a/test/plugins/CMakeLists.txt +++ b/test/plugins/CMakeLists.txt @@ -17,6 +17,7 @@ endif() foreach(plugin IN LISTS plugins) target_link_libraries(${plugin} PRIVATE + gz-physics-test ${PROJECT_LIBRARY_TARGET_NAME}) endforeach() @@ -27,8 +28,6 @@ if (DART_FOUND) set(DART_LIBRARIES_NO_BULLET ${DART_LIBRARIES}) list(REMOVE_ITEM DART_LIBRARIES_NO_BULLET dart-collision-bullet) target_link_libraries(MockDoublePendulum PUBLIC ${DART_LIBRARIES_NO_BULLET}) - target_compile_definitions(MockDoublePendulum PRIVATE - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"") if (MSVC) # needed by DART, see https://github.com/dartsim/dart/issues/753 target_compile_options(MockDoublePendulum PUBLIC "/permissive-") diff --git a/test/plugins/DARTDoublePendulum.cc b/test/plugins/DARTDoublePendulum.cc index a5a0f9836..d18f8684a 100644 --- a/test/plugins/DARTDoublePendulum.cc +++ b/test/plugins/DARTDoublePendulum.cc @@ -26,7 +26,8 @@ #include #include -#include "../MockDoublePendulum.hh" +#include "mock/MockDoublePendulum.hh" +#include "test/Resources.hh" namespace mock { @@ -65,7 +66,8 @@ namespace mock lastId(0) { ::dart::utils::DartLoader loader; - this->robot = loader.parseSkeleton(GZ_PHYSICS_RESOURCE_DIR "/rrbot.xml"); + this->robot = loader.parseSkeleton( + gz::physics::test::resources::kRrbotXml); this->world->addSkeleton(this->robot); this->joint1 = this->robot->getJoint("joint1"); diff --git a/test/plugins/MockEntities.cc b/test/plugins/MockEntities.cc index 20bd674a6..7a9bd8c96 100644 --- a/test/plugins/MockEntities.cc +++ b/test/plugins/MockEntities.cc @@ -15,7 +15,7 @@ * */ -#include "../MockFeatures.hh" +#include "mock/MockFeatures.hh" #include diff --git a/test/plugins/MockJoints.cc b/test/plugins/MockJoints.cc index e80e671ce..54dd3b06c 100644 --- a/test/plugins/MockJoints.cc +++ b/test/plugins/MockJoints.cc @@ -20,7 +20,7 @@ #include -#include "../MockJoints.hh" +#include "mock/MockJoints.hh" namespace mock { diff --git a/test/plugins/frames.cc b/test/plugins/frames.cc index 1eb6e6fb0..5b02e3831 100644 --- a/test/plugins/frames.cc +++ b/test/plugins/frames.cc @@ -15,7 +15,7 @@ * */ -#include "../MockFrameSemantics.hh" +#include "mock/MockFrameSemantics.hh" #include #include diff --git a/resources/chassis.dae b/test/resources/chassis.dae similarity index 100% rename from resources/chassis.dae rename to test/resources/chassis.dae diff --git a/resources/heightmap_bowl.png b/test/resources/heightmap_bowl.png similarity index 100% rename from resources/heightmap_bowl.png rename to test/resources/heightmap_bowl.png diff --git a/resources/rrbot.xml b/test/resources/rrbot.xml similarity index 100% rename from resources/rrbot.xml rename to test/resources/rrbot.xml diff --git a/resources/volcano.tif b/test/resources/volcano.tif similarity index 100% rename from resources/volcano.tif rename to test/resources/volcano.tif diff --git a/tpe/plugin/CMakeLists.txt b/tpe/plugin/CMakeLists.txt index 3d73d6828..573fe8adb 100644 --- a/tpe/plugin/CMakeLists.txt +++ b/tpe/plugin/CMakeLists.txt @@ -55,20 +55,17 @@ gz_build_tests( SOURCES ${test_sources} LIB_DEPS ${features} - gz-plugin${GZ_PLUGIN_VER}::loader + gz-physics-test gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} ${PROJECT_LIBRARY_TARGET_NAME}-sdf ${PROJECT_LIBRARY_TARGET_NAME}-mesh TEST_LIST tests ENVIRONMENT - GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INCLUDE_DIRS + ${tpelib_dir}) foreach(test ${tests}) - - target_include_directories(${test} PRIVATE ${tpelib_dir}) target_compile_definitions(${test} PRIVATE - "tpe_plugin_LIB=\"$\"" - "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" - "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"") - + "tpe_plugin_LIB=\"$\"") endforeach() diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc index b9b592d86..eb619c82d 100644 --- a/tpe/plugin/src/SDFFeatures_TEST.cc +++ b/tpe/plugin/src/SDFFeatures_TEST.cc @@ -23,6 +23,7 @@ #include #include +#include "Worlds.hh" #include @@ -90,7 +91,7 @@ World LoadWorld(const std::string &_world) // Test that the tpe plugin loaded all the relevant information correctly. TEST(SDFFeatures_TEST, CheckTpeData) { - World world = LoadWorld(TEST_WORLD_DIR"/test.world"); + auto world = LoadWorld(tpe::worlds::kTestWorld); auto tpeWorld = world.GetTpeLibWorld(); ASSERT_NE(nullptr, tpeWorld); @@ -363,7 +364,7 @@ TEST(SDFFeatures_TEST, CheckTpeData) // Test that the tpe plugin loaded nested models correctly. TEST(SDFFeatures_TEST, NestedModel) { - World world = LoadWorld(TEST_WORLD_DIR"/nested_model.world"); + auto world = LoadWorld(tpe::worlds::kNestedModelWorld); auto tpeWorld = world.GetTpeLibWorld(); ASSERT_NE(nullptr, tpeWorld); diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 7fa3de841..81660996c 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -18,6 +18,7 @@ #include #include + #include #include @@ -42,6 +43,7 @@ #include #include +#include "Worlds.hh" #include "EntityManagementFeatures.hh" #include "FreeGroupFeatures.hh" @@ -49,6 +51,7 @@ #include "SimulationFeatures.hh" #include "World.hh" + using namespace gz; struct TestFeatureList : physics::FeatureList< @@ -273,7 +276,7 @@ TEST_P(SimulationFeatures_TEST, StepWorld) return; gzdbg << "Testing library " << library << std::endl; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world"); + const auto worlds = LoadWorlds(library, tpe::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -296,7 +299,7 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) if (library.empty()) return; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world"); + const auto worlds = LoadWorlds(library, tpe::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -435,7 +438,7 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) if (library.empty()) return; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world"); + const auto worlds = LoadWorlds(library, tpe::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -483,8 +486,7 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) if (library.empty()) return; - auto worlds = - LoadWorldsPiecemeal(library, TEST_WORLD_DIR "/nested_model.world"); + const auto worlds = LoadWorlds(library, tpe::worlds::kNestedModelWorld); for (const auto &world : worlds) { @@ -534,7 +536,10 @@ TEST_P(SimulationFeatures_TEST, CollideBitmasks) if (library.empty()) return; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes_bitmask.sdf"); + const auto worldPath = + gz::common::testing::SourceFile( + "tpe", "plugin", "worlds", "shapes_bitmask.sdf"); + const auto worlds = LoadWorlds(library, tpe::worlds::kShapesBitmaskSdf); for (const auto &world : worlds) { @@ -578,7 +583,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) if (library.empty()) return; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes.world"); + const auto worlds = LoadWorlds(library, tpe::worlds::kShapesWorld); for (const auto &world : worlds) { diff --git a/tpe/plugin/src/Worlds.hh b/tpe/plugin/src/Worlds.hh new file mode 100644 index 000000000..d700ba878 --- /dev/null +++ b/tpe/plugin/src/Worlds.hh @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef TPE_PLUGIN_WORLDS_HH_ +#define TPE_PLUGIN_WORLDS_HH_ + +#include + +#include + +// \brief retrieve a filename from the tpe/plugin/worlds directory +// \param[in] _world filename to retrieve +// \return full path to the request world +inline std::string TpeTestWorld(const std::string &_world) +{ + return gz::common::testing::SourceFile("tpe", "plugin", "worlds", _world); +} + +namespace tpe::worlds +{ +const auto kNestedModelWorld = TpeTestWorld("nested_model.world"); +const auto kShapesWorld = TpeTestWorld("shapes.world"); +const auto kShapesBitmaskSdf = TpeTestWorld("shapes_bitmask.sdf"); +const auto kTestWorld = TpeTestWorld("test.world"); +} // namespace tpe::worlds +#endif // TPE_PLUGIN_WORLDS_HH_ From 6b607d95dfb4ba6910d66f54dde5d21c69f0f628 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 11 Jan 2024 09:04:26 -0600 Subject: [PATCH 2/7] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Michael Carroll --- dartsim/src/Worlds.hh | 1 - test/common_test/collisions.cc | 2 ++ test/include/test/Resources.hh | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/dartsim/src/Worlds.hh b/dartsim/src/Worlds.hh index 201d437ad..0cc2331da 100644 --- a/dartsim/src/Worlds.hh +++ b/dartsim/src/Worlds.hh @@ -46,6 +46,5 @@ const auto kWorldWithNestedModelSdf = DartsimTestWorld("world_with_nested_model.sdf"); const auto kWorldWithNestedModelJointToWorldSdf = DartsimTestWorld("world_with_nested_model_joint_to_world.sdf"); - } // namespace dartsim::worlds #endif // DARTSIM_WORLDS_HH_ diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 4b987ab50..f78495126 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -16,6 +16,8 @@ */ #include +#include + #include #include diff --git a/test/include/test/Resources.hh b/test/include/test/Resources.hh index feab2acb9..8b2eff9c7 100644 --- a/test/include/test/Resources.hh +++ b/test/include/test/Resources.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2024 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. From 6c61ec7320d02691ab854797a91cff5298fabd31 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 29 Jan 2024 15:56:47 +0000 Subject: [PATCH 3/7] Remove duplicate cmake logic Signed-off-by: Michael Carroll --- test/common_test/CMakeLists.txt | 4 ---- 1 file changed, 4 deletions(-) diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index f119dcc34..a35e2127a 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -78,7 +78,3 @@ foreach(test ${tests}) configure_common_test("dartsim" ${test_executable}) configure_common_test("tpe" ${test_executable}) endforeach() - -if(TARGET COMMON_TEST_joint_transmitted_wrench_features) - target_include_directories(COMMON_TEST_joint_transmitted_wrench_features PRIVATE ${BULLET_INCLUDE_DIRS}) -endif() From 6607c912530d363908632106d1bac805ec173f41 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 29 Jan 2024 16:38:12 +0000 Subject: [PATCH 4/7] Mark testing component required Signed-off-by: Michael Carroll --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d55743e38..a2d1f57cc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,7 @@ gz_find_package(gz-common5 set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) # This is only used for test support -gz_find_package(gz-common5 COMPONENTS testing) +gz_find_package(gz-common5 REQUIRED COMPONENTS testing) #-------------------------------------- # Find gz-math From 537a85453951fce0186feb3525496bb0e314fc5a Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 29 Jan 2024 17:35:52 +0000 Subject: [PATCH 5/7] Consolidate duplicated worlds Signed-off-by: Michael Carroll --- dartsim/src/SDFFeatures_TEST.cc | 15 +- dartsim/src/WorldFeatures_TEST.cc | 6 +- dartsim/src/Worlds.hh | 7 - dartsim/worlds/empty.sdf | 5 - dartsim/worlds/falling.world | 75 ---- dartsim/worlds/shapes.sdf | 191 ---------- dartsim/worlds/shapes_bitmask.sdf | 154 --------- dartsim/worlds/test.world | 383 --------------------- dartsim/worlds/world_with_nested_model.sdf | 75 ---- tpe/plugin/src/SDFFeatures_TEST.cc | 43 ++- tpe/plugin/src/SimulationFeatures_TEST.cc | 26 +- tpe/plugin/src/Worlds.hh | 40 --- tpe/plugin/worlds/nested_model.world | 37 -- tpe/plugin/worlds/shapes.world | 169 --------- tpe/plugin/worlds/shapes_bitmask.sdf | 154 --------- tpe/plugin/worlds/test.world | 328 ------------------ 16 files changed, 48 insertions(+), 1660 deletions(-) delete mode 100644 dartsim/worlds/empty.sdf delete mode 100644 dartsim/worlds/falling.world delete mode 100644 dartsim/worlds/shapes.sdf delete mode 100644 dartsim/worlds/shapes_bitmask.sdf delete mode 100644 dartsim/worlds/test.world delete mode 100644 dartsim/worlds/world_with_nested_model.sdf delete mode 100644 tpe/plugin/src/Worlds.hh delete mode 100644 tpe/plugin/worlds/nested_model.world delete mode 100644 tpe/plugin/worlds/shapes.world delete mode 100644 tpe/plugin/worlds/shapes_bitmask.sdf delete mode 100644 tpe/plugin/worlds/test.world diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 83e6095cf..eda7a5520 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -53,7 +53,8 @@ #include #include -#include "dartsim/src/Worlds.hh" +#include +#include "Worlds.hh" using namespace gz; @@ -314,7 +315,7 @@ INSTANTIATE_TEST_SUITE_P(LoadWorld, SDFFeatures_TEST, // Test that the dartsim plugin loaded all the relevant information correctly. TEST_P(SDFFeatures_TEST, CheckDartsimData) { - WorldPtr world = this->LoadWorld(dartsim::worlds::kTestWorld); + WorldPtr world = this->LoadWorld(common_test::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -422,7 +423,7 @@ TEST_P(SDFFeatures_TEST, CheckDartsimData) // Test that joint limits are working by running the simulation TEST_P(SDFFeatures_TEST, CheckJointLimitEnforcement) { - WorldPtr world = this->LoadWorld(dartsim::worlds::kTestWorld); + WorldPtr world = this->LoadWorld(common_test::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -581,7 +582,7 @@ TEST_P(SDFFeatures_TEST, WorldIsParentOrChild) // TEST_P(SDFFeatures_TEST, WorldWithNestedModel) { - WorldPtr world = this->LoadWorld(dartsim::worlds::kWorldWithNestedModelSdf); + WorldPtr world = this->LoadWorld(common_test::worlds::kWorldWithNestedModelSdf); ASSERT_NE(nullptr, world); EXPECT_EQ(2u, world->GetModelCount()); @@ -690,7 +691,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) // Test that joint type falls back to fixed if the type is not supported TEST_P(SDFFeatures_TEST, FallbackToFixedJoint) { - WorldPtr world = this->LoadWorld(dartsim::worlds::kTestWorld); + WorldPtr world = this->LoadWorld(common_test::worlds::kTestWorld); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -964,7 +965,7 @@ TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) ///////////////////////////////////////////////// TEST_P(SDFFeatures_TEST, Shapes) { - WorldPtr world = this->LoadWorld(dartsim::worlds::kShapesSdf); + WorldPtr world = this->LoadWorld(common_test::worlds::kShapesWorld); ASSERT_NE(nullptr, world); auto dartWorld = world->GetDartsimWorld(); @@ -973,7 +974,7 @@ TEST_P(SDFFeatures_TEST, Shapes) ASSERT_EQ(5u, dartWorld->getNumSkeletons()); int count{0}; - for (auto name : {"box", "cylinder", "sphere", "capsule", "ellipsoid"}) + for (auto name : {"sphere", "box", "cylinder", "capsule", "ellipsoid"}) { const auto skeleton = dartWorld->getSkeleton(count++); ASSERT_NE(nullptr, skeleton); diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index 9f6fa75ba..fe0eef69a 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -32,7 +32,7 @@ #include #include "test/Utils.hh" -#include "Worlds.hh" +#include "test/common_test/Worlds.hh" struct TestFeatureList : gz::physics::FeatureList< gz::physics::CollisionDetector, @@ -83,7 +83,7 @@ TestWorldPtr LoadWorld( ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, CollisionDetector) { - const auto world = LoadWorld(this->engine, dartsim::worlds::kEmptySdf); + const auto world = LoadWorld(this->engine, common_test::worlds::kEmptySdf); EXPECT_EQ("ode", world->GetCollisionDetector()); world->SetCollisionDetector("banana"); @@ -105,7 +105,7 @@ TEST_F(WorldFeaturesFixture, CollisionDetector) ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, Solver) { - const auto world = LoadWorld(this->engine, dartsim::worlds::kEmptySdf); + const auto world = LoadWorld(this->engine, common_test::worlds::kEmptySdf); EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver()); diff --git a/dartsim/src/Worlds.hh b/dartsim/src/Worlds.hh index 0cc2331da..4ffa870da 100644 --- a/dartsim/src/Worlds.hh +++ b/dartsim/src/Worlds.hh @@ -33,17 +33,10 @@ inline std::string DartsimTestWorld(const std::string &_world) namespace dartsim::worlds { const auto kAddedMassSdf = DartsimTestWorld("added_mass.sdf"); -const auto kEmptySdf = DartsimTestWorld("empty.sdf"); -const auto kFallingWorld = DartsimTestWorld("falling.world"); const auto kJointAcrossNestedModelsSdf = DartsimTestWorld("joint_across_nested_models.sdf"); const auto kModelFramesSdf = DartsimTestWorld("model_frames.sdf"); -const auto kShapesSdf = DartsimTestWorld("shapes.sdf"); -const auto kShapesBitmaskWorld = DartsimTestWorld("shapes_bitmask.sdf"); -const auto kTestWorld = DartsimTestWorld("test.world"); const auto kWorldFramesSdf = DartsimTestWorld("world_frames.sdf"); -const auto kWorldWithNestedModelSdf = - DartsimTestWorld("world_with_nested_model.sdf"); const auto kWorldWithNestedModelJointToWorldSdf = DartsimTestWorld("world_with_nested_model_joint_to_world.sdf"); } // namespace dartsim::worlds diff --git a/dartsim/worlds/empty.sdf b/dartsim/worlds/empty.sdf deleted file mode 100644 index 62150c14a..000000000 --- a/dartsim/worlds/empty.sdf +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/dartsim/worlds/falling.world b/dartsim/worlds/falling.world deleted file mode 100644 index 1d2ea2f43..000000000 --- a/dartsim/worlds/falling.world +++ /dev/null @@ -1,75 +0,0 @@ - - - - - - - 0 0 2 0 0.78539816339 0 - - 0.0 0.0 0.0 0 0 0 - - - 0.4 - 0 - 0 - 0.4 - 0 - 0.4 - - 1.0 - - - 0.0 0.0 0.0 0 0 0 - - - 1 - - - - - 0.0 0.0 0.0 0 0 0 - - - 1 - - - - - - - true - 0 0 -0.5 0 0 0.78539816339 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - 0.0 0.0 0.0 0 0 0 - - - - 100 100 1 - - - - - - - - 100 100 1 - - - - - - - diff --git a/dartsim/worlds/shapes.sdf b/dartsim/worlds/shapes.sdf deleted file mode 100644 index 733c82ac2..000000000 --- a/dartsim/worlds/shapes.sdf +++ /dev/null @@ -1,191 +0,0 @@ - - - - - 0 0 0.5 0 0 0 - - - - 0.16666 - 0 - 0 - 0.16666 - 0 - 0.16666 - - 1.0 - - - - - 1 1 1 - - - - - - - - 1 1 1 - - - - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 - - - - - - - 0 -1.5 0.5 0 0 0 - - - - 0.1458 - 0 - 0 - 0.1458 - 0 - 0.125 - - 1.0 - - - - - 0.5 - 1.0 - - - - - - - - 0.5 - 1.0 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - - - - - 0 1.5 0.5 0 0 0 - - - - 0.1 - 0 - 0 - 0.1 - 0 - 0.1 - - 1.0 - - - - - 0.5 - - - - - - - - 0.5 - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - - - - - - 0 -3.0 0.5 0 0 0 - - - - 0.074154 - 0 - 0 - 0.074154 - 0 - 0.018769 - - 1.0 - - - - - 0.2 - 0.6 - - - - - - - 0.2 - 0.6 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - - - - - 0 3.0 0.5 0 0 0 - - - - 0.068 - 0 - 0 - 0.058 - 0 - 0.026 - - 1.0 - - - - - 0.2 0.3 0.5 - - - - - - - 0.2 0.3 0.5 - - - - 1 0 1 1 - 1 0 1 1 - 1 0 1 1 - - - - - - diff --git a/dartsim/worlds/shapes_bitmask.sdf b/dartsim/worlds/shapes_bitmask.sdf deleted file mode 100644 index 8aa289fce..000000000 --- a/dartsim/worlds/shapes_bitmask.sdf +++ /dev/null @@ -1,154 +0,0 @@ - - - - - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - - 0 0 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x01 - - - - - - - 1 1 1 - - - - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 - - - - - - - 0 0.75 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x02 - - - - - - - 1 1 1 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - - - - - 0 -0.75 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x03 - - - - - - - 1 1 1 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - - - - - diff --git a/dartsim/worlds/test.world b/dartsim/worlds/test.world deleted file mode 100644 index b75496857..000000000 --- a/dartsim/worlds/test.world +++ /dev/null @@ -1,383 +0,0 @@ - - - - - true - - - - - 0 0 1 - 100 100 - - - - - - 100 - 50 - - - - - - false - - - 0 0 1 - 100 100 - - - - 0.9 0.9 0.9 1 - - - - - - 1 0 0 0 0 0 - - - 100 - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - 1 1 0 1 - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - 1 1 0 1 - - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - - - 1.1 - - - - - - - - 0 0 2.1 -1.5708 0 0 - 0 - - 0 0 0.5 0 0 0 - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - 1 1 0 1 - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - - - 0.1 - - - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - 0.25 1.0 2.1 -2 0 0 - 0 - - 0 0 0.5 0 0 0 - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - base - upper_link - - 1.0 0 0 - - 3.0 - - - - - - upper_link - lower_link - - 1.0 0 0 - - 3.0 - - - - - - - 0.0 10.0 10.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - - - 1.0 - - - - - 0.8 - - - - - - - - 10 0 2 0 0 0 - - - 100 - - - 0 0 0 -1.57 0 0 - - - 0.1 - 0.2 - - - - - - 1 0 0 0 0 0 - - 1 - - - - - 0.1 0.1 0.1 - - - - - - world - base - - - -1 0 0 0 0 0 - base - bar - - 0 1 0 - - -0.5 - 0.5 - 100 - - - - - - - 10 10 2 0 0 0 - - - 100 - - - 0 0 0 -1.57 0 0 - - - 0.1 - 0.2 - - - - - - 0 0 -1 0 0 0 - - 1 - - - - - 0.1 0.1 0.1 - - - - - - world - base - - - 0 0 1 0 0 0 - base - bar - - 0 1 0 - - - - - - - - link0 - link1 - 2 - - - - - - - - link0 - link1 - - - - - link2 - link3 - - - - - link4 - link5 - - - - diff --git a/dartsim/worlds/world_with_nested_model.sdf b/dartsim/worlds/world_with_nested_model.sdf deleted file mode 100644 index 0b71ad0d7..000000000 --- a/dartsim/worlds/world_with_nested_model.sdf +++ /dev/null @@ -1,75 +0,0 @@ - - - - - - 1 2 2 0 0 0 - - 3 1 1 0 0 1.5707 - - - - 2 - - - - - - 0 1 0 1.5707 0 0 - - - - 2 - - - - - - nested_link1 - nested_link2 - - 1 0 0 - - - - - - - - 1 2 3 - - - - - - - 1 2 3 - - - - - - - link1 - nested_model::nested_link1 - - - - - - - - - - - - - - - - - - - - diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc index eb619c82d..ec92eb380 100644 --- a/tpe/plugin/src/SDFFeatures_TEST.cc +++ b/tpe/plugin/src/SDFFeatures_TEST.cc @@ -23,7 +23,7 @@ #include #include -#include "Worlds.hh" +#include #include @@ -91,11 +91,11 @@ World LoadWorld(const std::string &_world) // Test that the tpe plugin loaded all the relevant information correctly. TEST(SDFFeatures_TEST, CheckTpeData) { - auto world = LoadWorld(tpe::worlds::kTestWorld); + auto world = LoadWorld(common_test::worlds::kTestWorld); auto tpeWorld = world.GetTpeLibWorld(); ASSERT_NE(nullptr, tpeWorld); - ASSERT_EQ(6u, tpeWorld->GetChildCount()); + ASSERT_EQ(7u, tpeWorld->GetChildCount()); // check model 01 { @@ -254,7 +254,13 @@ TEST(SDFFeatures_TEST, CheckTpeData) link.GetId()); EXPECT_EQ("link", link.GetName()); EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); - EXPECT_EQ(0u, link.GetChildCount()); + EXPECT_EQ(1u, link.GetChildCount()); + + auto &collision = link.GetChildByName("collision1"); + ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), + collision.GetId()); + EXPECT_EQ("collision1", collision.GetName()); + EXPECT_EQ(0u, collision.GetChildCount()); } // check model 04 @@ -364,26 +370,26 @@ TEST(SDFFeatures_TEST, CheckTpeData) // Test that the tpe plugin loaded nested models correctly. TEST(SDFFeatures_TEST, NestedModel) { - auto world = LoadWorld(tpe::worlds::kNestedModelWorld); + auto world = LoadWorld(common_test::worlds::kWorldWithNestedModelSdf); auto tpeWorld = world.GetTpeLibWorld(); ASSERT_NE(nullptr, tpeWorld); - ASSERT_EQ(1u, tpeWorld->GetChildCount()); - EXPECT_EQ(1u, world.GetModelCount()); + ASSERT_EQ(2u, tpeWorld->GetChildCount()); + EXPECT_EQ(2u, world.GetModelCount()); // check top level model physics::tpelib::Entity &model = - tpeWorld->GetChildByName("model"); + tpeWorld->GetChildByName("parent_model"); ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), model.GetId()); - EXPECT_EQ("model", model.GetName()); + EXPECT_EQ("parent_model", model.GetName()); EXPECT_EQ(math::Pose3d::Zero, model.GetPose()); - EXPECT_EQ(2u, model.GetChildCount()); + EXPECT_EQ(4u, model.GetChildCount()); - physics::tpelib::Entity &link = model.GetChildByName("link"); + physics::tpelib::Entity &link = model.GetChildByName("link1"); ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), link.GetId()); - EXPECT_EQ("link", link.GetName()); + EXPECT_EQ("link1", link.GetName()); EXPECT_EQ(math::Pose3d::Zero, link.GetPose()); EXPECT_EQ(1u, link.GetChildCount()); @@ -401,27 +407,26 @@ TEST(SDFFeatures_TEST, NestedModel) nestedModel.GetId()); EXPECT_EQ("nested_model", nestedModel.GetName()); EXPECT_EQ(math::Pose3d(1, 2, 2, 0, 0, 0), nestedModel.GetPose()); - EXPECT_EQ(1u, nestedModel.GetChildCount()); + EXPECT_EQ(2u, nestedModel.GetChildCount()); physics::tpelib::Entity &nestedLink = - nestedModel.GetChildByName("nested_link"); + nestedModel.GetChildByName("nested_link1"); ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedLink.GetId()); - EXPECT_EQ("nested_link", nestedLink.GetName()); + EXPECT_EQ("nested_link1", nestedLink.GetName()); EXPECT_EQ(math::Pose3d(3, 1, 1, 0, 0, 1.5707), nestedLink.GetPose()); EXPECT_EQ(1u, nestedLink.GetChildCount()); physics::tpelib::Entity &nestedCollision = - nestedLink.GetChildByName("nested_collision"); + nestedLink.GetChildByName("nested_collision1"); ASSERT_NE(physics::tpelib::Entity::kNullEntity.GetId(), nestedCollision.GetId()); - EXPECT_EQ("nested_collision", nestedCollision.GetName()); + EXPECT_EQ("nested_collision1", nestedCollision.GetName()); EXPECT_EQ(math::Pose3d::Zero, nestedCollision.GetPose()); // canonical link - physics::tpelib::Model *m = - static_cast(&model); + auto *m = dynamic_cast(&model); physics::tpelib::Entity canLink = m->GetCanonicalLink(); EXPECT_EQ(link.GetId(), canLink.GetId()); } diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 81660996c..ddecee5de 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -41,9 +41,9 @@ #include #include +#include #include #include -#include "Worlds.hh" #include "EntityManagementFeatures.hh" #include "FreeGroupFeatures.hh" @@ -52,6 +52,7 @@ #include "World.hh" + using namespace gz; struct TestFeatureList : physics::FeatureList< @@ -276,7 +277,7 @@ TEST_P(SimulationFeatures_TEST, StepWorld) return; gzdbg << "Testing library " << library << std::endl; - const auto worlds = LoadWorlds(library, tpe::worlds::kShapesWorld); + const auto worlds = LoadWorlds(library, common_test::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -299,7 +300,7 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) if (library.empty()) return; - const auto worlds = LoadWorlds(library, tpe::worlds::kShapesWorld); + const auto worlds = LoadWorlds(library, common_test::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -438,7 +439,7 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) if (library.empty()) return; - const auto worlds = LoadWorlds(library, tpe::worlds::kShapesWorld); + const auto worlds = LoadWorlds(library, common_test::worlds::kShapesWorld); for (const auto &world : worlds) { @@ -486,7 +487,8 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) if (library.empty()) return; - const auto worlds = LoadWorlds(library, tpe::worlds::kNestedModelWorld); + const auto worlds = + LoadWorlds(library, common_test::worlds::kWorldWithNestedModelSdf); for (const auto &world : worlds) { @@ -494,7 +496,7 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) ASSERT_NE(nullptr, tpeWorld); // model free group test - auto model = world->GetModel("model"); + auto model = world->GetModel("parent_model"); ASSERT_NE(nullptr, model); auto freeGroup = model->FindFreeGroup(); ASSERT_NE(nullptr, freeGroup); @@ -504,7 +506,7 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) freeGroup->SetWorldPose(math::eigen3::convert(newPose)); { - auto link = model->GetLink("link"); + auto link = model->GetLink("link1"); ASSERT_NE(nullptr, link); auto frameData = link->FrameDataRelativeToWorld(); EXPECT_EQ(newPose, @@ -513,7 +515,7 @@ TEST_P(SimulationFeatures_TEST, NestedFreeGroup) { auto nestedModel = model->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); - auto nestedLink = nestedModel->GetLink("nested_link"); + auto nestedLink = nestedModel->GetLink("nested_link1"); ASSERT_NE(nullptr, nestedLink); // Poses from SDF @@ -536,10 +538,8 @@ TEST_P(SimulationFeatures_TEST, CollideBitmasks) if (library.empty()) return; - const auto worldPath = - gz::common::testing::SourceFile( - "tpe", "plugin", "worlds", "shapes_bitmask.sdf"); - const auto worlds = LoadWorlds(library, tpe::worlds::kShapesBitmaskSdf); + const auto worlds = + LoadWorlds(library, common_test::worlds::kShapesBitmaskWorld); for (const auto &world : worlds) { @@ -583,7 +583,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) if (library.empty()) return; - const auto worlds = LoadWorlds(library, tpe::worlds::kShapesWorld); + const auto worlds = LoadWorlds(library, common_test::worlds::kShapesWorld); for (const auto &world : worlds) { diff --git a/tpe/plugin/src/Worlds.hh b/tpe/plugin/src/Worlds.hh deleted file mode 100644 index d700ba878..000000000 --- a/tpe/plugin/src/Worlds.hh +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (C) 2024 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef TPE_PLUGIN_WORLDS_HH_ -#define TPE_PLUGIN_WORLDS_HH_ - -#include - -#include - -// \brief retrieve a filename from the tpe/plugin/worlds directory -// \param[in] _world filename to retrieve -// \return full path to the request world -inline std::string TpeTestWorld(const std::string &_world) -{ - return gz::common::testing::SourceFile("tpe", "plugin", "worlds", _world); -} - -namespace tpe::worlds -{ -const auto kNestedModelWorld = TpeTestWorld("nested_model.world"); -const auto kShapesWorld = TpeTestWorld("shapes.world"); -const auto kShapesBitmaskSdf = TpeTestWorld("shapes_bitmask.sdf"); -const auto kTestWorld = TpeTestWorld("test.world"); -} // namespace tpe::worlds -#endif // TPE_PLUGIN_WORLDS_HH_ diff --git a/tpe/plugin/worlds/nested_model.world b/tpe/plugin/worlds/nested_model.world deleted file mode 100644 index a2182472a..000000000 --- a/tpe/plugin/worlds/nested_model.world +++ /dev/null @@ -1,37 +0,0 @@ - - - - - true - - 1 2 2 0 0 0 - - 3 1 1 0 0 1.5707 - - - - 2 - - - - - - - - - - 1 2 3 - - - - - - - 1 2 3 - - - - - - - diff --git a/tpe/plugin/worlds/shapes.world b/tpe/plugin/worlds/shapes.world deleted file mode 100644 index cb3bce5d9..000000000 --- a/tpe/plugin/worlds/shapes.world +++ /dev/null @@ -1,169 +0,0 @@ - - - - - 0 1.5 0.5 0 0 0 - - - - 3 - 0 - 0 - 3 - 0 - 3 - - 1.0 - - - - - 1 - - - - - - - 1 - - - - - - - - true - 0 0 0.5 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 100 100 1 - - - - - - - 100 100 1 - - - - - - - - 0 -1.5 0.5 0 0 0 - - - - 2 - 0 - 0 - 2 - 0 - 2 - - 2.0 - - - - - 0.5 - 1.1 - - - - - - - 0.5 - 1.1 - - - - - - - - 0 -3.0 0.5 0 0 0 - - - - 0.074154 - 0 - 0 - 0.074154 - 0 - 0.018769 - - 1.0 - - - - - 0.2 - 0.6 - - - - - - - 0.2 - 0.6 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - - - - - 0 -5 0.7 0 0 0 - - - - 0.068 - 0 - 0 - 0.058 - 0 - 0.026 - - 1.0 - - - - - 0.2 0.3 0.5 - - - - - - - 0.2 0.3 0.5 - - - - - - - diff --git a/tpe/plugin/worlds/shapes_bitmask.sdf b/tpe/plugin/worlds/shapes_bitmask.sdf deleted file mode 100644 index 8aa289fce..000000000 --- a/tpe/plugin/worlds/shapes_bitmask.sdf +++ /dev/null @@ -1,154 +0,0 @@ - - - - - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - - 0 0 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x01 - - - - - - - 1 1 1 - - - - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 - - - - - - - 0 0.75 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x02 - - - - - - - 1 1 1 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - - - - - 0 -0.75 10.0 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - - - 0x03 - - - - - - - 1 1 1 - - - - 1 1 0 1 - 1 1 0 1 - 1 1 0 1 - - - - - - - diff --git a/tpe/plugin/worlds/test.world b/tpe/plugin/worlds/test.world deleted file mode 100644 index edbbe74cf..000000000 --- a/tpe/plugin/worlds/test.world +++ /dev/null @@ -1,328 +0,0 @@ - - - - - true - - - - - 0 0 1 - 100 100 - - - - - - 100 - 50 - - - - - - false - - - 0 0 1 - 100 100 - - - - 0.9 0.9 0.9 1 - - - - - - 1 0 0 0 0 0 - - - 100 - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - 1 1 0 1 - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - 1 1 0 1 - - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - - - 1.1 - - - - - - - - 0 0 2.1 -1.5708 0 0 - 0 - - 0 0 0.5 0 0 0 - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - 1 1 0 1 - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - - - 0.1 - - - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - 0.25 1.0 2.1 -2 0 0 - 0 - - 0 0 0.5 0 0 0 - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - 1 1 0 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 1 1 0 1 - - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - base - upper_link - - 1.0 0 0 - - 3.0 - - - - - - upper_link - lower_link - - 1.0 0 0 - - 3.0 - - - - - - - 0.0 10.0 10.0 0.0 0.0 0.0 - - 0.0 0.0 0.0 0.0 0.0 0.0 - - - - - 10 0 2 0 0 0 - - - 100 - - - 0 0 0 -1.57 0 0 - - - 0.1 - 0.2 - - - - - - 1 0 0 0 0 0 - - 1 - - - - - 0.1 0.1 0.1 - - - - - - world - base - - - -1 0 0 0 0 0 - base - bar - - 0 1 0 - - -0.5 - 0.5 - 100 - - - - - - - - - link0 - link1 - 2 - - - - - - - - link0 - link1 - - - - - link2 - link3 - - - - - link4 - link5 - - - - From e84feed1139fb3e2a6c26e41562666236c7f5d95 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 29 Jan 2024 18:03:23 +0000 Subject: [PATCH 6/7] Lint Signed-off-by: Michael Carroll --- dartsim/src/SDFFeatures_TEST.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index eda7a5520..21d698201 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -582,7 +582,8 @@ TEST_P(SDFFeatures_TEST, WorldIsParentOrChild) // TEST_P(SDFFeatures_TEST, WorldWithNestedModel) { - WorldPtr world = this->LoadWorld(common_test::worlds::kWorldWithNestedModelSdf); + WorldPtr world = + this->LoadWorld(common_test::worlds::kWorldWithNestedModelSdf); ASSERT_NE(nullptr, world); EXPECT_EQ(2u, world->GetModelCount()); From 1c806fd9344e5e0db4718b311ca99b890a1c2020 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 30 Jan 2024 13:31:25 +0000 Subject: [PATCH 7/7] Fix added upstream test Signed-off-by: Michael Carroll --- test/common_test/simulation_features.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 6c5150096..dfb657c1b 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -245,7 +245,7 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, auto world = LoadPluginAndWorld( this->loader, name, - gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); + common_test::worlds::kShapesWorld); auto checkedOutput = StepWorld( world, true, 1).first; EXPECT_TRUE(checkedOutput);