diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5812763bc..fe275a3cb 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,11 @@ name: Ubuntu -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'sdf[0-9]?' + - 'main' jobs: jammy-ci: @@ -8,7 +13,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@jammy diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index 82b7debb3..4de5a710c 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -51,7 +51,7 @@ jobs: working-directory: build run: | export PYTHONPATH=$PYTHONPATH:$(brew --prefix)/lib/python - cmake .. -DCMAKE_INSTALL_PREFIX=$(brew --prefix)/Cellar/${PACKAGE}/HEAD -DPython3_EXECUTABLE=$(brew --prefix)/bin/python3 + cmake .. -DCMAKE_INSTALL_PREFIX=$(brew --prefix)/Cellar/${PACKAGE}/HEAD -DPython3_EXECUTABLE=$(brew --prefix python3)/bin/python3 - run: make working-directory: build - run: make test diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml index 2c94852da..2332244bf 100644 --- a/.github/workflows/triage.yml +++ b/.github/workflows/triage.yml @@ -14,4 +14,3 @@ jobs: with: project-url: https://github.com/orgs/gazebosim/projects/7 github-token: ${{ secrets.TRIAGE_TOKEN }} - diff --git a/BUILD.bazel b/BUILD.bazel new file mode 100644 index 000000000..0e26e2c84 --- /dev/null +++ b/BUILD.bazel @@ -0,0 +1,200 @@ +load( + "@gz//bazel/skylark:build_defs.bzl", + "GZ_FEATURES", + "GZ_ROOT", + "GZ_VISIBILITY", + "add_lint_tests", + "gz_configure_file", + "gz_configure_header", + "gz_export_header", + "gz_include_header", + "gz_py_binary", +) + +package( + default_visibility = GZ_VISIBILITY, + features = GZ_FEATURES, +) + +licenses(["notice"]) + +exports_files(["LICENSE"]) + +gz_configure_header( + name = "config", + src = "include/sdf/config.hh.in", + cmakelists = ["CMakeLists.txt"], + defines = { + "CMAKE_INSTALL_FULL_DATAROOTDIR": "unused", + }, + package = "sdformat", +) + +gz_py_binary( + name = "embed_sdf", + srcs = ["sdf/embedSdf.py"], + main = "sdf/embedSdf.py", +) + +genrule( + name = "embed_sdf_genrule", + srcs = glob([ + "sdf/**/*.sdf", + "sdf/**/*.convert", + ]), + outs = ["EmbeddedSdf.cc"], + cmd = "$(execpath :embed_sdf) --output-file $@ --sdf-root sdformat/sdf/ --input-files $(SRCS)", # noqa + tools = [":embed_sdf"], +) + +public_headers_no_gen = glob([ + "include/sdf/*.h", + "include/sdf/*.hh", +]) + +private_headers = glob(["src/*.hh"]) + +sources = glob( + ["src/*.cc"], + exclude = [ + "src/*_TEST.cc", + "src/gz.cc", + ], +) + +gz_export_header( + name = "include/sdf/Export.hh", + export_base = "GZ_SDFORMAT", + lib_name = "sdf", + visibility = ["//visibility:private"], +) + +gz_include_header( + name = "sdformat_hh_genrule", + out = "include/sdformat.hh", + hdrs = public_headers_no_gen + [ + "include/sdf/config.hh", + "include/sdf/Export.hh", + ], +) + +public_headers = public_headers_no_gen + [ + "include/sdf/Export.hh", + "include/sdf/config.hh", + "include/sdformat.hh", +] + +cc_library( + name = "urdf", + srcs = [ + "src/urdf/urdf_parser/joint.cpp", + "src/urdf/urdf_parser/link.cpp", + "src/urdf/urdf_parser/model.cpp", + "src/urdf/urdf_parser/pose.cpp", + "src/urdf/urdf_parser/twist.cpp", + "src/urdf/urdf_parser/urdf_model_state.cpp", + "src/urdf/urdf_parser/urdf_sensor.cpp", + "src/urdf/urdf_parser/world.cpp", + ], + hdrs = glob( + ["src/urdf/**/*.h"], + ), + copts = ["-Wno-unknown-pragmas"], + includes = ["src/urdf"], + deps = [ + "@tinyxml2", + ], +) + +cc_library( + name = "sdformat", + srcs = sources + private_headers + ["EmbeddedSdf.cc"], + hdrs = public_headers, + defines = [ + 'SDF_SHARE_PATH=\\".\\"', + 'SDF_VERSION_PATH=\\"sdformat\\"', + ], + includes = [ + "include", + "src", + ], + deps = [ + ":urdf", + GZ_ROOT + "math", + GZ_ROOT + "utils", + "@tinyxml2", + ], +) + +cc_library( + name = "sdformat_internal", + srcs = [ + "src/gz.cc", + "src/gz.hh", + ], + visibility = ["//visibility:private"], + deps = [":sdformat"], +) + +test_sources = glob( + ["src/*_TEST.cc"], + exclude = ["src/gz_TEST.cc"], +) + +[cc_test( + name = src.replace("/", "_").replace(".cc", "").replace("src_", ""), + srcs = [src], + data = [ + "sdf", + GZ_ROOT + "sdformat/test:integration", + GZ_ROOT + "sdformat/test:sdf", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "sdformat", + }, + deps = [ + ":sdformat", + GZ_ROOT + "sdformat/test:test_utils", + "@gtest", + "@gtest//:gtest_main", + ], +) for src in test_sources] + +gz_configure_file( + name = "sdformat.rb", + src = "src/cmd/cmdsdformat.rb.in", + out = "cmdsdformat.rb", + cmakelists = ["CMakeLists.txt"], + defines = [ + "library_location=libgz-sdformat.so", + ], + package = "sdformat", + visibility = [GZ_ROOT + "tools:__pkg__"], +) + +gz_configure_file( + name = "sdformat_yaml", + src = "conf/sdformat.yaml.in", + out = "sdformat.yaml", + cmakelists = ["CMakeLists.txt"], + defines = [ + "gz_library_path=gz/sdformat/cmdsdformat.rb", + ], + package = "sdformat", + visibility = [GZ_ROOT + "tools:__pkg__"], +) + +cc_binary( + name = "libgz-sdformat.so", + srcs = [":sdformat_internal"], + linkshared = True, + visibility = [GZ_ROOT + "tools:__pkg__"], + deps = [ + ":sdformat", + ], +) + +exports_files(["sdf"]) + +add_lint_tests() diff --git a/Changelog.md b/Changelog.md index a7527acc9..091d2da88 100644 --- a/Changelog.md +++ b/Changelog.md @@ -529,6 +529,30 @@ ## libsdformat 12.X +### libsdformat 12.7.2 (2023-09-01) + +1. Fixed 1.9/light.sdf + * [Pull request #1281](https://github.com/gazebosim/sdformat/pull/1281) + +1. URDF->SDF handle links with no inertia or small mass + * [Pull request #1238](https://github.com/gazebosim/sdformat/pull/1238) + +1. Fix Element::Set method return value + * [Pull request #1256](https://github.com/gazebosim/sdformat/pull/1256) + +1. Add missing values in Surace ToElement method + * [Pull request #1263](https://github.com/gazebosim/sdformat/pull/1263) + +1. Rename COPYING to LICENSE + * [Pull request #1252](https://github.com/gazebosim/sdformat/pull/1252) + +1. Infrastructure + * [Pull request #1245](https://github.com/gazebosim/sdformat/pull/1245) + * [Pull request #1271](https://github.com/gazebosim/sdformat/pull/1271) + +1. Allow relative paths in URDF + * [Pull request #1213](https://github.com/gazebosim/sdformat/pull/1213) + ### libsdformat 12.7.1 (2023-02-28) 1. Fix camera info topic default value @@ -1606,6 +1630,27 @@ ## libsdformat 9.X +### libsdformat 9.10.1 (2024-01-05) + +1. URDF->SDF handle links with no inertia or small mass + * [Pull request #1238](https://github.com/gazebosim/sdformat/pull/1238) + +1. Fix Element::Set method return value + * [Pull request #1256](https://github.com/gazebosim/sdformat/pull/1256) + +1. Allowing relative paths in URDF + * [Pull request #1213](https://github.com/gazebosim/sdformat/pull/1213) + +1. Use `File.exist?` for Ruby 3.2 compatibility + * [Pull request #1216](https://github.com/gazebosim/sdformat/pull/1216) + +1. Infrastructure + * [Pull request #1217](https://github.com/gazebosim/sdformat/pull/1217) + * [Pull request #1225](https://github.com/gazebosim/sdformat/pull/1225) + * [Pull request #1271](https://github.com/gazebosim/sdformat/pull/1271) + * [Pull request #1345](https://github.com/gazebosim/sdformat/pull/1345) + * [Pull request #1252](https://github.com/gazebosim/sdformat/pull/1252) + ### libsdformat 9.10.0 (2022-11-30) 1. Ign to gz header migration. diff --git a/README.md b/README.md index 28808b151..373f69fee 100644 --- a/README.md +++ b/README.md @@ -10,16 +10,16 @@ Note: The branch name in the codecov URL & library version should be updated whe --> Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/gazebosim/sdformat/branch/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/sdformat/branch/main) -Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-focal-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-focal-amd64) +Test coverage | [![codecov](https://codecov.io/gh/gazebosim/sdformat/tree/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/sdformat/tree/main) +Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-jammy-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-jammy-amd64) Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-homebrew-amd64) -Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-windows7-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-windows7-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-main-win)](https://build.osrfoundation.org/job/sdformat-main-win) SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications. SDFormat is capable of representing and describing different physic engines, lighting properties, terrain, static -or dynamic objects, and articulated robots with various sensors, and acutators. +or dynamic objects, and articulated robots with various sensors, and actuators. The format of SDFormat is also described by XML, which facilitates updates and allows conversion from previous versions. diff --git a/conf/CMakeLists.txt b/conf/CMakeLists.txt index e42216a9f..7ac7c9e15 100644 --- a/conf/CMakeLists.txt +++ b/conf/CMakeLists.txt @@ -1,12 +1,16 @@ # Used only for internal testing. -set(gz_library_path "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${PROJECT_NAME}") +set(gz_library_path "${CMAKE_BINARY_DIR}/test/lib/$/ruby/gz/cmd${PROJECT_NAME}") # Generate a configuration file for internal testing. # Note that the major version of the library is included in the name. # Ex: sdformat0.yaml configure_file( "${PROJECT_NAME_NO_VERSION_LOWER}.yaml.in" - "${CMAKE_BINARY_DIR}/test/conf/${PROJECT_NAME}.yaml" @ONLY) + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.yaml.configured" @ONLY) + +file(GENERATE + OUTPUT "${CMAKE_BINARY_DIR}/test/conf/$/${PROJECT_NAME}.yaml" + INPUT "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.yaml.configured") # Used for the installed version. set(gz_library_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/gz/cmd${PROJECT_NAME}") diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 019149959..66d4caa6a 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -15,7 +15,7 @@ if (DOXYGEN_FOUND) ${CMAKE_BINARY_DIR}/doxygen/html/search COMMAND make -C ${CMAKE_BINARY_DIR}/doxygen/latex COMMAND mv ${CMAKE_BINARY_DIR}/doxygen/latex/refman.pdf - ${CMAKE_BINARY_DIR}/doxygen/latex/sdf-${SDF_VERSION_FULL}.pdf + ${CMAKE_BINARY_DIR}/doxygen/latex/sdf-${PROJECT_VERSION_FULL}.pdf COMMENT "Generating API documentation with Doxygen" VERBATIM) endif() diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index d6b8470c9..2ab4114ce 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -18,6 +18,7 @@ #define SDF_COLLISION_HH_ #include +#include #include #include #include @@ -78,6 +79,11 @@ namespace sdf /// \param[in] _name Name of the collision. public: void SetName(const std::string &_name); + /// \brief Get the default density of a collision if its density is not + /// specified. + /// \return Default density. + public: static double DensityDefault(); + /// \brief Get the density of the collision. /// \return Density of the collision. public: double Density() const; @@ -145,13 +151,32 @@ namespace sdf /// \brief Calculate and return the MassMatrix for the collision /// \param[out] _errors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors - /// \param[in] _config Custom parser configuration /// \param[out] _inertial An inertial object which will be set with the /// calculated inertial values + /// \param[in] _config Custom parser configuration public: void CalculateInertial(sdf::Errors &_errors, gz::math::Inertiald &_inertial, const ParserConfig &_config); + /// \brief Calculate and return the MassMatrix for the collision + /// \param[out] _errors A vector of Errors objects. Each errors contains an + /// Error code and a message. An empty errors vector indicates no errors + /// \param[out] _inertial An inertial object which will be set with the + /// calculated inertial values + /// \param[in] _config Custom parser configuration + /// \param[in] _density An optional density value to override the default + /// collision density. This value is used instead of DefaultDensity() + // if this collision's density has not been explicitly set. + /// \param[in] _autoInertiaParams An ElementPtr to the auto_inertia_params + /// element to be used if the auto_inertia_params have not been set in this + /// collision. + public: void CalculateInertial( + sdf::Errors &_errors, + gz::math::Inertiald &_inertial, + const ParserConfig &_config, + const std::optional &_density, + sdf::ElementPtr _autoInertiaParams); + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index c92d8d18d..3e5260e73 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -18,6 +18,7 @@ #define SDF_LINK_HH_ #include +#include #include #include #include @@ -79,6 +80,27 @@ namespace sdf /// \param[in] _name Name of the link. public: void SetName(const std::string &_name); + /// \brief Get the density of the inertial if it has been set. + /// \return Density of the inertial if it has been set, + /// otherwise std::nullopt. + public: std::optional Density() const; + + /// \brief Set the density of the inertial. + /// \param[in] _density Density of the inertial. + public: void SetDensity(double _density); + + /// \brief Get the ElementPtr to the element + /// This element can be used as a parent element to hold user-defined + /// params for the custom moment of inertia calculator. + /// \return ElementPtr object for the element. + public: sdf::ElementPtr AutoInertiaParams() const; + + /// \brief Function to set the auto inertia params using a + /// sdf ElementPtr object + /// \param[in] _autoInertiaParams ElementPtr to + /// element + public: void SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams); + /// \brief Get the number of visuals. /// \return Number of visuals contained in this Link object. public: uint64_t VisualCount() const; diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index 084ed4b6a..281b738a9 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -85,6 +85,11 @@ namespace sdf : val(_val), precision(_precision) {} }; + // Template deduction guide for ParamVariant + template + ParamStreamer(const ParamVariant &_val, int _precision) + -> ParamStreamer; + template std::ostream& operator<<(std::ostream &os, ParamStreamer s) { diff --git a/include/sdf/config.hh.in b/include/sdf/config.hh.in index d1b3db532..6a32cb8bc 100644 --- a/include/sdf/config.hh.in +++ b/include/sdf/config.hh.in @@ -47,7 +47,12 @@ #cmakedefine SDFORMAT_DISABLE_CONSOLE_LOGFILE 1 +#ifndef SDF_SHARE_PATH #define SDF_SHARE_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" -#define SDF_VERSION_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${SDF_MAJOR_VERSION}/${SDF_PKG_VERSION}" +#endif + +#ifndef SDF_VERSION_PATH +#define SDF_VERSION_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/${PROJECT_VERSION}" +#endif #endif // #ifndef SDF_CONFIG_HH_ diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt index 90509ffa4..10e789bfa 100644 --- a/sdf/CMakeLists.txt +++ b/sdf/CMakeLists.txt @@ -40,7 +40,7 @@ if (GZ_PROGRAM) add_custom_command( OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf COMMAND - ${CMAKE_COMMAND} -E env GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf + ${CMAKE_COMMAND} -E env GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf/$ ${GZ_PROGRAM} ARGS sdf -d ${desc_ver} > ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf COMMENT "Generating full description for spec ${desc_ver}" diff --git a/sdf/embedSdf.py b/sdf/embedSdf.py index 9764ee230..733871d99 100644 --- a/sdf/embedSdf.py +++ b/sdf/embedSdf.py @@ -161,10 +161,10 @@ def generate_map_content(paths: List[Path], relative_to: Optional[str] = None) - for path in paths: with open(path, "r", encoding="utf8") as input_sdf: file_content = input_sdf.read() - # Strip relative path if requested if relative_to is not None: - path = path.relative_to(relative_to) + _, relative_path = str(path).split(relative_to) + path = relative_path # dir separator is hardcoded to '/' in C++ mapping posix_path = PurePosixPath(path) content.append(embed_sdf_content(str(posix_path), file_content)) @@ -203,4 +203,4 @@ def main(args=None) -> int: if __name__ == "__main__": - sys.exit(main()) \ No newline at end of file + sys.exit(main()) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index feab8e716..dfd569186 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -5,10 +5,9 @@ gz_get_libsources_and_unittests(sources gtest_sources) # Add the source file auto-generated into the build folder from sdf/CMakeLists.txt list(APPEND sources EmbeddedSdf.cc) -# Use interface library to deduplicate cmake logic for URDF linking -add_library(using_parser_urdf INTERFACE) +# When using the internal URDF parser, we build its sources with the core library if (USE_INTERNAL_URDF) - set(sources ${sources} + set(urdf_internal_sources urdf/urdf_parser/model.cpp urdf/urdf_parser/link.cpp urdf/urdf_parser/joint.cpp @@ -17,60 +16,48 @@ if (USE_INTERNAL_URDF) urdf/urdf_parser/urdf_model_state.cpp urdf/urdf_parser/urdf_sensor.cpp urdf/urdf_parser/world.cpp) - target_include_directories(using_parser_urdf INTERFACE - ${CMAKE_CURRENT_SOURCE_DIR}/urdf) - if (WIN32) - target_compile_definitions(using_parser_urdf INTERFACE -D_USE_MATH_DEFINES) - endif() -else() - target_link_libraries(using_parser_urdf INTERFACE - GzURDFDOM::GzURDFDOM) + set(sources ${sources} ${urdf_internal_sources}) endif() -if (BUILD_TESTING) - # Build this test file only if Gazebo Tools is installed. - if (NOT GZ_PROGRAM) - list(REMOVE_ITEM gtest_sources gz_TEST.cc) - endif() +gz_create_core_library(SOURCES ${sources} + CXX_STANDARD 17 + LEGACY_PROJECT_PREFIX SDFormat + ) - # Skip tests that don't work on Windows - if (WIN32) - list(REMOVE_ITEM gtest_sources Converter_TEST.cc) - list(REMOVE_ITEM gtest_sources FrameSemantics_TEST.cc) - list(REMOVE_ITEM gtest_sources ParamPassing_TEST.cc) - list(REMOVE_ITEM gtest_sources Utils_TEST.cc) - list(REMOVE_ITEM gtest_sources XmlUtils_TEST.cc) - list(REMOVE_ITEM gtest_sources parser_urdf_TEST.cc) +target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} + PUBLIC + gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} + gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} + PRIVATE + TINYXML2::TINYXML2) + + if (USE_INTERNAL_URDF) + target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/urdf) + if (WIN32) + target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE -D_USE_MATH_DEFINES) + endif() + else() + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE + GzURDFDOM::GzURDFDOM) endif() - gz_build_tests( - TYPE UNIT - SOURCES ${gtest_sources} - INCLUDE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR} - ${PROJECT_SOURCE_DIR}/test - ) +if (WIN32 AND USE_INTERNAL_URDF) + target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE URDFDOM_STATIC) +endif() - if (TARGET UNIT_Converter_TEST) - target_link_libraries(UNIT_Converter_TEST - TINYXML2::TINYXML2) - target_sources(UNIT_Converter_TEST PRIVATE - Converter.cc - EmbeddedSdf.cc - Utils.cc - XmlUtils.cc) - endif() +target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) - if (TARGET UNIT_FrameSemantics_TEST) - target_sources(UNIT_FrameSemantics_TEST PRIVATE FrameSemantics.cc Utils.cc) - target_link_libraries(UNIT_FrameSemantics_TEST TINYXML2::TINYXML2) +if (BUILD_TESTING) + # Build this test file only if Gazebo Tools is installed. + if (NOT GZ_PROGRAM) + list(REMOVE_ITEM gtest_sources gz_TEST.cc) endif() - if (TARGET UNIT_ParamPassing_TEST) - target_link_libraries(UNIT_ParamPassing_TEST - TINYXML2::TINYXML2 - using_parser_urdf) - target_sources(UNIT_ParamPassing_TEST PRIVATE + add_library(library_for_tests OBJECT Converter.cc EmbeddedSdf.cc FrameSemantics.cc @@ -79,53 +66,47 @@ if (BUILD_TESTING) Utils.cc XmlUtils.cc parser.cc - parser_urdf.cc) - endif() + parser_urdf.cc + ) - if (TARGET UNIT_Utils_TEST) - target_sources(UNIT_Utils_TEST PRIVATE Utils.cc) - target_link_libraries(UNIT_Utils_TEST TINYXML2::TINYXML2) + if (USE_INTERNAL_URDF) + target_sources(library_for_tests PRIVATE ${urdf_internal_sources}) endif() - if (TARGET UNIT_XmlUtils_TEST) - target_link_libraries(UNIT_XmlUtils_TEST - TINYXML2::TINYXML2) - target_sources(UNIT_XmlUtils_TEST PRIVATE XmlUtils.cc) - endif() + # Link against the publicly and privately linked libraries of the core library + target_link_libraries(library_for_tests + $ + ) - if (TARGET UNIT_parser_urdf_TEST) - target_link_libraries(UNIT_parser_urdf_TEST - TINYXML2::TINYXML2 - using_parser_urdf) - target_sources(UNIT_parser_urdf_TEST PRIVATE - SDFExtension.cc - XmlUtils.cc - parser_urdf.cc) - endif() -endif() + # Use the include flags from the core library + target_include_directories(library_for_tests PUBLIC + $ + ) -gz_create_core_library(SOURCES ${sources} - CXX_STANDARD 17 - LEGACY_PROJECT_PREFIX SDFormat + # Use the private compile flags from the core library. Also set GZ_SDFORMAT_STATIC_DEFINE to avoid + # inconsistent linkage issues on windows. Setting the define will cause the SDFORMAT_VISIBLE/SDFORMAT_HIDDEN macros + # to expand to nothing when building a static library + target_compile_definitions(library_for_tests PUBLIC + $ + -DGZ_SDFORMAT_STATIC_DEFINE ) -target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} - PUBLIC - gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} - gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} - PRIVATE - TINYXML2::TINYXML2 - using_parser_urdf) + gz_build_tests( + TYPE UNIT + SOURCES ${gtest_sources} + INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR} + ${PROJECT_SOURCE_DIR}/test + LIB_DEPS + library_for_tests + ) -if (WIN32) - target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE URDFDOM_STATIC) + if (TARGET UNIT_gz_TEST) + target_compile_definitions(UNIT_gz_TEST PRIVATE + -DGZ_PATH="${GZ_PROGRAM}" + -DDETAIL_GZ_CONFIG_PATH="${CMAKE_BINARY_DIR}/test/conf/$" + -DGZ_TEST_LIBRARY_PATH="${PROJECT_BINARY_DIR}/src") + endif() endif() -target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) - -if(NOT WIN32) - add_subdirectory(cmd) -endif() +add_subdirectory(cmd) diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index 4cb39a851..0c77fcff7 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -159,7 +159,7 @@ TEST(DOMCapsule, Load) // Add a radius element sdf::ElementPtr radiusDesc(new sdf::Element()); radiusDesc->SetName("radius"); - radiusDesc->AddValue("double", "1.0", "1", "radius"); + radiusDesc->AddValue("double", "1.0", true, "radius"); sdf->AddElementDescription(radiusDesc); sdf::ElementPtr radiusElem = sdf->AddElement("radius"); radiusElem->Set(2.0); diff --git a/src/Collision.cc b/src/Collision.cc index c2b2a15ec..ca944712d 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -49,13 +49,10 @@ class sdf::Collision::Implementation /// \brief The collision's surface parameters. public: sdf::Surface surface; - /// \brief Density of the collision. Default is 1000.0 - public: double density{1000.0}; + /// \brief Density of the collision if it has been set. + public: std::optional density; - /// \brief True if density was set during load from sdf. - public: bool densitySetAtLoad = false; - - /// \brief SDF element pointer to tag + /// \brief SDF element pointer to tag public: sdf::ElementPtr autoInertiaParams{nullptr}; /// \brief The SDF element pointer used during load. @@ -130,7 +127,6 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) if (_sdf->HasElement("density")) { this->dataPtr->density = _sdf->Get("density"); - this->dataPtr->densitySetAtLoad = true; } // Load the auto_inertia_params element @@ -155,10 +151,20 @@ void Collision::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +double Collision::DensityDefault() +{ + return 1000.0; +} + ///////////////////////////////////////////////// double Collision::Density() const { - return this->dataPtr->density; + if (this->dataPtr->density) + { + return *this->dataPtr->density; + } + return DensityDefault(); } ///////////////////////////////////////////////// @@ -256,23 +262,59 @@ void Collision::CalculateInertial( gz::math::Inertiald &_inertial, const ParserConfig &_config) { - // Check if density was not set during load & send a warning - // about the default value being used - if (!this->dataPtr->densitySetAtLoad) + this->CalculateInertial( + _errors, _inertial, _config, std::nullopt, ElementPtr()); +} + +///////////////////////////////////////////////// +void Collision::CalculateInertial( + sdf::Errors &_errors, + gz::math::Inertiald &_inertial, + const ParserConfig &_config, + const std::optional &_density, + sdf::ElementPtr _autoInertiaParams) +{ + // Order of precedence for density: + double density; + // 1. Density explicitly set in this collision, either from the + // `//collision/density` element or from Collision::SetDensity. + if (this->dataPtr->density) + { + density = *this->dataPtr->density; + } + // 2. Density passed into this function, which likely comes from the + // `//link/inertial/density` element or from Link::SetDensity. + else if (_density) + { + density = *_density; + } + // 3. DensityDefault value. + else { + // If density was not explicitly set, send a warning + // about the default value being used + density = DensityDefault(); Error densityMissingErr( ErrorCode::ELEMENT_MISSING, "Collision is missing a child element. " - "Using a default density value of 1000.0 kg/m^3. " + "Using a default density value of " + + std::to_string(DensityDefault()) + " kg/m^3. " ); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), densityMissingErr, _errors ); } + // If this Collision's auto inertia params have not been set, then use the + // params passed into this function. + sdf::ElementPtr autoInertiaParams = this->dataPtr->autoInertiaParams; + if (!autoInertiaParams) + { + autoInertiaParams = _autoInertiaParams; + } auto geomInertial = this->dataPtr->geom.CalculateInertial(_errors, _config, - this->dataPtr->density, this->dataPtr->autoInertiaParams); + density, autoInertiaParams); if (!geomInertial) { @@ -309,6 +351,7 @@ sdf::ElementPtr Collision::Element() const return this->dataPtr->sdf; } +///////////////////////////////////////////////// sdf::ElementPtr Collision::ToElement() const { sdf::Errors errors; @@ -335,8 +378,11 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const poseElem->Set(_errors, this->RawPose()); // Set the density - sdf::ElementPtr densityElem = elem->GetElement("density", _errors); - densityElem->Set(this->Density()); + if (this->dataPtr->density.has_value()) + { + sdf::ElementPtr densityElem = elem->GetElement("density", _errors); + densityElem->Set(this->Density()); + } // Set the geometry elem->InsertElement(this->dataPtr->geom.ToElement(_errors), true); diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 07f894a58..1ab3d1b18 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -38,6 +38,7 @@ TEST(DOMcollision, Construction) EXPECT_EQ(nullptr, collision.Element()); EXPECT_TRUE(collision.Name().empty()); EXPECT_EQ(collision.Density(), 1000.0); + EXPECT_EQ(collision.DensityDefault(), 1000.0); collision.SetName("test_collison"); EXPECT_EQ(collision.Name(), "test_collison"); @@ -58,12 +59,12 @@ TEST(DOMcollision, Construction) EXPECT_DOUBLE_EQ(collision.Density(), 1240.0); EXPECT_EQ(collision.AutoInertiaParams(), nullptr); - sdf::ElementPtr autoInertialParamsElem(new sdf::Element()); - autoInertialParamsElem->SetName("auto_inertial_params"); - collision.SetAutoInertiaParams(autoInertialParamsElem); - EXPECT_EQ(collision.AutoInertiaParams(), autoInertialParamsElem); + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + autoInertiaParamsElem->SetName("auto_inertia_params"); + collision.SetAutoInertiaParams(autoInertiaParamsElem); + EXPECT_EQ(collision.AutoInertiaParams(), autoInertiaParamsElem); EXPECT_EQ(collision.AutoInertiaParams()->GetName(), - autoInertialParamsElem->GetName()); + autoInertiaParamsElem->GetName()); collision.SetRawPose({-10, -20, -30, GZ_PI, GZ_PI, GZ_PI}); EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, GZ_PI, GZ_PI, GZ_PI), @@ -420,6 +421,8 @@ TEST(DOMCollision, ToElement) sdf::ElementPtr elem = collision.ToElement(); ASSERT_NE(nullptr, elem); + // Expect no density element + EXPECT_FALSE(elem->HasElement("density")); sdf::Collision collision2; collision2.Load(elem); @@ -434,6 +437,19 @@ TEST(DOMCollision, ToElement) ASSERT_NE(nullptr, surface2->Friction()); ASSERT_NE(nullptr, surface2->Friction()->ODE()); EXPECT_DOUBLE_EQ(1.23, surface2->Friction()->ODE()->Mu()); + + // Now set density in collision + const double kDensity = 1234.5; + collision.SetDensity(kDensity); + sdf::ElementPtr elemWithDensity = collision.ToElement(); + ASSERT_NE(nullptr, elemWithDensity); + // Expect density element + ASSERT_TRUE(elemWithDensity->HasElement("density")); + EXPECT_DOUBLE_EQ(kDensity, elemWithDensity->Get("density")); + + sdf::Collision collision3; + collision3.Load(elem); + EXPECT_DOUBLE_EQ(kDensity, collision.Density()); } ///////////////////////////////////////////////// diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index 5c7d04ead..b07f298dd 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -155,7 +155,7 @@ TEST(DOMCylinder, Load) // Add a radius element sdf::ElementPtr radiusDesc(new sdf::Element()); radiusDesc->SetName("radius"); - radiusDesc->AddValue("double", "1.0", "1", "radius"); + radiusDesc->AddValue("double", "1.0", true, "radius"); sdf->AddElementDescription(radiusDesc); sdf::ElementPtr radiusElem = sdf->AddElement("radius"); radiusElem->Set(2.0); diff --git a/src/Link.cc b/src/Link.cc index 40a64012f..085feab93 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -69,6 +69,13 @@ class sdf::Link::Implementation /// \brief The projectors specified in this link. public: std::vector projectors; + /// \brief Density of the inertial which will be used for auto-inertial + /// calculations if the collision density has not been set. + public: std::optional density; + + /// \brief SDF element pointer to tag + public: sdf::ElementPtr autoInertiaParams{nullptr}; + /// \brief The inertial information for this link. public: gz::math::Inertiald inertial {{1.0, gz::math::Vector3d::One, gz::math::Vector3d::Zero}, @@ -180,6 +187,18 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { sdf::ElementPtr inertialElem = _sdf->GetElement("inertial"); + if (inertialElem->HasElement("density")) + { + this->dataPtr->density = inertialElem->Get("density"); + } + + // Load the auto_inertia_params element + if (inertialElem->HasElement("auto_inertia_params")) + { + this->dataPtr->autoInertiaParams = + inertialElem->GetElement("auto_inertia_params"); + } + if (inertialElem->Get("auto")) { this->dataPtr->autoInertia = true; @@ -309,6 +328,30 @@ void Link::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +std::optional Link::Density() const +{ + return this->dataPtr->density; +} + +///////////////////////////////////////////////// +void Link::SetDensity(double _density) +{ + this->dataPtr->density = _density; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Link::AutoInertiaParams() const +{ + return this->dataPtr->autoInertiaParams; +} + +///////////////////////////////////////////////// +void Link::SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams) +{ + this->dataPtr->autoInertiaParams = _autoInertiaParams; +} + ///////////////////////////////////////////////// uint64_t Link::VisualCount() const { @@ -620,7 +663,9 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors, for (sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; - collision.CalculateInertial(_errors, collisionInertia, _config); + collision.CalculateInertial(_errors, collisionInertia, _config, + this->dataPtr->density, + this->dataPtr->autoInertiaParams); totalInertia = totalInertia + collisionInertia; } @@ -942,6 +987,11 @@ sdf::ElementPtr Link::ToElement() const inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz()); inertiaElem->GetElement("izz")->Set(massMatrix.Izz()); + if (this->dataPtr->density.has_value()) + { + inertialElem->GetElement("density")->Set(*this->dataPtr->density); + } + if (this->dataPtr->inertial.FluidAddedMass().has_value()) { auto addedMass = this->dataPtr->inertial.FluidAddedMass().value(); diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 5e0968b75..2182ff2e3 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -125,6 +125,20 @@ TEST(DOMLink, Construction) EXPECT_EQ(inertial.BodyMatrix(), inertial.SpatialMatrix()); EXPECT_TRUE(inertial.MassMatrix().IsValid()); + EXPECT_FALSE(link.Density().has_value()); + const double density = 123.0; + link.SetDensity(density); + ASSERT_TRUE(link.Density().has_value()); + EXPECT_DOUBLE_EQ(density, *link.Density()); + + EXPECT_EQ(link.AutoInertiaParams(), nullptr); + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + autoInertiaParamsElem->SetName("auto_inertia_params"); + link.SetAutoInertiaParams(autoInertiaParamsElem); + EXPECT_EQ(link.AutoInertiaParams(), autoInertiaParamsElem); + EXPECT_EQ(link.AutoInertiaParams()->GetName(), + autoInertiaParamsElem->GetName()); + EXPECT_EQ(0u, link.CollisionCount()); EXPECT_EQ(nullptr, link.CollisionByIndex(0)); EXPECT_EQ(nullptr, link.CollisionByIndex(1)); @@ -302,6 +316,355 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink) EXPECT_NE(nullptr, root.Element()); } +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithDifferentDensity) +{ + const std::string sdfString = R"( + + + + 0 0 1.0 0 0 0 + + 0 1.0 0 0 0 0 + + 12.0 + + + 1.0 0 0 0 0 0 + + + 1 1 1 + + + + + + 0 2.0 0 0 0 0 + + + 1.0 0 0 0 0 0 + 24.0 + + + 1 1 1 + + + + + + 0 3.0 0 0 0 0 + + 12.0 + + + 1.0 0 0 0 0 0 + 24.0 + + + 1 1 1 + + + + + + 0 4.0 0 0 0 0 + + + 1.0 0 0 0 0 0 + + + 1 1 1 + + + + + + )"; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdfString, sdfParserConfig); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ(4u, model->LinkCount()); + + // ResolveAutoInertials should run by default during Root::Load. + + { + const sdf::Link *link = model->LinkByName("link_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + ASSERT_TRUE(linkDensity.has_value()); + EXPECT_DOUBLE_EQ(12.0, *linkDensity); + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(12.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(2, 2, 2), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } + + { + const sdf::Link *link = model->LinkByName("collision_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + EXPECT_FALSE(linkDensity.has_value()); + // assume Collision density is properly set to 24 + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } + + { + const sdf::Link *link = + model->LinkByName("collision_density_overrides_link_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + ASSERT_TRUE(linkDensity.has_value()); + EXPECT_DOUBLE_EQ(12.0, *linkDensity); + // assume Collision density is properly set to 24 and overrides the link + // density + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } + + { + const sdf::Link *link = + model->LinkByName("default_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + EXPECT_FALSE(linkDensity.has_value()); + // assume density is the default value of 1000.0 + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(1000.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d::One * 500.0 / 3.0, + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } +} + +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithDifferentAutoInertiaParams) +{ + const std::string sdfString = R"( + + + + 0 0 1.0 0 0 0 + + 0 1.0 0 0 0 0 + + + 12 + 1 1 1 + + + + 1.0 0 0 0 0 0 + + + uri + + + + + + 0 2.0 0 0 0 0 + + + 2.0 0 0 0 0 0 + + 24 + 1 1 1 + + + + uri + + + + + + 0 3.0 0 0 0 0 + + + 12 + 1 1 1 + + + + 3.0 0 0 0 0 0 + + 24 + 1 1 1 + + + + uri + + + + + + 0 4.0 0 0 0 0 + + + 4.0 0 0 0 0 0 + + + uri + + + + + + )"; + + // Lambda function for registering as custom inertia calculator + auto customMeshInertiaCalculator = []( + sdf::Errors &, + const sdf::CustomInertiaCalcProperties &_inertiaProps + ) -> std::optional + { + auto autoInertiaParams = _inertiaProps.AutoInertiaParams(); + if (!autoInertiaParams || + !autoInertiaParams->HasElement("gz:density") || + !autoInertiaParams->HasElement("gz:box_size")) + { + // return default inertial values + gz::math::Inertiald meshInertial; + + meshInertial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + + return meshInertial; + } + + gz::math::Inertiald meshInerial; + + double gzDensity = autoInertiaParams->Get("gz:density"); + gz::math::Vector3d gzBoxSize = + autoInertiaParams->Get("gz:box_size"); + gz::math::Material material = gz::math::Material(gzDensity); + + gz::math::MassMatrix3d massMatrix; + massMatrix.SetFromBox(gz::math::Material(gzDensity), gzBoxSize); + + meshInerial.SetMassMatrix(massMatrix); + + return meshInerial; + }; + + + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + sdfParserConfig.RegisterCustomInertiaCalc(customMeshInertiaCalculator); + sdf::Errors errors = root.LoadSdfString(sdfString, sdfParserConfig); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ(4u, model->LinkCount()); + + // ResolveAutoInertials should run by default during Root::Load. + + { + const sdf::Link *link = model->LinkByName("link_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //inertial/auto_inertia_params + // * gz:density == 12 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(12.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(2, 2, 2), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = model->LinkByName("collision_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //collision/auto_inertia_params + // * gz:density == 24 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(2, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = + model->LinkByName("collision_auto_inertia_params_overrides"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //collision/auto_inertia_params + // * gz:density == 24 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(3, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = + model->LinkByName("default_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify default inertial values + auto inertial = link->Inertial(); + EXPECT_DOUBLE_EQ(1.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(1, 1, 1), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(4, 0, 0, 0, 0, 0), inertial.Pose()); + } +} + ///////////////////////////////////////////////// TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions) { @@ -621,12 +984,18 @@ TEST(DOMLink, ToElement) sdf::ElementPtr elem = link.ToElement(); ASSERT_NE(nullptr, elem); + // Expect no density element + { + auto inertialElem = elem->FindElement("inertial"); + EXPECT_FALSE(inertialElem->HasElement("density")); + } sdf::Link link2; link2.Load(elem); EXPECT_EQ(link.Name(), link2.Name()); EXPECT_EQ(link.Inertial(), link2.Inertial()); + EXPECT_EQ(link.Density(), link2.Density()); EXPECT_EQ(link.RawPose(), link2.RawPose()); EXPECT_EQ(link.EnableWind(), link2.EnableWind()); EXPECT_EQ(link.CollisionCount(), link2.CollisionCount()); @@ -652,6 +1021,23 @@ TEST(DOMLink, ToElement) EXPECT_EQ(link.ProjectorCount(), link2.ProjectorCount()); for (uint64_t i = 0; i < link2.ProjectorCount(); ++i) EXPECT_NE(nullptr, link2.ProjectorByIndex(i)); + + // Now set density in link + const double kDensity = 1234.5; + link.SetDensity(kDensity); + sdf::ElementPtr elemWithDensity = link.ToElement(); + ASSERT_NE(nullptr, elemWithDensity); + // Expect density element + { + auto inertialElem = elemWithDensity->FindElement("inertial"); + ASSERT_TRUE(inertialElem->HasElement("density")); + EXPECT_DOUBLE_EQ(kDensity, inertialElem->Get("density")); + } + + sdf::Link link3; + link3.Load(elemWithDensity); + ASSERT_TRUE(link3.Density().has_value()); + EXPECT_DOUBLE_EQ(kDensity, *link3.Density()); } ///////////////////////////////////////////////// diff --git a/src/Mesh.cc b/src/Mesh.cc index a3d4d1274..fa180b2bb 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -199,14 +199,6 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, double _density, const sdf::ElementPtr _autoInertiaParams, const ParserConfig &_config) { - if (this->dataPtr->filePath.empty()) - { - _errors.push_back({ - sdf::ErrorCode::WARNING, - "File Path for the mesh was empty. Could not calculate inertia"}); - return std::nullopt; - } - const auto &customCalculator = _config.CustomInertiaCalc(); if (!customCalculator) diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 7a8c4c4ca..51c1751f4 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -201,9 +201,6 @@ TEST(DOMMesh, CalcualteInertial) double density = 0; sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); - // File Path needs to be considered as a valid mesh - mesh.SetFilePath("/some_path"); - // Test Lambda function for registering as custom inertia calculator auto customMeshInertiaCalculator = []( sdf::Errors &_errors, diff --git a/src/ParamPassing_TEST.cc b/src/ParamPassing_TEST.cc index 00f9ff48a..16f24c208 100644 --- a/src/ParamPassing_TEST.cc +++ b/src/ParamPassing_TEST.cc @@ -101,6 +101,15 @@ TEST(ParamPassing, GetElementByNameWarningOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + std::ostringstream stream; stream << "" << "" @@ -149,7 +158,7 @@ TEST(ParamPassing, GetElementByNameWarningOutput) EXPECT_NE(std::string::npos, buffer.str().find( "The original element [model] contains the attribute 'name' but none " "was provided in the element modifier. The assumed element to be " - "modified is: ")); + "modified is: ")) << buffer.str(); } //////////////////////////////////////// @@ -160,6 +169,15 @@ TEST(ParamPassing, ModifyChildrenNameWarningOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + std::ostringstream stream; stream << "" << "" @@ -212,5 +230,5 @@ TEST(ParamPassing, ModifyChildrenNameWarningOutput) EXPECT_NE(std::string::npos, buffer.str().find( "No modifications for element \n provided, " "skipping modification for:\n\n" - " \n")); + " \n")) << buffer.str(); } diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index ea93941a0..81439ffed 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -145,7 +145,7 @@ TEST(DOMPlane, Load) // Add a normal element sdf::ElementPtr normalDesc(new sdf::Element()); normalDesc->SetName("normal"); - normalDesc->AddValue("vector3", "0 0 1", "1", "normal"); + normalDesc->AddValue("vector3", "0 0 1", true, "normal"); sdf->AddElementDescription(normalDesc); sdf::ElementPtr normalElem = sdf->AddElement("normal"); normalElem->Set({1, 0, 0}); diff --git a/src/SDF.cc b/src/SDF.cc index 07056d792..7a43d0f9a 100644 --- a/src/SDF.cc +++ b/src/SDF.cc @@ -43,6 +43,15 @@ inline namespace SDF_VERSION_NAMESPACE // returns the version string when possible. std::string SDF::version = SDF_VERSION; // NOLINT(runtime/string) +std::string sdfSharePath() +{ +#ifdef SDF_SHARE_PATH + if (std::string(SDF_SHARE_PATH) != "/") + return SDF_SHARE_PATH; +#endif + return ""; +} + ///////////////////////////////////////////////// void setFindCallback(std::function _cb) { @@ -109,16 +118,17 @@ std::string findFile(const std::string &_filename, bool _searchLocalPath, } // Next check the install path. - std::string path = sdf::filesystem::append(SDF_SHARE_PATH, filename); + std::string path = sdf::filesystem::append(sdfSharePath(), filename); if (sdf::filesystem::exists(path)) { return path; } // Next check the versioned install path. - path = sdf::filesystem::append(SDF_SHARE_PATH, - "sdformat" SDF_MAJOR_VERSION_STR, - sdf::SDF::Version(), filename); + path = sdf::filesystem::append(sdfSharePath(), + "sdformat" + std::string(SDF_MAJOR_VERSION_STR), + sdf::SDF::Version(), filename); + if (sdf::filesystem::exists(path)) { return path; diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc index 43d2e4732..44bb9f284 100644 --- a/src/SDF_TEST.cc +++ b/src/SDF_TEST.cc @@ -16,7 +16,10 @@ */ #include + #include +#include + #include #include #include @@ -284,7 +287,7 @@ TEST(SDF, SetRoot) sdf::ElementPtr elem; elem.reset(new sdf::Element()); - elem->AddValue("bool", "true", "0", "description"); + elem->AddValue("bool", "true", false, "description"); s.SetRoot(elem); EXPECT_EQ(elem, s.Root()); @@ -306,56 +309,56 @@ TEST(SDF, EmptyValues) elem.reset(new sdf::Element()); EXPECT_FALSE(elem->Get(emptyString)); - elem->AddValue("bool", "true", "0", "description"); + elem->AddValue("bool", "true", false, "description"); EXPECT_TRUE(elem->Get(emptyString)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), 0); - elem->AddValue("int", "12", "0", "description"); + elem->AddValue("int", "12", false, "description"); EXPECT_EQ(elem->Get(emptyString), 12); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), static_cast(0)); - elem->AddValue("unsigned int", "123", "0", "description"); + elem->AddValue("unsigned int", "123", false, "description"); EXPECT_EQ(elem->Get(emptyString), static_cast(123)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), '\0'); - elem->AddValue("char", "a", "0", "description"); + elem->AddValue("char", "a", false, "description"); EXPECT_EQ(elem->Get(emptyString), 'a'); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), ""); - elem->AddValue("string", "hello", "0", "description"); + elem->AddValue("string", "hello", false, "description"); EXPECT_EQ(elem->Get(emptyString), "hello"); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Vector2d()); - elem->AddValue("vector2d", "1 2", "0", "description"); + elem->AddValue("vector2d", "1 2", false, "description"); EXPECT_EQ(elem->Get(emptyString), gz::math::Vector2d(1, 2)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Vector3d()); - elem->AddValue("vector3", "1 2 3", "0", "description"); + elem->AddValue("vector3", "1 2 3", false, "description"); EXPECT_EQ(elem->Get(emptyString), gz::math::Vector3d(1, 2, 3)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Quaterniond()); - elem->AddValue("quaternion", "1 2 3", "0", "description"); + elem->AddValue("quaternion", "1 2 3", false, "description"); EXPECT_EQ(elem->Get(emptyString), gz::math::Quaterniond(-2.14159, 1.14159, -0.141593)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Pose3d()); - elem->AddValue("pose", "1.0 2.0 3.0 4.0 5.0 6.0", "0", "description"); + elem->AddValue("pose", "1.0 2.0 3.0 4.0 5.0 6.0", false, "description"); EXPECT_EQ(elem->Get(emptyString).Pos(), gz::math::Pose3d(1, 2, 3, 4, 5, 6).Pos()); EXPECT_EQ(elem->Get(emptyString).Rot().Euler(), @@ -364,23 +367,23 @@ TEST(SDF, EmptyValues) elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Color()); - elem->AddValue("color", ".1 .2 .3 1.0", "0", "description"); + elem->AddValue("color", ".1 .2 .3 1.0", false, "description"); EXPECT_EQ(elem->Get(emptyString), gz::math::Color(.1f, .2f, .3f, 1.0f)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), sdf::Time()); - elem->AddValue("time", "1 2", "0", "description"); + elem->AddValue("time", "1 2", false, "description"); EXPECT_EQ(elem->Get(emptyString), sdf::Time(1, 2)); elem.reset(new sdf::Element()); EXPECT_NEAR(elem->Get(emptyString), 0.0, 1e-6); - elem->AddValue("float", "12.34", "0", "description"); + elem->AddValue("float", "12.34", false, "description"); EXPECT_NEAR(elem->Get(emptyString), 12.34, 1e-6); elem.reset(new sdf::Element()); EXPECT_NEAR(elem->Get(emptyString), 0.0, 1e-6); - elem->AddValue("double", "12.34", "0", "description"); + elem->AddValue("double", "12.34", false, "description"); EXPECT_NEAR(elem->Get(emptyString), 12.34, 1e-6); } @@ -788,17 +791,17 @@ TEST(SDF, WriteURIPath) ASSERT_EQ(std::remove(tempFile.c_str()), 0); ASSERT_EQ(rmdir(tempDir.c_str()), 0); } - ///////////////////////////////////////////////// TEST(SDF, FindFileModelSDFCurrDir) { - std::string currDir; - - // Get current directory path from $PWD env variable - currDir = sdf::filesystem::current_path(); + // Change to a temporary directory before running test + auto prevPath = std::filesystem::current_path(); + std::string tmpDir; + ASSERT_TRUE(sdf::testing::TestTmpPath(tmpDir)); + std::filesystem::current_path(tmpDir); // A file named model.sdf in current directory - auto tempFile = currDir + "/model.sdf"; + auto tempFile = tmpDir + "/model.sdf"; sdf::SDF tempSDF; tempSDF.Write(tempFile); @@ -815,5 +818,6 @@ TEST(SDF, FindFileModelSDFCurrDir) // Cleanup ASSERT_EQ(std::remove(tempFile.c_str()), 0); + std::filesystem::current_path(prevPath); } #endif // _WIN32 diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index 0c4c1a2f0..a869d5fca 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -2,8 +2,10 @@ # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. # Ex: cmdsdformat0.rb -set(cmd_script_generated_test "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${PROJECT_NAME}.rb") -set(cmd_script_configured_test "${cmd_script_generated_test}.configured") +set(cmd_script_generated_test + "${CMAKE_BINARY_DIR}/test/lib/$/ruby/gz/cmd${PROJECT_NAME}.rb") +set(cmd_script_configured_test + "${CMAKE_CURRENT_BINARY_DIR}/test_cmd${PROJECT_NAME}.rb.configured") # Set the library_location variable to the full path of the library file within # the build directory. @@ -24,8 +26,8 @@ file(GENERATE # Generate the ruby script that gets installed. # Note that the major version of the library is included in the name. # Ex: cmdsdformat0.rb -set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmd${PROJECT_NAME}.rb") -set(cmd_script_configured "${cmd_script_generated}.configured") +set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/$/cmd${PROJECT_NAME}.rb") +set(cmd_script_configured "${CMAKE_CURRENT_BINARY_DIR}/cmd${PROJECT_NAME}.rb.configured") # Set the library_location variable to the relative path to the library file # within the install directory structure. diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index d656f2efe..816882f56 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -22,6 +22,7 @@ #include #include +#include #include "sdf/Error.hh" #include "sdf/Filesystem.hh" @@ -36,6 +37,9 @@ #define pclose _pclose #endif +// DETAIL_GZ_CONFIG_PATH is compiler definition set in CMake. +#define GZ_CONFIG_PATH DETAIL_GZ_CONFIG_PATH + static std::string SdfVersion() { return " --force-version " + std::string(SDF_VERSION_FULL); @@ -71,12 +75,10 @@ std::string custom_exec_str(std::string _cmd) ///////////////////////////////////////////////// TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - // Check an SDFormat file with unrecognized elements { - std::string path = pathBase +"/unrecognized_elements.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "unrecognized_elements.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -101,7 +103,8 @@ TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDFormat file with unrecognized elements with XML namespaces { - std::string path = pathBase +"/unrecognized_elements_with_namespace.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "unrecognized_elements_with_namespace.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -119,12 +122,10 @@ TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ///////////////////////////////////////////////// TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - // Check a good SDF file { - std::string path = pathBase +"/box_plane_low_friction_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_plane_low_friction_test.world"); // Check box_plane_low_friction_test.world std::string output = @@ -134,7 +135,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a bad SDF file { - std::string path = pathBase +"/box_bad_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_bad_test.world"); // Check box_bad_test.world std::string output = @@ -146,7 +148,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (world) // that have duplicate names. { - std::string path = pathBase +"/world_duplicate.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "world_duplicate.sdf"); // Check world_duplicate.sdf std::string output = @@ -158,7 +161,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of different types (model, light) // that have duplicate names. { - std::string path = pathBase +"/world_sibling_same_names.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "world_sibling_same_names.sdf"); // Check world_sibling_same_names.sdf std::string output = @@ -168,7 +172,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } // Check an SDF world file is allowed to have duplicate plugin names { - std::string path = pathBase +"/world_duplicate_plugins.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "world_duplicate_plugins.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -178,7 +183,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (link) // that have duplicate names. { - std::string path = pathBase +"/model_duplicate_links.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_duplicate_links.sdf"); // Check model_duplicate_links.sdf std::string output = @@ -190,7 +196,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (joint) // that have duplicate names. { - std::string path = pathBase +"/model_duplicate_joints.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_duplicate_joints.sdf"); // Check model_duplicate_joints.sdf std::string output = @@ -202,7 +209,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of different types (link, joint) // that have duplicate names. { - std::string path = pathBase +"/model_link_joint_same_name.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_link_joint_same_name.sdf"); // Check model_link_joint_same_name.sdf std::string output = @@ -213,7 +221,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF model file is allowed to have duplicate plugin names { - std::string path = pathBase +"/model_duplicate_plugins.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_duplicate_plugins.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -223,7 +232,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (collision) // that have duplicate names. { - std::string path = pathBase +"/link_duplicate_sibling_collisions.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "link_duplicate_sibling_collisions.sdf"); // Check link_duplicate_sibling_collisions.sdf std::string output = @@ -236,7 +247,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (visual) // that have duplicate names. { - std::string path = pathBase +"/link_duplicate_sibling_visuals.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "link_duplicate_sibling_visuals.sdf"); // Check link_duplicate_sibling_visuals.sdf std::string output = @@ -248,7 +261,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with cousin elements of the same type (collision) // that have duplicate names. This is a valid file. { - std::string path = pathBase +"/link_duplicate_cousin_collisions.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "link_duplicate_cousin_collisions.sdf"); // Check link_duplicate_cousin_collisions.sdf std::string output = @@ -259,7 +274,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with cousin elements of the same type (visual) // that have duplicate names. This is a valid file. { - std::string path = pathBase +"/link_duplicate_cousin_visuals.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "link_duplicate_cousin_visuals.sdf"); // Check link_duplicate_cousin_visuals.sdf std::string output = @@ -269,7 +286,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint with an invalid child link. { - std::string path = pathBase +"/joint_invalid_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_invalid_child.sdf"); // Check joint_invalid_child.sdf std::string output = @@ -282,7 +300,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint with an invalid parent link. { - std::string path = pathBase +"/joint_invalid_parent.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_invalid_parent.sdf"); // Check joint_invalid_parent.sdf std::string output = @@ -295,7 +314,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint that names itself as the child frame. { - std::string path = pathBase +"/joint_invalid_self_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_invalid_self_child.sdf"); // Check joint_invalid_self_child.sdf std::string output = @@ -307,7 +327,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint that names itself as the parent frame. { - std::string path = pathBase +"/joint_invalid_self_parent.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_invalid_self_parent.sdf"); // Check joint_invalid_self_parent.sdf std::string output = @@ -320,7 +341,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint with identical parent and child. { - std::string path = pathBase +"/joint_invalid_parent_same_as_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "joint_invalid_parent_same_as_child.sdf"); // Check joint_invalid_parent_same_as_child.sdf std::string output = @@ -334,8 +357,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint with parent parent frame that resolves // to the same value as the child. { - std::string path = - pathBase + "/joint_invalid_resolved_parent_same_as_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "joint_invalid_resolved_parent_same_as_child.sdf"); // Check joint_invalid_resolved_parent_same_as_child.sdf std::string output = @@ -348,7 +372,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with the world specified as a child link. { - std::string path = pathBase +"/joint_child_world.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_child_world.sdf"); // Check joint_child_world.sdf std::string output = @@ -364,7 +389,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with the world specified as a parent link. // This is a valid file. { - std::string path = pathBase +"/joint_parent_world.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_parent_world.sdf"); // Check joint_parent_world.sdf std::string output = @@ -375,7 +401,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a frame specified as the joint child. // This is a valid file. { - std::string path = pathBase +"/joint_child_frame.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_child_frame.sdf"); // Check joint_child_frame.sdf std::string output = @@ -386,7 +413,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a frame specified as the joint parent. // This is a valid file. { - std::string path = pathBase +"/joint_parent_frame.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_parent_frame.sdf"); // Check joint_parent_frame.sdf std::string output = @@ -397,7 +425,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with the infinite values for joint axis limits. // This is a valid file. { - std::string path = pathBase +"/joint_axis_infinite_limits.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_axis_infinite_limits.sdf"); // Check joint_axis_infinite_limits.sdf std::string output = @@ -408,7 +437,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with the second link specified as the canonical link. // This is a valid file. { - std::string path = pathBase +"/model_canonical_link.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_canonical_link.sdf"); // Check model_canonical_link.sdf std::string output = @@ -418,7 +448,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with an invalid link specified as the canonical link. { - std::string path = pathBase +"/model_invalid_canonical_link.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_invalid_canonical_link.sdf"); // Check model_invalid_canonical_link.sdf std::string output = @@ -430,7 +461,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with an invalid model without links. { - std::string path = pathBase +"/model_without_links.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_without_links.sdf"); // Check model_without_links.sdf std::string output = @@ -441,7 +474,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a nested model. { - std::string path = pathBase +"/nested_model.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_model.sdf"); // Check nested_model.sdf std::string output = @@ -451,7 +486,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a model that has a nested canonical link. { - std::string path = pathBase +"/nested_canonical_link.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_canonical_link.sdf"); // Check nested_canonical_link.sdf std::string output = @@ -463,7 +500,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // that is explicitly specified by //model/@canonical_link using :: // syntax. { - std::string path = pathBase +"/nested_explicit_canonical_link.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_explicit_canonical_link.sdf"); // Check nested_explicit_canonical_link.sdf std::string output = @@ -473,7 +512,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a model that a nested model without a link. { - std::string path = pathBase +"/nested_without_links_invalid.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_without_links_invalid.sdf"); // Check nested_without_links_invalid.sdf std::string output = @@ -484,7 +525,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an invalid SDF file that uses reserved names. { - std::string path = pathBase +"/model_invalid_reserved_names.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_reserved_names.sdf"); // Check model_invalid_reserved_names.sdf std::string output = @@ -510,7 +553,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check that validity checks are disabled inside elements { - std::string path = pathBase +"/ignore_sdf_in_plugin.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "ignore_sdf_in_plugin.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -519,7 +564,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check that validity checks are disabled inside namespaced elements { - std::string path = pathBase +"/ignore_sdf_in_namespaced_elements.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "ignore_sdf_in_namespaced_elements.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -529,7 +576,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames using the attached_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_frame_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_attached_to.sdf"); // Check model_frame_attached_to.sdf std::string output = @@ -540,7 +589,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames attached_to joints. // This is a valid file. { - std::string path = pathBase +"/model_frame_attached_to_joint.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_attached_to_joint.sdf"); // Check model_frame_attached_to_joint.sdf std::string output = @@ -551,7 +602,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames attached_to a nested model. // This is a valid file. { - std::string path = pathBase +"/model_frame_attached_to_nested_model.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_attached_to_nested_model.sdf"); // Check model_frame_attached_to_nested_model.sdf std::string output = @@ -561,7 +614,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames with invalid attached_to attributes. { - std::string path = pathBase +"/model_frame_invalid_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_invalid_attached_to.sdf"); // Check model_frame_invalid_attached_to.sdf std::string output = @@ -579,7 +634,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a cycle in its FrameAttachedTo graph { - std::string path = pathBase +"/model_frame_invalid_attached_to_cycle.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_invalid_attached_to_cycle.sdf"); // Check model_frame_invalid_attached_to_cycle.sdf std::string output = @@ -597,7 +654,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames using the attached_to attribute. // This is a valid file. { - std::string path = pathBase +"/world_frame_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_frame_attached_to.sdf"); // Check world_frame_attached_to.sdf std::string output = @@ -607,7 +666,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with world frames with invalid attached_to attributes. { - std::string path = pathBase +"/world_frame_invalid_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_frame_invalid_attached_to.sdf"); // Check world_frame_invalid_attached_to.sdf std::string output = @@ -627,7 +688,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with links using the relative_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_link_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_link_relative_to.sdf"); // Check model_link_relative_to.sdf std::string output = @@ -637,7 +700,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model links with invalid relative_to attributes. { - std::string path = pathBase +"/model_invalid_link_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_link_relative_to.sdf"); // Check model_invalid_link_relative_to.sdf std::string output = @@ -656,7 +721,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with nested_models using the relative_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_nested_model_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_nested_model_relative_to.sdf"); // Check model_nested_model_relative_to.sdf std::string output = @@ -668,7 +735,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // parent or child frames. // This is a valid file. { - std::string path = pathBase +"/joint_nested_parent_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "joint_nested_parent_child.sdf"); // Check model_nested_model_relative_to.sdf std::string output = @@ -679,7 +748,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with joints using the relative_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_joint_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_joint_relative_to.sdf"); // Check model_joint_relative_to.sdf std::string output = @@ -689,7 +760,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model joints with invalid relative_to attributes. { - std::string path = pathBase +"/model_invalid_joint_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_joint_relative_to.sdf"); // Check model_invalid_joint_relative_to.sdf std::string output = @@ -708,7 +781,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames using the relative_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_frame_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_relative_to.sdf"); // Check model_frame_relative_to.sdf std::string output = @@ -719,7 +794,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames relative_to joints. // This is a valid file. { - std::string path = pathBase +"/model_frame_relative_to_joint.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_relative_to_joint.sdf"); // Check model_frame_relative_to_joint.sdf std::string output = @@ -729,7 +806,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames with invalid relative_to attributes. { - std::string path = pathBase +"/model_invalid_frame_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_frame_relative_to.sdf"); // Check model_invalid_frame_relative_to.sdf std::string output = @@ -747,7 +826,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a cycle in its PoseRelativeTo graph { - std::string path = pathBase +"/model_invalid_frame_relative_to_cycle.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_frame_relative_to_cycle.sdf"); // Check model_invalid_frame_relative_to_cycle.sdf std::string output = @@ -765,7 +846,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames using the attached_to attribute. // This is a valid file. { - std::string path = pathBase +"/world_frame_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_frame_relative_to.sdf"); // Check world_frame_relative_to.sdf std::string output = @@ -775,7 +858,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with world frames with invalid relative_to attributes. { - std::string path = pathBase +"/world_frame_invalid_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_frame_invalid_relative_to.sdf"); // Check world_frame_invalid_relative_to.sdf std::string output = @@ -804,7 +889,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with an invalid frame specified as the placement_frame // attribute. { - std::string path = pathBase + "/model_invalid_placement_frame.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_placement_frame.sdf"); // Check model_invalid_placement_frame.sdf std::string output = @@ -817,7 +904,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with an valid nested model cross references { - std::string path = pathBase + "/nested_model_cross_references.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_model_cross_references.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -826,7 +915,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF model file with an invalid usage of __root__ { - std::string path = pathBase + "/model_invalid_root_reference.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_root_reference.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -845,8 +936,11 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { // Set SDF_PATH so that included models can be found gz::utils::setenv( - "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); - std::string path = pathBase + "/world_invalid_root_reference.sdf"; + "SDF_PATH", sdf::testing::TestFile("integration", "model")); + + const auto path = + sdf::testing::TestFile("sdf", + "world_invalid_root_reference.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -893,7 +987,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } // Check an SDF world file with an valid usage of __root__ { - std::string path = pathBase + "/world_valid_root_reference.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_valid_root_reference.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -901,7 +997,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } // Check an SDF with an invalid relative frame at the top level model { - std::string path = pathBase + "/model_invalid_top_level_frame.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_top_level_frame.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -915,11 +1013,10 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ///////////////////////////////////////////////// TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - { - std::string path = pathBase +"/shapes.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "shapes.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -927,7 +1024,9 @@ TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } { - std::string path = pathBase +"/shapes_world.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "shapes_world.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -938,12 +1037,11 @@ TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ///////////////////////////////////////////////// TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/integration/model/box"; - // Check a good SDF file by passing the absolute path { - std::string path = pathBase +"/model.sdf"; + const auto path = + sdf::testing::TestFile("integration", + "model", "box", "model.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -952,7 +1050,9 @@ TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a good SDF file from the same folder by passing a relative path { - std::string path = "model.sdf"; + const auto pathBase = sdf::testing::TestFile("integration", + "model", "box"); + const auto path = "model.sdf"; std::string output = custom_exec_str("cd " + pathBase + " && " + @@ -976,12 +1076,10 @@ TEST(describe, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ///////////////////////////////////////////////// TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - // Check a good SDF file { - std::string path = pathBase +"/box_plane_low_friction_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_plane_low_friction_test.world"); sdf::SDFPtr sdf(new sdf::SDF()); EXPECT_TRUE(sdf::init(sdf)); EXPECT_TRUE(sdf::readFile(path, sdf)); @@ -994,7 +1092,8 @@ TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a bad SDF file { - std::string path = pathBase +"/box_bad_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_bad_test.world"); // Check box_bad_test.world std::string output = @@ -1356,7 +1455,7 @@ TEST(print_includes_rotations_in_quaternions, { // Set SDF_PATH so that included models can be found gz::utils::setenv( - "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); + "SDF_PATH", sdf::testing::TestFile("integration", "model")); const auto path = sdf::testing::TestFile( "sdf", "includes_rotations_in_quaternions.sdf"); @@ -1634,11 +1733,9 @@ TEST(print_snap_to_degrees_tolerance_too_high, ///////////////////////////////////////////////// TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) { - const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; - // world pose relative_to graph const std::string path = - pathBase + "/world_relative_to_nested_reference.sdf"; + sdf::testing::TestFile("sdf", "world_relative_to_nested_reference.sdf"); const std::string output = custom_exec_str(GzCommand() + " sdf -g pose " + path + SdfVersion()); @@ -1685,8 +1782,9 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) ///////////////////////////////////////////////// TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) { - const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; - const std::string path = pathBase + "/model_relative_to_nested_reference.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_relative_to_nested_reference.sdf"); + const std::string output = custom_exec_str(GzCommand() + " sdf -g pose " + path + SdfVersion()); @@ -1761,8 +1859,8 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) ///////////////////////////////////////////////// TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) { - const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; - const std::string path = pathBase + "/world_nested_frame_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "world_nested_frame_attached_to.sdf"); const std::string output = custom_exec_str(GzCommand() + " sdf -g frame " + path + SdfVersion()); @@ -1807,8 +1905,9 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) ///////////////////////////////////////////////// TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) { - const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; - const std::string path = pathBase + "/model_nested_frame_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_nested_frame_attached_to.sdf"); + const std::string output = custom_exec_str(GzCommand() + " sdf -g frame " + path + SdfVersion()); @@ -1858,9 +1957,6 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) ///////////////////////////////////////////////// TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - std::string expectedOutput = "Inertial statistics for model: test_model\n" "---\n" @@ -1879,7 +1975,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a good SDF file by passing the absolute path { - std::string path = pathBase +"/inertial_stats.sdf"; + const auto path = sdf::testing::TestFile("sdf", "inertial_stats.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf --inertial-stats " + @@ -1890,6 +1986,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a good SDF file from the same folder by passing a relative path { std::string path = "inertial_stats_auto.sdf"; + const auto pathBase = sdf::testing::TestFile("sdf"); std::string output = custom_exec_str("cd " + pathBase + " && " + @@ -1920,7 +2017,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an invalid SDF file by passing the absolute path { - std::string path = pathBase +"/inertial_invalid.sdf"; + const auto path = sdf::testing::TestFile("sdf", "inertial_invalid.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf --inertial-stats " + @@ -1932,7 +2029,8 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) "Error: Expected a model file but received a world file.\n"; // Check a valid world file. { - std::string path = pathBase +"/box_plane_low_friction_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_plane_low_friction_test.world"); std::string output = custom_exec_str(GzCommand() + " sdf --inertial-stats " + @@ -1951,9 +2049,8 @@ TEST(HelpVsCompletionFlags, SDF) std::string helpOutput = custom_exec_str(GzCommand() + " sdf --help"); // Call the output function in the bash completion script - std::string scriptPath = PROJECT_SOURCE_PATH; - scriptPath = sdf::filesystem::append(scriptPath, "src", "cmd", - "sdf.bash_completion.sh"); + const auto scriptPath = + sdf::testing::SourceFile("src", "cmd", "sdf.bash_completion.sh"); // Equivalent to: // sh -c "bash -c \". /path/to/sdf.bash_completion.sh; _gz_sdf_flags\"" diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index 7eb54d5e4..f8a614e34 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -23,11 +23,12 @@ #include "sdf/Element.hh" #include "sdf/Console.hh" #include "sdf/Filesystem.hh" -#include "test_config.hh" -#include "test_utils.hh" #include +#include "test_config.hh" +#include "test_utils.hh" + ///////////////////////////////////////////////// TEST(Parser, initStringTrim) { @@ -418,10 +419,18 @@ TEST(Parser, IncludesErrors) const auto path = sdf::testing::TestFile("sdf", "includes_missing_model.sdf"); + sdf::setFindCallback([&](const std::string &/*_file*/) + { + return ""; + }); + sdf::SDFPtr sdf(new sdf::SDF()); sdf::init(sdf); sdf::readFile(path, sdf); + + std::cout << buffer.str() << std::endl; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Error Code " + std::to_string( @@ -937,7 +946,14 @@ int main(int argc, char **argv) { // temporarily set HOME std::string homeDir; - sdf::testing::TestSetHomePath(homeDir); + sdf::testing::TestTmpPath(homeDir); + +#ifdef _WIN32 + gz::utils::setenv("HOMEPATH", homeDir); +#else + gz::utils::setenv("HOME", homeDir); +#endif + sdf::Console::Clear(); ::testing::InitGoogleTest(&argc, argv); diff --git a/test/BUILD.bazel b/test/BUILD.bazel new file mode 100644 index 000000000..bb1baa410 --- /dev/null +++ b/test/BUILD.bazel @@ -0,0 +1,72 @@ +load( + "@gz//bazel/skylark:build_defs.bzl", + "GZ_FEATURES", + "GZ_ROOT", + "GZ_VISIBILITY", + "cmake_configure_file", +) + +package( + default_visibility = GZ_VISIBILITY, + features = GZ_FEATURES, +) + +licenses(["notice"]) + +cmake_configure_file( + name = "config", + src = "test_config.hh.in", + out = "test_config.hh", + cmakelists = ["CMakeLists.txt"], + defines = [], +) + +cc_library( + name = "test_utils", + hdrs = [ + "test_config.hh", + "test_utils.hh", + ], + includes = ["."], + deps = [ + GZ_ROOT + "utils:utils", + GZ_ROOT + "sdformat:sdformat", + ], +) + +integration_test_sources = glob( + ["integration/*.cc"], + exclude = [ + "integration/schema_test.cc", + "integration/element_memory_leak.cc", + ], +) + +[cc_test( + name = src.replace("/", "_").replace(".cc", "").replace("integration_", "INTEGRATION_"), + srcs = [ + src, + "integration/toml_parser.hh", + ], + data = [ + GZ_ROOT + "sdformat:sdf", + "integration", + "sdf", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "sdformat", + }, + includes = ["integration"], + deps = [ + GZ_ROOT + "sdformat:sdformat", + ":test_utils", + "@gtest", + "@gtest//:gtest_main", + ], +) for src in integration_test_sources] + +exports_files([ + "sdf", + "integration", +]) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 9bad38ea4..b7d846e95 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -74,11 +74,22 @@ else() gz_build_warning("xmllint not found. schema_test won't be run") endif() -gz_build_tests(TYPE ${TEST_TYPE} SOURCES ${tests} INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test) +gz_build_tests(TYPE ${TEST_TYPE} + SOURCES ${tests} + INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test +) -if (EXISTS ${XMLLINT_EXE}) - # Need to run schema target (build .xsd files) before running schema_test - add_dependencies(${TEST_TYPE}_schema_test schema) + +if (TARGET ${TEST_TYPE}_schema_test) + target_compile_definitions(${TEST_TYPE}_schema_test + PRIVATE + -DSDF_ROOT_SCHEMA="${PROJECT_BINARY_DIR}/sdf/${SDF_PROTOCOL_VERSION}/root.xsd" + ) + + if (EXISTS ${XMLLINT_EXE}) + # Need to run schema target (build .xsd files) before running schema_test + add_dependencies(${TEST_TYPE}_schema_test schema) + endif() endif() # Test symbols having the right name on linux only diff --git a/test/integration/converter.cc b/test/integration/converter.cc index ec062d5ad..1509de722 100644 --- a/test/integration/converter.cc +++ b/test/integration/converter.cc @@ -156,7 +156,7 @@ void ParserStringConverter(const std::string &_version) TEST(ConverterIntegration, UnflattenConversion) { const std::string filename = - sdf::testing::SourceFile("test", "sdf", + sdf::testing::TestFile("sdf", "flattened_test_nested_model_with_frames.sdf"); sdf::SDFPtr sdf(new sdf::SDF()); diff --git a/test/integration/default_elements.cc b/test/integration/default_elements.cc index efad8d8ad..f07cee0c8 100644 --- a/test/integration/default_elements.cc +++ b/test/integration/default_elements.cc @@ -26,14 +26,14 @@ #include "sdf/Root.hh" #include "sdf/World.hh" #include "sdf/Filesystem.hh" + #include "test_config.hh" ////////////////////////////////////////////////// TEST(ExplicitlySetInFile, EmptyRoadSphCoords) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty_road_sph_coords.sdf"); + const auto testFile = + sdf::testing::TestFile("sdf", "empty_road_sph_coords.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -109,9 +109,8 @@ TEST(ExplicitlySetInFile, EmptyRoadSphCoords) ////////////////////////////////////////////////// TEST(ExplicitlySetInFile, EmptyAxis) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty_axis.sdf"); + const auto testFile = + sdf::testing::TestFile("sdf", "empty_axis.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -157,9 +156,8 @@ TEST(ExplicitlySetInFile, EmptyAxis) ////////////////////////////////////////////////// TEST(ExplicitlySetInFile, ToString) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty_road_sph_coords.sdf"); + const auto testFile = + sdf::testing::TestFile("sdf", "empty_road_sph_coords.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 9e0438273..1ed67cd3a 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -1299,8 +1299,7 @@ TEST(DOMFrame, WorldIncludeModel) TEST(Frame, IncludeFrameWithSubmodel) { const std::string MODEL_PATH = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "model", "box_with_submodel"); + sdf::testing::TestFile("integration", "model", "box_with_submodel"); std::ostringstream stream; std::string version = SDF_VERSION; diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index ec9181c4e..0c63cbc43 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -623,7 +623,7 @@ TEST(NestedModel, NestedModelWithFramesDirectComparison) const std::string expectedSdfPath = sdf::testing::TestFile( "integration", "nested_model_with_frames_expected.sdf"); - std::fstream fs; + std::ifstream fs; fs.open(expectedSdfPath); EXPECT_TRUE(fs.is_open()); std::stringstream expected; @@ -779,7 +779,7 @@ TEST(NestedModel, TwoLevelNestedModelWithFramesDirectComparison) const std::string expectedSdfPath = sdf::testing::TestFile( "integration", "two_level_nested_model_with_frames_expected.sdf"); - std::fstream fs; + std::ifstream fs; fs.open(expectedSdfPath); EXPECT_TRUE(fs.is_open()); std::stringstream expected; diff --git a/test/integration/nested_multiple_elements_error.cc b/test/integration/nested_multiple_elements_error.cc index 46c0e975a..7831d2271 100644 --- a/test/integration/nested_multiple_elements_error.cc +++ b/test/integration/nested_multiple_elements_error.cc @@ -26,17 +26,17 @@ #include "sdf/Model.hh" #include "sdf/Root.hh" #include "sdf/World.hh" + #include "test_config.hh" const auto g_testPath = sdf::testing::TestFile(); -const auto g_modelsPath = - sdf::filesystem::append(g_testPath, "integration", "model"); -const auto g_sdfPath = sdf::filesystem::append(g_testPath, "sdf"); +const auto g_modelsPath = sdf::testing::TestFile("integration", "model"); +const auto g_sdfPath = sdf::testing::TestFile("sdf"); ///////////////////////////////////////////////// std::string findFileCb(const std::string &_input) { - return sdf::filesystem::append(g_testPath, "integration", "model", _input); + return sdf::filesystem::append(g_modelsPath, _input); } ////////////////////////////////////////////////// diff --git a/test/integration/parser_config.cc b/test/integration/parser_config.cc index 577bbd01a..31a3e2a64 100644 --- a/test/integration/parser_config.cc +++ b/test/integration/parser_config.cc @@ -50,10 +50,10 @@ TEST(ParserConfig, GlobalConfig) ASSERT_TRUE(sdf::ParserConfig::GlobalConfig().FindFileCallback()); EXPECT_EQ("test/dir2", - sdf::ParserConfig::GlobalConfig().FindFileCallback()("empty")); + sdf::ParserConfig::GlobalConfig().FindFileCallback()("should_not_exist")); // sdf::findFile requires explicitly enabling callbacks - EXPECT_EQ("test/dir2", sdf::findFile("empty", false, true)); - EXPECT_EQ("test/dir2", sdf::findFile("empty", true, true)); + EXPECT_EQ("test/dir2", sdf::findFile("should_not_exist", false, true)); + EXPECT_EQ("test/dir2", sdf::findFile("should_not_exist", true, true)); } ///////////////////////////////////////////////// @@ -81,9 +81,10 @@ TEST(ParserConfig, NonGlobalConfig) EXPECT_EQ(it->second.front(), testDir); ASSERT_TRUE(config.FindFileCallback()); - EXPECT_EQ("test/dir2", config.FindFileCallback()("empty")); - EXPECT_EQ("test/dir2", sdf::findFile("empty", false, true, config)); - EXPECT_EQ("test/dir2", sdf::findFile("empty", true, true, config)); + EXPECT_EQ("test/dir2", config.FindFileCallback()("should_not_exist")); + EXPECT_EQ("test/dir2", + sdf::findFile("should_not_exist", false, true, config)); + EXPECT_EQ("test/dir2", sdf::findFile("should_not_exist", true, true, config)); EXPECT_TRUE(sdf::ParserConfig::GlobalConfig().URIPathMap().empty()); EXPECT_FALSE(sdf::ParserConfig::GlobalConfig().FindFileCallback()); diff --git a/test/integration/schema_test.cc b/test/integration/schema_test.cc index 1fa5e5a8f..99a309fdb 100644 --- a/test/integration/schema_test.cc +++ b/test/integration/schema_test.cc @@ -31,9 +31,7 @@ class SDFSchemaGenerator : public testing::Test public: void runXMLlint(const std::string & model) { - const std::string sdfRootSchema = sdf::filesystem::append( - PROJECT_BINARY_DIR, "sdf", SDF_PROTOCOL_VERSION, "root.xsd"); - + const auto sdfRootSchema = sdf::filesystem::append(SDF_ROOT_SCHEMA); std::string xmllintCmd = "xmllint --noout --schema " + sdfRootSchema + " " + model; std::cout << "CMD[" << xmllintCmd << "]\n"; diff --git a/test/integration/toml_parser.hh b/test/integration/toml_parser.hh index ee3f68ba1..528563cab 100644 --- a/test/integration/toml_parser.hh +++ b/test/integration/toml_parser.hh @@ -135,8 +135,8 @@ struct Document: public Value Document parseToml(const std::string &_filePath, sdf::Errors &_errors) { - std::fstream fs; - fs.open(_filePath, std::fstream::in); + std::ifstream fs; + fs.open(_filePath); if (!fs.good()) { _errors.emplace_back(sdf::ErrorCode::FILE_READ, diff --git a/test/sdf/inertial_stats_auto.sdf b/test/sdf/inertial_stats_auto.sdf index 010313b5d..239b9a340 100644 --- a/test/sdf/inertial_stats_auto.sdf +++ b/test/sdf/inertial_stats_auto.sdf @@ -25,9 +25,10 @@ 5 0 0 0 0 0 - - + 6 + + 1 1 1 diff --git a/test/test_config.hh.in b/test/test_config.hh.in index a84d1cddd..303eab375 100644 --- a/test/test_config.hh.in +++ b/test/test_config.hh.in @@ -18,18 +18,25 @@ #ifndef SDF_TEST_CONFIG_HH_ #define SDF_TEST_CONFIG_HH_ -#define GZ_CONFIG_PATH "@CMAKE_BINARY_DIR@/test/conf" -#define GZ_PATH "@GZ_PROGRAM@" -#define GZ_TEST_LIBRARY_PATH "${PROJECT_BINARY_DIR}/src:"\ - "@GZ-MSGS_LIBRARY_DIRS@:" -#define PROJECT_SOURCE_PATH "${PROJECT_SOURCE_DIR}" -#define PROJECT_BINARY_DIR "${CMAKE_BINARY_DIR}" -#define SDF_PROTOCOL_VERSION "${SDF_PROTOCOL_VERSION}" -#define SDF_TMP_DIR "tmp-sdf/" - #include #include + +#cmakedefine PROJECT_SOURCE_DIR "@PROJECT_SOURCE_DIR@" +#cmakedefine PROJECT_BINARY_DIR "@PROJECT_BINARY_DIR@" + +#ifdef PROJECT_SOURCE_DIR +constexpr const char* kProjectSourceDir = PROJECT_SOURCE_DIR; +#else +constexpr const char* kProjectSourceDir = ""; +#endif + +#ifdef PROJECT_BINARY_DIR +constexpr const char* kProjectBinaryDir = PROJECT_BINARY_DIR; +#else +constexpr const char* kProjectBinaryDir= ""; +#endif + namespace sdf { namespace testing @@ -41,17 +48,18 @@ namespace sdf /// \return True if directory is set correctly, false otherwise bool ProjectSourcePath(std::string &_sourceDir) { + std::string bazel_path; // Bazel builds set TEST_SRCDIR - if(gz::utils::env("TEST_SRCDIR", _sourceDir)) + if(gz::utils::env("TEST_SRCDIR", _sourceDir) && + gz::utils::env("GZ_BAZEL_PATH", bazel_path)) { _sourceDir = sdf::filesystem::append( - _sourceDir, "__main__", "sdformat"); + _sourceDir, "gz", bazel_path); return true; } else { - // CMake builds set PROJECT_SOURCE_PATH - _sourceDir = PROJECT_SOURCE_PATH; + _sourceDir = kProjectSourceDir; return true; } @@ -71,7 +79,8 @@ namespace sdf } else { - _tmpDir = sdf::filesystem::append(PROJECT_BINARY_DIR, SDF_TMP_DIR); + _tmpDir = sdf::filesystem::append(kProjectBinaryDir, "sdf-tmp"); + sdf::filesystem::create_directory(_tmpDir); return true; } } @@ -93,7 +102,7 @@ namespace sdf } else { - _homeDir = PROJECT_BINARY_DIR; + _homeDir = kProjectBinaryDir; // Set both for linux and windows return gz::utils::setenv("HOME", _homeDir) && gz::utils::setenv("HOMEPATH", _homeDir);