diff --git a/.github/ci/after_make_test.sh b/.github/ci/after_make_test.sh new file mode 100644 index 000000000..b6bdbd725 --- /dev/null +++ b/.github/ci/after_make_test.sh @@ -0,0 +1,16 @@ +#!/bin/sh -l + +set -x + +# Install +make install + +# Compile examples +curdir=$PWD +cd ../examples +mkdir build; +cd build; +cmake ..; +make; +./simple ../simple.sdf; +cd $curdir diff --git a/.github/ci/between_cmake_make.sh b/.github/ci/between_cmake_make.sh new file mode 100644 index 000000000..f52a3c693 --- /dev/null +++ b/.github/ci/between_cmake_make.sh @@ -0,0 +1,5 @@ +#!/bin/sh -l + +set -x + +make sdf_descriptions diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt new file mode 100644 index 000000000..0d407603d --- /dev/null +++ b/.github/ci/packages.apt @@ -0,0 +1,9 @@ +libignition-cmake2-dev +libignition-math6-dev +libignition-tools-dev +libignition-utils-dev +libtinyxml2-dev +liburdfdom-dev +libxml2-utils +python-psutil +ruby-dev diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 000000000..2f752261f --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,26 @@ +name: Ubuntu + +on: [push, pull_request] + +jobs: + bionic-ci: + runs-on: ubuntu-latest + name: Ubuntu Bionic CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@master + with: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + focal-ci: + runs-on: ubuntu-latest + name: Ubuntu Focal CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@focal + diff --git a/.github/workflows/linux-ubuntu-bionic.yml b/.github/workflows/linux-ubuntu-bionic.yml deleted file mode 100644 index 236f50289..000000000 --- a/.github/workflows/linux-ubuntu-bionic.yml +++ /dev/null @@ -1,72 +0,0 @@ -name: Ubuntu Bionic / Linux - -on: [push, pull_request] - -jobs: - build: - - runs-on: ubuntu-18.04 - - steps: - - uses: actions/checkout@v2 - - name: Install base dependencies - run: | - sudo apt update; - sudo apt -y install wget lsb-release gnupg; - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" > /etc/apt/sources.list.d/gazebo-stable.list'; - sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743; - sudo apt-get update; - sudo apt -y install cmake build-essential curl g++-8 git mercurial libtinyxml-dev libxml2-utils ruby-dev python-psutil cppcheck; - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8; - # workaround for https://github.com/rubygems/rubygems/issues/3068 - # suggested in https://github.com/rubygems/rubygems/issues/3068#issuecomment-574775885 - sudo gem update --system 3.0.6; - - name: Install lcov - run: | - git clone https://github.com/linux-test-project/lcov.git -b v1.14; - cd lcov; - sudo make install; - cd ..; - - name: Static checking before building - fail fast - run: sh tools/code_check.sh - - name: Install ignition dependencies - run: | - sudo apt -y install \ - libignition-cmake2-dev \ - libignition-math6-dev \ - libignition-tools-dev \ - liburdfdom-dev; - - name: cmake - run: | - mkdir build; - cd build; - cmake .. -DCMAKE_BUILD_TYPE=coverage; - - name: make sdf_descriptions - working-directory: build - run: make sdf_descriptions - - name: make - working-directory: build - run: make - - name: make test - env: - CTEST_OUTPUT_ON_FAILURE: 1 - working-directory: build - run: make test - - name: make coverage - working-directory: build - run: make coverage VERBOSE=1 - - name: Upload to codecov - working-directory: build - # disable gcov output with `-X gcovout -X gcov` - run: bash <(curl -s https://codecov.io/bash) -X gcovout -X gcov - - name: make install - working-directory: build - run: sudo make install - - name: Compile example code - working-directory: examples - run: | - mkdir build; - cd build; - cmake ..; - make; - ./simple ../simple.sdf; diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml new file mode 100644 index 000000000..77e6c2066 --- /dev/null +++ b/.github/workflows/macos.yml @@ -0,0 +1,45 @@ +name: macOS latest + +on: [push, pull_request] + +jobs: + build: + + env: + PACKAGE: sdformat11 + runs-on: macos-latest + steps: + - uses: actions/checkout@v2 + - name: Set up Homebrew + id: set-up-homebrew + uses: Homebrew/actions/setup-homebrew@master + - run: brew config + + - name: Install base dependencies + run: | + brew tap osrf/simulation; + brew install --only-dependencies ${PACKAGE}; + + - run: mkdir build + - name: cmake + working-directory: build + run: cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local/Cellar/${PACKAGE}/HEAD + - run: make + working-directory: build + - run: make test + working-directory: build + env: + CTEST_OUTPUT_ON_FAILURE: 1 + - name: make install + working-directory: build + run: | + make install; + brew link ${PACKAGE}; + - name: Compile example code + working-directory: examples + run: | + mkdir build; + cd build; + cmake ..; + make; + ./simple ../simple.sdf; diff --git a/.github/workflows/pr-collection-labeler.yml b/.github/workflows/pr-collection-labeler.yml new file mode 100644 index 000000000..7d7b4e179 --- /dev/null +++ b/.github/workflows/pr-collection-labeler.yml @@ -0,0 +1,13 @@ +name: PR Collection Labeler + +on: pull_request_target + +jobs: + pr_collection_labeler: + runs-on: ubuntu-latest + steps: + - name: Add collection labels + if: github.event.action == 'opened' + uses: ignition-tooling/pr-collection-labeler@v1 + with: + github-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/CMakeLists.txt b/CMakeLists.txt index 60987b758..b80c3fe21 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,8 @@ set(sdf_import_target_name ${PROJECT_EXPORT_NAME}::${sdf_target}) set(sdf_target_output_filename "${sdf_target}-targets.cmake") +OPTION(SDFORMAT_DISABLE_CONSOLE_LOGFILE "Disable the sdformat console logfile" OFF) + if (USE_FULL_RPATH) # use, i.e. don't skip the full RPATH for the build tree set(CMAKE_SKIP_BUILD_RPATH FALSE) @@ -109,24 +111,8 @@ set (build_warnings "" CACHE INTERNAL "build warnings" FORCE) set (sdf_cmake_dir ${PROJECT_SOURCE_DIR}/cmake CACHE PATH "Location of CMake scripts") -include (${sdf_cmake_dir}/SDFUtils.cmake) - -if (UNIX) - option (USE_EXTERNAL_TINYXML "Use external TinyXML" ON) -elseif(WIN32) - option (USE_EXTERNAL_TINYXML "Use external TinyXML" OFF) -else() - message (STATUS "Unknown platform") - BUILD_ERROR("Unknown platform") -endif() - message (STATUS "\n\n====== Finding 3rd Party Packages ======") - # Use of tinyxml. System installation on UNIX. Internal copy on WIN -if (USE_EXTERNAL_TINYXML) - message (STATUS "Using system tinyxml") -else() - message (STATUS "Using internal tinyxml code") -endif() +include (${sdf_cmake_dir}/SDFUtils.cmake) include (${sdf_cmake_dir}/SearchForStuff.cmake) message (STATUS "----------------------------------------\n") @@ -162,6 +148,10 @@ else() endif() endif() +#============================================================================ +# Ask whether we should make a shared or static library. +option(BUILD_SHARED_LIBS "Set this to true to generate shared libraries (recommended), or false for static libraries" ON) + ##################################### # Handle CFlags unset (CMAKE_C_FLAGS_ALL CACHE) @@ -269,7 +259,15 @@ else (build_errors) link_directories(${PROJECT_BINARY_DIR}/src) - add_subdirectory(test) + if (NOT DEFINED BUILD_TESTING OR BUILD_TESTING) + set(BUILD_SDF_TEST TRUE) + else() + set(BUILD_SDF_TEST FALSE) + endif() + + if (BUILD_SDF_TEST) + add_subdirectory(test) + endif() add_subdirectory(src) add_subdirectory(include/sdf) add_subdirectory(sdf) @@ -296,6 +294,7 @@ else (build_errors) set(sdf_version_output "cmake/${sdf_target}-config-version.cmake") set(sdf_config_install_dir "${LIB_INSTALL_DIR}/cmake/${PROJECT_NAME_LOWER}/") + include(CMakePackageConfigHelpers) #-------------------------------------- # Configure and install the config file configure_package_config_file( diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 000000000..820e43fdb --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,76 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +In the interest of fostering an open and welcoming environment, we as +contributors and maintainers pledge to making participation in our project +and our community a harassment-free experience for everyone, regardless of +age, body size, disability, ethnicity, sex characteristics, gender identity +and expression, level of experience, education, socio-economic status, +nationality, personal appearance, race, religion, or sexual identity and +orientation. + +## Our Standards + +Examples of behavior that contributes to creating a positive environment +include: + +* Using welcoming and inclusive language +* Being respectful of differing viewpoints and experiences +* Gracefully accepting constructive criticism +* Focusing on what is best for the community +* Showing empathy towards other community members + +Examples of unacceptable behavior by participants include: + +* The use of sexualized language or imagery and unwelcome sexual attention or + advances +* Trolling, insulting/derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or electronic + address, without explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Our Responsibilities + +Project maintainers are responsible for clarifying the standards of acceptable +behavior and are expected to take appropriate and fair corrective action in +response to any instances of unacceptable behavior. + +Project maintainers have the right and responsibility to remove, edit, or +reject comments, commits, code, wiki edits, issues, and other contributions +that are not aligned to this Code of Conduct, or to ban temporarily or +permanently any contributor for other behaviors that they deem inappropriate, +threatening, offensive, or harmful. + +## Scope + +This Code of Conduct applies both within project spaces and in public spaces +when an individual is representing the project or its community. Examples of +representing a project or community include using an official project e-mail +address, posting via an official social media account, or acting as an appointed +representative at an online or offline event. Representation of a project may be +further defined and clarified by project maintainers. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported by contacting the project team at [https://ignitionrobotics.org/support](https://ignitionrobotics.org/support). All +complaints will be reviewed and investigated and will result in a response that +is deemed necessary and appropriate to the circumstances. The project team is +obligated to maintain confidentiality with regard to the reporter of an incident. +Further details of specific enforcement policies may be posted separately. + +Project maintainers who do not follow or enforce the Code of Conduct in good +faith may face temporary or permanent repercussions as determined by other +members of the project's leadership. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, available at https://www.contributor-covenant.org/version/1/4/code-of-conduct.html + +[homepage]: https://www.contributor-covenant.org + +For answers to common questions about this code of conduct, see +https://www.contributor-covenant.org/faq diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 000000000..0a9fb33a7 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +See the +[Ignition Robotics contributing guide](https://ignitionrobotics.org/docs/all/contributing) +for how to contribute to SDFormat. diff --git a/Changelog.md b/Changelog.md index 9052ba625..d1b799bb9 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,51 @@ ### libsdformat 11.0.0 (202X-XX-XX) +1. Allow files paths for include URIs + * [Pull request 448](https://github.com/osrf/sdformat/pull/448) + +1. SDFormat 1.8: Add ellipsoid geometry type. + * [Pull request 434](https://github.com/osrf/sdformat/pull/434) + +1. SDFormat 1.8: Add capsule geometry type. + * [Pull request 389](https://github.com/osrf/sdformat/pull/389) + +1. Allow only one of actor/light/model for tags. + * [Pull request 433](https://github.com/osrf/sdformat/pull/433) + * [Pull request 444](https://github.com/osrf/sdformat/pull/444) + +1. Add support for building static library. + * [Pull request 394](https://github.com/osrf/sdformat/pull/394) + +1. Add force torque sensor. + * [Pull request 393](https://github.com/osrf/sdformat/pull/393) + +1. Simplify data embedding. + * [Pull request 270](https://github.com/osrf/sdformat/pull/270) + +1. Properly handle the requirement of C++17 at the CMake exported target level. + * [Pull request 251](https://github.com/osrf/sdformat/pull/251) + +1. Update documentation for Cylinder length. + * [Pull request 318](https://github.com/osrf/sdformat/pull/318) + +1. Update CI. + * [Pull request 452](https://github.com/osrf/sdformat/pull/452) + * [Pull request 255](https://github.com/osrf/sdformat/pull/255) + +1. Update README, Changelog, Contributing Guide, and Code of Conduct. + * [Pull request 431](https://github.com/osrf/sdformat/pull/431) + * [Pull request 275](https://github.com/osrf/sdformat/pull/275) + * [Pull request 250](https://github.com/osrf/sdformat/pull/250) + * [Pull request 243](https://github.com/osrf/sdformat/pull/243) + +1. Implement SDFormat 1.8 Model Composition. + * [Pull request 426](https://github.com/osrf/sdformat/pull/426) + * [Pull request 381](https://github.com/osrf/sdformat/pull/381) + * [Pull request 355](https://github.com/osrf/sdformat/pull/355) + * [Pull request 324](https://github.com/osrf/sdformat/pull/324) + * [Pull request 304](https://github.com/osrf/sdformat/pull/304) + 1. Initial version of SDFormat 1.8 specification. * [BitBucket pull request 682](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/682) @@ -14,7 +59,78 @@ ### libsdformat 10.X.X (202X-XX-XX) -### libsdformat 10.0.0 (202X-XX-XX) +### libsdformat 10.2.0 (2021-01-12) + +1. Disable ign test on Windows + + [Pull request 456](https://github.com/osrf/sdformat/pull/456) + +1. Add Heightmap class + + [Pull request 388](https://github.com/osrf/sdformat/pull/388) + +1. Added `render_order` to material + + [Pull request 446](https://github.com/osrf/sdformat/pull/446) + +### libsdformat 10.1.0 (2020-12-15) + +1. Fix supported shader types (`normal_map_X_space`) + * [Pull request 383](https://github.com/osrf/sdformat/pull/383) + +1. Prefix nested model names when flattening + * [Pull request 399](https://github.com/osrf/sdformat/pull/399) + +1. Move list of debian dependencies to packages.apt + * [Pull request 392](https://github.com/osrf/sdformat/pull/392) + +1. Remove custom element warning/error. + * [Pull request 402](https://github.com/osrf/sdformat/pull/402) + +1. Add Sky DOM. + * [Pull request 397](https://github.com/osrf/sdformat/pull/397) + +1. Add `` to material spec. + * [Pull request 410](https://github.com/osrf/sdformat/pull/410) + +1. Decrease far clip lower bound. + * [Pull request 437](https://github.com/osrf/sdformat/pull/437) + +1. Enable/disable tests for issue #202, add macOS workflow. + * [Pull request 414](https://github.com/osrf/sdformat/pull/414) + * [Pull request 438](https://github.com/osrf/sdformat/pull/438) + * [Issue 202](https://github.com/osrf/sdformat/issues/202) + +1. Make labeler work with PRs from forks. + * [Pull request 390](https://github.com/osrf/sdformat/pull/390) + +1. Test included model folder missing model.config + * [Pull request 422](https://github.com/osrf/sdformat/pull/422) + +1. Add lightmap to 1.7 spec and PBR material DOM + * [Pull request 429](https://github.com/osrf/sdformat/pull/429) + +### libsdformat 10.0.0 (2020-09-28) + +1. Return positive `INF` instead of `-1` in DOM API for unbounded symmetric joint limits. + * [Pull request 357](https://github.com/osrf/sdformat/pull/357) + +1. Add cmake option to disable console logfile. + * [Pull request 348](https://github.com/osrf/sdformat/pull/348) + +1. CMake fixes: include CMakePackageConfigHelpers and use modern cmake target for ignition math. + * [Pull request 358](https://github.com/osrf/sdformat/pull/358) + +1. Cmake: add tinyxml2 to Config names. + * [Pull request 360](https://github.com/osrf/sdformat/pull/360) + +1. Define `PATH_MAX` for Debian Hurd system. + * [Pull request 369](https://github.com/osrf/sdformat/pull/369) + +1. Normalize joint axis xyz vector when parsing from SDFormat. + * [Pull request 312](https://github.com/osrf/sdformat/pull/312) + +1. Migrate to using TinyXML2. + * [Pull request 264](https://github.com/osrf/sdformat/pull/264) + * [Pull request 321](https://github.com/osrf/sdformat/pull/321) + * [Pull request 359](https://github.com/osrf/sdformat/pull/359) 1. Enforce minimum/maximum values specified in SDFormat description files. * [Pull request 303](https://github.com/osrf/sdformat/pull/303) @@ -22,12 +138,15 @@ 1. Make parsing of values syntactically more strict with bad values generating an error. * [Pull request 244](https://github.com/osrf/sdformat/pull/244) -1. Don't install deprecated parser_urdf.hh header file, fix cmake warning about newline file, fix cmake warning about newlines. +1. Don't install deprecated parser\_urdf.hh header file, fix cmake warning about newline file, fix cmake warning about newlines. * [Pull request 276](https://github.com/osrf/sdformat/pull/276) 1. Remove deprecated Pose(), PoseFrame() functions from DOM objects. * [Pull request 308](https://github.com/osrf/sdformat/pull/308) +1. Remove deprecated UseParentModelFrame methods from JointAxis DOM. + * [Pull request 379](https://github.com/osrf/sdformat/pull/379) + 1. Changed the default radius of a Cylinder from 1.0 to 0.5 meters. * [BitBucket pull request 643](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/643) @@ -35,9 +154,37 @@ ### libsdformat 9.X.X (202X-XX-XX) +### libsdformat 9.3.0 (2020-09-07) + +1. Store material file path information. + + [Pull request 349](https://github.com/osrf/sdformat/pull/349) + +1. Support nested models in DOM and frame semantics. + * [Pull request 316](https://github.com/osrf/sdformat/pull/316) + + [Pull request 341](https://github.com/osrf/sdformat/pull/341) + +1. Find python3 in cmake, fix cmake warning. + * [Pull request 328](https://github.com/osrf/sdformat/pull/328) + +1. Fix Actor copy operators and increase test coverage. + * [Pull request 301](https://github.com/osrf/sdformat/pull/301) + +1. GitHub Actions CI, pull request labels. + * [Pull request 311](https://github.com/osrf/sdformat/pull/311) + * [Pull request 363](https://github.com/osrf/sdformat/pull/363) + 1. Change bitbucket links to GitHub. * [Pull request 240](https://github.com/osrf/sdformat/pull/240) +1. Param\_TEST: test parsing +Inf and -Inf. + * [Pull request 277](https://github.com/osrf/sdformat/pull/277) + +1. SearchForStuff: add logic to find urdfdom without pkg-config. + * [Pull request 245](https://github.com/osrf/sdformat/pull/245) + +1. Observe the CMake variable `BUILD_TESTING` if it is defined. + * [Pull request 269](https://github.com/osrf/sdformat/pull/269) + 1. Collision: don't load Surface without ``. * [Pull request 268](https://github.com/osrf/sdformat/pull/268) @@ -238,6 +385,21 @@ ### libsdformat 8.X.X (202X-XX-XX) +### libsdformat 8.9.0 (2020-09-04) + +1. Find python3 in cmake, fix warning + * [Pull request 328](https://github.com/osrf/sdformat/pull/328) + +1. Store material file path information + * [Pull request 349](https://github.com/osrf/sdformat/pull/349) + +1. Fix Actor copy operators and increase test coverage. + * [Pull request 301](https://github.com/osrf/sdformat/pull/301) + +1. Migration to GitHub: CI, links... + * [Pull request 239](https://github.com/osrf/sdformat/pull/239) + * [Pull request 310](https://github.com/osrf/sdformat/pull/310) + 1. Increase output precision of URDF to SDF conversion, output -0 as 0. * [BitBucket pull request 675](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/675) diff --git a/Migration.md b/Migration.md index 0029c6f84..7adbbea20 100644 --- a/Migration.md +++ b/Migration.md @@ -1,8 +1,8 @@ -# Migration Guide for SDF Protocol +# Migration Guide for SDFormat Specification This document contains information about migrating -between different versions of the SDF protocol. -The SDF protocol version number is specified in the `version` attribute -of the `sdf` element (1.4, 1.5, 1.6, etc.) +between different versions of the SDFormat specification. +The SDFormat specification version number is specified in the `version` +attribute of the `sdf` element (1.4, 1.5, 1.6, etc.) and is distinct from sdformat library version (2.3, 3.0, 4.0, etc.). @@ -12,10 +12,68 @@ forward programmatically. This document aims to contain similar information to those files but with improved human-readability.. +## SDFormat 10.x to 11.0 + +### Additions + +1. + Depend on ignition-utils1 for the ImplPtr and UniqueImplPtr. + + [Pull request 474](https://github.com/osrf/sdformat/pull/474) + +1. **sdf/Joint.hh** + + Errors ResolveChildLink(std::string&) const + + Errors ResolveParentLink(std::string&) const + +1. **sdf/Model.hh**: + + std::pair CanonicalLinkAndRelativeName() const; + +1. **sdf/Root.hh** sdf::Root elements can now only contain one of either Model, + Light or Actor since multiple items would conflict with overrides + specified in an tag. + + const sdf::Model \*Model(); + + const sdf::Light \*Light(); + + const sdf::Actor \*Actor(); + +### Modifications + +1. **sdf/Model.hh**: the following methods now accept nested names relative to + the model's scope that can begin with a sequence of nested model names + separated by `::` and may end with the name of an object of the specified + type. + + const Frame \*FrameByName(const std::string &) const + + const Joint \*JointByName(const std::string &) const + + const Link \*LinkByName(const std::string &) const + + bool FrameNameExists(const std::string &) const + + bool JointNameExists(const std::string &) const + + bool LinkNameExists(const std::string &) const + +1. **sdf/Heightmap.hh**: sampling now defaults to 1 instead of 2. + +### Deprecations + +1. **src/Root.hh**: The following methods have been deprecated in favor of the + new methods. For now the behavior is unchanged, but Root elements should + only contain one or none of Model/Light/Actor. + + const sdf::Model \*ModelByIndex(); + + uint64_t ModelCount(); + + bool ModelNameExists(const std::string &\_name) const; + + const sdf::Light \*LightByIndex(); + + uint64_t LightCount(); + + bool LightNameExists(const std::string &\_name) const; + + const sdf::Actor \*ActorByIndex(); + + uint64_t ActorCount(); + + bool ActorNameExists(const std::string &\_name) const; + ## SDFormat 9.x to 10.0 ### Modifications +1. Axis vectors specified in are normalized if their norm is + greater than 0. A vector with 0 norm generates an error + * [Pull request 312](https://github.com/osrf/sdformat/pull/312) + +1. + Depend on tinyxml2 instead of tinyxml for XML parsing. + + [Pull request 264](https://github.com/osrf/sdformat/pull/264) + 1. + Minimum/maximum values specified in SDFormat description files are now enforced + [Pull request 303](https://github.com/osrf/sdformat/pull/303) @@ -27,12 +85,16 @@ but with improved human-readability.. 1. + Removed the `parser_urdf.hh` header file and its `URDF2SDF` class + [Pull request 276](https://github.com/osrf/sdformat/pull/276) -1. + Removed the deprecated `Pose()`, `SetPose(), and `*PoseFrame()` API's in all DOM classes: +1. + Removed the deprecated `Pose()`, `SetPose()`, and `*PoseFrame()` API's in all DOM classes: + const ignition::math::Pose3d &Pose() + void SetPose(const ignition::math::Pose3d &) + const std::string &PoseFrame() + void SetPoseFrame(const std::string &) +1. + Removed deprecated functions from **sdf/JointAxis.hh**: + + bool UseParentModelFrame() + + void SetUseParentModelFrame(bool) + ### Additions 1. **sdf/Element.hh** @@ -44,6 +106,21 @@ but with improved human-readability.. + std::optional GetMaxValueAsString() const; + bool ValidateValue() const; +## SDFormat 9.0 to 9.3 + +### Additions + +1. **sdf/Model.hh** + + uint64\_t ModelCount() const + + const Model \*ModelByIndex(const uint64\_t) const + + const Model \*ModelByName(const std::string &) const + + bool ModelNameExists(const std::string &) const + +### Modifications + +1. Permit models without links if they contain a nested canonical link. + + [pull request 341](https://github.com/osrf/sdformat/pull/341) + ## SDFormat 8.x to 9.0 ### Additions @@ -189,7 +266,7 @@ but with improved human-readability.. ### Additions -1. **New SDF protocol version 1.6** +1. **New SDFormat specification version 1.6** + Details about the 1.5 to 1.6 transition are explained below in this same document @@ -199,40 +276,44 @@ but with improved human-readability.. + All boost pointers, boost::function in the public API have been replaced by their std:: equivalents (C++11 standard) -1. **`gravity` and `magnetic_field` elements are moved from `physics` to `world`** - + In physics element: gravity and `magnetic_field` tags have been moved - from Physics to World element. - + [BitBucket pull request 247](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/247) - + [BitBucket gazebo pull request 2090](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2090) - -1. **New noise for IMU** - + A new style for representing the noise properties of an `imu` was implemented - in [BitBucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) - for sdf 1.5 and the old style was declared as deprecated. - The old style has been removed from sdf 1.6 with the conversion script - updating to the new style. - + [BitBucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) - + [BitBucket pull request 243](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/243) - + [BitBucket pull request 244](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/244) - 1. **Lump:: prefix in link names** + Changed to `_fixed_joint_lump__` to avoid confusion with scoped names + [BitBucket pull request 245](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/245) -## SDF protocol 1.7 to 1.8 +## SDFormat specification 1.7 to 1.8 + +### Additions + +1. **capsule.sdf and ellipsoid.sdf** new shape types included in `//geometry` + + `capsule.sdf`: A shape consisting of a cylinder capped by hemispheres + with parameters for the `radius` and `length` of cylindrical section. + + `ellipsoid.sdf`: A convex shape with up to three radii defining its + shape in of the form (x^2/a^2 + y^2/b^2 + z^2/c^2 = 1). + * [Pull request 389](https://github.com/osrf/sdformat/pull/389) + * [Pull request 434](https://github.com/osrf/sdformat/pull/434) + +### Modifications + +1. **joint.sdf** `child` and `parent` elements accept frame names instead of only link names + * [Issue 204](https://github.com/osrf/sdformat/issues/204) + +1. **heightmap.sdf**: sampling now defaults to 1 instead of 2. ### Deprecations +1. **inerial.sdf** `//inertial/pose/@relative_to` attribute is removed + + [Pull request 480](https://github.com/osrf/sdformat/pull/480) + 1. **joint.sdf** `initial_position` element in `` and `` is deprecated -## SDF protocol 1.6 to 1.7 +## SDFormat specification 1.6 to 1.7 ### Additions 1. **frame.sdf** `//frame/@attached_to` attribute + description: Name of the link or frame to which this frame is attached. If a frame is specified, recursively following the attached\_to attributes - of the specified frames must lead to the name of a link or the world frame. + of the specified frames must lead to the name of a link, a model, or the world frame. + type: string + default: "" + required: * @@ -248,6 +329,14 @@ but with improved human-readability.. + required: 0 + [BitBucket pull request 589](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/589) +1. **material.sdf** `//material/double_sided` element + + description: Flag to indicate whether the mesh that this material is applied to + will be rendered as double sided. + + type: bool + + default: false + + required: 0 + + [pull request 418](https://github.com/osrf/sdformat/pull/418) + 1. **model.sdf** `//model/@canonical_link` attribute + description: The name of the canonical link in this model to which the model's implicit frame is attached. This implies that a model must have @@ -344,7 +433,7 @@ but with improved human-readability.. 1. **world.sdf** `//world/joint` was removed as it has never been used. + [BitBucket pull request 637](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/637) -## SDF protocol 1.5 to 1.6 +## SDFormat specification 1.5 to 1.6 ### Additions @@ -368,6 +457,16 @@ but with improved human-readability.. + description: Camera intrinsic parameters for setting a custom perspective projection matrix. + [BitBucket pull request 496](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/496) +1. **heightmap_shape.sdf** `sampling` element + + description: Samples per heightmap datum. + For rasterized heightmaps, this indicates the number of samples to take per pixel. + Using a lower value, e.g. 1, will generally improve the performance + of the heightmap but lower the heightmap quality. + + type: unsigned int + + default: 2 + + required: 0 + + [Bitbucket pull request 293](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/293) + 1. **link.sdf** `enable_wind` element + description: If true, the link is affected by the wind + type: bool @@ -411,6 +510,20 @@ but with improved human-readability.. + required: 0 + [BitBucket pull request 369](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/369) +1. **physics.sdf** `friction_model` element + + description: Name of ODE friction model to use. Valid values include: + + pyramid_model: (default) friction forces limited in two directions + in proportion to normal force. + + box_model: friction forces limited to constant in two directions. + + cone_model: friction force magnitude limited in proportion to normal force. + See [gazebo pull request 1522](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1522) + (merged in [gazebo 8c05ad64967c](https://github.com/osrf/gazebo/commit/968dccafdfbfca09c9b3326f855612076fed7e6f)) + for the implementation of this feature. + + type: string + + default: "pyramid_model" + + required: 0 + + [Bitbucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/294) + 1. **physics.sdf** `island_threads` element under `ode::solver` + description: Number of threads to use for "islands" of disconnected models. + type: int @@ -455,3 +568,21 @@ but with improved human-readability.. + default: "0 0 0" + required: 0 + [BitBucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) + +### Modifications + +1. **`gravity` and `magnetic_field` elements are moved from `physics` to `world`** + + In physics element: gravity and `magnetic_field` tags have been moved + from Physics to World element. + + [BitBucket pull request 247](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/247) + + [BitBucket gazebo pull request 2090](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2090) + +1. **New noise for IMU** + + A new style for representing the noise properties of an `imu` was implemented + in [BitBucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) + for sdf 1.5 and the old style was declared as deprecated. + The old style has been removed from sdf 1.6 with the conversion script + updating to the new style. + + [BitBucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) + + [BitBucket pull request 243](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/243) + + [BitBucket pull request 244](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/244) diff --git a/README.md b/README.md index 976578d66..5788fb081 100644 --- a/README.md +++ b/README.md @@ -35,6 +35,10 @@ TODO(eric.cousineau): Move terminology section to sdf_tutorials? ## Installation +**Note:** the `master` branch is under development for `libsdformat11` and is +currently unstable. A release branch (`sdf10`, `sdf9`, etc.) is recommended +for most users. + Standard installation can be performed in UNIX systems using the following steps: diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index b2ebf1b7e..e1dad3b7d 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -10,7 +10,7 @@ pipelines: - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' - wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - - apt-get update - - apt -y install cmake build-essential curl git libtinyxml-dev libxml2-utils ruby-dev python-psutil cppcheck + - apt -y install cmake build-essential curl git libtinyxml2-dev libxml2-utils ruby-dev python-psutil cppcheck - gcc -v - g++ -v - gcov -v diff --git a/cmake/Modules/FindTinyXML2.cmake b/cmake/Modules/FindTinyXML2.cmake new file mode 100644 index 000000000..fd3571e9b --- /dev/null +++ b/cmake/Modules/FindTinyXML2.cmake @@ -0,0 +1,42 @@ +# CMake Logic to find system TinyXML2, sourced from: +# ros2/tinyxml2_vendor +# https://github.com/ros2/tinyxml2_vendor/commit/fde8000d31d68ff555431d63af3c324afba9f117#diff-120198e331f1dd3e7806c31af0cfb425 + +# The CMake Logic here is licensed under Apache License 2.0 +# TinyXML2 itself is licensed under the zlib License + +# TinyXML2_FOUND +# TinyXML2_INCLUDE_DIRS +# TinyXML2_LIBRARIES + +# try to find the CMake config file for TinyXML2 first +find_package(TinyXML2 CONFIG NAMES tinyxml2 QUIET) +if(TinyXML2_FOUND) + message(STATUS "Found TinyXML2 via Config file: ${TinyXML2_DIR}") + if(NOT TINYXML2_LIBRARY) + # in this case, we're probably using TinyXML2 version 5.0.0 or greater + # in which case tinyxml2 is an exported target and we should use that + if(TARGET tinyxml2) + set(TINYXML2_LIBRARY tinyxml2) + elseif(TARGET tinyxml2::tinyxml2) + set(TINYXML2_LIBRARY tinyxml2::tinyxml2) + endif() + endif() +else() + find_path(TINYXML2_INCLUDE_DIR NAMES tinyxml2.h) + + find_library(TINYXML2_LIBRARY tinyxml2) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(TinyXML2 DEFAULT_MSG TINYXML2_LIBRARY TINYXML2_INCLUDE_DIR) + + mark_as_advanced(TINYXML2_INCLUDE_DIR TINYXML2_LIBRARY) +endif() + +# Set mixed case INCLUDE_DIRS and LIBRARY variables from upper case ones. +if(NOT TinyXML2_INCLUDE_DIRS) + set(TinyXML2_INCLUDE_DIRS ${TINYXML2_INCLUDE_DIR}) +endif() +if(NOT TinyXML2_LIBRARIES) + set(TinyXML2_LIBRARIES ${TINYXML2_LIBRARY}) +endif() diff --git a/cmake/SDFUtils.cmake b/cmake/SDFUtils.cmake index 16155dcce..21ac67827 100644 --- a/cmake/SDFUtils.cmake +++ b/cmake/SDFUtils.cmake @@ -52,8 +52,11 @@ endmacro (BUILD_WARNING) ################################################# macro (sdf_add_library _name) set(LIBS_DESTINATION ${PROJECT_BINARY_DIR}/src) - set_source_files_properties(${ARGN} PROPERTIES COMPILE_DEFINITIONS "BUILDING_DLL") - add_library(${_name} SHARED ${ARGN}) + add_library(${_name} ${ARGN}) + set_target_properties(${_name} PROPERTIES DEFINE_SYMBOL "BUILDING_SDFORMAT_SHARED") + if(NOT BUILD_SHARED_LIBS) + target_compile_definitions(${_name} PUBLIC SDFORMAT_STATIC_DEFINE) + endif() set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${LIBS_DESTINATION}) if (MSVC) set_target_properties( ${_name} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${LIBS_DESTINATION}) @@ -135,18 +138,9 @@ macro (sdf_build_tests) string(REGEX REPLACE ".cc" "" BINARY_NAME ${GTEST_SOURCE_file}) set(BINARY_NAME ${TEST_TYPE}_${BINARY_NAME}) - if (NOT USE_EXTERNAL_TINYXML) - set(tinyxml_SRC - ${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinystr.cpp - ${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinyxmlerror.cpp - ${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinyxml.cpp - ${PROJECT_SOURCE_DIR}/src/win/tinyxml/tinyxmlparser.cpp) - endif() - add_executable(${BINARY_NAME} ${GTEST_SOURCE_file} ${SDF_BUILD_TESTS_EXTRA_EXE_SRCS} - ${tinyxml_SRC} ) add_dependencies(${BINARY_NAME} @@ -154,11 +148,7 @@ macro (sdf_build_tests) ) link_directories(${IGNITION-MATH_LIBRARY_DIRS}) - - if (USE_EXTERNAL_TINYXML) - target_link_libraries(${BINARY_NAME} PRIVATE - ${tinyxml_LIBRARIES}) - endif() + target_link_libraries(${BINARY_NAME} ${tinyxml2_LIBRARIES}) if (UNIX) target_link_libraries(${BINARY_NAME} PRIVATE diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake index 965f1ecf7..65b875cfd 100644 --- a/cmake/SearchForStuff.cmake +++ b/cmake/SearchForStuff.cmake @@ -5,36 +5,10 @@ include (${project_cmake_dir}/TargetArch.cmake) target_architecture(ARCH) message(STATUS "Building for arch: ${ARCH}") -if (USE_EXTERNAL_TINYXML) - ################################################# - # Find tinyxml. Only debian distributions package tinyxml with a pkg-config - # Use pkg_check_modules and fallback to manual detection (needed, at least, for MacOS) - pkg_check_modules(tinyxml tinyxml) - if (NOT tinyxml_FOUND) - find_path (tinyxml_INCLUDE_DIRS tinyxml.h ${tinyxml_INCLUDE_DIRS} ENV CPATH) - find_library(tinyxml_LIBRARIES NAMES tinyxml) - set (tinyxml_FAIL False) - if (NOT tinyxml_INCLUDE_DIRS) - message (STATUS "Looking for tinyxml headers - not found") - set (tinyxml_FAIL True) - endif() - if (NOT tinyxml_LIBRARIES) - message (STATUS "Looking for tinyxml library - not found") - set (tinyxml_FAIL True) - endif() - endif() - - if (tinyxml_FAIL) - message (STATUS "Looking for tinyxml.h - not found") - BUILD_ERROR("Missing: tinyxml") - endif() -else() - # Needed in WIN32 since in UNIX the flag is added in the code installed - add_definitions(-DTIXML_USE_STL) - include_directories (${PROJECT_SOURCE_DIR}/src/win/tinyxml) - set (tinyxml_LIBRARIES "tinyxml") - set (tinyxml_LIBRARY_DIRS "") -endif() +################################################# +# Find tinyxml2. +list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules") +find_package(TinyXML2 REQUIRED) ################################################ # Find urdfdom parser. Logic: @@ -57,7 +31,13 @@ if (NOT DEFINED USE_INTERNAL_URDF OR NOT USE_INTERNAL_URDF) pkg_check_modules(URDF urdfdom>=1.0) if (NOT URDF_FOUND) - if (NOT DEFINED USE_INTERNAL_URDF) + find_package(urdfdom) + if (urdfdom_FOUND) + set(URDF_INCLUDE_DIRS ${urdfdom_INCLUDE_DIRS}) + # ${urdfdom_LIBRARIES} already contains absolute library filenames + set(URDF_LIBRARY_DIRS "") + set(URDF_LIBRARIES ${urdfdom_LIBRARIES}) + elseif (NOT DEFINED USE_INTERNAL_URDF) message(STATUS "Couldn't find urdfdom >= 1.0, using internal copy") set(USE_INTERNAL_URDF true) else() @@ -84,7 +64,7 @@ endif() ################################################ # Find the Python interpreter for running the # check_test_ran.py script -find_package(PythonInterp QUIET) +find_package(PythonInterp 3 QUIET) ################################################ # Find psutil python package for memory tests @@ -120,10 +100,17 @@ macro (check_gcc_visibility) check_cxx_compiler_flag(-fvisibility=hidden GCC_SUPPORTS_VISIBILITY) endmacro() +######################################## +# Find ignition cmake2 +# Only for using the testing macros, not really +# being use to configure the whole project +find_package(ignition-cmake2 2.3 REQUIRED) +set(IGN_CMAKE_VER ${ignition-cmake2_VERSION_MAJOR}) + ######################################## # Find ignition math # Set a variable for generating ProjectConfig.cmake -find_package(ignition-math6 QUIET) +find_package(ignition-math6 6.8 QUIET) if (NOT ignition-math6_FOUND) message(STATUS "Looking for ignition-math6-config.cmake - not found") BUILD_ERROR ("Missing: Ignition math (libignition-math6-dev)") @@ -131,3 +118,16 @@ else() set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR}) message(STATUS "Looking for ignition-math${IGN_MATH_VER}-config.cmake - found") endif() + +######################################## +# Find ignition utils +# Set a variable for generating ProjectConfig.cmake +find_package(ignition-utils1 QUIET) +if (NOT ignition-utils1_FOUND) + message(STATUS "Looking for ignition-utils1-config.cmake - not found") + BUILD_ERROR ("Missing: Ignition utils(libignition-utils1-dev)") +else() + set(IGN_UTILS_VER ${ignition-utils1_VERSION_MAJOR}) + message(STATUS "Looking for ignition-utils${IGN_UTILS_VER}-config.cmake - found") +endif() + diff --git a/cmake/sdf_config.cmake.in b/cmake/sdf_config.cmake.in index f4c511fa9..4fdf9f208 100644 --- a/cmake/sdf_config.cmake.in +++ b/cmake/sdf_config.cmake.in @@ -37,4 +37,8 @@ find_package(ignition-math@IGN_MATH_VER@) list(APPEND @PKG_NAME@_INCLUDE_DIRS ${IGNITION-MATH_INCLUDE_DIRS}) list(APPEND @PKG_NAME@_LIBRARY_DIRS ${IGNITION-MATH_LIBRARY_DIRS}) +find_package(ignition-utils@IGN_UTILS_VER@) +list(APPEND @PKG_NAME@_INCLUDE_DIRS ${IGNITION-UTILS_INCLUDE_DIRS}) +list(APPEND @PKG_NAME@_LIBRARY_DIRS ${IGNITION-UTILS_LIBRARY_DIRS}) + list(APPEND @PKG_NAME@_LDFLAGS "-L@CMAKE_INSTALL_PREFIX@/@LIB_INSTALL_DIR@") diff --git a/cmake/sdf_config.h.in b/cmake/sdf_config.h.in index c3ce23362..524088664 100644 --- a/cmake/sdf_config.h.in +++ b/cmake/sdf_config.h.in @@ -31,6 +31,7 @@ #cmakedefine BUILD_TYPE_RELEASE 1 #cmakedefine HAVE_URDFDOM 1 #cmakedefine USE_INTERNAL_URDF 1 +#cmakedefine SDFORMAT_DISABLE_CONSOLE_LOGFILE 1 #define SDF_SHARE_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" #define SDF_VERSION_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${SDF_MAJOR_VERSION}/${SDF_PKG_VERSION}" diff --git a/examples/simple.sdf b/examples/simple.sdf index 76aee66c8..f54e96328 100644 --- a/examples/simple.sdf +++ b/examples/simple.sdf @@ -3,4 +3,4 @@ -" + diff --git a/include/sdf/Actor.hh b/include/sdf/Actor.hh index 630d64e7b..d14cd04f1 100644 --- a/include/sdf/Actor.hh +++ b/include/sdf/Actor.hh @@ -21,6 +21,7 @@ #include #include +#include #include "sdf/Element.hh" #include "sdf/Types.hh" @@ -34,46 +35,12 @@ namespace sdf // Inline bracke to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // - - // Forward declare private data class. - class AnimationPrivate; - - // Forward declare private data class. - class WaypointPrivate; - - // Forward declare private data class. - class TrajectoryPrivate; - - // Forward declare private data class. - class ActorPrivate; - /// \brief Animation in Actor. class SDFORMAT_VISIBLE Animation { /// \brief Default constructor public: Animation(); - /// \brief Copy constructor - /// \param[in] _animation Animation to copy. - public: Animation(const Animation &_animation); - - /// \brief Move constructor - /// \param[in] _animation Animation to move. - public: Animation(Animation &&_animation) noexcept; - - /// \brief Destructor - public: ~Animation(); - - /// \brief Move assignment operator. - /// \param[in] _animation Animation to move. - /// \return Reference to this. - public: Animation &operator=(Animation &&_animation); - - /// \brief Assignment operator. - /// \param[in] _animation The animation to set values from. - /// \return *this - public: Animation &operator=(const Animation &_animation); - /// \brief Load the animation based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -122,12 +89,8 @@ namespace sdf /// \param[in] _interpolateX True to indicate interpolation on X. public: void SetInterpolateX(bool _interpolateX); - /// \brief Copy animation from an Animation instance. - /// \param[in] _animation The animation to set values from. - public: void CopyFrom(const Animation &_animation); - /// \brief Private data pointer. - private: AnimationPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; /// \brief Waypoint for Trajectory. @@ -136,27 +99,6 @@ namespace sdf /// \brief Default constructor public: Waypoint(); - /// \brief Copy constructor - /// \param[in] _waypoint Waypoint to copy. - public: Waypoint(const Waypoint &_waypoint); - - /// \brief Move constructor - /// \param[in] _waypoint Waypoint to move. - public: Waypoint(Waypoint &&_waypoint) noexcept; - - /// \brief Destructor - public: ~Waypoint(); - - /// \brief Move assignment operator. - /// \param[in] _waypoint Waypoint to move. - /// \return Reference to this. - public: Waypoint &operator=(Waypoint &&_waypoint); - - /// \brief Assignment operator. - /// \param[in] _waypoint The waypoint to set values from. - /// \return *this - public: Waypoint &operator=(const Waypoint &_waypoint); - /// \brief Load the waypoint based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -181,12 +123,8 @@ namespace sdf /// \param[in] _pose Pose to be reached. public: void SetPose(const ignition::math::Pose3d &_pose); - /// \brief Copy waypoint from an Waypoint instance. - /// \param[in] _waypoint The waypoint to set values from. - public: void CopyFrom(const Waypoint &_waypoint); - /// \brief Private data pointer. - private: WaypointPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; /// \brief Trajectory for Animation. @@ -195,27 +133,6 @@ namespace sdf /// \brief Default constructor public: Trajectory(); - /// \brief Copy constructor - /// \param[in] _trajectory Trajectory to copy. - public: Trajectory(const Trajectory &_trajectory); - - /// \brief Move constructor - /// \param[in] _trajectory Trajectory to move. - public: Trajectory(Trajectory &&_trajectory) noexcept; - - /// \brief Destructor - public: ~Trajectory(); - - /// \brief Move assignment operator. - /// \param[in] _trajectory Trajectory to move. - /// \return Reference to this. - public: Trajectory &operator=(Trajectory &&_trajectory); - - /// \brief Assignment operator. - /// \param[in] _trajectory The trajectory to set values from. - /// \return *this - public: Trajectory &operator=(const Trajectory &_trajectory); - /// \brief Load the trajectory based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -264,12 +181,8 @@ namespace sdf /// \param[in] _waypoint Waypoint to be added. public: void AddWaypoint(const Waypoint &_waypoint); - /// \brief Copy trajectory from a trajectory instance. - /// \param[in] _trajectory The trajectory to set values from. - public: void CopyFrom(const Trajectory &_trajectory); - /// \brief Private data pointer. - private: TrajectoryPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; @@ -279,31 +192,6 @@ namespace sdf /// \brief Default constructor public: Actor(); - /// \brief Copy constructor - /// \param[in] _actor Actor to copy. - public: Actor(const Actor &_actor); - - /// \brief Move constructor - /// \param[in] _actor Actor to move. - public: Actor(Actor &&_actor) noexcept; - - /// \brief Destructor - public: ~Actor(); - - /// \brief Move assignment operator. - /// \param[in] _actor Actor to move. - /// \return Reference to this. - public: Actor &operator=(Actor &&_actor); - - /// \brief Assignment operator. - /// \param[in] _actor The actor to set values from. - /// \return *this - public: Actor &operator=(const Actor &_actor); - - /// \brief Copy dataPtr from an actor instance. - /// \param[in] _actor The actor to set values from. - public: void CopyFrom(const Actor &_actor); - /// \brief Load the actor based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -314,7 +202,7 @@ namespace sdf /// \brief Get the name of the actor. /// \return Name of the actor. - public: std::string &Name() const; + public: const std::string &Name() const; /// \brief Set the name of the actor. /// \param[in] _name Name of the actor. @@ -473,7 +361,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Private data pointer. - private: ActorPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/AirPressure.hh b/include/sdf/AirPressure.hh index c800382be..05f656a5d 100644 --- a/include/sdf/AirPressure.hh +++ b/include/sdf/AirPressure.hh @@ -17,6 +17,8 @@ #ifndef SDF_AIRPRESSURE_HH_ #define SDF_AIRPRESSURE_HH_ +#include + #include #include #include @@ -26,9 +28,6 @@ namespace sdf { // Inline bracke to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - class AirPressurePrivate; - /// \brief AirPressure contains information about a general /// purpose fluid pressure sensor. /// This sensor can be attached to a link. @@ -37,29 +36,6 @@ namespace sdf /// \brief Default constructor public: AirPressure(); - /// \brief Copy constructor - /// \param[in] _airPressure AirPressure to copy. - public: AirPressure(const AirPressure &_sensor); - - /// \brief Move constructor - /// \param[in] _airPressure AirPressure to move. - public: AirPressure(AirPressure &&_sensor); - - /// \brief Destructor - public: ~AirPressure(); - - /// \brief Assignment operator. - /// \param[in] _airPressure The airPressure to set values - /// from. - /// \return *this - public: AirPressure &operator=(const AirPressure &_sensor); - - /// \brief Move assignment operator. - /// \param[in] _airPressure The airPressure to set values - /// from. - /// \return *this - public: AirPressure &operator=(AirPressure &&_sensor); - /// \brief Load the airPressure based on an element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. @@ -109,7 +85,7 @@ namespace sdf public: bool operator!=(const AirPressure &_air) const; /// \brief Private data pointer. - private: AirPressurePrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Altimeter.hh b/include/sdf/Altimeter.hh index c6baa2486..03bbbf0fa 100644 --- a/include/sdf/Altimeter.hh +++ b/include/sdf/Altimeter.hh @@ -17,6 +17,8 @@ #ifndef SDF_ALTIMETER_HH_ #define SDF_ALTIMETER_HH_ +#include + #include #include #include @@ -26,9 +28,6 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - class AltimeterPrivate; - /// \brief Altimeter contains information about an altimeter sensor. /// This sensor can be attached to a link. class SDFORMAT_VISIBLE Altimeter @@ -36,27 +35,6 @@ namespace sdf /// \brief Default constructor public: Altimeter(); - /// \brief Copy constructor - /// \param[in] _altimeter Altimeter to copy. - public: Altimeter(const Altimeter &_altimeter); - - /// \brief Move constructor - /// \param[in] _altimeter Altimeter to move. - public: Altimeter(Altimeter &&_altimeter) noexcept; - - /// \brief Destructor - public: ~Altimeter(); - - /// \brief Assignment operator. - /// \param[in] _altimeter The altimeter to set values from. - /// \return *this - public: Altimeter &operator=(const Altimeter &_altimeter); - - /// \brief Move assignment operator. - /// \param[in] _altimeter The altimeter to set values from. - /// \return *this - public: Altimeter &operator=(Altimeter &&_altimeter) noexcept; - /// \brief Load the altimeter based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -99,7 +77,7 @@ namespace sdf public: bool operator!=(const Altimeter &_alt) const; /// \brief Private data pointer. - private: AltimeterPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Atmosphere.hh b/include/sdf/Atmosphere.hh index af3e73566..3cee963ba 100644 --- a/include/sdf/Atmosphere.hh +++ b/include/sdf/Atmosphere.hh @@ -18,16 +18,18 @@ #define SDF_ATMOSPHERE_HH_ #include +#include + #include "sdf/Element.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" + namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // /// \enum AtmosphereType /// \brief The set of atmosphere model types. enum class AtmosphereType @@ -36,9 +38,6 @@ namespace sdf ADIABATIC = 0, }; - // Forward declarations. - class AtmospherePrivate; - /// \brief The Atmosphere class contains information about /// an atmospheric model and related parameters such as temperature /// and pressure at sea level. An Atmosphere instance is optionally part of @@ -48,27 +47,6 @@ namespace sdf /// \brief Default constructor public: Atmosphere(); - /// \brief Copy constructor - /// \param[in] _atmosphere Atmosphere to copy. - public: Atmosphere(const Atmosphere &_atmosphere); - - /// \brief Move constructor - /// \param[in] _atmosphere Atmosphere to move. - public: Atmosphere(Atmosphere &&_atmosphere) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _atmosphere Atmosphere to move. - /// \return Reference to this. - public: Atmosphere &operator=(Atmosphere &&_atmosphere); - - /// \brief Copy assignment operator. - /// \param[in] _atmosphere Atmosphere to copy. - /// \return Reference to this. - public: Atmosphere &operator=(const Atmosphere &_atmosphere); - - /// \brief Destructor - public: ~Atmosphere(); - /// \brief Load the atmosphere based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -118,7 +96,7 @@ namespace sdf public: bool operator==(const Atmosphere &_atmosphere); /// \brief Private data pointer. - private: AtmospherePrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 62b5a003e..9ac7fa219 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -27,10 +28,6 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - // Forward declare private data class. - class BoxPrivate; - /// \brief Box represents a box shape, and is usually accessed through a /// Geometry. class SDFORMAT_VISIBLE Box @@ -38,27 +35,6 @@ namespace sdf /// \brief Constructor public: Box(); - /// \brief Copy constructor - /// \param[in] _box Box to copy. - public: Box(const Box &_box); - - /// \brief Move constructor - /// \param[in] _box Box to move. - public: Box(Box &&_box) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _box Box to move. - /// \return Reference to this. - public: Box &operator=(Box &&_box); - - /// \brief Destructor - public: virtual ~Box(); - - /// \brief Assignment operator. - /// \param[in] _box The box to set values from. - /// \return *this - public: Box &operator=(const Box &_box); - /// \brief Load the box geometry based on a element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. @@ -90,7 +66,7 @@ namespace sdf public: ignition::math::Boxd &Shape(); /// \brief Private data pointer. - private: BoxPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index ed0930a5a..b2e6d91a2 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -8,16 +8,20 @@ set (headers Atmosphere.hh Box.hh Camera.hh + Capsule.hh Collision.hh Console.hh Cylinder.hh Element.hh + Ellipsoid.hh Error.hh Exception.hh Filesystem.hh + ForceTorque.hh Frame.hh Geometry.hh Gui.hh + Heightmap.hh Imu.hh Joint.hh JointAxis.hh @@ -39,6 +43,7 @@ set (headers SDFImpl.hh SemanticPose.hh Sensor.hh + Sky.hh Sphere.hh Surface.hh Types.hh diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 76c8dab07..5beed486b 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -29,10 +30,6 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - // Forward declare private data class. - class CameraPrivate; - /// \enum PixelFormatType /// \brief The set of pixel formats. This list should match /// ignition::common::Image::PixelFormatType. @@ -65,27 +62,6 @@ namespace sdf /// \brief Constructor public: Camera(); - /// \brief Copy constructor - /// \param[in] _camera Camera to copy. - public: Camera(const Camera &_camera); - - /// \brief Move constructor - /// \param[in] _camera Camera to move. - public: Camera(Camera &&_camera) noexcept; - - /// \brief Destructor - public: virtual ~Camera(); - - /// \brief Assignment operator. - /// \param[in] _camera The camera to set values from. - /// \return *this - public: Camera &operator=(const Camera &_camera); - - /// \brief Move assignment operator. - /// \param[in] _camera The camera to set values from. - /// \return *this - public: Camera &operator=(Camera &&_camera) noexcept; - /// \brief Return true if both Camera objects contain the same values. /// \param[_in] _alt Camera value to compare. /// \returen True if 'this' == _alt. @@ -290,8 +266,7 @@ namespace sdf /// \brief Set the distortion center or principal point. /// \param[in] _center Distortion center or principal point. - public: void SetDistortionCenter( - const ignition::math::Vector2d &_center) const; + public: void SetDistortionCenter(const ignition::math::Vector2d &_center); /// \brief Get the pose of the camer. This is the pose of the camera /// as specified in SDF ( ... ). @@ -460,7 +435,7 @@ namespace sdf public: void SetVisibilityMask(uint32_t _mask); /// \brief Private data pointer. - private: CameraPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh new file mode 100644 index 000000000..76a91f077 --- /dev/null +++ b/include/sdf/Capsule.hh @@ -0,0 +1,80 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_CAPSULE_HH_ +#define SDF_CAPSULE_HH_ + +#include +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + /// \brief Capsule represents a capsule shape, and is usually accessed + /// through a Geometry. + class SDFORMAT_VISIBLE Capsule + { + /// \brief Constructor + public: Capsule(); + + /// \brief Load the capsule geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the capsule's radius in meters. + /// \return The radius of the capsule in meters. + public: double Radius() const; + + /// \brief Set the capsule's radius in meters. + /// \param[in] _radius The radius of the capsule in meters. + public: void SetRadius(const double _radius); + + /// \brief Get the capsule's length in meters. + /// \return The length of the capsule in meters. + public: double Length() const; + + /// \brief Set the capsule's length in meters. + /// \param[in] _length The length of the capsule in meters. + public: void SetLength(const double _length); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the Ignition Math representation of this Capsule. + /// \return A const reference to an ignition::math::Sphered object. + public: const ignition::math::Capsuled &Shape() const; + + /// \brief Get a mutable Ignition Math representation of this Capsule. + /// \return A reference to an ignition::math::Capsuled object. + public: ignition::math::Capsuled &Shape(); + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } +} +#endif diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 275f97164..342985ff7 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -32,10 +33,10 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // // Forward declaration. - class CollisionPrivate; class Geometry; class Surface; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief A collision element descibes the collision properties associated /// with a link. This can be different from the visual properties of a link. @@ -46,27 +47,6 @@ namespace sdf /// \brief Default constructor public: Collision(); - /// \brief Copy constructor - /// \param[in] _collision Collision to copy. - public: Collision(const Collision &_collision); - - /// \brief Move constructor - /// \param[in] _collision Collision to move. - public: Collision(Collision &&_collision) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _collision Collision to move. - /// \return Reference to this. - public: Collision &operator=(Collision &&_collision); - - /// \brief Copy assignment operator. - /// \param[in] _collision Collision to copy. - /// \return Reference to this. - public: Collision &operator=(const Collision &_collision); - - /// \brief Destructor - public: ~Collision(); - /// \brief Load the collision based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -83,7 +63,7 @@ namespace sdf /// \brief Set the name of the collision. /// The name of the collision must be unique within the scope of a Link. /// \param[in] _name Name of the collision. - public: void SetName(const std::string &_name) const; + public: void SetName(const std::string &_name); /// \brief Get a pointer to the collisions's geometry. /// \return The collision's geometry. @@ -95,7 +75,7 @@ namespace sdf /// \brief Get a pointer to the collisions's surface parameters. /// \return The collision's surface parameters. - public: sdf::Surface *Surface() const; + public: const sdf::Surface *Surface() const; /// \brief Set the collision's surface parameters /// \param[in] _surface The surface parameters of the collision object @@ -141,12 +121,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is @@ -154,7 +134,7 @@ namespace sdf friend class Link; /// \brief Private data pointer. - private: CollisionPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Console.hh b/include/sdf/Console.hh index fa0d25c16..040f324de 100644 --- a/include/sdf/Console.hh +++ b/include/sdf/Console.hh @@ -122,7 +122,10 @@ namespace sdf const std::string &file, unsigned int line, int color); - /// \brief Use this to output a message to a log file + /// \brief Use this to output a message to a log file at + /// `$HOME/.sdformat/sdformat.log`. + /// To disable this log file, define the following symbol when + /// compiling: SDFORMAT_DISABLE_CONSOLE_LOGFILE /// \return Reference to output stream public: ConsoleStream &Log(const std::string &lbl, const std::string &file, diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index a6f1af1f7..9a48b59a5 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -18,6 +18,7 @@ #define SDF_CYLINDER_HH_ #include +#include #include #include #include @@ -26,11 +27,6 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - - // Forward declare private data class. - class CylinderPrivate; - /// \brief Cylinder represents a cylinder shape, and is usually accessed /// through a Geometry. class SDFORMAT_VISIBLE Cylinder @@ -38,27 +34,6 @@ namespace sdf /// \brief Constructor public: Cylinder(); - /// \brief Copy constructor - /// \param[in] _cylinder Cylinder to copy. - public: Cylinder(const Cylinder &_cylinder); - - /// \brief Move constructor - /// \param[in] _cylinder Cylinder to move. - public: Cylinder(Cylinder &&_cylinder) noexcept; - - /// \brief Destructor - public: virtual ~Cylinder(); - - /// \brief Move assignment operator. - /// \param[in] _cylinder Cylinder to move. - /// \return Reference to this. - public: Cylinder &operator=(Cylinder &&_cylinder); - - /// \brief Assignment operator. - /// \param[in] _cylinder The cylinder to set values from. - /// \return *this - public: Cylinder &operator=(const Cylinder &_cylinder); - /// \brief Load the cylinder geometry based on a element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. @@ -98,7 +73,7 @@ namespace sdf public: ignition::math::Cylinderd &Shape(); /// \brief Private data pointer. - private: CylinderPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh new file mode 100644 index 000000000..fadd4ed81 --- /dev/null +++ b/include/sdf/Ellipsoid.hh @@ -0,0 +1,72 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_ELLIPSOID_HH_ +#define SDF_ELLIPSOID_HH_ + +#include +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + /// \brief Ellipsoid represents a ellipsoid shape, and is usually accessed + /// through a Geometry. + class SDFORMAT_VISIBLE Ellipsoid + { + /// \brief Constructor + public: Ellipsoid(); + + /// \brief Load the ellipsoid geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the ellipsoid's radii in meters. + /// \return The radius of the ellipsoid in meters. + public: ignition::math::Vector3d Radii() const; + + /// \brief Set the ellipsoid's x, y, and z radii in meters. + /// \param[in] _radius Vector of radii (x, y, z) of the ellipsoid in meters. + public: void SetRadii(const ignition::math::Vector3d &_radii); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the Ignition Math representation of this Ellipsoid. + /// \return A const reference to an ignition::math::Ellipsoidd object. + public: const ignition::math::Ellipsoidd &Shape() const; + + /// \brief Get a mutable Ignition Math representation of this Ellipsoid. + /// \return A reference to an ignition::math::Ellipsoidd object. + public: ignition::math::Ellipsoidd &Shape(); + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } +} +#endif diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index 9ae088e9c..b1e8055dd 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -131,6 +131,9 @@ namespace sdf /// \brief Indicates that reading an SDF string failed. STRING_READ, + + /// \brief The specified placement frame is invalid + MODEL_PLACEMENT_FRAME_INVALID, }; class SDFORMAT_VISIBLE Error diff --git a/include/sdf/Exception.hh b/include/sdf/Exception.hh index 406a6897c..6adbc2956 100644 --- a/include/sdf/Exception.hh +++ b/include/sdf/Exception.hh @@ -24,16 +24,10 @@ #include #include +#include #include #include "sdf/system_util.hh" -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - namespace sdf { // Inline bracket to help doxygen filtering. @@ -49,8 +43,6 @@ namespace sdf throwStream << msg << std::endl << std::flush;\ throw sdf::Exception(__FILE__, __LINE__, throwStream.str()); } - class ExceptionPrivate; - /// \class Exception Exception.hh common/common.hh /// \brief Class for generating exceptions class SDFORMAT_VISIBLE Exception @@ -68,24 +60,24 @@ namespace sdf /// \brief Copy constructor /// \param[in] _e Exception to copy. - public: Exception(const Exception &_e); + public: Exception(const Exception &_e) = default; /// \brief Move constructor /// \param[in] _e Exception to move. - public: Exception(Exception &&_e) noexcept; + public: Exception(Exception &&_e) noexcept = default; /// \brief Assignment operator. /// \param[in] _exception The exception to set values from. /// \return *this - public: Exception &operator=(const Exception &_exception); + public: Exception &operator=(const Exception &_exception) = default; /// \brief Move assignment operator. /// \param[in] _exception Exception to move. /// \return Reference to this. - public: Exception &operator=(Exception &&_exception); + public: Exception &operator=(Exception &&_exception) noexcept = default; /// \brief Destructor - public: virtual ~Exception(); + public: virtual ~Exception() = default; /// \brief Return the error function /// \return The error function name @@ -109,7 +101,7 @@ namespace sdf } /// \brief Private data pointer. - private: std::unique_ptr dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; /// \class InternalError Exception.hh common/common.hh @@ -158,8 +150,4 @@ namespace sdf } } -#ifdef _WIN32 -#pragma warning(pop) -#endif - #endif diff --git a/include/sdf/Filesystem.hh b/include/sdf/Filesystem.hh index 3613f200b..a1e454c64 100644 --- a/include/sdf/Filesystem.hh +++ b/include/sdf/Filesystem.hh @@ -21,16 +21,10 @@ #include #include +#include #include #include "sdf/system_util.hh" -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - namespace sdf { // Inline bracke to help doxygen filtering. @@ -93,9 +87,6 @@ namespace sdf SDFORMAT_VISIBLE std::string basename(const std::string &_path); - /// \internal - class DirIterPrivate; - /// \class DirIter Filesystem.hh /// \brief A class for iterating over all items in a directory. class SDFORMAT_VISIBLE DirIter @@ -134,14 +125,10 @@ namespace sdf private: void close_handle(); /// \brief Private data. - private: std::unique_ptr dataPtr; + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } } } -#ifdef _WIN32 -#pragma warning(pop) -#endif - #endif diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh new file mode 100644 index 000000000..35889e811 --- /dev/null +++ b/include/sdf/ForceTorque.hh @@ -0,0 +1,115 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_FORCE_TORQUE_HH_ +#define SDF_FORCE_TORQUE_HH_ + +#include +#include +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + /// \enum ForceTorqueFrame + /// \brief The set of supported frames of the wrench values. + enum class ForceTorqueFrame : uint8_t + { + /// \brief Invalid frame + INVALID = 0, + + /// \brief Wrench expressed in the orientation of the parent link frame + PARENT = 1, + + /// \brief Wrench expressed in the orientation of the child link frame + CHILD = 2, + + /// \brief Wrench expressed in the orientation of the joint sensor frame + SENSOR = 3 + }; + + /// \enum ForceTorqueMeasureDirection + /// \brief The set of measure directions of the wrench values. + enum class ForceTorqueMeasureDirection : uint8_t + { + /// \brief Invalid frame + INVALID = 0, + + /// \brief Wrench measured as applied by the parent link on the child link + PARENT_TO_CHILD = 1, + + /// \brief Wrench measured as applied by the child link on the parent link + CHILD_TO_PARENT = 2 + }; + + /// \brief ForceTorque contains information about a force torque sensor. + /// This sensor can be attached to a joint. + class SDFORMAT_VISIBLE ForceTorque + { + /// \brief Default constructor + public: ForceTorque(); + + /// \brief Load the force torque sensor based on an element pointer. This is + /// *not* the usual entry point. Typical usage of the SDF DOM is through the + /// Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the frame in which the wrench values are reported. + /// \return The frame of the wrench values. + public: ForceTorqueFrame Frame() const; + + /// \brief Set the frame in which the wrench values are reported. + /// \param[in] _frame The frame of the wrench values. + public: void SetFrame(ForceTorqueFrame _frame); + + /// \brief Get the measure direction of the wrench values. + /// \return The measure direction of the wrench values. + public: ForceTorqueMeasureDirection MeasureDirection() const; + + /// \brief Set the measure direction of the wrench values. + /// \param[in] _direction The measure direction of the wrench values. + public: void SetMeasureDirection(ForceTorqueMeasureDirection _direction); + + /// \brief Return true if both force torque objects contain the same values. + /// \param[_in] _ft Force torque value to compare. + /// \returen True if 'this' == _ft. + public: bool operator==(const ForceTorque &_ft) const; + + /// \brief Return true this force torque object does not contain the same + /// values as the passed-in parameter. + /// \param[_in] _ft Force torque value to compare. + /// \returen True if 'this' != _ft. + public: bool operator!=(const ForceTorque &_ft) const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } +} + +#endif diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index e5097e8ca..3c327dab9 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -32,9 +33,9 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // // Forward declaration. - class FramePrivate; struct FrameAttachedToGraph; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief A Frame element descibes the properties associated with an /// explicit frame defined in a Model or World. @@ -43,27 +44,6 @@ namespace sdf /// \brief Default constructor public: Frame(); - /// \brief Copy constructor - /// \param[in] _frame Frame to copy. - public: Frame(const Frame &_frame); - - /// \brief Move constructor - /// \param[in] _frame Frame to move. - public: Frame(Frame &&_frame); - - /// \brief Destructor - public: ~Frame(); - - /// \brief Move assignment operator. - /// \param[in] _frame Frame to move. - /// \return Reference to this. - public: Frame &operator=(Frame &&_frame); - - /// \brief Assignment operator. - /// \param[in] _frame The frame to set values from. - /// \return *this - public: Frame &operator=(const Frame &_frame); - /// \brief Load the frame based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -80,7 +60,7 @@ namespace sdf /// \brief Set the name of the frame. /// The name of the frame must be unique within the scope of its siblings. /// \param[in] _name Name of the frame. - public: void SetName(const std::string &_name) const; + public: void SetName(const std::string &_name); /// \brief Get the name of the coordinate frame to which this /// frame is attached. The interpretation of an empty value depends @@ -133,9 +113,8 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Resolve the attached-to body of this frame from the - /// FrameAttachedToGraph. If this is in a __model__ scope, it returns - /// the name of a link. In the world scope, it returns the name of a - /// model or the world. + /// FrameAttachedToGraph. Generally, it resolves to the name of a link, but + /// if it is in the world scope, it can resolve to "world". /// \param[out] _body Name of body to which this frame is attached. /// \return Errors. public: Errors ResolveAttachedToBody(std::string &_body) const; @@ -145,26 +124,26 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the FrameAttachedToGraph to be used - /// for resolving attached bodies. This is private and is intended to - /// be called by Model::Load or World::Load. - /// \param[in] _graph Weak pointer to FrameAttachedToGraph. + /// \brief Give a scoped FrameAttachedToGraph to be used for resolving + /// attached bodies. This is private and is intended to be called by + /// Model::Load or World::Load. + /// \param[in] _graph scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load or World::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load or + /// World::Load. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load and World::Load to call SetPoseRelativeToGraph. friend class Model; friend class World; /// \brief Private data pointer. - private: FramePrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 148943ad3..6223a1737 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -17,6 +17,7 @@ #ifndef SDF_GEOMETRY_HH_ #define SDF_GEOMETRY_HH_ +#include #include #include #include @@ -28,9 +29,11 @@ namespace sdf // // Forward declare private data class. - class GeometryPrivate; class Box; + class Capsule; class Cylinder; + class Ellipsoid; + class Heightmap; class Mesh; class Plane; class Sphere; @@ -56,6 +59,15 @@ namespace sdf /// \brief A mesh geometry. MESH = 5, + + /// \brief A heightmap geometry. + HEIGHTMAP = 6, + + /// \brief A capsule geometry. + CAPSULE = 7, + + /// \brief An ellipsoid geometry + ELLIPSOID = 8, }; /// \brief Geometry provides access to a shape, such as a Box. Use the @@ -67,27 +79,6 @@ namespace sdf /// \brief Default constructor public: Geometry(); - /// \brief Copy constructor - /// \param[in] _geometry Geometry to copy. - public: Geometry(const Geometry &_geometry); - - /// \brief Move constructor - /// \param[in] _geometry Geometry to move. - public: Geometry(Geometry &&_geometry) noexcept; - - /// \brief Destructor - public: virtual ~Geometry(); - - /// \brief Assignment operator. - /// \param[in] _geometry The geometry to set values from. - /// \return *this - public: Geometry &operator=(const Geometry &_geometry); - - /// \brief Move assignment operator. - /// \param[in] _geometry The geometry to move from. - /// \return *this - public: Geometry &operator=(Geometry &&_geometry); - /// \brief Load the geometry based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -115,6 +106,17 @@ namespace sdf /// \param[in] _box The box shape. public: void SetBoxShape(const Box &_box); + /// \brief Get the capsule geometry, or nullptr if the contained + /// geometry is not a capsule. + /// \return Pointer to the capsule geometry, or nullptr if the + /// geometry is not a capsule. + /// \sa GeometryType Type() const + public: const Capsule *CapsuleShape() const; + + /// \brief Set the capsule shape. + /// \param[in] _capsule The capsule shape. + public: void SetCapsuleShape(const Capsule &_capsule); + /// \brief Get the cylinder geometry, or nullptr if the contained /// geometry is not a cylinder. /// \return Pointer to the visual's cylinder geometry, or nullptr if the @@ -126,6 +128,17 @@ namespace sdf /// \param[in] _cylinder The cylinder shape. public: void SetCylinderShape(const Cylinder &_cylinder); + /// \brief Get the ellipsoid geometry, or nullptr if the contained + /// geometry is not an ellipsoid. + /// \return Pointer to the ellipsoid geometry, or nullptr if the geometry is + /// not an ellipsoid. + /// \sa GeometryType Type() const + public: const Ellipsoid *EllipsoidShape() const; + + /// \brief Set the ellipsoid shape. + /// \param[in] _ellipsoid The ellipsoid shape. + public: void SetEllipsoidShape(const Ellipsoid &_ellipsoid); + /// \brief Get the sphere geometry, or nullptr if the contained geometry is /// not a sphere. /// \return Pointer to the visual's sphere geometry, or nullptr if the @@ -159,6 +172,17 @@ namespace sdf /// \param[in] _mesh The mesh shape. public: void SetMeshShape(const Mesh &_mesh); + /// \brief Get the heightmap geometry, or nullptr if the contained geometry + /// is not a heightmap. + /// \return Pointer to the heightmap geometry, or nullptr if the geometry is + /// not a heightmap. + /// \sa GeometryType Type() const + public: const Heightmap *HeightmapShape() const; + + /// \brief Set the heightmap shape. + /// \param[in] _heightmap The heightmap shape. + public: void SetHeightmapShape(const Heightmap &_heightmap); + /// \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 @@ -166,7 +190,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Private data pointer. - private: GeometryPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Gui.hh b/include/sdf/Gui.hh index afe52caa9..eec65d134 100644 --- a/include/sdf/Gui.hh +++ b/include/sdf/Gui.hh @@ -17,6 +17,7 @@ #ifndef SDF_GUI_HH_ #define SDF_GUI_HH_ +#include #include "sdf/Element.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -26,37 +27,11 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - - // Forward declarations. - class GuiPrivate; - class SDFORMAT_VISIBLE Gui { /// \brief Default constructor public: Gui(); - /// \brief Copy constructor - /// \param[in] _gui Gui element to copy. - public: Gui(const Gui &_gui); - - /// \brief Move constructor - /// \param[in] _gui Gui to move. - public: Gui(Gui &&_gui) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _gui Gui to move. - /// \return Reference to this. - public: Gui &operator=(Gui &&_gui); - - /// \brief Copy assignment operator. - /// \param[in] _gui Gui to copy. - /// \return Reference to this. - public: Gui &operator=(const Gui &_gui); - - /// \brief Destructor - public: ~Gui(); - /// \brief Load the gui based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -72,7 +47,7 @@ namespace sdf /// \brief Set whether the Gui should be full screen. /// \param[in] _fullscreen True indicates that the Gui should be /// fullscreen. - public: void SetFullscreen(const bool _fullscreen) const; + public: void SetFullscreen(const bool _fullscreen); /// \brief Equality operator that returns true if this Gui /// instance equals the given Gui instance. @@ -87,7 +62,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Private data pointer. - private: GuiPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Heightmap.hh b/include/sdf/Heightmap.hh new file mode 100644 index 000000000..11a664b56 --- /dev/null +++ b/include/sdf/Heightmap.hh @@ -0,0 +1,222 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef SDF_HEIGHTMAP_HH_ +#define SDF_HEIGHTMAP_HH_ + +#include +#include +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + /// \brief Texture to be used on heightmaps. + class SDFORMAT_VISIBLE HeightmapTexture + { + /// \brief Constructor + public: HeightmapTexture(); + + /// \brief Load the heightmap texture geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the heightmap texture's size. + /// \return The size of the heightmap texture in meters. + public: double Size() const; + + /// \brief Set the size of the texture in meters. + /// \param[in] _uri The size of the texture in meters. + public: void SetSize(double _uri); + + /// \brief Get the heightmap texture's diffuse map. + /// \return The diffuse map of the heightmap texture. + public: std::string Diffuse() const; + + /// \brief Set the filename of the diffuse map. + /// \param[in] _diffuse The diffuse map of the heightmap texture. + public: void SetDiffuse(const std::string &_diffuse); + + /// \brief Get the heightmap texture's normal map. + /// \return The normal map of the heightmap texture. + public: std::string Normal() const; + + /// \brief Set the filename of the normal map. + /// \param[in] _normal The normal map of the heightmap texture. + public: void SetNormal(const std::string &_normal); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + + /// \brief Blend information to be used between textures on heightmaps. + class SDFORMAT_VISIBLE HeightmapBlend + { + /// \brief Constructor + public: HeightmapBlend(); + + /// \brief Load the heightmap blend geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the heightmap blend's minimum height. + /// \return The minimum height of the blend layer. + public: double MinHeight() const; + + /// \brief Set the minimum height of the blend in meters. + /// \param[in] _uri The minimum height of the blend in meters. + public: void SetMinHeight(double _minHeight); + + /// \brief Get the heightmap blend's fade distance. + /// \return The fade distance of the heightmap blend in meters. + public: double FadeDistance() const; + + /// \brief Set the distance over which the blend occurs. + /// \param[in] _uri The distance in meters. + public: void SetFadeDistance(double _fadeDistance); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + + /// \brief Heightmap represents a shape defined by a 2D field, and is usually + /// accessed through a Geometry. + class SDFORMAT_VISIBLE Heightmap + { + /// \brief Constructor + public: Heightmap(); + + /// \brief Load the heightmap geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the heightmap's URI. + /// \return The URI of the heightmap data. + public: std::string Uri() const; + + /// \brief Set the URI to a grayscale image. + /// \param[in] _uri The URI of the heightmap. + public: void SetUri(const std::string &_uri); + + /// \brief The path to the file where this element was loaded from. + /// \return Full path to the file on disk. + public: const std::string &FilePath() const; + + /// \brief Set the path to the file where this element was loaded from. + /// \paramp[in] _filePath Full path to the file on disk. + public: void SetFilePath(const std::string &_filePath); + + /// \brief Get the heightmap's scaling factor. + /// \return The heightmap's size. + public: ignition::math::Vector3d Size() const; + + /// \brief Set the heightmap's scaling factor. Defaults to 1x1x1. + /// \return The heightmap's size factor. + public: void SetSize(const ignition::math::Vector3d &_size); + + /// \brief Get the heightmap's position offset. + /// \return The heightmap's position offset. + public: ignition::math::Vector3d Position() const; + + /// \brief Set the heightmap's position offset. + /// \return The heightmap's position offset. + public: void SetPosition(const ignition::math::Vector3d &_position); + + /// \brief Get whether the heightmap uses terrain paging. + /// \return True if the heightmap uses terrain paging. + public: bool UseTerrainPaging() const; + + /// \brief Set whether the heightmap uses terrain paging. Defaults to false. + /// \param[in] _use True to use. + public: void SetUseTerrainPaging(bool _use); + + /// \brief Get the heightmap's sampling per datum. + /// \return The heightmap's sampling. + public: unsigned int Sampling() const; + + /// \brief Set the heightmap's sampling. Defaults to 1. + /// \param[in] _sampling The heightmap's sampling per datum. + public: void SetSampling(unsigned int _sampling); + + /// \brief Get the number of heightmap textures. + /// \return Number of heightmap textures contained in this Heightmap object. + public: uint64_t TextureCount() const; + + /// \brief Get a heightmap texture based on an index. + /// \param[in] _index Index of the heightmap texture. The index should be in + /// the range [0..TextureCount()). + /// \return Pointer to the heightmap texture. Nullptr if the index does not + /// exist. + /// \sa uint64_t TextureCount() const + public: const HeightmapTexture *TextureByIndex(uint64_t _index) const; + + /// \brief Add a heightmap texture. + /// \param[in] _texture Texture to add. + public: void AddTexture(const HeightmapTexture &_texture); + + /// \brief Get the number of heightmap blends. + /// \return Number of heightmap blends contained in this Heightmap object. + public: uint64_t BlendCount() const; + + /// \brief Get a heightmap blend based on an index. + /// \param[in] _index Index of the heightmap blend. The index should be in + /// the range [0..BlendCount()). + /// \return Pointer to the heightmap blend. Nullptr if the index does not + /// exist. + /// \sa uint64_t BlendCount() const + public: const HeightmapBlend *BlendByIndex(uint64_t _index) const; + + /// \brief Add a heightmap blend. + /// \param[in] _blend Blend to add. + public: void AddBlend(const HeightmapBlend &_blend); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } +} +#endif diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index 1ff9c2f89..a6c4c47b0 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -18,6 +18,7 @@ #define SDF_IMU_HH_ #include +#include #include #include #include @@ -27,9 +28,6 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - class ImuPrivate; - /// \brief Imu contains information about an imu sensor. /// This sensor can be attached to a link. class SDFORMAT_VISIBLE Imu @@ -37,27 +35,6 @@ namespace sdf /// \brief Default constructor public: Imu(); - /// \brief Copy constructor - /// \param[in] _imu Imu to copy. - public: Imu(const Imu &_imu); - - /// \brief Move constructor - /// \param[in] _imu Imu to move. - public: Imu(Imu &&_imu) noexcept; - - /// \brief Destructor - public: ~Imu(); - - /// \brief Assignment operator. - /// \param[in] _imu The IMU to set values from. - /// \return *this - public: Imu &operator=(const Imu &_imu); - - /// \brief Move assignment operator. - /// \param[in] _imu The IMU to set values from. - /// \return *this - public: Imu &operator=(Imu &&_imu) noexcept; - /// \brief Load the IMU based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -138,7 +115,7 @@ namespace sdf /// X-axis. grav_dir_x is defined in the coordinate frame as defined by /// the parent_frame element. /// \return The gravity direction. - public: ignition::math::Vector3d &GravityDirX() const; + public: const ignition::math::Vector3d &GravityDirX() const; /// \brief Used when localization is set to GRAV_UP or GRAV_DOWN, a /// projection of this vector into a plane that is orthogonal to the @@ -146,7 +123,7 @@ namespace sdf /// X-axis. grav_dir_x is defined in the coordinate frame as defined by /// the parent_frame element. /// \param[in] _grav The gravity direction. - public: void SetGravityDirX(const ignition::math::Vector3d &_grav) const; + public: void SetGravityDirX(const ignition::math::Vector3d &_grav); /// \brief Get the name of parent frame which the GravityDirX vector is /// defined relative to. It can be any valid fully scoped link name or the @@ -160,7 +137,7 @@ namespace sdf /// special reserved "world" frame. If left empty, use the sensor's own /// local frame. /// \return The name of the parent frame. - public: void SetGravityDirXParentFrame(const std::string &_frame) const; + public: void SetGravityDirXParentFrame(const std::string &_frame); /// \brief This string represents special hardcoded use cases that are /// commonly seen with typical robot IMU's: @@ -239,7 +216,7 @@ namespace sdf /// \brief See CustomRpy() const. /// \param[in] Custom RPY vectory - public: void SetCustomRpy(const ignition::math::Vector3d &_rpy) const; + public: void SetCustomRpy(const ignition::math::Vector3d &_rpy); /// \brief Get the name of parent frame which the custom_rpy transform is /// defined relative to. It can be any valid fully scoped link name or the @@ -253,7 +230,7 @@ namespace sdf /// special reserved "world" frame. If left empty, use the sensor's own /// local frame. /// \param[in] _frame The name of the parent frame. - public: void SetCustomRpyParentFrame(const std::string &_frame) const; + public: void SetCustomRpyParentFrame(const std::string &_frame); /// \brief Return true if both Imu objects contain the same values. /// \param[_in] _imu Imu value to compare. @@ -267,7 +244,7 @@ namespace sdf public: bool operator!=(const Imu &_imu) const; /// \brief Private data pointer. - private: ImuPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 395977926..c53d54e19 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -34,8 +35,9 @@ namespace sdf // Forward declarations. class JointAxis; - class JointPrivate; + struct FrameAttachedToGraph; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum JointType /// \brief The set of joint types. INVALID indicates that joint type has @@ -84,27 +86,6 @@ namespace sdf /// \brief Default constructor public: Joint(); - /// \brief Copy constructor - /// \param[in] _joint Joint to copy. - public: Joint(const Joint &_joint); - - /// \brief Move constructor - /// \param[in] _joint Joint to move. - public: Joint(Joint &&_joint) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _joint Joint to move. - /// \return Reference to this. - public: Joint &operator=(Joint &&_joint); - - /// \brief Copy assignment operator. - /// \param[in] _joint Joint to copy. - /// \return Reference to this. - public: Joint &operator=(const Joint &_joint); - - /// \brief Destructor - public: ~Joint(); - /// \brief Load the joint based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -147,6 +128,18 @@ namespace sdf /// \param[in] _name Name of the child link. public: void SetChildLinkName(const std::string &_name); + /// \brief Resolve the name of the child link from the + /// FrameAttachedToGraph. + /// \param[out] _body Name of child link of this joint. + /// \return Errors. + public: Errors ResolveChildLink(std::string &_link) const; + + /// \brief Resolve the name of the parent link from the + /// FrameAttachedToGraph. It will return the name of a link or "world". + /// \param[out] _body Name of parent link of this joint. + /// \return Errors. + public: Errors ResolveParentLink(std::string &_link) const; + /// \brief Get a joint axis. /// \param[in] _index This value specifies which axis to get. A value of /// zero corresponds to the first axis, which is the SDF @@ -208,18 +201,24 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the scoped FrameAttachedToGraph to be used for resolving + /// parent and child link names. This is private and is intended to be + /// called by Model::Load. + /// \param[in] _graph scoped FrameAttachedToGraph object. + private: void SetFrameAttachedToGraph( + sdf::ScopedGraph _graph); + + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load to call SetPoseRelativeToGraph. friend class Model; /// \brief Private data pointer. - private: JointPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index 38b028020..d16aac72f 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -20,7 +20,9 @@ #include #include #include +#include #include "sdf/Element.hh" +#include "sdf/Exception.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" @@ -32,8 +34,8 @@ namespace sdf // // Forward declare private data class. - class JointAxisPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief Parameters related to the axis of rotation for rotational joints, /// and the axis of translation for prismatic joints. @@ -42,27 +44,6 @@ namespace sdf /// \brief Default constructor public: JointAxis(); - /// \brief Copy constructor - /// \param[in] _jointAxis Joint axis to copy. - public: JointAxis(const JointAxis &_jointAxis); - - /// \brief Move constructor - /// \param[in] _jointAxis Joint axis to move. - public: JointAxis(JointAxis &&_jointAxis) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _jointAxis JointAxis component to move. - /// \return Reference to this. - public: JointAxis &operator=(JointAxis &&_jointAxis); - - /// \brief Copy assignment operator. - /// \param[in] _jointAxis JointAxis component to copy. - /// \return Reference to this. - public: JointAxis &operator=(const JointAxis &_jointAxis); - - /// \brief Destructor - public: ~JointAxis(); - /// \brief Load the joint axis based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -83,8 +64,9 @@ namespace sdf public: void SetInitialPosition(const double _pos) SDF_DEPRECATED(10.0); /// \brief Get the x,y,z components of the axis unit vector. - /// The axis is expressed in the joint frame unless UseParentModelFrame - /// is true. The vector should be normalized. + /// The axis is expressed in the frame named in XyzExpressedIn() and + /// defaults to the joint frame if that method returns an empty string. + /// The vector should be normalized. /// The default value is ignition::math::Vector3d::UnitZ which equals /// (0, 0, 1). /// \return The x,y,z components of the axis unit vector. @@ -94,23 +76,9 @@ namespace sdf /// \brief Set the x,y,z components of the axis unit vector. /// \param[in] _xyz The x,y,z components of the axis unit vector. /// \sa ignition::math::Vector3d Xyz() const - public: void SetXyz(const ignition::math::Vector3d &_xyz); - - /// \brief Get whether to interpret the axis xyz value in the parent model - /// frame instead of joint frame. The default value is false. - /// \return True to interpret the axis xyz value in the parent model - /// frame, false to use the joint frame. - /// \sa void SetUseParentModelFrame(const bool _parentModelFrame) - public: bool UseParentModelFrame() const - SDF_DEPRECATED(9.0); - - /// \brief Set whether to interpret the axis xyz value in the parent model - /// instead of the joint frame. - /// \param[in] _parentModelFrame True to interpret the axis xyz value in - /// the parent model frame, false to use the joint frame. - /// \sa bool UseParentModelFrame() const - public: void SetUseParentModelFrame(const bool _parentModelFrame) - SDF_DEPRECATED(9.0); + /// \return Errors will have an entry if the norm of the xyz vector is 0. + public: [[nodiscard]] sdf::Errors SetXyz( + const ignition::math::Vector3d &_xyz); /// \brief Get the physical velocity dependent viscous damping coefficient /// of the joint axis. The default value is zero (0.0). @@ -185,30 +153,34 @@ namespace sdf /// axis is continuous. /// \param[in] _upper The upper joint axis limit. /// \sa double Upper() const - public: void SetUpper(const double _upper) const; + public: void SetUpper(const double _upper); - /// \brief Get the value for enforcing the maximum joint effort applied. - /// Limit is not enforced if value is negative. The default value is -1. - /// \return Effort limit. + /// \brief Get the value for enforcing the maximum absolute joint effort + /// that can be applied. + /// The limit is not enforced if the value is infinity. + /// The default value is infinity. + /// \return Symmetric effort limit. /// \sa void SetEffort(double _effort) public: double Effort() const; - /// \brief Set the value for enforcing the maximum joint effort applied. - /// Limit is not enforced if value is negative. - /// \param[in] _effort Effort limit. + /// \brief Set the value for enforcing the maximum absolute joint effort + /// that can be applied. + /// The limit is not enforced if the value is infinity. + /// \param[in] _effort Symmetric effort limit. /// \sa double Effort() const public: void SetEffort(double _effort); - /// \brief Get the value for enforcing the maximum joint velocity. The - /// default value is -1. - /// \return The value for enforcing the maximum joint velocity. + /// \brief Get the value for enforcing the maximum absolute joint velocity. + /// The default value is infinity. + /// \return The value for enforcing the maximum absolute joint velocity. /// \sa void SetVelocity(const double _velocity) const public: double MaxVelocity() const; - /// \brief Set the value for enforcing the maximum joint velocity. - /// \param[in] _velocity The value for enforcing the maximum joint velocity. - /// \sa double Velocity() const - public: void SetMaxVelocity(const double _velocity) const; + /// \brief Set the value for enforcing the maximum absolute joint velocity. + /// \param[in] _velocity The value for enforcing the maximum absolute + /// joint velocity. + /// \sa double MaxVelocity() const + public: void SetMaxVelocity(const double _velocity); /// \brief Get the joint stop stiffness. The default value is 1e8. /// \return The joint stop stiffness. @@ -219,7 +191,7 @@ namespace sdf /// \param[in] _stiffness The joint stop stiffness. /// \return The joint stop stiffness. /// \sa double Stiffness() const - public: void SetStiffness(const double _stiffness) const; + public: void SetStiffness(const double _stiffness); /// \brief Get the joint stop dissipation. The default value is 1.0. /// \return The joint stop dissipation. @@ -229,7 +201,7 @@ namespace sdf /// \brief Set the joint stop dissipation. /// \param[in] _dissipation The joint stop dissipation. /// \sa double Dissipation() const - public: void SetDissipation(const double _dissipation) const; + public: void SetDissipation(const double _dissipation); /// Get the name of the coordinate frame in which this joint axis's /// unit vector is expressed. An empty value implies the parent (joint) @@ -266,18 +238,18 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called - /// by Joint::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by + /// Joint::SetPoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Joint::SetPoseRelativeToGraph to propagate. friend class Joint; /// \brief Private data pointer - private: JointAxisPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Lidar.hh b/include/sdf/Lidar.hh index f04a1538e..1595fb8bd 100644 --- a/include/sdf/Lidar.hh +++ b/include/sdf/Lidar.hh @@ -17,19 +17,20 @@ #ifndef SDF_LIDAR_HH_ #define SDF_LIDAR_HH_ +#include +#include + #include #include #include #include -#include namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // - class LidarPrivate; /// \brief Lidar contains information about a Lidar sensor. /// This sensor can be attached to a link. The Lidar sensor can be defined @@ -106,27 +107,6 @@ namespace sdf /// \brief Default constructor public: Lidar(); - /// \brief Copy constructor - /// \param[in] _lidar Lidar to copy. - public: Lidar(const Lidar &_lidar); - - /// \brief Move constructor - /// \param[in] _lidar Lidar to move. - public: Lidar(Lidar &&_lidar) noexcept; - - /// \brief Destructor - public: ~Lidar(); - - /// \brief Assignment operator - /// \param[in] _lidar The lidar to set values from. - /// \return *this - public: Lidar &operator=(const Lidar &_lidar); - - /// \brief Move assignment operator - /// \param[in] _lidar The lidar to set values from. - /// \return *this - public: Lidar &operator=(Lidar &&_lidar) noexcept; - /// \brief Load the lidar based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -253,7 +233,7 @@ namespace sdf public: bool operator!=(const Lidar &_lidar) const; /// \brief Private data pointer. - private: LidarPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index c34f33979..885bf0465 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -21,6 +21,7 @@ #include #include #include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" @@ -35,8 +36,8 @@ namespace sdf // // Forward declare private data class. - class LightPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum LightType /// \brief The set of light types. INVALID indicates that light type has @@ -64,27 +65,6 @@ namespace sdf /// \brief Default constructor public: Light(); - /// \brief Copy constructor - /// \param[in] _light Light to copy. - public: Light(const Light &_light); - - /// \brief Move constructor - /// \param[in] _light Light to move. - public: Light(Light &&_light) noexcept; - - /// \brief Destructor - public: ~Light(); - - /// \brief Move assignment operator. - /// \param[in] _light Light to move. - /// \return Reference to this. - public: Light &operator=(Light &&_light); - - /// \brief Assignment operator. - /// \param[in] _light The light to set values from. - /// \return *this - public: Light &operator=(const Light &_light); - /// \brief Load the light based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -107,7 +87,7 @@ namespace sdf /// \brief Set the name of the light. /// \param[in] _name Name of the light. - public: void SetName(const std::string &_name) const; + public: void SetName(const std::string &_name); /// \brief Get the pose of the light. This is the pose of the light /// as specified in SDF ( ... ), and is @@ -156,7 +136,7 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Diffuse color. - public: void SetDiffuse(const ignition::math::Color &_color) const; + public: void SetDiffuse(const ignition::math::Color &_color); /// \brief Get the specular color. The specular color is /// specified by a set of three numbers representing red/green/blue, @@ -168,7 +148,7 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Specular color. - public: void SetSpecular(const ignition::math::Color &_color) const; + public: void SetSpecular(const ignition::math::Color &_color); /// \brief Get the range of the light source in meters. /// \return Range of the light source in meters. @@ -273,12 +253,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph or World::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph or World::Load to call /// SetXmlParentName and SetPoseRelativeToGraph, @@ -288,7 +268,7 @@ namespace sdf friend class World; /// \brief Private data pointer. - private: LightPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 7b8532ac5..4dd1cc50c 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -35,38 +36,16 @@ namespace sdf // Forward declarations. class Collision; class Light; - class LinkPrivate; class Sensor; class Visual; - class LinkPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Link { /// \brief Default constructor public: Link(); - /// \brief Copy constructor - /// \param[in] _link Link to copy. - public: Link(const Link &_link); - - /// \brief Move constructor - /// \param[in] _link Link to move. - public: Link(Link &&_link) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _link Link to move. - /// \return Reference to this. - public: Link &operator=(Link &&_link); - - /// \brief Copy assignment operator. - /// \param[in] _link Link to copy. - /// \return Reference to this. - public: Link &operator=(const Link &_link); - - /// \brief Destructor - public: ~Link(); - /// \brief Load the link based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -83,7 +62,7 @@ namespace sdf /// \brief Set the name of the link. /// The name of a link must be unique within the scope of a Model. /// \param[in] _name Name of the link. - public: void SetName(const std::string &_name) const; + public: void SetName(const std::string &_name); /// \brief Get the number of visuals. /// \return Number of visuals contained in this Link object. @@ -225,12 +204,11 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load to call SetPoseRelativeToGraph. friend class Model; @@ -248,7 +226,7 @@ namespace sdf public: void SetEnableWind(bool _enableWind); /// \brief Private data pointer. - private: LinkPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Magnetometer.hh b/include/sdf/Magnetometer.hh index eb987df7a..642960dfe 100644 --- a/include/sdf/Magnetometer.hh +++ b/include/sdf/Magnetometer.hh @@ -17,6 +17,7 @@ #ifndef SDF_MAGNETOMETER_HH_ #define SDF_MAGNETOMETER_HH_ +#include #include #include #include @@ -27,7 +28,6 @@ namespace sdf // Inline bracke to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // - class MagnetometerPrivate; /// \brief Magnetometer contains information about a magnetometer sensor. /// This sensor can be attached to a link. @@ -36,27 +36,6 @@ namespace sdf /// \brief Default constructor public: Magnetometer(); - /// \brief Copy constructor - /// \param[in] _magnetometer Magnetometer to copy. - public: Magnetometer(const Magnetometer &_magnetometer); - - /// \brief Move constructor - /// \param[in] _magnetometer Magnetometer to move. - public: Magnetometer(Magnetometer &&_magnetometer) noexcept; - - /// \brief Destructor - public: ~Magnetometer(); - - /// \brief Assignment operator. - /// \param[in] _magnetometer The magnetometer to set values from. - /// \return *this - public: Magnetometer &operator=(const Magnetometer &_magnetometer); - - /// \brief Move assignment operator. - /// \param[in] _magnetometer The magnetometer to set values from. - /// \return *this - public: Magnetometer &operator=(Magnetometer &&_magnetometer); - /// \brief Load the magnetometer based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -107,7 +86,7 @@ namespace sdf public: bool operator!=(const Magnetometer &_mag) const; /// \brief Private data pointer. - private: MagnetometerPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index 9b3245554..0e1b3b8f1 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -18,6 +18,7 @@ #define SDF_MATERIAL_HH_ #include +#include #include "sdf/Element.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -30,7 +31,6 @@ namespace sdf // // Forward declarations. - class MaterialPrivate; class Pbr; enum class ShaderType : int @@ -47,27 +47,6 @@ namespace sdf /// \brief Default constructor public: Material(); - /// \brief Copy constructor - /// \param[in] _material Material to copy. - public: Material(const Material &_material); - - /// \brief Move constructor - /// \param[in] _material Material to move. - public: Material(Material &&_material) noexcept; - - /// \brief Destructor - public: ~Material(); - - /// \brief Assignment operator. - /// \param[in] _material The material to set values from. - /// \return *this - public: Material &operator=(const Material &_material); - - /// \brief Move assignment operator. - /// \param[in] _material The material to move from. - /// \return *this - public: Material &operator=(Material &&_material); - /// \brief Load the material based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -86,7 +65,7 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Ambient color. - public: void SetAmbient(const ignition::math::Color &_color) const; + public: void SetAmbient(const ignition::math::Color &_color); /// \brief Get the diffuse color. The diffuse color is /// specified by a set of three numbers representing red/green/blue, @@ -98,7 +77,7 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Diffuse color. - public: void SetDiffuse(const ignition::math::Color &_color) const; + public: void SetDiffuse(const ignition::math::Color &_color); /// \brief Get the specular color. The specular color is /// specified by a set of three numbers representing red/green/blue, @@ -110,7 +89,7 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Specular color. - public: void SetSpecular(const ignition::math::Color &_color) const; + public: void SetSpecular(const ignition::math::Color &_color); /// \brief Get the emissive color. The emissive color is /// specified by a set of three numbers representing red/green/blue, @@ -122,7 +101,15 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Emissive color. - public: void SetEmissive(const ignition::math::Color &_color) const; + public: void SetEmissive(const ignition::math::Color &_color); + + /// \brief Get render order + /// \return Render order + public: float RenderOrder() const; + + /// \brief Set render order + /// \param[in] _renderOrder render order + public: void SetRenderOrder(const float _renderOrder); /// \brief Get whether dynamic lighting is enabled. The default /// value is true. @@ -133,6 +120,15 @@ namespace sdf /// \param[in] _lighting False disables dynamic lighting. public: void SetLighting(const bool _lighting); + /// \brief Get whether double sided material is enabled. The default + /// value is false. + /// \return False if double sided material should be disabled. + public: bool DoubleSided() const; + + /// \brief Set whether double sided material is enabled. + /// \param[in] _lighting False disables double sided material. + public: void SetDoubleSided(bool _doubleSided); + /// \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 @@ -184,10 +180,18 @@ namespace sdf /// \brief Get the Physically Based Rendering (PBR) material /// \return Pointer to the PBR material. Null if it does not exist. - public: Pbr *PbrMaterial() const; + public: const Pbr *PbrMaterial() const; + + /// \brief The path to the file where this element was loaded from. + /// \return Full path to the file on disk. + public: const std::string &FilePath() const; + + /// \brief Set the path to the file where this element was loaded from. + /// \paramp[in] _filePath Full path to the file on disk. + public: void SetFilePath(const std::string &_filePath); /// \brief Private data pointer. - private: MaterialPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 1e61d8c2c..b75349200 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -29,9 +30,6 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // - // Forward declare private data class. - class MeshPrivate; - /// \brief Mesh represents a mesh shape, and is usually accessed through a /// Geometry. class SDFORMAT_VISIBLE Mesh @@ -39,27 +37,6 @@ namespace sdf /// \brief Constructor public: Mesh(); - /// \brief Copy constructor - /// \param[in] _mesh Mesh to copy. - public: Mesh(const Mesh &_mesh); - - /// \brief Move constructor - /// \param[in] _mesh Mesh to move. - public: Mesh(Mesh &&_mesh) noexcept; - - /// \brief Destructor - public: virtual ~Mesh(); - - /// \brief Move assignment operator. - /// \param[in] _mesh Mesh to move. - /// \return Reference to this. - public: Mesh &operator=(Mesh &&_mesh); - - /// \brief Copy Assignment operator. - /// \param[in] _mesh The mesh to set values from. - /// \return *this - public: Mesh &operator=(const Mesh &_mesh); - /// \brief Load the mesh geometry based on a element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. @@ -121,7 +98,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Private data pointer. - private: MeshPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 81d7a474d..83a37d93e 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -19,7 +19,9 @@ #include #include +#include #include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -36,35 +38,15 @@ namespace sdf class Frame; class Joint; class Link; - class ModelPrivate; struct PoseRelativeToGraph; + struct FrameAttachedToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Model { /// \brief Default constructor public: Model(); - /// \brief Copy constructor - /// \param[in] _model Model to copy. - public: Model(const Model &_model); - - /// \brief Move constructor - /// \param[in] _model Model to move. - public: Model(Model &&_model) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _model Model to move. - /// \return Reference to this. - public: Model &operator=(Model &&_model); - - /// \brief Copy assignment operator. - /// \param[in] _model Model to copy. - /// \return Reference to this. - public: Model &operator=(const Model &_model); - - /// \brief Destructor - public: ~Model(); - /// \brief Load the model based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -131,11 +113,14 @@ namespace sdf /// should be subject to wind. public: void SetEnableWind(bool _enableWind); - /// \brief Get the number of links. + /// \brief Get the number of links that are immediate (not nested) children + /// of this Model object. + /// \remark LinkByName() can find links that are not immediate children of + /// this Model object. /// \return Number of links contained in this Model object. public: uint64_t LinkCount() const; - /// \brief Get a link based on an index. + /// \brief Get an immediate (not nested) child link based on an index. /// \param[in] _index Index of the link. The index should be in the /// range [0..LinkCount()). /// \return Pointer to the link. Nullptr if the index does not exist. @@ -144,19 +129,26 @@ namespace sdf /// \brief Get a link based on a name. /// \param[in] _name Name of the link. + /// To get a link in a nested model, prefix the link name with the + /// sequence of nested models containing this link, delimited by "::". /// \return Pointer to the link. Nullptr if the name does not exist. public: const Link *LinkByName(const std::string &_name) const; /// \brief Get whether a link name exists. /// \param[in] _name Name of the link to check. + /// To check for a link in a nested model, prefix the link name with + /// the sequence of nested models containing this link, delimited by "::". /// \return True if there exists a link with the given name. public: bool LinkNameExists(const std::string &_name) const; - /// \brief Get the number of joints. + /// \brief Get the number of joints that are immediate (not nested) children + /// of this Model object. + /// \remark JointByName() can find joints that are not immediate children of + /// this Model object. /// \return Number of joints contained in this Model object. public: uint64_t JointCount() const; - /// \brief Get a joint based on an index. + /// \brief Get an immediate (not nested) child joint based on an index. /// \param[in] _index Index of the joint. The index should be in the /// range [0..JointCount()). /// \return Pointer to the joint. Nullptr if the index does not exist. @@ -165,21 +157,29 @@ namespace sdf /// \brief Get whether a joint name exists. /// \param[in] _name Name of the joint to check. + /// To check for a joint in a nested model, prefix the joint name with + /// the sequence of nested models containing this joint, delimited by "::". /// \return True if there exists a joint with the given name. public: bool JointNameExists(const std::string &_name) const; /// \brief Get a joint based on a name. /// \param[in] _name Name of the joint. + /// To get a joint in a nested model, prefix the joint name with the + /// sequence of nested models containing this joint, delimited by "::". /// \return Pointer to the joint. Nullptr if a joint with the given name /// does not exist. /// \sa bool JointNameExists(const std::string &_name) const public: const Joint *JointByName(const std::string &_name) const; - /// \brief Get the number of explicit frames. + /// \brief Get the number of explicit frames that are immediate (not nested) + /// children of this Model object. + /// \remark FrameByName() can find explicit frames that are not immediate + /// children of this Model object. /// \return Number of explicit frames contained in this Model object. public: uint64_t FrameCount() const; - /// \brief Get an explicit frame based on an index. + /// \brief Get an immediate (not nested) child explicit frame based on an + /// index. /// \param[in] _index Index of the explicit frame. The index should be in /// the range [0..FrameCount()). /// \return Pointer to the explicit frame. Nullptr if the index does not @@ -189,15 +189,50 @@ namespace sdf /// \brief Get an explicit frame based on a name. /// \param[in] _name Name of the explicit frame. + /// To get a frame in a nested model, prefix the frame name with the + /// sequence of nested models containing this frame, delimited by "::". /// \return Pointer to the explicit frame. Nullptr if the name does not /// exist. public: const Frame *FrameByName(const std::string &_name) const; /// \brief Get whether an explicit frame name exists. /// \param[in] _name Name of the explicit frame to check. + /// To check for a frame in a nested model, prefix the frame name with + /// the sequence of nested models containing this frame, delimited by "::". /// \return True if there exists an explicit frame with the given name. public: bool FrameNameExists(const std::string &_name) const; + /// \brief Get the number of nested models that are immediate (not + /// recursively nested) children of this Model object. + /// \remark ModelByName() can find nested models that are not immediate + /// children of this Model object. + /// \return Number of nested models contained in this Model object. + public: uint64_t ModelCount() const; + + /// \brief Get an immediate (not recursively nested) child model based on an + /// index. + /// \param[in] _index Index of the nested model. The index should be in the + /// range [0..ModelCount()). + /// \return Pointer to the model. Nullptr if the index does not exist. + /// \sa uint64_t ModelCount() const + public: const Model *ModelByIndex(const uint64_t _index) const; + + /// \brief Get whether a nested model name exists. + /// \param[in] _name Name of the nested model to check. + /// To check for a model nested in other models, prefix the model name + /// with the sequence of nested model names, delimited by "::". + /// \return True if there exists a nested model with the given name. + public: bool ModelNameExists(const std::string &_name) const; + + /// \brief Get a nested model based on a name. + /// \param[in] _name Name of the nested model. + /// To get a model nested in other models, prefix the model name + /// with the sequence of nested model names, delimited by "::". + /// \return Pointer to the model. Nullptr if a model with the given name + /// does not exist. + /// \sa bool ModelNameExists(const std::string &_name) const + public: const Model *ModelByName(const std::string &_name) const; + /// \brief Get the pose of the model. This is the pose of the model /// as specified in SDF ( ... ), and is /// typically used to express the position and rotation of a model in a @@ -215,12 +250,14 @@ namespace sdf public: const Link *CanonicalLink() const; /// \brief Get the name of the model's canonical link. An empty value - /// indicates that the first link in the model is the canonical link. + /// indicates that the first link in the model or the first link found + /// in a depth first search of nested models is the canonical link. /// \return The name of the canonical link. public: const std::string &CanonicalLinkName() const; /// \brief Set the name of the model's canonical link. An empty value - /// indicates that the first link in the model is the canonical link. + /// indicates that the first link in the model or the first link found + /// in a depth first search of nested models is the canonical link. /// \param[in] _canonicalLink The name of the canonical link. public: void SetCanonicalLinkName(const std::string &_canonicalLink); @@ -247,18 +284,46 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// World::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Get the name of the placement frame of the model. + /// \return Name of the placement frame attribute of the model. + public: const std::string &PlacementFrameName() const; + + /// \brief Set the name of the placement frame of the model. + /// The specified placement frame must exist within the model. + /// \param[in] _name Name of the placement frame. + public: void SetPlacementFrameName(const std::string &_name); + + /// \brief Get the model's canonical link and the nested name of the link + /// relative to the current model, delimited by "::". + /// \return An immutable pointer to the canonical link and the nested + /// name of the link relative to the current model. + public: std::pair CanonicalLinkAndRelativeName() + const; + + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Root::Load or + /// World::SetPoseRelativeToGraph if this is a standalone model and + /// Model::SetPoseRelativeToGraph if this is a nested model. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); - - /// \brief Allow World::Load to call SetPoseRelativeToGraph. + sdf::ScopedGraph _graph); + + /// \brief Give the scoped FrameAttachedToGraph to be used for resolving + /// attached bodies. This is private and is intended to be called by + /// Root::Load or World::SetFrameAttachedToGraph if this is a standalone + /// model and Model::SetFrameAttachedToGraph if this is a nested model. + /// \param[in] _graph scoped FrameAttachedToGraph object. + private: void SetFrameAttachedToGraph( + sdf::ScopedGraph _graph); + + /// \brief Allow Root::Load, World::SetPoseRelativeToGraph, or + /// World::SetFrameAttachedToGraph to call SetPoseRelativeToGraph and + /// SetFrameAttachedToGraph + friend class Root; friend class World; /// \brief Private data pointer. - private: ModelPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Noise.hh b/include/sdf/Noise.hh index a51b92763..c0d8382f8 100644 --- a/include/sdf/Noise.hh +++ b/include/sdf/Noise.hh @@ -17,6 +17,7 @@ #ifndef SDF_NOISE_HH_ #define SDF_NOISE_HH_ +#include #include #include #include @@ -25,9 +26,6 @@ namespace sdf { // Inline bracke to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // Forward declare private data class. - class NoisePrivate; - /// \enum NoiseType /// \brief The set of noise types. enum class NoiseType @@ -51,27 +49,6 @@ namespace sdf /// \brief Default constructor public: Noise(); - /// \brief Copy constructor - /// \param[in] _noise Noise to copy. - public: Noise(const Noise &_noise); - - /// \brief Move constructor - /// \param[in] _noise Noise to move. - public: Noise(Noise &&_noise) noexcept; - - /// \brief Destructor - public: ~Noise(); - - /// \brief Assignment operator. - /// \param[in] _noise The noise to set values from. - /// \return *this - public: Noise &operator=(const Noise &_noise); - - /// \brief Move assignment operator. - /// \param[in] _noise The noise to set values from. - /// \return *this - public: Noise &operator=(Noise &&_noise); - /// \brief Return true if both Noise objects contain the same values. /// \param[_in] _noise Noise value to compare. /// \return True if 'this' == _noise. @@ -186,7 +163,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Private data pointer. - private: NoisePrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Pbr.hh b/include/sdf/Pbr.hh index 564315d85..acc906dc8 100644 --- a/include/sdf/Pbr.hh +++ b/include/sdf/Pbr.hh @@ -18,6 +18,7 @@ #define SDF_PBR_HH_ #include +#include #include "sdf/Element.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -28,9 +29,6 @@ namespace sdf // Inline bracke to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // - // Forward declarations. - class PbrPrivate; - class PbrWorkflowPrivate; /// \brief Type of PBR workflow. enum class PbrWorkflowType : int @@ -62,17 +60,6 @@ namespace sdf /// \brief Default constructor public: PbrWorkflow(); - /// \brief Copy constructor - /// \param[in] _workflow Workflow to copy. - public: PbrWorkflow(const PbrWorkflow &_workflow); - - /// \brief Move constructor - /// \param[in] _workflow to move. - public: PbrWorkflow(PbrWorkflow &&_workflow) noexcept; - - /// \brief Destructor - public: ~PbrWorkflow(); - /// \brief Load the pbr workflow based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -81,16 +68,6 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); - /// \brief Assignment operator. - /// \param[in] _workflow The workflow to set values from. - /// \return *this - public: PbrWorkflow &operator=(const PbrWorkflow &_workflow); - - /// \brief Move assignment operator. - /// \param[in] _workflow The workflow to move from. - /// \return *this - public: PbrWorkflow &operator=(PbrWorkflow &&_workflow); - /// \brief Return true if both PbrWorkflow objects contain the same values. /// \param[_in] _workflow PbrWorkflow value to compare. /// \returen True if 'this' == _workflow. @@ -179,6 +156,21 @@ namespace sdf /// \param[in] _map Filename of the emissive map. public: void SetEmissiveMap(const std::string &_map); + /// \brief Get the light map filename. This will be an empty string + /// if an light map has not been set. + /// \return Filename of the light map, or empty string if a light + /// map has not been specified. + public: std::string LightMap() const; + + /// \brief Set the light map filename. + /// \param[in] _map Filename of the light map. + /// \param[in] _uvSet Index of the light map texture coordinate set + public: void SetLightMap(const std::string &_map, unsigned int _uvSet = 0u); + + /// \brief Get the light map texture coordinate set. + /// \return Index of the texture coordinate set + public: unsigned int LightMapTexCoordSet() const; + /// \brief Get the metalness value of the material for metal workflow /// \return metalness value of the material public: double Metalness() const; @@ -238,7 +230,7 @@ namespace sdf public: void SetType(PbrWorkflowType _type); /// \brief Private data pointer. - private: PbrWorkflowPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; /// \brief This class provides access to Physically-Based-Rendering (PBR) @@ -248,27 +240,6 @@ namespace sdf /// \brief Default constructor public: Pbr(); - /// \brief Copy constructor - /// \param[in] _pbr Pbr to copy. - public: Pbr(const Pbr &_pbr); - - /// \brief Move constructor - /// \param[in] _pbr Pbr to move. - public: Pbr(Pbr &&_pbr) noexcept; - - /// \brief Destructor - public: ~Pbr(); - - /// \brief Assignment operator. - /// \param[in] _pbr The pbr to set values from. - /// \return *this - public: Pbr &operator=(const Pbr &_pbr); - - /// \brief Move assignment operator. - /// \param[in] _pbr The pbr to move values from. - /// \return *this - public: Pbr &operator=(Pbr &&_pbr); - /// \brief Load the pbr based on an element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -288,10 +259,10 @@ namespace sdf /// \param[in] _type Type of PBR workflow /// \return Workflow of the specified type. /// \sa PbrWorkflowType - public: PbrWorkflow *Workflow(PbrWorkflowType _type) const; + public: const PbrWorkflow *Workflow(PbrWorkflowType _type) const; /// \brief Private data pointer. - private: PbrPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Physics.hh b/include/sdf/Physics.hh index bcc1385c5..58331113b 100644 --- a/include/sdf/Physics.hh +++ b/include/sdf/Physics.hh @@ -18,6 +18,7 @@ #define SDF_PHYSICS_HH_ #include +#include #include "sdf/Element.hh" #include "sdf/Types.hh" @@ -30,9 +31,6 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // - // Forward declare private data class. - class PhysicsPrivate; - /// \brief The physics element specifies the type and properties of a /// dynamics engine. class SDFORMAT_VISIBLE Physics @@ -40,27 +38,6 @@ namespace sdf /// \brief Default constructor public: Physics(); - /// \brief Copy constructor - /// \param[in] _physics Physics to copy. - public: Physics(const Physics &_physics); - - /// \brief Move constructor - /// \param[in] _physics Physics to move. - public: Physics(Physics &&_physics) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _physics Physics to move. - /// \return Reference to this. - public: Physics &operator=(Physics &&_physics); - - /// \brief Copy assignment operator. - /// \param[in] _physics Physics to copy. - /// \return Reference to this. - public: Physics &operator=(const Physics &_physics); - - /// \brief Destructor - public: ~Physics(); - /// \brief Load the physics based on an element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -75,7 +52,7 @@ namespace sdf /// \brief Set the name of this set of physics parameters. /// \param[in] _name Name of the physics profile. - public: void SetName(const std::string &_name) const; + public: void SetName(const std::string &_name); /// \brief Get a pointer to the SDF element that was used during /// load. @@ -93,7 +70,7 @@ namespace sdf /// \brief Set whether this physics profile is the default. /// \param[in] _default True to make this profile default. - public: void SetDefault(const bool _default) const; + public: void SetDefault(const bool _default); /// \brief Get the physics profile dynamics engine type. /// Current options are ode, bullet, simbody and dart. Defaults to ode if @@ -126,7 +103,7 @@ namespace sdf public: void SetRealTimeFactor(const double _factor); /// \brief Private data pointer. - private: PhysicsPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Plane.hh b/include/sdf/Plane.hh index fea04517a..c0ea5a4d7 100644 --- a/include/sdf/Plane.hh +++ b/include/sdf/Plane.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -30,9 +31,6 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // - // Forward declare private data class. - class PlanePrivate; - /// \brief Plane represents a plane shape, and is usually accessed through a /// Geometry. class SDFORMAT_VISIBLE Plane @@ -40,27 +38,6 @@ namespace sdf /// \brief Constructor public: Plane(); - /// \brief Copy constructor - /// \param[in] _plane Plane to copy. - public: Plane(const Plane &_plane); - - /// \brief Move constructor - /// \param[in] _plane Plane to move. - public: Plane(Plane &&_plane) noexcept; - - /// \brief Destructor - public: virtual ~Plane(); - - /// \brief Move assignment operator. - /// \param[in] _plane Plane to move. - /// \return Reference to this. - public: Plane &operator=(Plane &&_plane); - - /// \brief Assignment operator. - /// \param[in] _plane The plane to set values from. - /// \return *this - public: Plane &operator=(const Plane &_plane); - /// \brief Load the plane geometry based on a element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. @@ -104,7 +81,7 @@ namespace sdf public: ignition::math::Planed &Shape(); /// \brief Private data pointer. - private: PlanePrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index ae88cd037..895ccde19 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -18,6 +18,7 @@ #define SDF_ROOT_HH_ #include +#include #include "sdf/SDFImpl.hh" #include "sdf/Types.hh" @@ -34,7 +35,6 @@ namespace sdf class Actor; class Light; class Model; - class RootPrivate; class World; /// \brief Root class that acts as an entry point to the SDF document @@ -56,9 +56,6 @@ namespace sdf /// \brief Default constructor public: Root(); - /// \brief Destructor - public: ~Root(); - /// \brief Parse the given SDF file, and generate objects based on types /// specified in the SDF file. /// \param[in] _filename Name of the SDF file to parse. @@ -107,53 +104,95 @@ namespace sdf /// \return True if there exists a world with the given name. public: bool WorldNameExists(const std::string &_name) const; - /// \brief Get the number of models. + /// \brief Get the number of models that are immediate (not nested) children + /// of this Root object. /// \return Number of models contained in this Root object. - public: uint64_t ModelCount() const; + public: uint64_t ModelCount() const SDF_DEPRECATED(11.0); /// \brief Get a model based on an index. /// \param[in] _index Index of the model. The index should be in the /// range [0..ModelCount()). /// \return Pointer to the model. Nullptr if the index does not exist. /// \sa uint64_t ModelCount() const - public: const Model *ModelByIndex(const uint64_t _index) const; + public: const sdf::Model *ModelByIndex(const uint64_t _index) const + SDF_DEPRECATED(11.0); /// \brief Get whether a model name exists. /// \param[in] _name Name of the model to check. /// \return True if there exists a model with the given name. - public: bool ModelNameExists(const std::string &_name) const; + public: bool ModelNameExists(const std::string &_name) const + SDF_DEPRECATED(11.0); + + /// \brief Get a pointer to the model object if it exists. + /// + /// If there is more than one model, this will return the first element. + /// This method is preferred to ModelByIndex, as its behavior is + /// consistent with the planned future API. Having more than one Model, or + /// more than one of Model/Actor/Light is now considered deprecated and + /// should not be relied upon going forward. + /// + /// \return A pointer to the model, nullptr if it doesn't exist + public: const sdf::Model *Model() const; /// \brief Get the number of lights. /// \return Number of lights contained in this Root object. - public: uint64_t LightCount() const; + public: uint64_t LightCount() const SDF_DEPRECATED(11.0); /// \brief Get a light based on an index. /// \param[in] _index Index of the light. The index should be in the /// range [0..LightCount()). /// \return Pointer to the light. Nullptr if the index does not exist. /// \sa uint64_t LightCount() const - public: const Light *LightByIndex(const uint64_t _index) const; + public: const sdf::Light *LightByIndex(const uint64_t _index) const + SDF_DEPRECATED(11.0); /// \brief Get whether a light name exists. /// \param[in] _name Name of the light to check. /// \return True if there exists a light with the given name. - public: bool LightNameExists(const std::string &_name) const; + public: bool LightNameExists(const std::string &_name) const + SDF_DEPRECATED(11.0); + + /// \brief Get a pointer to the light object if it exists. + /// + /// If there is more than one light, this will return the first element. If + /// there is already a model element, this will return null. + /// This method is preferred to LightByIndex, as its behavior is + /// consistent with the planned future API. Having more than one Light, or + /// more than one of Model/Actor/Light is now considered deprecated and + /// should not be relied upon going forward. + /// + /// \return A pointer to the light, nullptr if it doesn't exist + public: const sdf::Light *Light() const; /// \brief Get the number of actors. /// \return Number of actors contained in this Root object. - public: uint64_t ActorCount() const; + public: uint64_t ActorCount() const SDF_DEPRECATED(11.0); /// \brief Get an actor based on an index. /// \param[in] _index Index of the actor. The actor should be in the /// range [0..ActorCount()). /// \return Pointer to the actor. Nullptr if the index does not exist. /// \sa uint64_t ActorCount() const - public: const Actor *ActorByIndex(const uint64_t _index) const; + public: const sdf::Actor *ActorByIndex(const uint64_t _index) const + SDF_DEPRECATED(11.0); /// \brief Get whether an actor name exists. /// \param[in] _name Name of the actor to check. /// \return True if there exists an actor with the given name. - public: bool ActorNameExists(const std::string &_name) const; + public: bool ActorNameExists(const std::string &_name) const + SDF_DEPRECATED(11.0); + + /// \brief Get a pointer to the actor object if it exists. + /// + /// If there is more than one actor, this will return the first element. If + /// there is already a model or light element, this will return null. + /// This method is preferred to ActorByIndex, as its behavior is + /// consistent with the planned future API. Having more than one Actor, or + /// more than one of Model/Actor/Light is now considered deprecated and + /// should not be relied upon going forward. + /// + /// \return A pointer to the actor, nullptr if it doesn't exist + public: const sdf::Actor *Actor() const; /// \brief Get a pointer to the SDF element that was generated during /// load. @@ -162,7 +201,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Private data pointer - private: RootPrivate *dataPtr = nullptr; + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index b4fea303b..2a137760c 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -18,8 +18,10 @@ #define SDF_SCENE_HH_ #include +#include #include "sdf/Element.hh" +#include "sdf/Sky.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" @@ -28,37 +30,11 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - - // Forward declarations. - class ScenePrivate; - class SDFORMAT_VISIBLE Scene { /// \brief Default constructor public: Scene(); - /// \brief Copy constructor - /// \param[in] _scene Scene element to copy. - public: Scene(const Scene &_scene); - - /// \brief Move constructor - /// \param[in] _scene Scene to move. - public: Scene(Scene &&_scene) noexcept; - - /// \brief Destructor - public: ~Scene(); - - /// \brief Assignment operator. - /// \param[in] _scene The scene to set values from. - /// \return *this - public: Scene &operator=(const Scene &_scene); - - /// \brief Move assignment operator. - /// \param[in] _workflow The scene to move from. - /// \return *this - public: Scene &operator=(Scene &&_scene); - /// \brief Load the scene based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -107,6 +83,14 @@ namespace sdf /// \param[in] enabled True to enable shadows public: void SetShadows(const bool _shadows); + /// \brief Set sky + /// \param[in] _sky Sky to set to + public: void SetSky(const Sky &_sky); + + /// \brief Get sky + /// \return Sky + public: const sdf::Sky *Sky() const; + /// \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 @@ -114,7 +98,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Private data pointer. - private: ScenePrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 6baa76035..cca0e35b6 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -40,8 +41,8 @@ namespace sdf // // Forward declare private data class. - class SemanticPosePrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief SemanticPose is a data structure that can be used by different /// DOM objects to resolve poses on a PoseRelativeToGraph. This object holds @@ -78,33 +79,30 @@ namespace sdf /// raw pose is applied. /// \param[in] _defaultResolveTo Default frame to resolve-to in Resolve() /// if no frame is specified. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph A scoped PoseRelativeToGraph object. private: SemanticPose( const ignition::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, - std::weak_ptr _graph); + const sdf::ScopedGraph &_graph); - /// \brief Destructor - public: ~SemanticPose(); - - /// \brief Copy constructor - /// \param[in] _semanticpose SemanticPose to copy. - public: SemanticPose(const SemanticPose &_semanticPose); - - /// \brief Move constructor - /// \param[in] _semanticpose SemanticPose to move. - public: SemanticPose(SemanticPose &&_semanticPose) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _semanticpose SemanticPose to move. - /// \return Reference to this. - public: SemanticPose &operator=(SemanticPose &&_semanticPose); - - /// \brief Copy assignment operator. - /// \param[in] _semanticpose SemanticPose to copy. - /// \return Reference to this. - public: SemanticPose &operator=(const SemanticPose &_semanticPose); + /// \brief Private constructor that is used by object that represent a frame + /// in the PoseRelativeTo graph. Examples are Model, Frame, Link and not + /// Collision or Visual. + /// \param[in] _name Name of object. This should also be the name of the + /// frame represented by this object in the PoseRelativeTo graph. + /// \param[in] _pose Raw pose of object. + /// \param[in] _relativeTo Name of frame in graph relative-to which the + /// raw pose is applied. + /// \param[in] _defaultResolveTo Default frame to resolve-to in Resolve() + /// if no frame is specified. + /// \param[in] _graph A scoped PoseRelativeToGraph object. + private: SemanticPose( + const std::string &_name, + const ignition::math::Pose3d &_pose, + const std::string &_relativeTo, + const std::string &_defaultResolveTo, + const sdf::ScopedGraph &_graph); friend class Collision; friend class Frame; @@ -116,7 +114,7 @@ namespace sdf friend class Visual; /// \brief Private data pointer. - private: std::unique_ptr dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 33a07fa16..748b6ffe5 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -20,9 +20,11 @@ #include #include #include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" +#include "sdf/sdf_config.h" #include "sdf/system_util.hh" namespace sdf @@ -35,11 +37,12 @@ namespace sdf class AirPressure; class Altimeter; class Camera; + class ForceTorque; class Imu; class Lidar; class Magnetometer; - class SensorPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum SensorType /// \brief The set of sensor types. @@ -118,17 +121,6 @@ namespace sdf /// \brief Default constructor public: Sensor(); - /// \brief Copy constructor - /// \param[in] _sensor Sensor to copy. - public: Sensor(const Sensor &_sensor); - - /// \brief Move constructor - /// \param[in] _sensor Sensor to move. - public: Sensor(Sensor &&_sensor) noexcept; - - /// \brief Destructor - public: ~Sensor(); - /// \brief Load the sensor based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -222,16 +214,6 @@ namespace sdf /// \param[in] _rate The update rate in Hz. public: void SetUpdateRate(double _hz); - /// \brief Assignment operator. - /// \param[in] _sensor The sensor to set values from. - /// \return *this - public: Sensor &operator=(const Sensor &_sensor); - - /// \brief Move assignment operator. - /// \param[in] _sensor The sensor to set values from. - /// \return *this - public: Sensor &operator=(Sensor &&_sensor); - /// \brief Return true if both Sensor objects contain the same values. /// \param[_in] _sensor Sensor object to compare. /// \returen True if 'this' == _sensor. @@ -287,6 +269,17 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; + /// \brief Set the force torque sensor. + /// \param[in] _ft The force torque sensor. + public: void SetForceTorqueSensor(const ForceTorque &_ft); + + /// \brief Get a pointer to a force torque sensor, or nullptr if the sensor + /// does not contain a force torque sensor. + /// \return Pointer to the force torque sensor, or nullptr if the sensor + /// is not a force torque sensor. + /// \sa SensorType Type() const + public: const ForceTorque *ForceTorqueSensor() const; + /// \brief Set the IMU sensor. /// \param[in] _imu The IMU sensor. public: void SetImuSensor(const Imu &_imu); @@ -315,12 +308,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is @@ -328,7 +321,7 @@ namespace sdf friend class Link; /// \brief Private data pointer. - private: SensorPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Sky.hh b/include/sdf/Sky.hh new file mode 100644 index 000000000..cd76cfd47 --- /dev/null +++ b/include/sdf/Sky.hh @@ -0,0 +1,121 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef SDF_SKY_HH_ +#define SDF_SKY_HH_ + +#include +#include + +#include "sdf/Element.hh" +#include "sdf/Types.hh" +#include "sdf/sdf_config.h" +#include "sdf/system_util.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + class SDFORMAT_VISIBLE Sky + { + /// \brief Default constructor + public: Sky(); + + /// \brief Get time of day [0..24] + /// \return Time of day + public: double Time() const; + + /// \brief Set time of day + /// \param[in] _time Time of day [0..24] + public: void SetTime(double _time); + + /// \brief Get sunrise time + /// \return sunrise time [0..24] + public: double Sunrise() const; + + /// \brief Set Sunrise time + /// \param[in] _time Sunrise time [0..24] + public: void SetSunrise(double _time); + + /// \brief Get sunset time + /// \return sunset time [0..24] + public: double Sunset() const; + + /// \brief Set Sunset time + /// \param[in] _time Sunset time [0..24] + public: void SetSunset(double _time); + + /// \brief Get cloud speed + /// \return cloud speed in meters per second + public: double CloudSpeed() const; + + /// \brief Set cloud speed + /// \param[in] _speed cloud speed in meters per second. + public: void SetCloudSpeed(double _speed); + + /// \brief Get cloud direction angle (angle around up axis) + /// \return cloud direction angle in world frame + public: ignition::math::Angle CloudDirection() const; + + /// \brief Set cloud direction angle (angle around up axis) + /// \param[in] _angle Cloud direction angle in world frame. + public: void SetCloudDirection(const ignition::math::Angle &_angle); + + /// \brief Get cloud humidity + /// \return cloud humidity [0..1] + public: double CloudHumidity() const; + + /// \brief Set cloud humidity + /// \param[in] _humidity cloud humidity [0..1] + public: void SetCloudHumidity(double _humidity); + + /// \brief Get cloud mean size + /// \return cloud mean size [0..1] + public: double CloudMeanSize() const; + + /// \brief Set cloud mean siz + /// \param[in] _size cloud mean size [0..1] + public: void SetCloudMeanSize(double _size); + + /// \brief Get cloud ambient color + /// \return cloud ambient color + public: ignition::math::Color CloudAmbient() const; + + /// \brief Set cloud ambient color + /// \param[in] _ambient cloud ambient color + public: void SetCloudAmbient(const ignition::math::Color &_ambient); + + /// \brief Load the sky based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \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 + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } +} +#endif diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 1fff9ec0c..10d91b897 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -18,6 +18,7 @@ #define SDF_SPHERE_HH_ #include +#include #include #include @@ -27,11 +28,6 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - - // Forward declare private data class. - class SpherePrivate; - /// \brief Sphere represents a sphere shape, and is usually accessed through a /// Geometry. class SDFORMAT_VISIBLE Sphere @@ -39,27 +35,6 @@ namespace sdf /// \brief Constructor public: Sphere(); - /// \brief Copy constructor - /// \param[in] _sphere Sphere to copy. - public: Sphere(const Sphere &_sphere); - - /// \brief Move constructor - /// \param[in] _sphere Sphere to move. - public: Sphere(Sphere &&_sphere) noexcept; - - /// \brief Destructor - public: virtual ~Sphere(); - - /// \brief Assignment operator. - /// \param[in] _sphere The sphere to set values from. - /// \return *this - public: Sphere &operator=(const Sphere &_sphere); - - /// \brief Move assignment operator. - /// \param[in] _sphere Sphere to move. - /// \return Reference to this. - public: Sphere &operator=(Sphere &&_sphere); - /// \brief Load the sphere geometry based on a element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. @@ -91,7 +66,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Private data pointer. - private: SpherePrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index b4930a457..dd39aea94 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -17,6 +17,7 @@ #ifndef SDF_SURFACE_HH_ #define SDF_SURFACE_HH_ +#include #include "sdf/Element.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -26,38 +27,12 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - // Forward declaration. - class ContactPrivate; - class SurfacePrivate; - /// \brief Contact information for a surface. class SDFORMAT_VISIBLE Contact { /// \brief Default constructor public: Contact(); - /// \brief Copy constructor - /// \param[in] _contact Contact to copy. - public: Contact(const Contact &_contact); - - /// \brief Move constructor - /// \param[in] _contact Contact to move. - public: Contact(Contact &&_contact) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _contact Contact to move. - /// \return Reference to this. - public: Contact &operator=(Contact &&_contact); - - /// \brief Copy assignment operator. - /// \param[in] _contact Contact to copy. - /// \return Reference to this. - public: Contact &operator=(const Contact &_contact); - - /// \brief Destructor - public: ~Contact(); - /// \brief Load the contact based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -80,7 +55,7 @@ namespace sdf public: void SetCollideBitmask(const uint16_t _bitmask); /// \brief Private data pointer. - private: ContactPrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; /// \brief Surface information for a collision. @@ -89,27 +64,6 @@ namespace sdf /// \brief Default constructor public: Surface(); - /// \brief Copy constructor - /// \param[in] _surface Surface to copy. - public: Surface(const Surface &_surface); - - /// \brief Move constructor - /// \param[in] _surface Surface to move. - public: Surface(Surface &&_surface) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _surface Surface to move. - /// \return Reference to this. - public: Surface &operator=(Surface &&_surface); - - /// \brief Copy assignment operator. - /// \param[in] _surface Surface to copy. - /// \return Reference to this. - public: Surface &operator=(const Surface &_surface); - - /// \brief Destructor - public: ~Surface(); - /// \brief Load the surface based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -127,14 +81,14 @@ namespace sdf /// \brief Get the associated contact object /// \returns Pointer to the associated Contact object, /// nullptr if the Surface doesn't contain a Contact element. - public: sdf::Contact *Contact() const; + public: const sdf::Contact *Contact() const; /// \brief Set the associated contact object. /// \param[in] _cont The contact object. public: void SetContact(const sdf::Contact &_contact); /// \brief Private data pointer. - private: SurfacePrivate *dataPtr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh index 607f7712f..d1bc921a4 100644 --- a/include/sdf/Types.hh +++ b/include/sdf/Types.hh @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -60,6 +61,8 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // + const std::string kSdfScopeDelimiter = "::"; + /// \brief Split a string using the delimiter in splitter. /// \param[in] str The string to split. /// \param[in] splitter The delimiter to use. @@ -94,6 +97,13 @@ namespace sdf /// \brief A vector of Error. using Errors = std::vector; + /// \brief Output operator for a collection of errors. + /// \param[in,out] _out The output stream. + /// \param[in] _err The errors to output. + /// \return Reference to the given output stream + SDFORMAT_VISIBLE std::ostream &operator<<( + std::ostream &_out, const sdf::Errors &_errs); + /// \brief Defines a color class SDFORMAT_VISIBLE Color { @@ -221,6 +231,23 @@ namespace sdf /// \param[in] _in String to convert to lowercase /// \return Lowercase equilvalent of _in. std::string SDFORMAT_VISIBLE lowercase(const std::string &_in); + + /// \brief Split a name into a two strings based on the '::' delimeter + /// \param[in] _absoluteName The fully qualified absolute name + /// \return A pair with the absolute name minus the leaf node name, and the + /// leaf name + SDFORMAT_VISIBLE + std::pair SplitName( + const std::string &_absoluteName); + + /// \brief Join two strings with the '::' delimiter. + /// This checks for edge cases and is safe to use with any valid names + /// \param[in] _scopeName the left-hand-side component + /// \param[in] _localName the right-hand-side component + /// \return A full string with the names joined by the '::' delimeter. + SDFORMAT_VISIBLE + std::string JoinName( + const std::string &_scopeName, const std::string &_localName); } } #endif diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index deff10053..454993857 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include "sdf/Box.hh" #include "sdf/Cylinder.hh" #include "sdf/Element.hh" @@ -38,36 +39,15 @@ namespace sdf // // Forward declarations. - class VisualPrivate; class Geometry; struct PoseRelativeToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Visual { /// \brief Default constructor public: Visual(); - /// \brief Copy constructor - /// \param[in] _visual Visual to copy. - public: Visual(const Visual &_visual); - - /// \brief Move constructor - /// \param[in] _visual Visual to move. - public: Visual(Visual &&_visual) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _visual Visual to move. - /// \return Reference to this. - public: Visual &operator=(Visual &&_visual); - - /// \brief Copy assignment operator. - /// \param[in] _visual Visual to copy. - /// \return Reference to this. - public: Visual &operator=(const Visual &_visual); - - /// \brief Destructor - public: ~Visual(); - /// \brief Load the visual based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -84,7 +64,7 @@ namespace sdf /// \brief Set the name of the visual. /// The name of the visual must be unique within the scope of a Link. /// \param[in] _name Name of the visual. - public: void SetName(const std::string &_name) const; + public: void SetName(const std::string &_name); /// \brief Get whether the visual casts shadows /// \return True if the visual casts shadows, false otherwise @@ -148,7 +128,7 @@ namespace sdf /// be a nullptr if material properties have not been set. /// \return Pointer to the visual's material properties. Nullptr /// indicates that material properties have not been set. - public: sdf::Material *Material() const; + public: const sdf::Material *Material() const; /// \brief Set the visual's material /// \param[in] _material The material of the visual object @@ -168,12 +148,12 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is @@ -181,7 +161,7 @@ namespace sdf friend class Link; /// \brief Private data pointer. - private: VisualPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 2f513eaaa..82387351f 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -19,6 +19,7 @@ #include #include +#include #include "sdf/Atmosphere.hh" #include "sdf/Element.hh" @@ -40,34 +41,15 @@ namespace sdf class Light; class Model; class Physics; - class WorldPrivate; + struct PoseRelativeToGraph; + struct FrameAttachedToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE World { /// \brief Default constructor public: World(); - /// \brief Copy constructor - /// \param[in] _world World to copy. - public: World(const World &_world); - - /// \brief Move constructor - /// \param[in] _world World to move. - public: World(World &&_world) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _world World to move. - /// \return Reference to this. - public: World &operator=(World &&_world); - - /// \brief Copy assignment operator. - /// \param[in] _world World to copy. - /// \return Reference to this. - public: World &operator=(const World &_world); - - /// \brief Destructor - public: ~World(); - /// \brief Load the world based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -82,7 +64,7 @@ namespace sdf /// \brief Set the name of the world. /// \param[in] _name Name of the world. - public: void SetName(const std::string &_name) const; + public: void SetName(const std::string &_name); /// \brief Get the audio device name. The audio device can be used to /// playback audio files. A value of "default" or an empty string @@ -136,11 +118,12 @@ namespace sdf /// \sa SphericalCoordinates public: void SetMagneticField(const ignition::math::Vector3d &_mag); - /// \brief Get the number of models. + /// \brief Get the number of models that are immediate (not nested) children + /// of this World object. /// \return Number of models contained in this World object. public: uint64_t ModelCount() const; - /// \brief Get a model based on an index. + /// \brief Get an immediate (not nested) child model based on an index. /// \param[in] _index Index of the model. The index should be in the /// range [0..ModelCount()). /// \return Pointer to the model. Nullptr if the index does not exist. @@ -173,11 +156,13 @@ namespace sdf /// \return True if there exists an actor with the given name. public: bool ActorNameExists(const std::string &_name) const; - /// \brief Get the number of explicit frames. + /// \brief Get the number of explicit frames that are immediate (not nested) + /// children of this World object. /// \return Number of explicit frames contained in this World object. public: uint64_t FrameCount() const; - /// \brief Get an explicit frame based on an index. + /// \brief Get an immediate (not nested) child explicit frame based on an + /// index. /// \param[in] _index Index of the explicit frame. The index should be in /// the range [0..FrameCount()). /// \return Pointer to the explicit frame. Nullptr if the index does not @@ -220,13 +205,13 @@ namespace sdf /// \brief Set the atmosphere model associated with this world. /// \param[in] _atmosphere The new atmosphere model for this world. - public: void SetAtmosphere(const sdf::Atmosphere &_atmosphere) const; + public: void SetAtmosphere(const sdf::Atmosphere &_atmosphere); /// \brief Get a pointer to the Gui associated with this /// world. A nullptr indicates that a Gui element has not been specified. /// \return Pointer to this world's Gui parameters. Nullptr inidicates /// that there are no Gui parameters. - public: sdf::Gui *Gui() const; + public: const sdf::Gui *Gui() const; /// \brief Set the Gui parameters associated with this world. /// \param[in] _gui The new Gui parameter for this world @@ -269,8 +254,26 @@ namespace sdf /// \return True if there exists a physics profile with the given name. public: bool PhysicsNameExists(const std::string &_name) const; + /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child + /// entities for resolving poses. This is private and is intended to be + /// called by Root::Load. + /// \param[in] _graph Scoped PoseRelativeToGraph object. + private: void SetPoseRelativeToGraph( + sdf::ScopedGraph _graph); + + /// \brief Give the Scoped FrameAttachedToGraph to be passed on to child + /// entities for attached bodes. This is private and is intended to be + /// called by Root::Load. + /// \param[in] _graph Scoped FrameAttachedToGraph object. + private: void SetFrameAttachedToGraph( + sdf::ScopedGraph _graph); + + /// \brief Allow Root::Load to call SetPoseRelativeToGraph and + /// SetFrameAttachedToGraph + friend class Root; + /// \brief Private data pointer. - private: WorldPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index d17bdcdb8..5e33a8ef7 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -185,31 +185,6 @@ namespace sdf SDFORMAT_VISIBLE std::string getModelFilePath(const std::string &_modelDirPath); - /// \brief Copy the contents of the first model element from one ElementPtr - /// to another ElementPtr, prepending the copied model name with `::` to - /// link and joint names, and apply the model pose to the copied link poses. - /// If //xyz/@expressed_in == "__model__" for the axes of any copied joints, - /// then apply the model pose rotation to those joint axes. - /// \param[in] _sdf ElementPtr for model into which the elements will be - /// copied. - /// \param[in] _includeSDF The first model element from this ElementPtr will - /// be copied to _sdf with the mentioned name and pose transformations. - SDFORMAT_VISIBLE - void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF); - - /// \brief Copy the contents of the first model element from one ElementPtr - /// to another ElementPtr, prepending the copied model name with `::` to - /// link and joint names, and apply the model pose to the copied link poses. - /// If //xyz/@expressed_in == "__model__" for the axes of any copied joints, - /// then apply the model pose rotation to those joint axes. - /// \param[in] _sdf ElementPtr for model into which the elements will be - /// copied. - /// \param[in] _includeSDF The first model element from this ElementPtr will - /// be copied to _sdf with the mentioned name and pose transformations. - /// \param[out] _errors Any errors will be appended to this variable. - SDFORMAT_VISIBLE - void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors); - /// \brief Convert an SDF file to a specific SDF version. /// \param[in] _filename Name of the SDF file to convert. /// \param[in] _version Version to convert _filename to. diff --git a/include/sdf/system_util.hh b/include/sdf/system_util.hh index a94b9f46e..c2488dbb9 100644 --- a/include/sdf/system_util.hh +++ b/include/sdf/system_util.hh @@ -26,20 +26,13 @@ */ #if defined _WIN32 || defined __CYGWIN__ - #ifdef BUILDING_DLL - #ifdef __GNUC__ - #define SDFORMAT_VISIBLE __attribute__ ((dllexport)) - #else - #define SDFORMAT_VISIBLE __declspec(dllexport) - #endif + #ifdef BUILDING_SDFORMAT_SHARED + #define SDFORMAT_VISIBLE __declspec(dllexport) + #elif !defined SDFORMAT_STATIC_DEFINE + #define SDFORMAT_VISIBLE __declspec(dllimport) #else - #ifdef __GNUC__ - #define SDFORMAT_VISIBLE __attribute__ ((dllimport)) - #else - #define SDFORMAT_VISIBLE __declspec(dllimport) - #endif + #define SDFORMAT_VISIBLE #endif - #define SDFORMAT_HIDDEN #else #if __GNUC__ >= 4 && !defined SDFORMAT_STATIC_DEFINE #define SDFORMAT_VISIBLE __attribute__ ((visibility ("default"))) diff --git a/sdf/1.2/visual.sdf b/sdf/1.2/visual.sdf index 6b4913260..550fe4448 100644 --- a/sdf/1.2/visual.sdf +++ b/sdf/1.2/visual.sdf @@ -41,7 +41,7 @@ - vertex, pixel, normal_map_objectspace, normal_map_tangentspace + vertex, pixel, normal_map_object_space, normal_map_tangent_space diff --git a/sdf/1.3/visual.sdf b/sdf/1.3/visual.sdf index 029ef22bd..10dfb3a56 100644 --- a/sdf/1.3/visual.sdf +++ b/sdf/1.3/visual.sdf @@ -41,7 +41,7 @@ - vertex, pixel, normal_map_objectspace, normal_map_tangentspace + vertex, pixel, normal_map_object_space, normal_map_tangent_space diff --git a/sdf/1.4/cylinder_shape.sdf b/sdf/1.4/cylinder_shape.sdf index 8e25a5311..771596ba4 100644 --- a/sdf/1.4/cylinder_shape.sdf +++ b/sdf/1.4/cylinder_shape.sdf @@ -4,6 +4,6 @@ Radius of the cylinder - Length of the cylinder + Length of the cylinder along the z axis diff --git a/sdf/1.4/sensor.sdf b/sdf/1.4/sensor.sdf index df4749333..1d815338e 100644 --- a/sdf/1.4/sensor.sdf +++ b/sdf/1.4/sensor.sdf @@ -33,6 +33,7 @@ + @@ -40,6 +41,5 @@ - diff --git a/sdf/1.4/visual.sdf b/sdf/1.4/visual.sdf index 644bff44a..f8043dfca 100644 --- a/sdf/1.4/visual.sdf +++ b/sdf/1.4/visual.sdf @@ -41,7 +41,7 @@ - vertex, pixel, normal_map_objectspace, normal_map_tangentspace + vertex, pixel, normal_map_object_space, normal_map_tangent_space diff --git a/sdf/1.5/cylinder_shape.sdf b/sdf/1.5/cylinder_shape.sdf index 8e25a5311..771596ba4 100644 --- a/sdf/1.5/cylinder_shape.sdf +++ b/sdf/1.5/cylinder_shape.sdf @@ -4,6 +4,6 @@ Radius of the cylinder - Length of the cylinder + Length of the cylinder along the z axis diff --git a/sdf/1.5/forcetorque.sdf b/sdf/1.5/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.5/forcetorque.sdf +++ b/sdf/1.5/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.5/material.sdf b/sdf/1.5/material.sdf index 137436eea..fdf5fa6a7 100644 --- a/sdf/1.5/material.sdf +++ b/sdf/1.5/material.sdf @@ -17,7 +17,7 @@ - vertex, pixel, normal_map_objectspace, normal_map_tangentspace + vertex, pixel, normal_map_object_space, normal_map_tangent_space diff --git a/sdf/1.5/sensor.sdf b/sdf/1.5/sensor.sdf index 5891cfa7f..e58ae79b3 100644 --- a/sdf/1.5/sensor.sdf +++ b/sdf/1.5/sensor.sdf @@ -49,6 +49,7 @@ + @@ -58,6 +59,5 @@ - diff --git a/sdf/1.6/camera.sdf b/sdf/1.6/camera.sdf index b5a906d65..5f7bc220a 100644 --- a/sdf/1.6/camera.sdf +++ b/sdf/1.6/camera.sdf @@ -29,7 +29,7 @@ Near clipping plane - + Far clipping plane diff --git a/sdf/1.6/cylinder_shape.sdf b/sdf/1.6/cylinder_shape.sdf index 8e25a5311..771596ba4 100644 --- a/sdf/1.6/cylinder_shape.sdf +++ b/sdf/1.6/cylinder_shape.sdf @@ -4,6 +4,6 @@ Radius of the cylinder - Length of the cylinder + Length of the cylinder along the z axis diff --git a/sdf/1.6/forcetorque.sdf b/sdf/1.6/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.6/forcetorque.sdf +++ b/sdf/1.6/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.6/material.sdf b/sdf/1.6/material.sdf index f17690759..7cd011598 100644 --- a/sdf/1.6/material.sdf +++ b/sdf/1.6/material.sdf @@ -17,7 +17,7 @@ - vertex, pixel, normal_map_objectspace, normal_map_tangentspace + vertex, pixel, normal_map_object_space, normal_map_tangent_space diff --git a/sdf/1.6/sensor.sdf b/sdf/1.6/sensor.sdf index eb8cff4e7..978522289 100644 --- a/sdf/1.6/sensor.sdf +++ b/sdf/1.6/sensor.sdf @@ -57,6 +57,7 @@ + @@ -67,6 +68,5 @@ - diff --git a/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf index 273f07191..448f8a516 100644 --- a/sdf/1.7/camera.sdf +++ b/sdf/1.7/camera.sdf @@ -29,7 +29,7 @@ Near clipping plane - + Far clipping plane diff --git a/sdf/1.7/cylinder_shape.sdf b/sdf/1.7/cylinder_shape.sdf index 8e25a5311..771596ba4 100644 --- a/sdf/1.7/cylinder_shape.sdf +++ b/sdf/1.7/cylinder_shape.sdf @@ -4,6 +4,6 @@ Radius of the cylinder - Length of the cylinder + Length of the cylinder along the z axis diff --git a/sdf/1.7/forcetorque.sdf b/sdf/1.7/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.7/forcetorque.sdf +++ b/sdf/1.7/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.7/frame.sdf b/sdf/1.7/frame.sdf index be28ffe9b..dcc1c3397 100644 --- a/sdf/1.7/frame.sdf +++ b/sdf/1.7/frame.sdf @@ -10,7 +10,7 @@ Name of the link or frame to which this frame is attached. If a frame is specified, recursively following the attached_to attributes - of the specified frames must lead to the name of a link or the world frame. + of the specified frames must lead to the name of a link, a model, or the world frame. diff --git a/sdf/1.7/material.sdf b/sdf/1.7/material.sdf index f17690759..285b476ee 100644 --- a/sdf/1.7/material.sdf +++ b/sdf/1.7/material.sdf @@ -17,7 +17,7 @@ - vertex, pixel, normal_map_objectspace, normal_map_tangentspace + vertex, pixel, normal_map_object_space, normal_map_tangent_space @@ -25,6 +25,10 @@ + + Set render order for coplanar polygons. The higher value will be rendered on top of the other coplanar polygons + + If false, dynamic lighting will be disabled @@ -45,6 +49,11 @@ The emissive color of a material specified by set of four numbers representing red/green/blue, each in the range of [0,1]. + + If true, the mesh that this material is applied to will be rendered as double sided + + + Physically Based Rendering (PBR) material. There are two PBR workflows: metal and specular. While both workflows and their parameters can be specified at the same time, typically only one of them will be used (depending on the underlying renderer capability). It is also recommended to use the same workflow for all materials in the world. @@ -90,6 +99,14 @@ Filename of the emissive map. + + + + Index of the texture coordinate set to use. + + Filename of the light map. The light map is a prebaked light texture that is applied over the albedo map + + @@ -126,6 +143,13 @@ Filename of the emissive map. + + + + Index of the texture coordinate set to use. + + Filename of the light map. The light map is a prebaked light texture that is applied over the albedo map + diff --git a/sdf/1.7/sensor.sdf b/sdf/1.7/sensor.sdf index d5f8c938b..0944cd333 100644 --- a/sdf/1.7/sensor.sdf +++ b/sdf/1.7/sensor.sdf @@ -56,6 +56,7 @@ + @@ -66,6 +67,5 @@ - diff --git a/sdf/1.8/CMakeLists.txt b/sdf/1.8/CMakeLists.txt index 7ad9f795f..a1a70717b 100644 --- a/sdf/1.8/CMakeLists.txt +++ b/sdf/1.8/CMakeLists.txt @@ -7,9 +7,11 @@ set (sdfs battery.sdf box_shape.sdf camera.sdf + capsule_shape.sdf collision.sdf contact.sdf cylinder_shape.sdf + ellipsoid_shape.sdf frame.sdf forcetorque.sdf geometry.sdf diff --git a/sdf/1.8/capsule_shape.sdf b/sdf/1.8/capsule_shape.sdf new file mode 100644 index 000000000..2831099ba --- /dev/null +++ b/sdf/1.8/capsule_shape.sdf @@ -0,0 +1,9 @@ + + Capsule shape + + Radius of the capsule + + + Length of the cylindrical portion of the capsule along the z axis + + diff --git a/sdf/1.8/cylinder_shape.sdf b/sdf/1.8/cylinder_shape.sdf index 8e25a5311..771596ba4 100644 --- a/sdf/1.8/cylinder_shape.sdf +++ b/sdf/1.8/cylinder_shape.sdf @@ -4,6 +4,6 @@ Radius of the cylinder - Length of the cylinder + Length of the cylinder along the z axis diff --git a/sdf/1.8/ellipsoid_shape.sdf b/sdf/1.8/ellipsoid_shape.sdf new file mode 100644 index 000000000..821aadf12 --- /dev/null +++ b/sdf/1.8/ellipsoid_shape.sdf @@ -0,0 +1,6 @@ + + Ellipsoid shape + + The three radii of the ellipsoid. The origin of the ellipsoid is in its geometric center (inside the center of the ellipsoid). + + diff --git a/sdf/1.8/forcetorque.sdf b/sdf/1.8/forcetorque.sdf index 4fd5e742b..18e5b8f71 100644 --- a/sdf/1.8/forcetorque.sdf +++ b/sdf/1.8/forcetorque.sdf @@ -13,7 +13,7 @@ Direction of the wrench measured by the sensor. The supported options are: - "parent_to_child" if the measured wrench is the one applied by parent link on the child link, + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. diff --git a/sdf/1.8/geometry.sdf b/sdf/1.8/geometry.sdf index 5fe95ed88..884902afb 100644 --- a/sdf/1.8/geometry.sdf +++ b/sdf/1.8/geometry.sdf @@ -7,7 +7,9 @@ + + diff --git a/sdf/1.8/heightmap_shape.sdf b/sdf/1.8/heightmap_shape.sdf index 8db8c670b..7a7462298 100644 --- a/sdf/1.8/heightmap_shape.sdf +++ b/sdf/1.8/heightmap_shape.sdf @@ -37,8 +37,8 @@ Set if the rendering engine will use terrain paging - - Samples per heightmap datum. For rasterized heightmaps, this indicates the number of samples to take per pixel. Using a lower value, e.g. 1, will generally improve the performance of the heightmap but lower the heightmap quality. + + Samples per heightmap datum. For rasterized heightmaps, this indicates the number of samples to take per pixel. Using a higher value, e.g. 2, will generally improve the quality of the heightmap but lower performance. diff --git a/sdf/1.8/inertial.sdf b/sdf/1.8/inertial.sdf index 963f4b2c3..a6568f58a 100644 --- a/sdf/1.8/inertial.sdf +++ b/sdf/1.8/inertial.sdf @@ -6,9 +6,9 @@ The mass of the link. - - This is the pose of the inertial reference frame, relative to the specified reference frame. The origin of the inertial reference frame needs to be at the center of gravity. The axes of the inertial reference frame do not need to be aligned with the principal axes of the inertia. - + + This is the pose of the inertial reference frame. The origin of the inertial reference frame needs to be at the center of gravity. The axes of the inertial reference frame do not need to be aligned with the principal axes of the inertia. + The 3x3 rotational inertia matrix. Because the rotational inertia matrix is symmetric, only 6 above-diagonal elements of this matrix are specified here, using the attributes ixx, ixy, ixz, iyy, iyz, izz. diff --git a/sdf/1.8/joint.sdf b/sdf/1.8/joint.sdf index 42a8cfe99..f58fa75fd 100644 --- a/sdf/1.8/joint.sdf +++ b/sdf/1.8/joint.sdf @@ -21,11 +21,11 @@ - Name of the parent link or "world". + Name of the parent frame or "world". - Name of the child link. The value "world" may not be specified. + Name of the child frame. The value "world" may not be specified. diff --git a/sdf/1.8/material.sdf b/sdf/1.8/material.sdf index f17690759..7cd011598 100644 --- a/sdf/1.8/material.sdf +++ b/sdf/1.8/material.sdf @@ -17,7 +17,7 @@ - vertex, pixel, normal_map_objectspace, normal_map_tangentspace + vertex, pixel, normal_map_object_space, normal_map_tangent_space diff --git a/sdf/1.8/model.sdf b/sdf/1.8/model.sdf index bd6fe4122..a6d30f392 100644 --- a/sdf/1.8/model.sdf +++ b/sdf/1.8/model.sdf @@ -14,6 +14,9 @@ as the canonical link. + + The frame inside this model whose pose will be set by the pose element of the model. i.e, the pose element specifies the pose of this frame instead of the model frame. + If set to true, the model is immovable. Otherwise the model is simulated in the dynamics engine. @@ -35,7 +38,9 @@ - Include resources from a URI. This can be used to nest models. + + Include resources from a URI. This can be used to nest models. Included resources can only contain one 'model', 'light' or 'actor' element. The URI can point to a directory or a file. If the URI is a directory, it must conform to the model database structure (see /tutorials?tut=composition&cat=specification&#defining-models-in-separate-files). + URI to a resource, such as a model @@ -49,6 +54,10 @@ Override the static value of the included model. + + + The frame inside the included model whose pose will be set by the specified pose element. If this element is specified, the pose must be specified. + diff --git a/sdf/1.8/root.sdf b/sdf/1.8/root.sdf index 346435979..d543c973b 100644 --- a/sdf/1.8/root.sdf +++ b/sdf/1.8/root.sdf @@ -1,13 +1,13 @@ - SDFormat base element that can include 0-N models, actors, lights, and/or worlds. A user of multiple worlds could run parallel instances of simulation, or offer selection of a world at runtime. + SDFormat base element that can include one model, actor, light, or worlds. A user of multiple worlds could run parallel instances of simulation, or offer selection of a world at runtime. Version number of the SDFormat specification. - - - + + + diff --git a/sdf/1.8/sensor.sdf b/sdf/1.8/sensor.sdf index d5f8c938b..0944cd333 100644 --- a/sdf/1.8/sensor.sdf +++ b/sdf/1.8/sensor.sdf @@ -56,6 +56,7 @@ + @@ -66,6 +67,5 @@ - diff --git a/sdf/1.8/world.sdf b/sdf/1.8/world.sdf index 3214518cc..991eaf083 100644 --- a/sdf/1.8/world.sdf +++ b/sdf/1.8/world.sdf @@ -22,7 +22,9 @@ - Include resources from a URI + + Include resources from a URI. Included resources can only contain one 'model', 'light' or 'actor' element. The URI can point to a directory or a file. If the URI is a directory, it must conform to the model database structure (see /tutorials?tut=composition&cat=specification&#defining-models-in-separate-files). + URI to a resource, such as a model @@ -36,6 +38,10 @@ + + + The frame inside the included entity whose pose will be set by the specified pose element. If this element is specified, the pose must be specified. + diff --git a/sdf/Migration.md b/sdf/Migration.md deleted file mode 100644 index 189391a10..000000000 --- a/sdf/Migration.md +++ /dev/null @@ -1,97 +0,0 @@ -# Migration Guide for SDFormat Specification -This document contains information about migrating -between different versions of the SDFormat specification. -The SDFormat specification version number is specified in the `version` attribute -of the `sdf` element (1.4, 1.5, 1.6, etc.) -and is distinct from libsdformat library version -(2.3, 3.0, 4.0, etc.). - -# Note on backward compatibility -There are `*.convert` files that allow old sdf files to be migrated -forward programmatically. -This document aims to contain similar information to those files -but with improved human-readability. - -## SDFormat specification 1.7 to 1.8 - -## SDFormat specification 1.6 to 1.7 - -## SDFormat specification 1.5 to 1.6 - -### Additions - -1. **heightmap_shape.sdf** `sampling` element - + description: Samples per heightmap datum. - For rasterized heightmaps, this indicates the number of samples to take per pixel. - Using a lower value, e.g. 1, will generally improve the performance - of the heightmap but lower the heightmap quality. - + type: unsigned int - + default: 2 - + required: 0 - + [Bitbucket pull request 293](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/293) - -1. **link.sdf** `enable_wind` element - + description: If true, the link is affected by the wind - + type: bool - + default: false - + required: 0 - + [Bitbucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) - -1. **model.sdf** `enable_wind` element - + description: If set to true, all links in the model - will be affected by the wind. - Can be overriden by the link wind property. - + type: bool - + default: false - + required: 0 - + [Bitbucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) - -1. **model_state.sdf** `scale` element - + description: Scale for the 3 dimensions of the model. - + type: vector3 - + default: "1 1 1" - + required: 0 - + [Bitbucket pull request 246](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/246) - -1. **physics.sdf** `friction_model` element - + description: Name of ODE friction model to use. Valid values include: - + pyramid_model: (default) friction forces limited in two directions - in proportion to normal force. - + box_model: friction forces limited to constant in two directions. - + cone_model: friction force magnitude limited in proportion to normal force. - See [gazebo pull request 1522](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1522) - (merged in [gazebo 8c05ad64967c](https://github.com/osrf/gazebo/commit/968dccafdfbfca09c9b3326f855612076fed7e6f)) - for the implementation of this feature. - + type: string - + default: "pyramid_model" - + required: 0 - + [Bitbucket pull request 294](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/294) - -1. **world.sdf** `wind` element - + description: The wind tag specifies the type and properties of the wind. - + required: 0 - + [Bitbucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) - -1. **world.sdf** `wind::linear_velocity` element - + description: Linear velocity of the wind. - + type: vector3 - + default: "0 0 0" - + required: 0 - + [Bitbucket pull request 240](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/240) - -### Modifications - -1. `gravity` and `magnetic_field` elements are moved - from `physics` to `world` - + [Bitbucket pull request 247](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/247) - + [gazebo pull request 2090](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2090) - -1. A new style for representing the noise properties of an `imu` was implemented - in [Bitbucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) - for sdf 1.5 and the old style was declared as deprecated. - The old style has been removed from sdf 1.6 with the conversion script - updating to the new style. - + [Bitbucket pull request 199](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/199) - + [Bitbucket pull request 243](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/243) - + [Bitbucket pull request 244](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/244) - diff --git a/src/Actor.cc b/src/Actor.cc index e2744cb21..2f10ae7b4 100644 --- a/src/Actor.cc +++ b/src/Actor.cc @@ -24,7 +24,7 @@ using namespace sdf; /// \brief Animation private data. -class sdf::AnimationPrivate +class sdf::Animation::Implementation { /// \brief Unique name for animation. public: std::string name = "__default__"; @@ -43,7 +43,7 @@ class sdf::AnimationPrivate }; /// \brief Waypoint private data. -class sdf::WaypointPrivate +class sdf::Waypoint::Implementation { /// \brief Time to indicate when the pose should be reached. public: double time = 0.0; @@ -53,7 +53,7 @@ class sdf::WaypointPrivate }; /// \brief Trajectory private data. -class sdf::TrajectoryPrivate +class sdf::Trajectory::Implementation { /// \brief Unique id for a trajectory. public: uint64_t id = 0; @@ -69,7 +69,7 @@ class sdf::TrajectoryPrivate }; /// \brief Actor private data. -class sdf::ActorPrivate +class sdf::Actor::Implementation { /// \brief Name of the actor. public: std::string name = "__default__"; @@ -116,53 +116,10 @@ class sdf::ActorPrivate ///////////////////////////////////////////////// Animation::Animation() - : dataPtr(new AnimationPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Animation::~Animation() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Animation::Animation(const Animation &_animation) - : dataPtr(new AnimationPrivate) -{ - this->CopyFrom(_animation); -} - -///////////////////////////////////////////////// -Animation::Animation(Animation &&_animation) noexcept - : dataPtr(std::exchange(_animation.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Animation &Animation::operator=(const Animation &_animation) -{ - return *this = Animation(_animation); -} - -////////////////////////////////////////////////// -Animation &Animation::operator=(Animation &&_animation) -{ - std::swap(this->dataPtr, _animation.dataPtr); - return *this; -} - -///////////////////////////////////////////////// -void Animation::CopyFrom(const Animation &_animation) -{ - this->dataPtr->name = _animation.dataPtr->name; - this->dataPtr->filename = _animation.dataPtr->filename; - this->dataPtr->scale = _animation.dataPtr->scale; - this->dataPtr->interpolateX = _animation.dataPtr->interpolateX; - this->dataPtr->filePath = _animation.dataPtr->filePath; -} - ///////////////////////////////////////////////// Errors Animation::Load(ElementPtr _sdf) { @@ -256,48 +213,8 @@ void Animation::SetFilePath(const std::string &_filePath) ///////////////////////////////////////////////// Waypoint::Waypoint() - : dataPtr(new WaypointPrivate) -{ -} - -///////////////////////////////////////////////// -Waypoint::~Waypoint() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Waypoint::Waypoint(const Waypoint &_waypoint) - : dataPtr(new WaypointPrivate) -{ - this->CopyFrom(_waypoint); -} - -///////////////////////////////////////////////// -Waypoint::Waypoint(Waypoint &&_waypoint) noexcept - : dataPtr(std::exchange(_waypoint.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Waypoint &Waypoint::operator=(const Waypoint &_waypoint) -{ - return *this = Waypoint(_waypoint); -} - -////////////////////////////////////////////////// -Waypoint &Waypoint::operator=(Waypoint &&_waypoint) -{ - std::swap(this->dataPtr, _waypoint.dataPtr); - return *this; -} - -///////////////////////////////////////////////// -void Waypoint::CopyFrom(const Waypoint &_waypoint) + : dataPtr(ignition::utils::MakeImpl()) { - this->dataPtr->time = _waypoint.dataPtr->time; - this->dataPtr->pose = _waypoint.dataPtr->pose; } ///////////////////////////////////////////////// @@ -351,52 +268,10 @@ void Waypoint::SetPose(const ignition::math::Pose3d &_pose) ///////////////////////////////////////////////// Trajectory::Trajectory() - : dataPtr(new TrajectoryPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Trajectory::~Trajectory() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Trajectory::Trajectory(const Trajectory &_trajectory) - : dataPtr(new TrajectoryPrivate) -{ - this->CopyFrom(_trajectory); -} - -///////////////////////////////////////////////// -Trajectory::Trajectory(Trajectory &&_trajectory) noexcept - : dataPtr(std::exchange(_trajectory.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Trajectory &Trajectory::operator=(const Trajectory &_trajectory) -{ - return *this = Trajectory(_trajectory); -} - -////////////////////////////////////////////////// -Trajectory &Trajectory::operator=(Trajectory &&_trajectory) -{ - std::swap(this->dataPtr, _trajectory.dataPtr); - return *this; -} - -///////////////////////////////////////////////// -void Trajectory::CopyFrom(const Trajectory &_trajectory) -{ - this->dataPtr->id = _trajectory.dataPtr->id; - this->dataPtr->type = _trajectory.dataPtr->type; - this->dataPtr->tension = _trajectory.dataPtr->tension; - this->dataPtr->waypoints = _trajectory.dataPtr->waypoints; -} - ///////////////////////////////////////////////// Errors Trajectory::Load(ElementPtr _sdf) { @@ -488,57 +363,8 @@ void Trajectory::AddWaypoint(const Waypoint &_waypoint) ///////////////////////////////////////////////// Actor::Actor() - : dataPtr(new ActorPrivate) -{ -} - -///////////////////////////////////////////////// -Actor::~Actor() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Actor::Actor(const Actor &_actor) - : dataPtr(new ActorPrivate) -{ - this->CopyFrom(_actor); -} - -///////////////////////////////////////////////// -Actor::Actor(Actor &&_actor) noexcept - : dataPtr(std::exchange(_actor.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Actor &Actor::operator=(const Actor &_actor) -{ - return *this = Actor(_actor); -} - -////////////////////////////////////////////////// -Actor &Actor::operator=(Actor &&_actor) -{ - std::swap(this->dataPtr, _actor.dataPtr); - return *this; -} - -////////////////////////////////////////////////// -void Actor::CopyFrom(const Actor &_actor) + : dataPtr(ignition::utils::MakeImpl()) { - this->dataPtr->name = _actor.dataPtr->name; - this->dataPtr->pose = _actor.dataPtr->pose; - this->dataPtr->poseRelativeTo = _actor.dataPtr->poseRelativeTo; - this->dataPtr->skinFilename = _actor.dataPtr->skinFilename; - this->dataPtr->skinScale = _actor.dataPtr->skinScale; - this->dataPtr->animations = _actor.dataPtr->animations; - this->dataPtr->scriptLoop = _actor.dataPtr->scriptLoop; - this->dataPtr->scriptDelayStart = _actor.dataPtr->scriptDelayStart; - this->dataPtr->scriptAutoStart = _actor.dataPtr->scriptAutoStart; - this->dataPtr->trajectories = _actor.dataPtr->trajectories; - this->dataPtr->filePath = _actor.dataPtr->filePath; } ///////////////////////////////////////////////// @@ -631,7 +457,7 @@ Errors Actor::Load(ElementPtr _sdf) } ///////////////////////////////////////////////// -std::string &Actor::Name() const +const std::string &Actor::Name() const { return this->dataPtr->name; } diff --git a/src/Actor_TEST.cc b/src/Actor_TEST.cc index a19683ce8..bac434933 100644 --- a/src/Actor_TEST.cc +++ b/src/Actor_TEST.cc @@ -19,6 +19,127 @@ #include #include "sdf/Actor.hh" +///////////////////////////////////////////////// +sdf::Animation CreateDummyAnimation() +{ + sdf::Animation animation; + animation.SetName("animation"); + animation.SetFilename("animation_filename"); + animation.SetFilePath("animation_filepath"); + animation.SetScale(1.234); + animation.SetInterpolateX(true); + return animation; +} + +///////////////////////////////////////////////// +bool AnimationsEqual(const sdf::Animation &_anim1, const sdf::Animation &_anim2) +{ + constexpr double EPS = 1e-6; + return (_anim1.Name() == _anim2.Name()) && + (_anim1.Filename() == _anim2.Filename()) && + (_anim1.FilePath() == _anim2.FilePath()) && + (std::abs(_anim1.Scale() - _anim2.Scale()) < EPS) && + (_anim1.InterpolateX() == _anim2.InterpolateX()); +} + +///////////////////////////////////////////////// +sdf::Trajectory CreateDummyTrajectory() +{ + sdf::Trajectory trajectory; + sdf::Waypoint waypoint; + waypoint.SetTime(0.12); + waypoint.SetPose({3, 2, 1, 0, IGN_PI, 0}); + trajectory.SetId(1234); + trajectory.SetType("trajectory_type"); + trajectory.SetTension(4.567); + trajectory.AddWaypoint(waypoint); + return trajectory; +} + +///////////////////////////////////////////////// +bool TrajectoriesEqual(const sdf::Trajectory &_traj1, + const sdf::Trajectory &_traj2) +{ + constexpr double EPS = 1e-6; + bool waypointsEqual = true; + if (_traj1.WaypointCount() != _traj2.WaypointCount()) + { + return false; + } + for (uint64_t wp_idx = 0; wp_idx < _traj1.WaypointCount(); ++wp_idx) + { + auto wp1 = _traj1.WaypointByIndex(wp_idx); + auto wp2 = _traj2.WaypointByIndex(wp_idx); + waypointsEqual &= (std::abs(wp1->Time() - wp2->Time()) < EPS) && + wp1->Pose() == wp2->Pose(); + } + return waypointsEqual && + (_traj1.Id() == _traj2.Id()) && + (_traj1.Type() == _traj2.Type()) && + (std::abs(_traj1.Tension() - _traj2.Tension()) < EPS); +} + +///////////////////////////////////////////////// +sdf::Actor CreateDummyActor() +{ + sdf::Actor actor; + actor.SetName("test_dummy_actor"); + actor.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); + actor.SetPoseRelativeTo("ground_plane"); + actor.SetSkinFilename("walk.dae"); + actor.SetSkinScale(2.0); + actor.SetScriptLoop(true); + actor.SetScriptDelayStart(2.8); + actor.SetScriptAutoStart(false); + actor.SetFilePath("/home/path/to/model.sdf"); + // Add a dummy trajectory and animation as well + actor.AddTrajectory(CreateDummyTrajectory()); + actor.AddAnimation(CreateDummyAnimation()); + return actor; +} + +///////////////////////////////////////////////// +bool ActorsEqual(const sdf::Actor &_actor1, const sdf::Actor &_actor2) +{ + constexpr double EPS = 1e-6; + // Check animations, trajectories and properties + bool animationsEqual = true; + if (_actor1.AnimationCount() != _actor2.AnimationCount()) + { + return false; + } + for (uint64_t anim_idx = 0; anim_idx < _actor1.AnimationCount(); ++anim_idx) + { + animationsEqual &= AnimationsEqual( + *_actor1.AnimationByIndex(anim_idx), + *_actor2.AnimationByIndex(anim_idx)); + } + bool trajectoriesEqual = true; + if (_actor1.TrajectoryCount() != _actor2.TrajectoryCount()) + { + return false; + } + for (uint64_t traj_idx = 0; traj_idx < _actor1.TrajectoryCount(); ++traj_idx) + { + trajectoriesEqual &= TrajectoriesEqual( + *_actor1.TrajectoryByIndex(traj_idx), + *_actor2.TrajectoryByIndex(traj_idx)); + } + return animationsEqual && trajectoriesEqual && + (_actor1.Name() == _actor2.Name()) && + (_actor1.RawPose() == _actor2.RawPose()) && + (_actor1.PoseRelativeTo() == _actor2.PoseRelativeTo()) && + (_actor1.SkinFilename() == _actor2.SkinFilename()) && + (std::abs(_actor1.SkinScale() - _actor2.SkinScale()) < EPS) && + (_actor1.ScriptLoop() == _actor2.ScriptLoop()) && + (std::abs(_actor1.ScriptDelayStart() - _actor2.ScriptDelayStart()) < EPS) && + (_actor1.ScriptAutoStart() == _actor2.ScriptAutoStart()) && + (_actor1.FilePath() == _actor2.FilePath()) && + (_actor1.LinkCount() == _actor2.LinkCount()) && + (_actor1.JointCount() == _actor2.JointCount()) && + (_actor1.Element() == _actor2.Element()); +} + ///////////////////////////////////////////////// TEST(DOMActor, DefaultConstruction) { @@ -63,227 +184,42 @@ TEST(DOMActor, DefaultConstruction) ///////////////////////////////////////////////// TEST(DOMActor, CopyConstructor) { - sdf::Actor actor; - actor.SetName("test_copy_actor"); - actor.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); - actor.SetPoseRelativeTo("ground_plane"); - actor.SetSkinFilename("walk.dae"); - actor.SetSkinScale(2.0); - actor.SetScriptLoop(true); - actor.SetScriptDelayStart(2.8); - actor.SetScriptAutoStart(false); - actor.SetFilePath("/home/path/to/model.sdf"); - + sdf::Actor actor = CreateDummyActor(); sdf::Actor actor2(actor); - EXPECT_EQ("test_copy_actor", actor2.Name()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), actor2.RawPose()); - EXPECT_EQ("ground_plane", actor2.PoseRelativeTo()); - EXPECT_EQ("/home/path/to/model.sdf", actor2.FilePath()); - - EXPECT_EQ(0u, actor2.AnimationCount()); - EXPECT_EQ(nullptr, actor2.AnimationByIndex(0)); - EXPECT_EQ(nullptr, actor2.AnimationByIndex(1)); - EXPECT_FALSE(actor2.AnimationNameExists("")); - EXPECT_FALSE(actor2.AnimationNameExists("default")); - - EXPECT_EQ("walk.dae", actor2.SkinFilename()); - EXPECT_DOUBLE_EQ(2.0, actor2.SkinScale()); - - EXPECT_EQ(0u, actor2.TrajectoryCount()); - EXPECT_EQ(nullptr, actor2.TrajectoryByIndex(0)); - EXPECT_EQ(nullptr, actor2.TrajectoryByIndex(1)); - EXPECT_FALSE(actor2.TrajectoryIdExists(0)); - EXPECT_FALSE(actor2.TrajectoryIdExists(1)); - - EXPECT_TRUE(actor2.ScriptLoop()); - EXPECT_DOUBLE_EQ(2.8, actor2.ScriptDelayStart()); - EXPECT_FALSE(actor2.ScriptAutoStart()); - - EXPECT_EQ(0u, actor2.LinkCount()); - EXPECT_EQ(nullptr, actor2.LinkByIndex(0)); - EXPECT_EQ(nullptr, actor2.LinkByIndex(1)); - EXPECT_FALSE(actor2.LinkNameExists("")); - - EXPECT_EQ(0u, actor2.JointCount()); - EXPECT_EQ(nullptr, actor2.JointByIndex(0)); - EXPECT_EQ(nullptr, actor2.JointByIndex(1)); - EXPECT_FALSE(actor2.JointNameExists("")); + EXPECT_TRUE(ActorsEqual(actor, actor2)); } ///////////////////////////////////////////////// TEST(DOMActor, CopyAssignmentOperator) { - sdf::Actor actor; - actor.SetName("test_actor_assignment"); - actor.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); - actor.SetPoseRelativeTo("ground_plane"); - actor.SetSkinFilename("walk.dae"); - actor.SetSkinScale(2.0); - actor.SetScriptLoop(true); - actor.SetScriptDelayStart(2.8); - actor.SetScriptAutoStart(false); - actor.SetFilePath("/home/path/to/model.sdf"); - + sdf::Actor actor = CreateDummyActor(); sdf::Actor actor2; actor2 = actor; - EXPECT_EQ("test_actor_assignment", actor2.Name()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), actor2.RawPose()); - EXPECT_EQ("ground_plane", actor2.PoseRelativeTo()); - EXPECT_EQ("/home/path/to/model.sdf", actor2.FilePath()); - - EXPECT_EQ(0u, actor2.AnimationCount()); - EXPECT_EQ(nullptr, actor2.AnimationByIndex(0)); - EXPECT_EQ(nullptr, actor2.AnimationByIndex(1)); - EXPECT_FALSE(actor2.AnimationNameExists("")); - EXPECT_FALSE(actor2.AnimationNameExists("default")); - - EXPECT_EQ("walk.dae", actor2.SkinFilename()); - EXPECT_DOUBLE_EQ(2.0, actor2.SkinScale()); - - EXPECT_EQ(0u, actor2.TrajectoryCount()); - EXPECT_EQ(nullptr, actor2.TrajectoryByIndex(0)); - EXPECT_EQ(nullptr, actor2.TrajectoryByIndex(1)); - EXPECT_FALSE(actor2.TrajectoryIdExists(0)); - EXPECT_FALSE(actor2.TrajectoryIdExists(1)); - - EXPECT_TRUE(actor2.ScriptLoop()); - EXPECT_DOUBLE_EQ(2.8, actor2.ScriptDelayStart()); - EXPECT_FALSE(actor2.ScriptAutoStart()); - - EXPECT_EQ(0u, actor2.LinkCount()); - EXPECT_EQ(nullptr, actor2.LinkByIndex(0)); - EXPECT_EQ(nullptr, actor2.LinkByIndex(1)); - EXPECT_FALSE(actor2.LinkNameExists("")); - - EXPECT_EQ(0u, actor2.JointCount()); - EXPECT_EQ(nullptr, actor2.JointByIndex(0)); - EXPECT_EQ(nullptr, actor2.JointByIndex(1)); - EXPECT_FALSE(actor2.JointNameExists("")); + EXPECT_TRUE(ActorsEqual(actor, actor2)); } ///////////////////////////////////////////////// TEST(DOMActor, MoveConstructor) { - sdf::Actor actor; - actor.SetName("test_actor_assignment"); - actor.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); - actor.SetPoseRelativeTo("ground_plane"); - actor.SetSkinFilename("walk.dae"); - actor.SetSkinScale(2.0); - actor.SetScriptLoop(true); - actor.SetScriptDelayStart(2.8); - actor.SetScriptAutoStart(false); - actor.SetFilePath("/home/path/to/model.sdf"); - + sdf::Actor actor = CreateDummyActor(); sdf::Actor actor2(std::move(actor)); - EXPECT_EQ("test_actor_assignment", actor2.Name()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), actor2.RawPose()); - EXPECT_EQ("ground_plane", actor2.PoseRelativeTo()); - EXPECT_EQ("/home/path/to/model.sdf", actor2.FilePath()); - - EXPECT_EQ(0u, actor2.AnimationCount()); - EXPECT_EQ(nullptr, actor2.AnimationByIndex(0)); - EXPECT_EQ(nullptr, actor2.AnimationByIndex(1)); - EXPECT_FALSE(actor2.AnimationNameExists("")); - EXPECT_FALSE(actor2.AnimationNameExists("default")); - - EXPECT_EQ("walk.dae", actor2.SkinFilename()); - EXPECT_DOUBLE_EQ(2.0, actor2.SkinScale()); - - EXPECT_EQ(0u, actor2.TrajectoryCount()); - EXPECT_EQ(nullptr, actor2.TrajectoryByIndex(0)); - EXPECT_EQ(nullptr, actor2.TrajectoryByIndex(1)); - EXPECT_FALSE(actor2.TrajectoryIdExists(0)); - EXPECT_FALSE(actor2.TrajectoryIdExists(1)); - - EXPECT_TRUE(actor2.ScriptLoop()); - EXPECT_DOUBLE_EQ(2.8, actor2.ScriptDelayStart()); - EXPECT_FALSE(actor2.ScriptAutoStart()); - - EXPECT_EQ(0u, actor2.LinkCount()); - EXPECT_EQ(nullptr, actor2.LinkByIndex(0)); - EXPECT_EQ(nullptr, actor2.LinkByIndex(1)); - EXPECT_FALSE(actor2.LinkNameExists("")); - - EXPECT_EQ(0u, actor2.JointCount()); - EXPECT_EQ(nullptr, actor2.JointByIndex(0)); - EXPECT_EQ(nullptr, actor2.JointByIndex(1)); - EXPECT_FALSE(actor2.JointNameExists("")); + EXPECT_TRUE(ActorsEqual(CreateDummyActor(), actor2)); } ///////////////////////////////////////////////// TEST(DOMActor, MoveAssignment) { - sdf::Actor actor; - actor.SetName("test_actor_assignment"); - actor.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); - actor.SetPoseRelativeTo("ground_plane"); - actor.SetSkinFilename("walk.dae"); - actor.SetSkinScale(2.0); - actor.SetScriptLoop(true); - actor.SetScriptDelayStart(2.8); - actor.SetScriptAutoStart(false); - actor.SetFilePath("/home/path/to/model.sdf"); - + sdf::Actor actor = CreateDummyActor(); sdf::Actor actor2; actor2 = std::move(actor); - EXPECT_EQ("test_actor_assignment", actor2.Name()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), actor2.RawPose()); - EXPECT_EQ("ground_plane", actor2.PoseRelativeTo()); - EXPECT_EQ("/home/path/to/model.sdf", actor2.FilePath()); - - EXPECT_EQ(0u, actor2.AnimationCount()); - EXPECT_EQ(nullptr, actor2.AnimationByIndex(0)); - EXPECT_EQ(nullptr, actor2.AnimationByIndex(1)); - EXPECT_FALSE(actor2.AnimationNameExists("")); - EXPECT_FALSE(actor2.AnimationNameExists("default")); - - EXPECT_EQ("walk.dae", actor2.SkinFilename()); - EXPECT_DOUBLE_EQ(2.0, actor2.SkinScale()); - - EXPECT_EQ(0u, actor2.TrajectoryCount()); - EXPECT_EQ(nullptr, actor2.TrajectoryByIndex(0)); - EXPECT_EQ(nullptr, actor2.TrajectoryByIndex(1)); - EXPECT_FALSE(actor2.TrajectoryIdExists(0)); - EXPECT_FALSE(actor2.TrajectoryIdExists(1)); - - EXPECT_TRUE(actor2.ScriptLoop()); - EXPECT_DOUBLE_EQ(2.8, actor2.ScriptDelayStart()); - EXPECT_FALSE(actor2.ScriptAutoStart()); - - EXPECT_EQ(0u, actor2.LinkCount()); - EXPECT_EQ(nullptr, actor2.LinkByIndex(0)); - EXPECT_EQ(nullptr, actor2.LinkByIndex(1)); - EXPECT_FALSE(actor2.LinkNameExists("")); - - EXPECT_EQ(0u, actor2.JointCount()); - EXPECT_EQ(nullptr, actor2.JointByIndex(0)); - EXPECT_EQ(nullptr, actor2.JointByIndex(1)); - EXPECT_FALSE(actor2.JointNameExists("")); + EXPECT_TRUE(ActorsEqual(CreateDummyActor(), actor2)); } ///////////////////////////////////////////////// TEST(DOMActor, CopyAssignmentAfterMove) { - sdf::Actor actor1; - actor1.SetName("actor1"); - actor1.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); - actor1.SetPoseRelativeTo("ground_plane_1"); - actor1.SetSkinFilename("walk.dae"); - actor1.SetSkinScale(2.0); - actor1.SetScriptLoop(true); - actor1.SetScriptDelayStart(2.8); - actor1.SetScriptAutoStart(false); - + sdf::Actor actor1 = CreateDummyActor(); sdf::Actor actor2; - actor2.SetName("actor2"); - actor2.SetRawPose({1, 2, 3, 0, IGN_PI, 0}); - actor2.SetPoseRelativeTo("ground_plane_2"); - actor2.SetSkinFilename("run.dae"); - actor2.SetSkinScale(0.5); - actor2.SetScriptLoop(false); - actor2.SetScriptDelayStart(0.8); - actor2.SetScriptAutoStart(true); // This is similar to what std::swap does except it uses std::move for each // assignment @@ -291,29 +227,8 @@ TEST(DOMActor, CopyAssignmentAfterMove) actor1 = actor2; actor2 = tmp; - EXPECT_EQ("actor2", actor1.Name()); - EXPECT_EQ("actor1", actor2.Name()); - - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, IGN_PI, 0), actor1.RawPose()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), actor2.RawPose()); - - EXPECT_EQ("ground_plane_2", actor1.PoseRelativeTo()); - EXPECT_EQ("ground_plane_1", actor2.PoseRelativeTo()); - - EXPECT_EQ("run.dae", actor1.SkinFilename()); - EXPECT_EQ("walk.dae", actor2.SkinFilename()); - - EXPECT_DOUBLE_EQ(0.5, actor1.SkinScale()); - EXPECT_DOUBLE_EQ(2.0, actor2.SkinScale()); - - EXPECT_FALSE(actor1.ScriptLoop()); - EXPECT_TRUE(actor2.ScriptLoop()); - - EXPECT_DOUBLE_EQ(0.8, actor1.ScriptDelayStart()); - EXPECT_DOUBLE_EQ(2.8, actor2.ScriptDelayStart()); - - EXPECT_TRUE(actor1.ScriptAutoStart()); - EXPECT_FALSE(actor2.ScriptAutoStart()); + EXPECT_TRUE(ActorsEqual(sdf::Actor(), actor1)); + EXPECT_TRUE(ActorsEqual(CreateDummyActor(), actor2)); } ////////////////////////////////////////////////// @@ -380,3 +295,210 @@ TEST(DOMActor, Add) EXPECT_EQ(456u, actor.TrajectoryByIndex(1)->Id()); EXPECT_EQ("trajectory2", actor.TrajectoryByIndex(1)->Type()); } + +////////////////////////////////////////////////// +TEST(DOMAnimation, DefaultConstruction) +{ + sdf::Animation anim; + + EXPECT_EQ(anim.Name(), "__default__"); + EXPECT_EQ(anim.Filename(), "__default__"); + EXPECT_EQ(anim.FilePath(), ""); + EXPECT_DOUBLE_EQ(anim.Scale(), 1.0); + EXPECT_EQ(anim.InterpolateX(), false); +} + +////////////////////////////////////////////////// +TEST(DOMAnimation, CopyConstructor) +{ + sdf::Animation anim1 = CreateDummyAnimation(); + sdf::Animation anim2(anim1); + EXPECT_TRUE(AnimationsEqual(anim1, anim2)); +} + +////////////////////////////////////////////////// +TEST(DOMAnimation, CopyAssignmentOperator) +{ + sdf::Animation anim1 = CreateDummyAnimation(); + sdf::Animation anim2; + anim2 = anim1; + EXPECT_TRUE(AnimationsEqual(anim1, anim2)); +} + +////////////////////////////////////////////////// +TEST(DOMAnimation, MoveConstructor) +{ + sdf::Animation anim1 = CreateDummyAnimation(); + sdf::Animation anim2(std::move(anim1)); + EXPECT_TRUE(AnimationsEqual(CreateDummyAnimation(), anim2)); +} + +////////////////////////////////////////////////// +TEST(DOMAnimation, MoveAssignment) +{ + sdf::Animation anim1 = CreateDummyAnimation(); + sdf::Animation anim2; + anim2 = std::move(anim1); + EXPECT_TRUE(AnimationsEqual(CreateDummyAnimation(), anim2)); +} + +///////////////////////////////////////////////// +TEST(DOMAnimation, CopyAssignmentAfterMove) +{ + sdf::Animation anim1 = CreateDummyAnimation(); + sdf::Animation anim2; + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Animation tmp = std::move(anim1); + anim1 = anim2; + anim2 = tmp; + + EXPECT_TRUE(AnimationsEqual(sdf::Animation(), anim1)); + EXPECT_TRUE(AnimationsEqual(CreateDummyAnimation(), anim2)); +} + +////////////////////////////////////////////////// +TEST(DOMWaypoint, DefaultConstruction) +{ + sdf::Waypoint waypoint; + EXPECT_DOUBLE_EQ(waypoint.Time(), 0.0); + EXPECT_EQ(waypoint.Pose(), ignition::math::Pose3d::Zero); +} + +////////////////////////////////////////////////// +TEST(DOMWaypoint, CopyConstructor) +{ + sdf::Waypoint waypoint1; + waypoint1.SetTime(1.23); + waypoint1.SetPose({3, 2, 1, 0, IGN_PI, 0}); + + sdf::Waypoint waypoint2(waypoint1); + EXPECT_DOUBLE_EQ(waypoint1.Time(), waypoint2.Time()); + EXPECT_EQ(waypoint1.Pose(), waypoint2.Pose()); +} + +////////////////////////////////////////////////// +TEST(DOMWaypoint, CopyAssignmentOperator) +{ + sdf::Waypoint waypoint1; + waypoint1.SetTime(1.23); + waypoint1.SetPose({3, 2, 1, 0, IGN_PI, 0}); + + sdf::Waypoint waypoint2; + waypoint2 = waypoint1; + EXPECT_DOUBLE_EQ(waypoint1.Time(), waypoint2.Time()); + EXPECT_EQ(waypoint1.Pose(), waypoint2.Pose()); +} + +////////////////////////////////////////////////// +TEST(DOMWaypoint, MoveConstructor) +{ + sdf::Waypoint waypoint1; + ignition::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); + waypoint1.SetTime(1.23); + waypoint1.SetPose(pose1); + + sdf::Waypoint waypoint2(std::move(waypoint1)); + EXPECT_DOUBLE_EQ(1.23, waypoint2.Time()); + EXPECT_EQ(pose1, waypoint2.Pose()); +} + +////////////////////////////////////////////////// +TEST(DOMWaypoint, MoveAssignment) +{ + sdf::Waypoint waypoint1; + ignition::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); + waypoint1.SetTime(1.23); + waypoint1.SetPose(pose1); + + sdf::Waypoint waypoint2; + waypoint2 = std::move(waypoint1); + EXPECT_DOUBLE_EQ(1.23, waypoint2.Time()); + EXPECT_EQ(pose1, waypoint2.Pose()); +} + +///////////////////////////////////////////////// +TEST(DOMWaypoint, CopyAssignmentAfterMove) +{ + sdf::Waypoint waypoint1; + ignition::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); + waypoint1.SetTime(1.23); + waypoint1.SetPose(pose1); + sdf::Waypoint waypoint2; + ignition::math::Pose3d pose2(1, 2, 3, 1, 2, IGN_PI); + waypoint2.SetTime(3.45); + waypoint2.SetPose(pose2); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Waypoint tmp = std::move(waypoint1); + waypoint1 = waypoint2; + waypoint2 = tmp; + + EXPECT_DOUBLE_EQ(1.23, waypoint2.Time()); + EXPECT_EQ(pose1, waypoint2.Pose()); + EXPECT_DOUBLE_EQ(3.45, waypoint1.Time()); + EXPECT_EQ(pose2, waypoint1.Pose()); +} + +////////////////////////////////////////////////// +TEST(DOMTrajectory, DefaultConstruction) +{ + sdf::Trajectory trajectory; + + EXPECT_EQ(trajectory.Id(), 0u); + EXPECT_EQ(trajectory.Type(), "__default__"); + EXPECT_DOUBLE_EQ(trajectory.Tension(), 0.0); + EXPECT_EQ(trajectory.WaypointCount(), 0u); +} + +////////////////////////////////////////////////// +TEST(DOMTrajectory, CopyConstructor) +{ + sdf::Trajectory trajectory1 = CreateDummyTrajectory(); + sdf::Trajectory trajectory2(trajectory1); + EXPECT_TRUE(TrajectoriesEqual(trajectory1, trajectory2)); +} + +////////////////////////////////////////////////// +TEST(DOMTrajectory, CopyAssignmentOperator) +{ + sdf::Trajectory trajectory1 = CreateDummyTrajectory(); + sdf::Trajectory trajectory2; + trajectory2 = trajectory1; + EXPECT_TRUE(TrajectoriesEqual(trajectory1, trajectory2)); +} + +////////////////////////////////////////////////// +TEST(DOMTrajectory, MoveConstructor) +{ + sdf::Trajectory trajectory1 = CreateDummyTrajectory(); + sdf::Trajectory trajectory2(std::move(trajectory1)); + EXPECT_TRUE(TrajectoriesEqual(CreateDummyTrajectory(), trajectory2)); +} + +////////////////////////////////////////////////// +TEST(DOMTrajectory, MoveAssignment) +{ + sdf::Trajectory trajectory1 = CreateDummyTrajectory(); + sdf::Trajectory trajectory2; + trajectory2 = std::move(trajectory1); + EXPECT_TRUE(TrajectoriesEqual(CreateDummyTrajectory(), trajectory2)); +} + +///////////////////////////////////////////////// +TEST(DOMTrajectory, CopyAssignmentAfterMove) +{ + sdf::Trajectory trajectory1 = CreateDummyTrajectory(); + sdf::Trajectory trajectory2; + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Trajectory tmp = std::move(trajectory1); + trajectory1 = trajectory2; + trajectory2 = tmp; + + EXPECT_TRUE(TrajectoriesEqual(sdf::Trajectory(), trajectory1)); + EXPECT_TRUE(TrajectoriesEqual(CreateDummyTrajectory(), trajectory2)); +} diff --git a/src/AirPressure.cc b/src/AirPressure.cc index 667fb164f..44d76a48c 100644 --- a/src/AirPressure.cc +++ b/src/AirPressure.cc @@ -21,7 +21,7 @@ using namespace sdf; /// \brief Private airPressure data. -class sdf::AirPressurePrivate +class sdf::AirPressure::Implementation { /// \brief The pressure noise. public: Noise noise; @@ -35,44 +35,10 @@ class sdf::AirPressurePrivate ////////////////////////////////////////////////// AirPressure::AirPressure() - : dataPtr(new AirPressurePrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -////////////////////////////////////////////////// -AirPressure::~AirPressure() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -AirPressure::AirPressure(const AirPressure &_sensor) - : dataPtr(new AirPressurePrivate(*_sensor.dataPtr)) -{ -} - -////////////////////////////////////////////////// -AirPressure::AirPressure(AirPressure &&_sensor) - : dataPtr(std::exchange(_sensor.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -AirPressure &AirPressure::operator=( - const AirPressure &_sensor) -{ - return *this = AirPressure(_sensor); -} - -////////////////////////////////////////////////// -AirPressure &AirPressure::operator=( - AirPressure &&_sensor) -{ - std::swap(this->dataPtr, _sensor.dataPtr); - return *this; -} - ////////////////////////////////////////////////// Errors AirPressure::Load(ElementPtr _sdf) { diff --git a/src/Altimeter.cc b/src/Altimeter.cc index 8af404e07..6ec32bc24 100644 --- a/src/Altimeter.cc +++ b/src/Altimeter.cc @@ -21,7 +21,7 @@ using namespace sdf; /// \brief Private altimeter data. -class sdf::AltimeterPrivate +class sdf::Altimeter::Implementation { /// \brief The vertical position noise value. public: Noise verticalPositionNoise; @@ -35,42 +35,10 @@ class sdf::AltimeterPrivate ////////////////////////////////////////////////// Altimeter::Altimeter() - : dataPtr(new AltimeterPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -////////////////////////////////////////////////// -Altimeter::~Altimeter() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Altimeter::Altimeter(const Altimeter &_altimeter) - : dataPtr(new AltimeterPrivate(*_altimeter.dataPtr)) -{ -} - -////////////////////////////////////////////////// -Altimeter::Altimeter(Altimeter &&_altimeter) noexcept - : dataPtr(std::exchange(_altimeter.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Altimeter &Altimeter::operator=(const Altimeter &_altimeter) -{ - return *this = Altimeter(_altimeter); -} - -////////////////////////////////////////////////// -Altimeter &Altimeter::operator=(Altimeter &&_altimeter) noexcept -{ - std::swap(this->dataPtr, _altimeter.dataPtr); - return *this; -} - ////////////////////////////////////////////////// Errors Altimeter::Load(ElementPtr _sdf) { diff --git a/src/Atmosphere.cc b/src/Atmosphere.cc index b8901957d..26ba420f3 100644 --- a/src/Atmosphere.cc +++ b/src/Atmosphere.cc @@ -21,7 +21,7 @@ using namespace sdf; -class sdf::AtmospherePrivate +class sdf::Atmosphere::Implementation { /// \brief The type of the atmosphere engine. /// Current options are adiabatic. Defaults to adiabatic if left unspecified. @@ -40,46 +40,10 @@ class sdf::AtmospherePrivate ////////////////////////////////////////////////// Atmosphere::Atmosphere() - : dataPtr(new AtmospherePrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -////////////////////////////////////////////////// -Atmosphere::~Atmosphere() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Atmosphere::Atmosphere(const Atmosphere &_atmosphere) - : dataPtr(new AtmospherePrivate) -{ - this->dataPtr->type = _atmosphere.dataPtr->type; - this->dataPtr->temperature = _atmosphere.dataPtr->temperature; - this->dataPtr->temperatureGradient = _atmosphere.dataPtr->temperatureGradient; - this->dataPtr->pressure = _atmosphere.dataPtr->pressure; -} - -////////////////////////////////////////////////// -Atmosphere::Atmosphere(Atmosphere &&_atmosphere) noexcept - : dataPtr(std::exchange(_atmosphere.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Atmosphere &Atmosphere::operator=(const Atmosphere &_atmosphere) -{ - return *this = Atmosphere(_atmosphere); -} - -///////////////////////////////////////////////// -Atmosphere &Atmosphere::operator=(Atmosphere &&_atmosphere) -{ - std::swap(this->dataPtr, _atmosphere.dataPtr); - return *this; -} - ////////////////////////////////////////////////// Errors Atmosphere::Load(ElementPtr _sdf) { diff --git a/src/Box.cc b/src/Box.cc index d16404c59..90ceb26e3 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -20,7 +20,7 @@ using namespace sdf; // Private data class -class sdf::BoxPrivate +class sdf::Box::Implementation { // Size of the box public: ignition::math::Boxd box{ignition::math::Vector3d::One}; @@ -31,45 +31,10 @@ class sdf::BoxPrivate ///////////////////////////////////////////////// Box::Box() - : dataPtr(new BoxPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Box::~Box() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Box::Box(const Box &_box) - : dataPtr(new BoxPrivate) -{ - this->dataPtr->box = _box.dataPtr->box; - this->dataPtr->sdf = _box.dataPtr->sdf; -} - - -////////////////////////////////////////////////// -Box::Box(Box &&_box) noexcept - : dataPtr(std::exchange(_box.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Box &Box::operator=(const Box &_box) -{ - return *this = Box(_box); -} - -///////////////////////////////////////////////// -Box &Box::operator=(Box &&_box) -{ - std::swap(this->dataPtr, _box.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Box::Load(ElementPtr _sdf) { diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 133276433..ce45a042c 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -8,10 +8,6 @@ if (NOT USE_INTERNAL_URDF) link_directories(${URDF_LIBRARY_DIRS}) endif() -if (USE_EXTERNAL_TINYXML) - link_directories(${tinyxml_LIBRARY_DIRS}) -endif() - set (sources Actor.cc AirPressure.cc @@ -19,19 +15,23 @@ set (sources Atmosphere.cc Box.cc Camera.cc + Capsule.cc Collision.cc Console.cc Converter.cc Cylinder.cc Element.cc + Ellipsoid.cc EmbeddedSdf.cc Error.cc Exception.cc Frame.cc FrameSemantics.cc Filesystem.cc + ForceTorque.cc Geometry.cc Gui.cc + Heightmap.cc ign.cc Imu.cc Joint.cc @@ -56,28 +56,17 @@ set (sources SDFExtension.cc SemanticPose.cc Sensor.cc + Sky.cc Sphere.cc Surface.cc Types.cc Utils.cc Visual.cc World.cc + XmlUtils.cc ) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -if (USE_EXTERNAL_TINYXML) - include_directories(${tinyxml_INCLUDE_DIRS}) -else() - set(sources ${sources} - win/tinyxml/tinystr.cpp - win/tinyxml/tinyxmlerror.cpp - win/tinyxml/tinyxml.cpp - win/tinyxml/tinyxmlparser.cpp) - - install (FILES win/tinyxml/tinyxml.h - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sdformat-${SDF_VERSION}) -endif() - if (USE_INTERNAL_URDF) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/urdf) set(sources ${sources} @@ -93,106 +82,132 @@ else() include_directories(${URDF_INCLUDE_DIRS}) endif() -set (gtest_sources - Actor_TEST.cc - AirPressure_TEST.cc - Altimeter_TEST.cc - Atmosphere_TEST.cc - Box_TEST.cc - Camera_TEST.cc - Collision_TEST.cc - Console_TEST.cc - Cylinder_TEST.cc - Element_TEST.cc - Error_TEST.cc - Exception_TEST.cc - Frame_TEST.cc - Filesystem_TEST.cc - Geometry_TEST.cc - Gui_TEST.cc - Imu_TEST.cc - Joint_TEST.cc - JointAxis_TEST.cc - Lidar_TEST.cc - Light_TEST.cc - Link_TEST.cc - Magnetometer_TEST.cc - Material_TEST.cc - Mesh_TEST.cc - Model_TEST.cc - Noise_TEST.cc - Param_TEST.cc - parser_TEST.cc - Pbr_TEST.cc - Physics_TEST.cc - Plane_TEST.cc - Root_TEST.cc - Scene_TEST.cc - SemanticPose_TEST.cc - SDF_TEST.cc - Sensor_TEST.cc - Sphere_TEST.cc - Surface_TEST.cc - Types_TEST.cc - Visual_TEST.cc - World_TEST.cc -) - -# Build this test file only if Ignition Tools is installed. -if (IGNITION-TOOLS_BINARY_DIRS) - set (gtest_sources ${gtest_sources} - ign_TEST.cc +if (BUILD_SDF_TEST) + set (gtest_sources + Actor_TEST.cc + AirPressure_TEST.cc + Altimeter_TEST.cc + Atmosphere_TEST.cc + Box_TEST.cc + Camera_TEST.cc + Capsule_TEST.cc + Collision_TEST.cc + Console_TEST.cc + Cylinder_TEST.cc + Element_TEST.cc + Ellipsoid_TEST.cc + Error_TEST.cc + Exception_TEST.cc + Frame_TEST.cc + Filesystem_TEST.cc + ForceTorque_TEST.cc + Geometry_TEST.cc + Gui_TEST.cc + Heightmap_TEST.cc + Imu_TEST.cc + Joint_TEST.cc + JointAxis_TEST.cc + Lidar_TEST.cc + Light_TEST.cc + Link_TEST.cc + Magnetometer_TEST.cc + Material_TEST.cc + Mesh_TEST.cc + Model_TEST.cc + Noise_TEST.cc + Param_TEST.cc + parser_TEST.cc + Pbr_TEST.cc + Physics_TEST.cc + Plane_TEST.cc + Root_TEST.cc + Scene_TEST.cc + SemanticPose_TEST.cc + SDF_TEST.cc + Sensor_TEST.cc + Sky_TEST.cc + Sphere_TEST.cc + Surface_TEST.cc + Types_TEST.cc + Visual_TEST.cc + World_TEST.cc ) -endif() -sdf_build_tests(${gtest_sources}) + # Build this test file only if Ignition Tools is installed. + if (IGNITION-TOOLS_BINARY_DIRS) + set (gtest_sources ${gtest_sources} + ign_TEST.cc + ) + endif() -if (NOT WIN32) - set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS Utils.cc) - sdf_build_tests(Utils_TEST.cc) -endif() + sdf_build_tests(${gtest_sources}) -if (NOT WIN32) - set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS FrameSemantics.cc) - sdf_build_tests(FrameSemantics_TEST.cc) -endif() + if (TARGET UNIT_ign_TEST) + # Link the libraries that we always need. + target_link_libraries("UNIT_ign_TEST" + PUBLIC + ignition-cmake${IGN_CMAKE_VER}::utilities + ) + endif() -if (NOT WIN32) - set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS Converter.cc EmbeddedSdf.cc) - sdf_build_tests(Converter_TEST.cc) -endif() + if (NOT WIN32) + set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS Utils.cc) + sdf_build_tests(Utils_TEST.cc) + endif() + + if (NOT WIN32) + set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS XmlUtils.cc) + sdf_build_tests(XmlUtils_TEST.cc) + target_link_libraries(UNIT_XmlUtils_TEST PRIVATE + ${TinyXML2_LIBRARIES}) + endif() + + if (NOT WIN32) + set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS FrameSemantics.cc) + sdf_build_tests(FrameSemantics_TEST.cc) + endif() + + if (NOT WIN32) + set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS Converter.cc EmbeddedSdf.cc XmlUtils.cc) + sdf_build_tests(Converter_TEST.cc) + target_link_libraries(UNIT_Converter_TEST PRIVATE + ${TinyXML2_LIBRARIES}) + endif() -if (NOT WIN32) - set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS SDFExtension.cc parser_urdf.cc) - sdf_build_tests(parser_urdf_TEST.cc) - if (NOT USE_INTERNAL_URDF) - target_compile_options(UNIT_parser_urdf_TEST PRIVATE ${URDF_CFLAGS}) - if (${CMAKE_VERSION} VERSION_GREATER 3.13) - target_link_options(UNIT_parser_urdf_TEST PRIVATE ${URDF_LDFLAGS}) + if (NOT WIN32) + set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS SDFExtension.cc parser_urdf.cc XmlUtils.cc) + sdf_build_tests(parser_urdf_TEST.cc) + if (NOT USE_INTERNAL_URDF) + target_compile_options(UNIT_parser_urdf_TEST PRIVATE ${URDF_CFLAGS}) + if (${CMAKE_VERSION} VERSION_GREATER 3.13) + target_link_options(UNIT_parser_urdf_TEST PRIVATE ${URDF_LDFLAGS}) + endif() + target_link_libraries(UNIT_parser_urdf_TEST PRIVATE ${URDF_LIBRARIES}) endif() - target_link_libraries(UNIT_parser_urdf_TEST PRIVATE ${URDF_LIBRARIES}) + target_link_libraries(UNIT_parser_urdf_TEST PRIVATE + ${TinyXML2_LIBRARIES}) endif() endif() sdf_add_library(${sdf_target} ${sources}) target_compile_features(${sdf_target} PUBLIC cxx_std_17) -target_link_libraries(${sdf_target} PUBLIC ${IGNITION-MATH_LIBRARIES}) +target_link_libraries(${sdf_target} + PUBLIC + ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER} + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} + PRIVATE + ${TinyXML2_LIBRARIES}) + +if (WIN32) + target_compile_definitions(${sdf_target} PRIVATE URDFDOM_STATIC) +endif() target_include_directories(${sdf_target} PUBLIC - ${IGNITION-MATH_INCLUDE_DIRS} $ $ ) -if (USE_EXTERNAL_TINYXML) - target_link_libraries(${sdf_target} PRIVATE ${tinyxml_LIBRARIES}) -else() - # Ignore the warnings from the internal library - set_target_properties(sdformat${SDF_MAJOR_VERSION} PROPERTIES - LINK_FLAGS "/IGNORE:4049 /IGNORE:4217") -endif() - message (STATUS "URDF_LIBRARY_DIRS=${URDF_LIBRARY_DIRS}") message (STATUS "URDF_LIBRARIES=${URDF_LIBRARIES}") diff --git a/src/Camera.cc b/src/Camera.cc index 0c42e63cb..12267f84c 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -46,7 +46,7 @@ static std::array kPixelFormatNames = }; // Private data class -class sdf::CameraPrivate +class sdf::Camera::Implementation { /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -168,42 +168,10 @@ class sdf::CameraPrivate ///////////////////////////////////////////////// Camera::Camera() - : dataPtr(new CameraPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Camera::~Camera() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Camera::Camera(const Camera &_camera) - : dataPtr(new CameraPrivate(*_camera.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Camera::Camera(Camera &&_camera) noexcept - : dataPtr(std::exchange(_camera.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Camera &Camera::operator=(const Camera &_camera) -{ - return *this = Camera(_camera); -} - -////////////////////////////////////////////////// -Camera &Camera::operator=(Camera &&_camera) noexcept -{ - std::swap(this->dataPtr, _camera.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Camera::Load(ElementPtr _sdf) { @@ -678,8 +646,7 @@ const ignition::math::Vector2d &Camera::DistortionCenter() const } ////////////////////////////////////////////////// -void Camera::SetDistortionCenter( - const ignition::math::Vector2d &_center) const +void Camera::SetDistortionCenter(const ignition::math::Vector2d &_center) { this->dataPtr->distortionCenter = _center; } diff --git a/src/Capsule.cc b/src/Capsule.cc new file mode 100644 index 000000000..507cbe0c4 --- /dev/null +++ b/src/Capsule.cc @@ -0,0 +1,136 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include "sdf/Capsule.hh" + +using namespace sdf; + +// Private data class +class sdf::Capsule::Implementation +{ + // A capsule with a length of 1 meter and radius if 0.5 meters. + public: ignition::math::Capsuled capsule{1.0, 0.5}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Capsule::Capsule() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Capsule::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a capsule, but the provided SDF " + "element is null."}); + return errors; + } + + // We need a capsule child element + if (_sdf->GetName() != "capsule") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a capsule geometry, but the provided SDF " + "element is not a ."}); + return errors; + } + + { + std::pair pair = _sdf->Get("radius", + this->dataPtr->capsule.Radius()); + + if (!pair.second) + { + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a radius of " + << this->dataPtr->capsule.Radius() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); + } + this->dataPtr->capsule.SetRadius(pair.first); + } + + { + std::pair pair = _sdf->Get("length", + this->dataPtr->capsule.Length()); + + if (!pair.second) + { + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a length of " + << this->dataPtr->capsule.Length() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); + } + this->dataPtr->capsule.SetLength(pair.first); + } + + return errors; +} + +////////////////////////////////////////////////// +double Capsule::Radius() const +{ + return this->dataPtr->capsule.Radius(); +} + +////////////////////////////////////////////////// +void Capsule::SetRadius(const double _radius) +{ + this->dataPtr->capsule.SetRadius(_radius); +} + +////////////////////////////////////////////////// +double Capsule::Length() const +{ + return this->dataPtr->capsule.Length(); +} + +////////////////////////////////////////////////// +void Capsule::SetLength(const double _length) +{ + this->dataPtr->capsule.SetLength(_length); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Capsule::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +const ignition::math::Capsuled &Capsule::Shape() const +{ + return this->dataPtr->capsule; +} + +///////////////////////////////////////////////// +ignition::math::Capsuled &Capsule::Shape() +{ + return this->dataPtr->capsule; +} diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc new file mode 100644 index 000000000..20b55c644 --- /dev/null +++ b/src/Capsule_TEST.cc @@ -0,0 +1,181 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/Capsule.hh" + +///////////////////////////////////////////////// +TEST(DOMCapsule, Construction) +{ + sdf::Capsule capsule; + EXPECT_EQ(nullptr, capsule.Element()); + // A default capsule has a length of 1 meter and radius if 0.5 meters. + EXPECT_DOUBLE_EQ(IGN_PI * std::pow(0.5, 2) * (1.0 + 4./3. * 0.5), + capsule.Shape().Volume()); + + EXPECT_DOUBLE_EQ(0.5, capsule.Radius()); + EXPECT_DOUBLE_EQ(1.0, capsule.Length()); + + capsule.SetRadius(0.5); + capsule.SetLength(2.3); + + EXPECT_DOUBLE_EQ(0.5, capsule.Radius()); + EXPECT_DOUBLE_EQ(2.3, capsule.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, MoveConstructor) +{ + sdf::Capsule capsule; + capsule.SetRadius(0.2); + capsule.SetLength(3.0); + EXPECT_DOUBLE_EQ(IGN_PI * std::pow(0.2, 2) * (3.0 + 4./3. * 0.2), + capsule.Shape().Volume()); + + sdf::Capsule capsule2(std::move(capsule)); + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); + + EXPECT_DOUBLE_EQ(IGN_PI * std::pow(0.2, 2) * (3.0 + 4./3. * 0.2), + capsule2.Shape().Volume()); + EXPECT_DOUBLE_EQ(0.2, capsule2.Shape().Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Shape().Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, CopyConstructor) +{ + sdf::Capsule capsule; + capsule.SetRadius(0.2); + capsule.SetLength(3.0); + + sdf::Capsule capsule2(capsule); + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, CopyAssignmentOperator) +{ + sdf::Capsule capsule; + capsule.SetRadius(0.2); + capsule.SetLength(3.0); + + sdf::Capsule capsule2; + capsule2 = capsule; + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, MoveAssignmentConstructor) +{ + sdf::Capsule capsule; + capsule.SetRadius(0.2); + capsule.SetLength(3.0); + + sdf::Capsule capsule2; + capsule2 = std::move(capsule); + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, CopyAssignmentAfterMove) +{ + sdf::Capsule capsule1; + capsule1.SetRadius(0.2); + capsule1.SetLength(3.0); + + sdf::Capsule capsule2; + capsule2.SetRadius(2); + capsule2.SetLength(30.0); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Capsule tmp = std::move(capsule1); + capsule1 = capsule2; + capsule2 = tmp; + + EXPECT_DOUBLE_EQ(2, capsule1.Radius()); + EXPECT_DOUBLE_EQ(30, capsule1.Length()); + + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, Load) +{ + sdf::Capsule capsule; + sdf::Errors errors; + + // Null element name + errors = capsule.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, capsule.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = capsule.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, capsule.Element()); + + // Missing and elements + sdf->SetName("capsule"); + errors = capsule.Load(sdf); + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[1].Code()); + EXPECT_NE(std::string::npos, errors[1].Message().find("Invalid ")) + << errors[1].Message(); + EXPECT_NE(nullptr, capsule.Element()); + + // Add a radius element + sdf::ElementPtr radiusDesc(new sdf::Element()); + radiusDesc->SetName("radius"); + radiusDesc->AddValue("double", "1.0", "1", "radius"); + sdf->AddElementDescription(radiusDesc); + sdf::ElementPtr radiusElem = sdf->AddElement("radius"); + radiusElem->Set(2.0); + + // Missing element + sdf->SetName("capsule"); + errors = capsule.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, Shape) +{ + sdf::Capsule capsule; + EXPECT_DOUBLE_EQ(0.5, capsule.Radius()); + EXPECT_DOUBLE_EQ(1.0, capsule.Length()); + + capsule.Shape().SetRadius(0.123); + capsule.Shape().SetLength(0.456); + EXPECT_DOUBLE_EQ(0.123, capsule.Radius()); + EXPECT_DOUBLE_EQ(0.456, capsule.Length()); +} diff --git a/src/Collision.cc b/src/Collision.cc index ee98baca8..da67eb5e4 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -22,11 +22,13 @@ #include "sdf/Geometry.hh" #include "sdf/Surface.hh" #include "sdf/Types.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; -class sdf::CollisionPrivate +class sdf::Collision::Implementation { /// \brief Name of the collision. public: std::string name = ""; @@ -41,7 +43,7 @@ class sdf::CollisionPrivate public: Geometry geom; /// \brief The collision's surface parameters. - public: Surface surface; + public: sdf::Surface surface; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -49,48 +51,16 @@ class sdf::CollisionPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// Collision::Collision() - : dataPtr(new CollisionPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Collision::~Collision() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Collision::Collision(const Collision &_collision) - : dataPtr(new CollisionPrivate(*_collision.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Collision::Collision(Collision &&_collision) noexcept - : dataPtr(std::exchange(_collision.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Collision &Collision::operator=(const Collision &_collision) -{ - return *this = Collision(_collision); -} - -///////////////////////////////////////////////// -Collision &Collision::operator=(Collision &&_collision) -{ - std::swap(this->dataPtr, _collision.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Collision::Load(ElementPtr _sdf) { @@ -146,7 +116,7 @@ std::string Collision::Name() const } ///////////////////////////////////////////////// -void Collision::SetName(const std::string &_name) const +void Collision::SetName(const std::string &_name) { this->dataPtr->name = _name; } @@ -164,7 +134,7 @@ void Collision::SetGeom(const Geometry &_geom) } ///////////////////////////////////////////////// -sdf::Surface *Collision::Surface() const +const sdf::Surface *Collision::Surface() const { return &this->dataPtr->surface; } @@ -207,7 +177,7 @@ void Collision::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Collision::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index c830f15aa..33b49267d 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -158,7 +158,9 @@ TEST(DOMcollision, SetSurface) sdf::Surface surface; ASSERT_NE(nullptr, surface.Contact()); - surface.Contact()->SetCollideBitmask(0x2); + sdf::Contact contact; + contact.SetCollideBitmask(0x2); + surface.SetContact(contact); collision.SetSurface(surface); diff --git a/src/Console.cc b/src/Console.cc index cde264ef5..51e8b7341 100644 --- a/src/Console.cc +++ b/src/Console.cc @@ -24,6 +24,7 @@ #include "sdf/Console.hh" #include "sdf/Filesystem.hh" #include "sdf/Types.hh" +#include "sdf/sdf_config.h" using namespace sdf; @@ -45,6 +46,7 @@ static Console::ConsoleStream g_NullStream(nullptr); Console::Console() : dataPtr(new ConsolePrivate) { +#ifndef SDFORMAT_DISABLE_CONSOLE_LOGFILE // Set up the file that we'll log to. #ifndef _WIN32 const char *home = std::getenv("HOME"); @@ -72,6 +74,7 @@ Console::Console() } std::string logFile = sdf::filesystem::append(logDir, "sdformat.log"); this->dataPtr->logFileStream.open(logFile.c_str(), std::ios::out); +#endif } ////////////////////////////////////////////////// diff --git a/src/Converter.cc b/src/Converter.cc index 4dc66a1d7..dff989ad2 100644 --- a/src/Converter.cc +++ b/src/Converter.cc @@ -30,6 +30,7 @@ #include "Converter.hh" #include "EmbeddedSdf.hh" +#include "XmlUtils.hh" using namespace sdf; @@ -42,12 +43,13 @@ bool EndsWith(const std::string& _a, const std::string& _b) } ///////////////////////////////////////////////// -bool Converter::Convert(TiXmlDocument *_doc, const std::string &_toVersion, +bool Converter::Convert(tinyxml2::XMLDocument *_doc, + const std::string &_toVersion, bool _quiet) { SDF_ASSERT(_doc != nullptr, "SDF XML doc is NULL"); - TiXmlElement *elem = _doc->FirstChildElement("sdf"); + tinyxml2::XMLElement *elem = _doc->FirstChildElement("sdf"); // Check that the element exists if (!elem) @@ -77,7 +79,7 @@ bool Converter::Convert(TiXmlDocument *_doc, const std::string &_toVersion, << " $ gz sdf -c [sdf_file]\n"; } - elem->SetAttribute("version", _toVersion); + elem->SetAttribute("version", _toVersion.c_str()); // The conversion recipes within the embedded files database are named, e.g., // "1.8/1_7.convert" to upgrade from 1.7 to 1.8. @@ -107,12 +109,12 @@ bool Converter::Convert(TiXmlDocument *_doc, const std::string &_toVersion, } // Parse and apply the conversion XML. - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(convertXml); if (xmlDoc.Error()) { sdferr << "Error parsing XML from string: " - << xmlDoc.ErrorDesc() << '\n'; + << xmlDoc.ErrorStr() << '\n'; return false; } ConvertImpl(elem, xmlDoc.FirstChildElement("convert")); @@ -130,7 +132,8 @@ bool Converter::Convert(TiXmlDocument *_doc, const std::string &_toVersion, } ///////////////////////////////////////////////// -void Converter::Convert(TiXmlDocument *_doc, TiXmlDocument *_convertDoc) +void Converter::Convert(tinyxml2::XMLDocument *_doc, + tinyxml2::XMLDocument *_convertDoc) { SDF_ASSERT(_doc != NULL, "SDF XML doc is NULL"); SDF_ASSERT(_convertDoc != NULL, "Convert XML doc is NULL"); @@ -139,28 +142,28 @@ void Converter::Convert(TiXmlDocument *_doc, TiXmlDocument *_convertDoc) } ///////////////////////////////////////////////// -void Converter::ConvertDescendantsImpl(TiXmlElement *_e, TiXmlElement *_c) +void Converter::ConvertDescendantsImpl(tinyxml2::XMLElement *_e, + tinyxml2::XMLElement *_c) { if (!_c->Attribute("descendant_name")) { return; } - if (_e->ValueStr() == "plugin") + if (strcmp(_e->Name(), "plugin") == 0) { return; } - if (_e->ValueStr().find(":") != std::string::npos) + if (strchr(_e->Name(), ':') != nullptr) { return; } - std::string name = _c->Attribute("descendant_name"); - TiXmlElement *e = _e->FirstChildElement(); + tinyxml2::XMLElement *e = _e->FirstChildElement(); while (e) { - if (name == e->ValueStr()) + if (strcmp(e->Name(), _c->Attribute("descendant_name")) == 0) { ConvertImpl(e, _c); } @@ -170,19 +173,20 @@ void Converter::ConvertDescendantsImpl(TiXmlElement *_e, TiXmlElement *_c) } ///////////////////////////////////////////////// -void Converter::ConvertImpl(TiXmlElement *_elem, TiXmlElement *_convert) +void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_convert) { SDF_ASSERT(_elem != NULL, "SDF element is NULL"); SDF_ASSERT(_convert != NULL, "Convert element is NULL"); CheckDeprecation(_elem, _convert); - for (TiXmlElement *convertElem = _convert->FirstChildElement("convert"); + for (auto *convertElem = _convert->FirstChildElement("convert"); convertElem; convertElem = convertElem->NextSiblingElement("convert")) { if (convertElem->Attribute("name")) { - TiXmlElement *elem = _elem->FirstChildElement( + tinyxml2::XMLElement *elem = _elem->FirstChildElement( convertElem->Attribute("name")); while (elem) { @@ -196,48 +200,51 @@ void Converter::ConvertImpl(TiXmlElement *_elem, TiXmlElement *_convert) } } - for (TiXmlElement *childElem = _convert->FirstChildElement(); + for (tinyxml2::XMLElement *childElem = _convert->FirstChildElement(); childElem; childElem = childElem->NextSiblingElement()) { - if (childElem->ValueStr() == "rename") + const auto name = std::string(childElem->Name()); + + if (name == "rename") { Rename(_elem, childElem); } - else if (childElem->ValueStr() == "copy") + else if (name == "copy") { Move(_elem, childElem, true); } - else if (childElem->ValueStr() == "map") + else if (name == "map") { Map(_elem, childElem); } - else if (childElem->ValueStr() == "move") + else if (name == "move") { Move(_elem, childElem, false); } - else if (childElem->ValueStr() == "add") + else if (name == "add") { Add(_elem, childElem); } - else if (childElem->ValueStr() == "remove") + else if (name == "remove") { Remove(_elem, childElem); } - else if (childElem->ValueStr() != "convert") + else if (name != "convert") { - sdferr << "Unknown convert element[" << childElem->ValueStr() << "]\n"; + sdferr << "Unknown convert element[" << name << "]\n"; } } } ///////////////////////////////////////////////// -void Converter::Rename(TiXmlElement *_elem, TiXmlElement *_renameElem) +void Converter::Rename(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_renameElem) { SDF_ASSERT(_elem != NULL, "SDF element is NULL"); SDF_ASSERT(_renameElem != NULL, "Rename element is NULL"); - TiXmlElement *fromConvertElem = _renameElem->FirstChildElement("from"); - TiXmlElement *toConvertElem = _renameElem->FirstChildElement("to"); + auto *fromConvertElem = _renameElem->FirstChildElement("from"); + auto *toConvertElem = _renameElem->FirstChildElement("to"); const char *fromElemName = fromConvertElem->Attribute("element"); const char *fromAttrName = fromConvertElem->Attribute("attribute"); @@ -257,44 +264,33 @@ void Converter::Rename(TiXmlElement *_elem, TiXmlElement *_renameElem) return; } - TiXmlElement *replaceTo = new TiXmlElement(toElemName); + auto *doc = _elem->GetDocument(); + tinyxml2::XMLElement *replaceTo = doc->NewElement(toElemName); if (toAttrName) { replaceTo->SetAttribute(toAttrName, value); } else { - TiXmlText *text = new TiXmlText(value); - // The tinyxml function LinkEndChild takes the pointer and takes ownership - // of the memory, so it is responsible for freeing it later. + tinyxml2::XMLText *text = doc->NewText(value); replaceTo->LinkEndChild(text); } if (fromElemName) { - TiXmlElement *replaceFrom = _elem->FirstChildElement(fromElemName); - if (_elem->ReplaceChild(replaceFrom, *replaceTo) == nullptr) - { - sdferr << "Failed to rename element\n"; - // fall through so we can reclaim memory - } - - // In this case, the tinyxml function ReplaceChild does a deep copy of the - // node that is passed in, so we want to free it here. - delete replaceTo; + tinyxml2::XMLElement *replaceFrom = _elem->FirstChildElement(fromElemName); + _elem->InsertAfterChild(replaceFrom, replaceTo); + _elem->DeleteChild(replaceFrom); } else if (fromAttrName) { - _elem->RemoveAttribute(fromAttrName); - // In this case, the tinyxml function LinkEndChild just takes the pointer - // and takes ownership of the memory, so it is responsible for freeing it - // later. + _elem->DeleteAttribute(fromAttrName); _elem->LinkEndChild(replaceTo); } } ///////////////////////////////////////////////// -void Converter::Add(TiXmlElement *_elem, TiXmlElement *_addElem) +void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) { SDF_ASSERT(_elem != NULL, "SDF element is NULL"); SDF_ASSERT(_addElem != NULL, "Add element is NULL"); @@ -324,10 +320,11 @@ void Converter::Add(TiXmlElement *_elem, TiXmlElement *_addElem) } else { - TiXmlElement *addElem = new TiXmlElement(elementName); + auto *doc = _elem->GetDocument(); + tinyxml2::XMLElement *addElem = doc->NewElement(elementName); if (value) { - TiXmlText *addText = new TiXmlText(value); + tinyxml2::XMLText *addText = doc->NewText(value); addElem->LinkEndChild(addText); } _elem->LinkEndChild(addElem); @@ -335,7 +332,8 @@ void Converter::Add(TiXmlElement *_elem, TiXmlElement *_addElem) } ///////////////////////////////////////////////// -void Converter::Remove(TiXmlElement *_elem, TiXmlElement *_removeElem) +void Converter::Remove(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_removeElem) { SDF_ASSERT(_elem != NULL, "SDF element is NULL"); SDF_ASSERT(_removeElem != NULL, "Move element is NULL"); @@ -352,27 +350,28 @@ void Converter::Remove(TiXmlElement *_elem, TiXmlElement *_removeElem) if (attributeName) { - _elem->RemoveAttribute(attributeName); + _elem->DeleteAttribute(attributeName); } else { - TiXmlElement *childElem = _elem->FirstChildElement(elementName); + tinyxml2::XMLElement *childElem = _elem->FirstChildElement(elementName); + while (childElem) { - _elem->RemoveChild(childElem); + _elem->DeleteChild(childElem); childElem = _elem->FirstChildElement(elementName); } } } ///////////////////////////////////////////////// -void Converter::Map(TiXmlElement *_elem, TiXmlElement *_mapElem) +void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) { SDF_ASSERT(_elem != nullptr, "SDF element is nullptr"); SDF_ASSERT(_mapElem != nullptr, "Map element is nullptr"); - TiXmlElement *fromConvertElem = _mapElem->FirstChildElement("from"); - TiXmlElement *toConvertElem = _mapElem->FirstChildElement("to"); + tinyxml2::XMLElement *fromConvertElem = _mapElem->FirstChildElement("from"); + tinyxml2::XMLElement *toConvertElem = _mapElem->FirstChildElement("to"); if (!fromConvertElem) { @@ -401,8 +400,8 @@ void Converter::Map(TiXmlElement *_elem, TiXmlElement *_mapElem) // create map of input and output values std::map valueMap; - TiXmlElement *fromValueElem = fromConvertElem->FirstChildElement("value"); - TiXmlElement *toValueElem = toConvertElem->FirstChildElement("value"); + auto *fromValueElem = fromConvertElem->FirstChildElement("value"); + auto *toValueElem = toConvertElem->FirstChildElement("value"); if (!fromValueElem) { sdferr << "Map: element requires at least one element.\n"; @@ -455,10 +454,10 @@ void Converter::Map(TiXmlElement *_elem, TiXmlElement *_mapElem) // empty string. Thus we don't check if the fromTokens or toTokens are empty. // get value of the 'from' element/attribute - TiXmlElement *fromElem = _elem; + tinyxml2::XMLElement *fromElem = _elem; for (unsigned int i = 0; i < fromTokens.size()-1; ++i) { - fromElem = fromElem->FirstChildElement(fromTokens[i]); + fromElem = fromElem->FirstChildElement(fromTokens[i].c_str()); if (!fromElem) { // Return when the tokens don't match. Don't output an error message @@ -496,11 +495,11 @@ void Converter::Map(TiXmlElement *_elem, TiXmlElement *_mapElem) // check if destination elements before leaf exist and create if necessary unsigned int newDirIndex = 0; - TiXmlElement *toElem = _elem; - TiXmlElement *childElem = NULL; + tinyxml2::XMLElement *toElem = _elem; + tinyxml2::XMLElement *childElem = NULL; for (unsigned int i = 0; i < toTokens.size()-1; ++i) { - childElem = toElem->FirstChildElement(toTokens[i]); + childElem = toElem->FirstChildElement(toTokens[i].c_str()); if (!childElem) { newDirIndex = i; @@ -519,6 +518,8 @@ void Converter::Map(TiXmlElement *_elem, TiXmlElement *_mapElem) } bool toAttribute = toLeaf[0] == '@'; + auto *doc = _elem->GetDocument(); + // found elements in 'to' string that are not present, so create new // elements if they aren't empty if (!childElem) @@ -532,7 +533,7 @@ void Converter::Map(TiXmlElement *_elem, TiXmlElement *_mapElem) return; } - TiXmlElement *newElem = new TiXmlElement(toTokens[newDirIndex]); + auto *newElem = doc->NewElement(toTokens[newDirIndex].c_str()); toElem->LinkEndChild(newElem); toElem = newElem; newDirIndex++; @@ -545,20 +546,21 @@ void Converter::Map(TiXmlElement *_elem, TiXmlElement *_mapElem) } else { - TiXmlText *text = new TiXmlText(toValue); + tinyxml2::XMLText *text = doc->NewText(toValue); toElem->LinkEndChild(text); } } ///////////////////////////////////////////////// -void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem, +void Converter::Move(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_moveElem, const bool _copy) { SDF_ASSERT(_elem != NULL, "SDF element is NULL"); SDF_ASSERT(_moveElem != NULL, "Move element is NULL"); - TiXmlElement *fromConvertElem = _moveElem->FirstChildElement("from"); - TiXmlElement *toConvertElem = _moveElem->FirstChildElement("to"); + tinyxml2::XMLElement *fromConvertElem = _moveElem->FirstChildElement("from"); + tinyxml2::XMLElement *toConvertElem = _moveElem->FirstChildElement("to"); const char *fromElemStr = fromConvertElem->Attribute("element"); const char *fromAttrStr = fromConvertElem->Attribute("attribute"); @@ -593,10 +595,10 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem, // empty string. Thus we don't check if the fromTokens or toTokens are empty. // get value of the 'from' element/attribute - TiXmlElement *fromElem = _elem; + tinyxml2::XMLElement *fromElem = _elem; for (unsigned int i = 0; i < fromTokens.size()-1; ++i) { - fromElem = fromElem->FirstChildElement(fromTokens[i]); + fromElem = fromElem->FirstChildElement(fromTokens[i].c_str()); if (!fromElem) { // Return when the tokens don't match. Don't output an error message @@ -606,16 +608,16 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem, } const char *fromName = fromTokens.back().c_str(); - const char *value = NULL; + const char *value = nullptr; unsigned int newDirIndex = 0; // get the new element/attribute name const char *toName = toTokens.back().c_str(); - TiXmlElement *toElem = _elem; - TiXmlElement *childElem = NULL; + tinyxml2::XMLElement *toElem = _elem; + tinyxml2::XMLElement *childElem = nullptr; for (unsigned int i = 0; i < toTokens.size()-1; ++i) { - childElem = toElem->FirstChildElement(toTokens[i]); + childElem = toElem->FirstChildElement(toTokens[i].c_str()); if (!childElem) { newDirIndex = i; @@ -628,10 +630,11 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem, // elements if (!childElem) { - int offset = toElemStr != NULL && toAttrStr != NULL ? 0 : 1; + int offset = toElemStr != nullptr && toAttrStr != nullptr ? 0 : 1; while (newDirIndex < (toTokens.size()-offset)) { - TiXmlElement *newElem = new TiXmlElement(toTokens[newDirIndex]); + auto *doc = toElem->GetDocument(); + auto *newElem = doc->NewElement(toTokens[newDirIndex].c_str()); toElem->LinkEndChild(newElem); toElem = newElem; newDirIndex++; @@ -642,7 +645,7 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem, // be specified in the sdf. if (fromElemStr) { - TiXmlElement *moveFrom = fromElem->FirstChildElement(fromName); + tinyxml2::XMLElement *moveFrom = fromElem->FirstChildElement(fromName); // No matching element, so return. if (!moveFrom) @@ -652,30 +655,32 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem, if (toElemStr && !toAttrStr) { - TiXmlElement *moveTo = static_cast(moveFrom->Clone()); + tinyxml2::XMLNode *cloned = DeepClone(moveFrom->GetDocument(), moveFrom); + tinyxml2::XMLElement *moveTo = static_cast(cloned); + moveTo->SetValue(toName); toElem->LinkEndChild(moveTo); } else { - value = GetValue(fromName, NULL, fromElem); + value = GetValue(fromName, nullptr, fromElem); if (!value) { return; } std::string valueStr = value; - toElem->SetAttribute(toAttrStr, valueStr); + toElem->SetAttribute(toAttrStr, valueStr.c_str()); } if (!_copy) { - fromElem->RemoveChild(moveFrom); + fromElem->DeleteChild(moveFrom); } } else if (fromAttrStr) { - value = GetValue(NULL, fromName, fromElem); + value = GetValue(nullptr, fromName, fromElem); if (!value) { @@ -686,26 +691,27 @@ void Converter::Move(TiXmlElement *_elem, TiXmlElement *_moveElem, if (toElemStr) { - TiXmlElement *moveTo = new TiXmlElement(toName); - TiXmlText *text = new TiXmlText(valueStr); + auto *doc = toElem->GetDocument(); + tinyxml2::XMLElement *moveTo = doc->NewElement(toName); + tinyxml2::XMLText *text = doc->NewText(valueStr.c_str()); moveTo->LinkEndChild(text); toElem->LinkEndChild(moveTo); } else if (toAttrStr) { - toElem->SetAttribute(toName, valueStr); + toElem->SetAttribute(toName, valueStr.c_str()); } if (!_copy && fromAttrStr) { - fromElem->RemoveAttribute(fromName); + fromElem->DeleteAttribute(fromName); } } } ///////////////////////////////////////////////// const char *Converter::GetValue(const char *_valueElem, const char *_valueAttr, - TiXmlElement *_elem) + tinyxml2::XMLElement *_elem) { if (_valueElem) { @@ -733,10 +739,11 @@ const char *Converter::GetValue(const char *_valueElem, const char *_valueAttr, } ///////////////////////////////////////////////// -void Converter::CheckDeprecation(TiXmlElement *_elem, TiXmlElement *_convert) +void Converter::CheckDeprecation(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_convert) { // Process deprecated elements - for (TiXmlElement *deprecatedElem = _convert->FirstChildElement("deprecated"); + for (auto *deprecatedElem = _convert->FirstChildElement("deprecated"); deprecatedElem; deprecatedElem = deprecatedElem->NextSiblingElement("deprecated")) { @@ -744,13 +751,13 @@ void Converter::CheckDeprecation(TiXmlElement *_elem, TiXmlElement *_convert) std::vector valueSplit = split(value, "/"); bool found = false; - TiXmlElement *e = _elem; + tinyxml2::XMLElement *e = _elem; std::ostringstream stream; std::string prefix = ""; for (unsigned int i = 0; i < valueSplit.size() && !found; ++i) { - if (e->FirstChildElement(valueSplit[i])) + if (e->FirstChildElement(valueSplit[i].c_str())) { if (stream.str().size() != 0) { @@ -759,9 +766,9 @@ void Converter::CheckDeprecation(TiXmlElement *_elem, TiXmlElement *_convert) } stream << prefix << "<" << valueSplit[i]; - e = e->FirstChildElement(valueSplit[i]); + e = e->FirstChildElement(valueSplit[i].c_str()); } - else if (e->Attribute(valueSplit[i])) + else if (e->Attribute(valueSplit[i].c_str())) { stream << " " << valueSplit[i] << "='" << e->Attribute(valueSplit[i].c_str()) << "'"; diff --git a/src/Converter.hh b/src/Converter.hh index 3237f2a15..c5f7c6e7c 100644 --- a/src/Converter.hh +++ b/src/Converter.hh @@ -17,7 +17,7 @@ #ifndef _SDF_CONVERTER_HH_ #define _SDF_CONVERTER_HH_ -#include +#include #include @@ -37,7 +37,7 @@ namespace sdf /// \param[in] _doc SDF xml doc /// \param[in] _toVersion Version number in string format /// \param[in] _quiet False to be more verbose - public: static bool Convert(TiXmlDocument *_doc, + public: static bool Convert(tinyxml2::XMLDocument *_doc, const std::string &_toVersion, bool _quiet = false); @@ -47,38 +47,38 @@ namespace sdf /// given Convert file. /// \param[in] _doc SDF xml doc /// \param[in] _convertDoc Convert xml doc - public: static void Convert(TiXmlDocument *_doc, - TiXmlDocument *_convertDoc); + public: static void Convert(tinyxml2::XMLDocument *_doc, + tinyxml2::XMLDocument *_convertDoc); /// \endcond /// \brief Implementation of Convert functionality. /// \param[in] _elem SDF xml element tree to convert. /// \param[in] _convert Convert xml element tree. - private: static void ConvertImpl(TiXmlElement *_elem, - TiXmlElement *_convert); + private: static void ConvertImpl(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_convert); /// \brief Recursive helper function for ConvertImpl that converts /// elements named by the descendant_name attribute. /// \param[in] _e SDF xml element tree to convert. /// \param[in] _c Convert xml element tree. - private: static void ConvertDescendantsImpl(TiXmlElement *_e, - TiXmlElement *_c); + private: static void ConvertDescendantsImpl(tinyxml2::XMLElement *_e, + tinyxml2::XMLElement *_c); /// \brief Rename an element or attribute. /// \param[in] _elem The element to be renamed, or the element which /// has the attribute to be renamed. /// \param[in] _renameElem A 'convert' element that describes the rename /// operation. - private: static void Rename(TiXmlElement *_elem, - TiXmlElement *_renameElem); + private: static void Rename(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_renameElem); /// \brief Map values from one element or attribute to another. /// \param[in] _elem Ancestor element of the element or attribute to /// be mapped. /// \param[in] _mapElem A 'convert' element that describes the map /// operation. - private: static void Map(TiXmlElement *_elem, - TiXmlElement *_mapElem); + private: static void Map(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_mapElem); /// \brief Move an element or attribute within a common ancestor element. /// \param[in] _elem Ancestor element of the element or attribute to @@ -86,28 +86,29 @@ namespace sdf /// \param[in] _moveElem A 'convert' element that describes the move /// operation. /// \param[in] _copy True to copy the element - private: static void Move(TiXmlElement *_elem, - TiXmlElement *_moveElem, + private: static void Move(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_moveElem, const bool _copy); /// \brief Add an element or attribute to an element. /// \param[in] _elem The element to receive the value. /// \param[in] _addElem A 'convert' element that describes the add /// operation. - private: static void Add(TiXmlElement *_elem, - TiXmlElement *_addElem); + private: static void Add(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_addElem); /// \brief Remove an element. /// \param[in] _elem The element that has the _removeElem child. /// \param[in] _removeElem The element to remove. - private: static void Remove(TiXmlElement *_elem, TiXmlElement *_removeElem); + private: static void Remove(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_removeElem); private: static const char *GetValue(const char *_valueElem, const char *_valueAttr, - TiXmlElement *_elem); + tinyxml2::XMLElement *_elem); - private: static void CheckDeprecation(TiXmlElement *_elem, - TiXmlElement *_convert); + private: static void CheckDeprecation(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_convert); }; } } diff --git a/src/Converter_TEST.cc b/src/Converter_TEST.cc index cdee13f2e..131535622 100644 --- a/src/Converter_TEST.cc +++ b/src/Converter_TEST.cc @@ -22,6 +22,8 @@ #include "sdf/Filesystem.hh" #include "Converter.hh" +#include "XmlUtils.hh" + #include "test_config.h" //////////////////////////////////////////////////// @@ -66,20 +68,20 @@ TEST(Converter, MoveElemElem) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test moving from elem to elem // Set up a convert file @@ -92,15 +94,15 @@ TEST(Converter, MoveElemElem) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemC")); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemE")); std::string elemValue = convertedElem->FirstChildElement("elemE")->GetText(); @@ -119,7 +121,7 @@ TEST(Converter, MoveElemAttr) std::string xmlString = getXmlString(); // Test moving from elem to attr - TiXmlDocument xmlDoc2; + tinyxml2::XMLDocument xmlDoc2; xmlDoc2.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -130,21 +132,21 @@ TEST(Converter, MoveElemAttr) << " " << " " << ""; - TiXmlDocument convertXmlDoc2; + tinyxml2::XMLDocument convertXmlDoc2; convertXmlDoc2.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc2, &convertXmlDoc2); - TiXmlElement *convertedElem = xmlDoc2.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc2.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); EXPECT_NE(nullptr, convertedElem->Attribute("attrE")); std::string attrValue = convertedElem->Attribute("attrE"); EXPECT_EQ(attrValue, "D"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_FALSE(convertedElem->FirstChildElement("elemD")); } @@ -157,7 +159,7 @@ TEST(Converter, MoveAttrAttr) std::string xmlString = getXmlString(); // Test moving from attr to attr - TiXmlDocument xmlDoc3; + tinyxml2::XMLDocument xmlDoc3; xmlDoc3.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -168,25 +170,25 @@ TEST(Converter, MoveAttrAttr) << " " << " " << ""; - TiXmlDocument convertXmlDoc3; + tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); - TiXmlElement *convertedElem = xmlDoc3.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); EXPECT_NE(nullptr, convertedElem->Attribute("attrE")); std::string attrValue = convertedElem->Attribute("attrE"); EXPECT_EQ(attrValue, "C"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_FALSE(convertedElem->Attribute("attrC")); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -198,7 +200,7 @@ TEST(Converter, MoveAttrElem) std::string xmlString = getXmlString(); // Test moving from attr to elem - TiXmlDocument xmlDoc4; + tinyxml2::XMLDocument xmlDoc4; xmlDoc4.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -209,15 +211,15 @@ TEST(Converter, MoveAttrElem) << " " << " " << ""; - TiXmlDocument convertXmlDoc4; + tinyxml2::XMLDocument convertXmlDoc4; convertXmlDoc4.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc4, &convertXmlDoc4); - TiXmlElement *convertedElem = xmlDoc4.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc4.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemE")); std::string elemValue = convertedElem->FirstChildElement("elemE")->GetText(); EXPECT_EQ(elemValue, "C"); @@ -227,7 +229,7 @@ TEST(Converter, MoveAttrElem) EXPECT_FALSE(convertedElem->Attribute("attrC")); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -239,7 +241,7 @@ TEST(Converter, MoveElemElemMultipleLevels) std::string xmlString = getXmlString(); // Test moving from elem to elem across multiple levels - TiXmlDocument xmlDoc5; + tinyxml2::XMLDocument xmlDoc5; xmlDoc5.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -248,12 +250,12 @@ TEST(Converter, MoveElemElemMultipleLevels) << " " << " " << ""; - TiXmlDocument convertXmlDoc5; + tinyxml2::XMLDocument convertXmlDoc5; convertXmlDoc5.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc5, &convertXmlDoc5); - TiXmlElement *convertedElem = xmlDoc5.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc5.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemE")); std::string elemValue = convertedElem->FirstChildElement("elemE")->GetText(); EXPECT_EQ(elemValue, "D"); @@ -261,7 +263,7 @@ TEST(Converter, MoveElemElemMultipleLevels) ASSERT_NE(nullptr, convertedElem); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_FALSE(convertedElem->FirstChildElement("elemD")); } @@ -274,7 +276,7 @@ TEST(Converter, MoveAttrAttrMultipleLevels) std::string xmlString = getXmlString(); // Test moving from attr to attr across multiple levels - TiXmlDocument xmlDoc6; + tinyxml2::XMLDocument xmlDoc6; xmlDoc6.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -283,24 +285,24 @@ TEST(Converter, MoveAttrAttrMultipleLevels) << " " << " " << ""; - TiXmlDocument convertXmlDoc6; + tinyxml2::XMLDocument convertXmlDoc6; convertXmlDoc6.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc6, &convertXmlDoc6); - TiXmlElement *convertedElem = xmlDoc6.FirstChildElement(); + tinyxml2::XMLElement *convertedElem = xmlDoc6.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + EXPECT_STREQ(convertedElem->Name(), "elemA"); std::string attrValue = convertedElem->Attribute("attrE"); EXPECT_EQ(attrValue, "C"); convertedElem = convertedElem->FirstChildElement("elemB"); ASSERT_NE(nullptr, convertedElem); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_FALSE(convertedElem->Attribute("attrC")); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -312,7 +314,7 @@ TEST(Converter, MoveElemAttrMultipleLevels) std::string xmlString = getXmlString(); // Test moving from elem to attr across multiple levels - TiXmlDocument xmlDoc7; + tinyxml2::XMLDocument xmlDoc7; xmlDoc7.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -321,20 +323,20 @@ TEST(Converter, MoveElemAttrMultipleLevels) << " " << " " << ""; - TiXmlDocument convertXmlDoc7; + tinyxml2::XMLDocument convertXmlDoc7; convertXmlDoc7.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc7, &convertXmlDoc7); - TiXmlElement *convertedElem = xmlDoc7.FirstChildElement(); + tinyxml2::XMLElement *convertedElem = xmlDoc7.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + EXPECT_STREQ(convertedElem->Name(), "elemA"); std::string attrValue = convertedElem->Attribute("attrE"); EXPECT_EQ(attrValue, "D"); convertedElem = convertedElem->FirstChildElement("elemB"); ASSERT_NE(nullptr, convertedElem); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_FALSE(convertedElem->FirstChildElement("elemD")); } @@ -347,7 +349,7 @@ TEST(Converter, MoveAttrElemMultipleLevels) std::string xmlString = getXmlString(); // Test moving from attr to elem across multiple levels - TiXmlDocument xmlDoc8; + tinyxml2::XMLDocument xmlDoc8; xmlDoc8.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -356,13 +358,13 @@ TEST(Converter, MoveAttrElemMultipleLevels) << " " << " " << ""; - TiXmlDocument convertXmlDoc8; + tinyxml2::XMLDocument convertXmlDoc8; convertXmlDoc8.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc8, &convertXmlDoc8); - TiXmlElement *convertedElem = xmlDoc8.FirstChildElement(); + tinyxml2::XMLElement *convertedElem = xmlDoc8.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + EXPECT_STREQ(convertedElem->Name(), "elemA"); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemE")); std::string elemValue = convertedElem->FirstChildElement("elemE")->GetText(); EXPECT_EQ(elemValue, "C"); @@ -370,11 +372,11 @@ TEST(Converter, MoveAttrElemMultipleLevels) ASSERT_NE(nullptr, convertedElem); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_FALSE(convertedElem->Attribute("attrC")); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -386,20 +388,20 @@ TEST(Converter, Add) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test adding element // Set up a convert file @@ -414,15 +416,15 @@ TEST(Converter, Add) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemC")); ASSERT_NE(nullptr, convertedElem->FirstChildElement("elemBB")); std::string elemValue = convertedElem->FirstChildElement("elemBB")->GetText(); @@ -441,7 +443,7 @@ TEST(Converter, AddNoElem) // Set up an xml string for testing std::string xmlString = getXmlString(); - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); // Test adding element @@ -457,23 +459,23 @@ TEST(Converter, AddNoElem) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); // Verify the xml - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -483,7 +485,7 @@ TEST(Converter, AddNoValue) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); // Test adding element @@ -499,22 +501,22 @@ TEST(Converter, AddNoValue) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -526,20 +528,20 @@ TEST(Converter, RemoveElement) std::string xmlString = getRepeatedXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test removing element // Set up a convert file @@ -551,15 +553,15 @@ TEST(Converter, RemoveElement) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemC")); convertedElem = convertedElem->FirstChildElement("elemC"); ASSERT_NE(nullptr, convertedElem); @@ -576,20 +578,20 @@ TEST(Converter, RemoveDescendantElement) std::string xmlString = getRepeatedXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test removing element // Set up a convert file @@ -599,15 +601,16 @@ TEST(Converter, RemoveDescendantElement) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemC")); convertedElem = convertedElem->FirstChildElement("elemC"); ASSERT_NE(nullptr, convertedElem); @@ -642,10 +645,10 @@ TEST(Converter, RemoveDescendantNestedElement) )"; // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertString.c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); @@ -659,15 +662,16 @@ TEST(Converter, RemoveDescendantNestedElement) )"; - TiXmlDocument expectedXmlDoc; + tinyxml2::XMLDocument expectedXmlDoc; expectedXmlDoc.Parse(expectedString.c_str()); - std::stringstream xmlDocOut; - xmlDocOut << xmlDoc; + tinyxml2::XMLPrinter xmlDocOut; + xmlDoc.Print(&xmlDocOut); - std::stringstream expectedXmlDocOut; - expectedXmlDocOut << expectedXmlDoc; - EXPECT_EQ(xmlDocOut.str(), expectedXmlDocOut.str()); + tinyxml2::XMLPrinter expectedXmlDocOut; + expectedXmlDoc.Print(&expectedXmlDocOut); + + EXPECT_STREQ(xmlDocOut.CStr(), expectedXmlDocOut.CStr()); } //////////////////////////////////////////////////// /// Ensure that Converter ignores descendants of or namespaced elements @@ -699,10 +703,10 @@ TEST(Converter, DescendantIgnorePluginOrNamespacedElements) )"; // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertString.c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); @@ -721,15 +725,16 @@ TEST(Converter, DescendantIgnorePluginOrNamespacedElements) )"; - TiXmlDocument expectedXmlDoc; + tinyxml2::XMLDocument expectedXmlDoc; expectedXmlDoc.Parse(expectedString.c_str()); - std::stringstream xmlDocOut; - xmlDocOut << xmlDoc; + tinyxml2::XMLPrinter xmlDocOut; + xmlDoc.Print(&xmlDocOut); + + tinyxml2::XMLPrinter expectedXmlDocOut; + expectedXmlDoc.Print(&expectedXmlDocOut); - std::stringstream expectedXmlDocOut; - expectedXmlDocOut << expectedXmlDoc; - EXPECT_EQ(xmlDocOut.str(), expectedXmlDocOut.str()); + EXPECT_STREQ(xmlDocOut.CStr(), expectedXmlDocOut.CStr()); } //////////////////////////////////////////////////// @@ -741,20 +746,20 @@ TEST(Converter, RemoveElementSubElement) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test adding element // Set up a convert file @@ -764,15 +769,15 @@ TEST(Converter, RemoveElementSubElement) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); ASSERT_TRUE(convertedElem->FirstChildElement("elemC") == nullptr); } @@ -785,20 +790,20 @@ TEST(Converter, RemoveAttr) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test adding element // Set up a convert file @@ -810,15 +815,15 @@ TEST(Converter, RemoveAttr) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); EXPECT_NE(nullptr, convertedElem->FirstChildElement("elemC")); convertedElem = convertedElem->FirstChildElement("elemC"); ASSERT_NE(nullptr, convertedElem); @@ -834,7 +839,7 @@ TEST(Converter, RemoveNoElement) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); // Test adding element @@ -847,22 +852,22 @@ TEST(Converter, RemoveNoElement) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -874,20 +879,20 @@ TEST(Converter, MoveInvalid) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Set up a convert file std::stringstream convertStream; @@ -899,7 +904,7 @@ TEST(Converter, MoveInvalid) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); @@ -907,18 +912,18 @@ TEST(Converter, MoveInvalid) // means that the conversion quietly failed. Make sure the new // document is the same as the original. // Verify the xml - TiXmlElement *convertElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *convertElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemA"); + EXPECT_STREQ(convertElem->Name(), "elemA"); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertElem->Name(), "elemB"); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertElem->Name(), "elemC"); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -930,20 +935,20 @@ TEST(Converter, MoveInvalidPrefix) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Set up a convert file std::stringstream convertStream; @@ -955,7 +960,7 @@ TEST(Converter, MoveInvalidPrefix) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); @@ -963,18 +968,18 @@ TEST(Converter, MoveInvalidPrefix) // means that the conversion quietly failed. Make sure the new // document is the same as the original. // Verify the xml - TiXmlElement *convertElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *convertElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemA"); + EXPECT_STREQ(convertElem->Name(), "elemA"); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertElem->Name(), "elemB"); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertElem->Name(), "elemC"); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -986,20 +991,20 @@ TEST(Converter, CopyElemElem) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test moving from elem to elem // Set up a convert file @@ -1012,22 +1017,22 @@ TEST(Converter, CopyElemElem) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); - TiXmlElement *elemB = convertedElem->FirstChildElement(); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); + tinyxml2::XMLElement *elemB = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, elemB); - EXPECT_EQ(elemB->ValueStr(), "elemB"); - TiXmlElement *elemC = elemB->FirstChild("elemC")->ToElement(); + EXPECT_STREQ(elemB->Name(), "elemB"); + tinyxml2::XMLElement *elemC = elemB->FirstChildElement("elemC"); ASSERT_NE(nullptr, elemC); - TiXmlElement *elemD = elemC->FirstChildElement(); + tinyxml2::XMLElement *elemD = elemC->FirstChildElement(); ASSERT_NE(nullptr, elemD); std::string elemValue = elemD->GetText(); EXPECT_EQ(elemValue, "D"); - TiXmlElement *elemE = elemB->FirstChild("elemE")->ToElement(); + tinyxml2::XMLElement *elemE = elemB->FirstChildElement("elemE"); ASSERT_NE(nullptr, elemE); elemValue = elemE->GetText(); EXPECT_EQ(elemValue, "D"); @@ -1042,20 +1047,20 @@ TEST(Converter, MapInvalid) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Set up an invalid convert file that should do nothing std::stringstream convertStream; @@ -1183,39 +1188,42 @@ TEST(Converter, MapInvalid) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - std::ostringstream xmlDocBefore; - xmlDocBefore << xmlDoc; + + tinyxml2::XMLPrinter printerBefore; + xmlDoc.Print(&printerBefore); + sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); // Only invalid conversion statements. // Make sure the new document is the same as the original. - std::ostringstream xmlDocAfter; - xmlDocAfter << xmlDoc; - EXPECT_EQ(xmlDocBefore.str(), xmlDocAfter.str()); + tinyxml2::XMLPrinter printerAfter; + xmlDoc.Print(&printerAfter); + + EXPECT_STREQ(printerBefore.CStr(), printerAfter.CStr()); // Verify the xml - TiXmlElement *convertElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *convertElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemA"); + EXPECT_STREQ(convertElem->Name(), "elemA"); ASSERT_NE(nullptr, convertElem->Attribute("attrA")); std::string attrValue = convertElem->Attribute("attrA"); EXPECT_EQ("A", attrValue); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertElem->Name(), "elemB"); ASSERT_NE(nullptr, convertElem->Attribute("attrB")); attrValue = convertElem->Attribute("attrB"); EXPECT_EQ("B", attrValue); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertElem->Name(), "elemC"); ASSERT_NE(nullptr, convertElem->Attribute("attrC")); attrValue = convertElem->Attribute("attrC"); EXPECT_EQ("C", attrValue); convertElem = convertElem->FirstChildElement(); ASSERT_NE(nullptr, convertElem); - EXPECT_EQ(convertElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertElem->Name(), "elemD"); ASSERT_NE(nullptr, convertElem->GetText()); std::string textValue = convertElem->GetText(); EXPECT_EQ("D", textValue); @@ -1230,20 +1238,20 @@ TEST(Converter, MapElemElem) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test moving from elem to elem // Set up a convert file @@ -1294,18 +1302,18 @@ TEST(Converter, MapElemElem) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); ASSERT_NE(nullptr, convertedElem->FirstChildElement("elemD")); std::string elemValue = convertedElem->FirstChildElement("elemD")->GetText(); EXPECT_EQ(elemValue, "D"); @@ -1330,7 +1338,7 @@ TEST(Converter, MapElemAttr) std::string xmlString = getXmlString(); // Test moving from elem to attr - TiXmlDocument xmlDoc2; + tinyxml2::XMLDocument xmlDoc2; xmlDoc2.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1345,22 +1353,22 @@ TEST(Converter, MapElemAttr) << " " << " " << ""; - TiXmlDocument convertXmlDoc2; + tinyxml2::XMLDocument convertXmlDoc2; convertXmlDoc2.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc2, &convertXmlDoc2); - TiXmlElement *convertedElem = xmlDoc2.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc2.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); // check for new attribute ASSERT_NE(nullptr, convertedElem->Attribute("attrE")); std::string attrValue = convertedElem->Attribute("attrE"); EXPECT_EQ(attrValue, "E"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_TRUE(convertedElem->FirstChildElement("elemD")); } @@ -1373,7 +1381,7 @@ TEST(Converter, MapAttrAttr) std::string xmlString = getXmlString(); // Test moving from attr to attr - TiXmlDocument xmlDoc3; + tinyxml2::XMLDocument xmlDoc3; xmlDoc3.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1388,15 +1396,15 @@ TEST(Converter, MapAttrAttr) << " " << " " << ""; - TiXmlDocument convertXmlDoc3; + tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); - TiXmlElement *convertedElem = xmlDoc3.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); // check for new attribute ASSERT_NE(nullptr, convertedElem->Attribute("attrE")); std::string attrValue = convertedElem->Attribute("attrE"); @@ -1407,11 +1415,11 @@ TEST(Converter, MapAttrAttr) EXPECT_EQ(attrValue, "B"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_TRUE(convertedElem->Attribute("attrC")); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -1423,7 +1431,7 @@ TEST(Converter, MapAttrElem) std::string xmlString = getXmlString(); // Test moving from attr to elem - TiXmlDocument xmlDoc4; + tinyxml2::XMLDocument xmlDoc4; xmlDoc4.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1438,15 +1446,15 @@ TEST(Converter, MapAttrElem) << " " << " " << ""; - TiXmlDocument convertXmlDoc4; + tinyxml2::XMLDocument convertXmlDoc4; convertXmlDoc4.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc4, &convertXmlDoc4); - TiXmlElement *convertedElem = xmlDoc4.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc4.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); ASSERT_NE(nullptr, convertedElem->FirstChildElement("elemE")); std::string elemValue = convertedElem->FirstChildElement("elemE")->GetText(); EXPECT_EQ(elemValue, "E"); @@ -1460,7 +1468,7 @@ TEST(Converter, MapAttrElem) EXPECT_TRUE(convertedElem->Attribute("attrC")); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -1472,7 +1480,7 @@ TEST(Converter, MapElemElemMultipleLevels) std::string xmlString = getXmlString(); // Test moving from elem to elem across multiple levels - TiXmlDocument xmlDoc5; + tinyxml2::XMLDocument xmlDoc5; xmlDoc5.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1485,13 +1493,14 @@ TEST(Converter, MapElemElemMultipleLevels) << " " << " " << ""; - TiXmlDocument convertXmlDoc5; + tinyxml2::XMLDocument convertXmlDoc5; convertXmlDoc5.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc5, &convertXmlDoc5); - TiXmlElement *convertedElem = xmlDoc5.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); - TiXmlElement *convertedElem2 = convertedElem->FirstChildElement("elemCC"); + tinyxml2::XMLElement *convertedElem = xmlDoc5.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); + tinyxml2::XMLElement *convertedElem2 = + convertedElem->FirstChildElement("elemCC"); ASSERT_NE(nullptr, convertedElem2); convertedElem2 = convertedElem2->FirstChildElement("elemDD"); ASSERT_NE(nullptr, convertedElem2); @@ -1502,7 +1511,7 @@ TEST(Converter, MapElemElemMultipleLevels) ASSERT_NE(nullptr, convertedElem); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_TRUE(convertedElem->FirstChildElement("elemD")); } @@ -1515,7 +1524,7 @@ TEST(Converter, MapAttrAttrMultipleLevels) std::string xmlString = getXmlString(); // Test moving from attr to attr across multiple levels - TiXmlDocument xmlDoc6; + tinyxml2::XMLDocument xmlDoc6; xmlDoc6.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1528,14 +1537,15 @@ TEST(Converter, MapAttrAttrMultipleLevels) << " " << " " << ""; - TiXmlDocument convertXmlDoc6; + tinyxml2::XMLDocument convertXmlDoc6; convertXmlDoc6.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc6, &convertXmlDoc6); - TiXmlElement *convertedElem = xmlDoc6.FirstChildElement(); + tinyxml2::XMLElement *convertedElem = xmlDoc6.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); - TiXmlElement *convertedElem2 = convertedElem->FirstChildElement("elemCC"); + EXPECT_STREQ(convertedElem->Name(), "elemA"); + tinyxml2::XMLElement *convertedElem2 = + convertedElem->FirstChildElement("elemCC"); ASSERT_NE(nullptr, convertedElem2); convertedElem2 = convertedElem2->FirstChildElement("elemDD"); ASSERT_NE(nullptr, convertedElem2); @@ -1545,11 +1555,11 @@ TEST(Converter, MapAttrAttrMultipleLevels) ASSERT_NE(nullptr, convertedElem); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_TRUE(convertedElem->Attribute("attrC")); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -1561,7 +1571,7 @@ TEST(Converter, MapElemAttrMultipleLevels) std::string xmlString = getXmlString(); // Test moving from elem to attr across multiple levels - TiXmlDocument xmlDoc7; + tinyxml2::XMLDocument xmlDoc7; xmlDoc7.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1574,14 +1584,15 @@ TEST(Converter, MapElemAttrMultipleLevels) << " " << " " << ""; - TiXmlDocument convertXmlDoc7; + tinyxml2::XMLDocument convertXmlDoc7; convertXmlDoc7.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc7, &convertXmlDoc7); - TiXmlElement *convertedElem = xmlDoc7.FirstChildElement(); + tinyxml2::XMLElement *convertedElem = xmlDoc7.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); - TiXmlElement *convertedElem2 = convertedElem->FirstChildElement("elemCC"); + EXPECT_STREQ(convertedElem->Name(), "elemA"); + tinyxml2::XMLElement *convertedElem2 = + convertedElem->FirstChildElement("elemCC"); ASSERT_NE(nullptr, convertedElem2); convertedElem2 = convertedElem2->FirstChildElement("elemDD"); ASSERT_NE(nullptr, convertedElem2); @@ -1591,7 +1602,7 @@ TEST(Converter, MapElemAttrMultipleLevels) ASSERT_NE(nullptr, convertedElem); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_TRUE(convertedElem->FirstChildElement("elemD")); } @@ -1604,7 +1615,7 @@ TEST(Converter, MapAttrElemMultipleLevels) std::string xmlString = getXmlString(); // Test moving from attr to elem across multiple levels - TiXmlDocument xmlDoc8; + tinyxml2::XMLDocument xmlDoc8; xmlDoc8.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1617,14 +1628,15 @@ TEST(Converter, MapAttrElemMultipleLevels) << " " << " " << ""; - TiXmlDocument convertXmlDoc8; + tinyxml2::XMLDocument convertXmlDoc8; convertXmlDoc8.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc8, &convertXmlDoc8); - TiXmlElement *convertedElem = xmlDoc8.FirstChildElement(); + tinyxml2::XMLElement *convertedElem = xmlDoc8.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); - TiXmlElement *convertedElem2 = convertedElem->FirstChildElement("elemCC"); + EXPECT_STREQ(convertedElem->Name(), "elemA"); + tinyxml2::XMLElement *convertedElem2 = + convertedElem->FirstChildElement("elemCC"); ASSERT_NE(nullptr, convertedElem2); convertedElem2 = convertedElem2->FirstChildElement("elemDD"); ASSERT_NE(nullptr, convertedElem2); @@ -1635,11 +1647,11 @@ TEST(Converter, MapAttrElemMultipleLevels) ASSERT_NE(nullptr, convertedElem); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); EXPECT_TRUE(convertedElem->Attribute("attrC")); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -1649,20 +1661,20 @@ TEST(Converter, RenameElemElem) std::string xmlString = getXmlString(); // Verify the xml - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - TiXmlElement *childElem = xmlDoc.FirstChildElement(); + tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemA"); + EXPECT_STREQ(childElem->Name(), "elemA"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemB"); + EXPECT_STREQ(childElem->Name(), "elemB"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemC"); + EXPECT_STREQ(childElem->Name(), "elemC"); childElem = childElem->FirstChildElement(); ASSERT_NE(nullptr, childElem); - EXPECT_EQ(childElem->ValueStr(), "elemD"); + EXPECT_STREQ(childElem->Name(), "elemD"); // Test moving from elem to elem // Set up a convert file @@ -1677,20 +1689,20 @@ TEST(Converter, RenameElemElem) << " " << " " << ""; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); - TiXmlElement *elemB = convertedElem->FirstChildElement(); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); + tinyxml2::XMLElement *elemB = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, elemB); - EXPECT_EQ(elemB->ValueStr(), "elemB"); - TiXmlElement *elemC = elemB->FirstChild("elemC")->ToElement(); + EXPECT_STREQ(elemB->Name(), "elemB"); + tinyxml2::XMLElement *elemC = elemB->FirstChildElement("elemC"); ASSERT_NE(nullptr, elemC); - TiXmlElement *elemE = elemC->FirstChildElement(); + tinyxml2::XMLElement *elemE = elemC->FirstChildElement(); ASSERT_NE(nullptr, elemE); - EXPECT_EQ(elemE->ValueStr(), "elemE"); + EXPECT_STREQ(elemE->Name(), "elemE"); std::string elemValue = elemE->GetText(); ASSERT_EQ(elemValue, "D"); } @@ -1702,7 +1714,7 @@ TEST(Converter, RenameAttrAttr) std::string xmlString = getXmlString(); // Test moving from attr to attr - TiXmlDocument xmlDoc3; + tinyxml2::XMLDocument xmlDoc3; xmlDoc3.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1715,19 +1727,19 @@ TEST(Converter, RenameAttrAttr) << " " << " " << ""; - TiXmlDocument convertXmlDoc3; + tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); - TiXmlElement *convertedElem = xmlDoc3.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); - convertedElem = convertedElem->FirstChild("elemE")->ToElement(); + EXPECT_STREQ(convertedElem->Name(), "elemC"); + convertedElem = convertedElem->FirstChildElement("elemE"); std::string attrValue = convertedElem->Attribute("attrE"); EXPECT_EQ(attrValue, "C"); } @@ -1739,7 +1751,7 @@ TEST(Converter, RenameNoFrom) std::string xmlString = getXmlString(); // Test failing to move since there is nothing specified in the "from" element - TiXmlDocument xmlDoc3; + tinyxml2::XMLDocument xmlDoc3; xmlDoc3.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1752,21 +1764,21 @@ TEST(Converter, RenameNoFrom) << " " << " " << ""; - TiXmlDocument convertXmlDoc3; + tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); - TiXmlElement *convertedElem = xmlDoc3.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -1776,7 +1788,7 @@ TEST(Converter, RenameNoTo) std::string xmlString = getXmlString(); // Test failing to move since there is nothing specified in the "to" element - TiXmlDocument xmlDoc3; + tinyxml2::XMLDocument xmlDoc3; xmlDoc3.Parse(xmlString.c_str()); std::stringstream convertStream; convertStream << "" @@ -1789,21 +1801,21 @@ TEST(Converter, RenameNoTo) << " " << " " << ""; - TiXmlDocument convertXmlDoc3; + tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); - TiXmlElement *convertedElem = xmlDoc3.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "elemA"); + tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "elemA"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemB"); + EXPECT_STREQ(convertedElem->Name(), "elemB"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemC"); + EXPECT_STREQ(convertedElem->Name(), "elemC"); convertedElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, convertedElem); - EXPECT_EQ(convertedElem->ValueStr(), "elemD"); + EXPECT_STREQ(convertedElem->Name(), "elemD"); } //////////////////////////////////////////////////// @@ -1814,7 +1826,7 @@ TEST(Converter, GazeboToSDF) << ""; std::string xmlString = stream.str(); - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); EXPECT_FALSE(sdf::Converter::Convert(&xmlDoc, "1.3")); } @@ -1822,8 +1834,8 @@ TEST(Converter, GazeboToSDF) //////////////////////////////////////////////////// TEST(Converter, NullDoc) { - TiXmlDocument xmlDoc; - TiXmlDocument convertXmlDoc; + tinyxml2::XMLDocument xmlDoc; + tinyxml2::XMLDocument convertXmlDoc; ASSERT_THROW(sdf::Converter::Convert(nullptr, &convertXmlDoc), sdf::AssertionInternalError); @@ -1838,7 +1850,7 @@ TEST(Converter, NoVersion) { std::string xmlString(""); - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); ASSERT_FALSE(sdf::Converter::Convert(&xmlDoc, "1.3")); @@ -1849,18 +1861,19 @@ TEST(Converter, SameVersion) { std::string xmlString(""); - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - std::ostringstream xmlDocBefore; - xmlDocBefore << xmlDoc; + tinyxml2::XMLPrinter printerBefore; + xmlDoc.Print(&printerBefore); ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.3")); - std::ostringstream xmlDocAfter; - xmlDocAfter << xmlDoc; + + tinyxml2::XMLPrinter printerAfter; + xmlDoc.Print(&printerAfter); // Expect xmlDoc to be unchanged after conversion - EXPECT_EQ(xmlDocBefore.str(), xmlDocAfter.str()); + EXPECT_STREQ(printerBefore.CStr(), printerAfter.CStr()); } //////////////////////////////////////////////////// @@ -1868,7 +1881,7 @@ TEST(Converter, NewerVersion) { std::string xmlString(""); - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.6")); @@ -1879,7 +1892,7 @@ TEST(Converter, MuchNewerVersion) { std::string xmlString(""); - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.6")); @@ -1925,42 +1938,42 @@ TEST(Converter, IMU_15_to_16) )"; - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); // Convert - TiXmlDocument convertXmlDoc; - convertXmlDoc.LoadFile(CONVERT_DOC_15_16); + tinyxml2::XMLDocument convertXmlDoc; + convertXmlDoc.LoadFile(CONVERT_DOC_15_16.c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); // Check some basic elements - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "sdf"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "sdf"); convertedElem = convertedElem->FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "world"); + EXPECT_STREQ(convertedElem->Name(), "world"); convertedElem = convertedElem->FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "model"); + EXPECT_STREQ(convertedElem->Name(), "model"); convertedElem = convertedElem->FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "link"); + EXPECT_STREQ(convertedElem->Name(), "link"); convertedElem = convertedElem->FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "sensor"); + EXPECT_STREQ(convertedElem->Name(), "sensor"); // Get the imu - TiXmlElement *imuElem = convertedElem->FirstChildElement(); - EXPECT_EQ(imuElem->ValueStr(), "imu"); + tinyxml2::XMLElement *imuElem = convertedElem->FirstChildElement(); + EXPECT_STREQ(imuElem->Name(), "imu"); // Get the angular_velocity - TiXmlElement *angVelElem = imuElem->FirstChildElement(); - EXPECT_EQ(angVelElem->ValueStr(), "angular_velocity"); + tinyxml2::XMLElement *angVelElem = imuElem->FirstChildElement(); + EXPECT_STREQ(angVelElem->Name(), "angular_velocity"); // Get the linear_acceleration - TiXmlElement *linAccElem = angVelElem->NextSiblingElement(); - EXPECT_EQ(linAccElem->ValueStr(), "linear_acceleration"); + tinyxml2::XMLElement *linAccElem = angVelElem->NextSiblingElement(); + EXPECT_STREQ(linAccElem->Name(), "linear_acceleration"); std::array axis = {'x', 'y', 'z'}; - TiXmlElement *angVelAxisElem = angVelElem->FirstChildElement(); - TiXmlElement *linAccAxisElem = linAccElem->FirstChildElement(); + tinyxml2::XMLElement *angVelAxisElem = angVelElem->FirstChildElement(); + tinyxml2::XMLElement *linAccAxisElem = linAccElem->FirstChildElement(); // Iterate over , , and elements under and // @@ -1969,11 +1982,11 @@ TEST(Converter, IMU_15_to_16) EXPECT_EQ(angVelAxisElem->Value()[0], a); EXPECT_EQ(linAccAxisElem->Value()[0], a); - TiXmlElement *angVelAxisNoiseElem = angVelAxisElem->FirstChildElement(); - TiXmlElement *linAccAxisNoiseElem = linAccAxisElem->FirstChildElement(); + auto *angVelAxisNoiseElem = angVelAxisElem->FirstChildElement(); + auto *linAccAxisNoiseElem = linAccAxisElem->FirstChildElement(); - EXPECT_EQ(angVelAxisNoiseElem->ValueStr(), "noise"); - EXPECT_EQ(linAccAxisNoiseElem->ValueStr(), "noise"); + EXPECT_STREQ(angVelAxisNoiseElem->Name(), "noise"); + EXPECT_STREQ(linAccAxisNoiseElem->Name(), "noise"); EXPECT_STREQ(angVelAxisNoiseElem->Attribute("type"), "gaussian"); EXPECT_STREQ(linAccAxisNoiseElem->Attribute("type"), "gaussian"); @@ -2019,33 +2032,33 @@ TEST(Converter, World_15_to_16) )"; - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); // Convert - TiXmlDocument convertXmlDoc; - convertXmlDoc.LoadFile(CONVERT_DOC_15_16); + tinyxml2::XMLDocument convertXmlDoc; + convertXmlDoc.LoadFile(CONVERT_DOC_15_16.c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); // Check some basic elements - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "sdf"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "sdf"); convertedElem = convertedElem->FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "world"); + EXPECT_STREQ(convertedElem->Name(), "world"); convertedElem = convertedElem->FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "physics"); + EXPECT_STREQ(convertedElem->Name(), "physics"); // gravity and magnetic_field should have been moved from physics to world EXPECT_EQ(nullptr, convertedElem->FirstChildElement("gravity")); EXPECT_EQ(nullptr, convertedElem->FirstChildElement("magnetic_field")); // Get the gravity - TiXmlElement *gravityElem = convertedElem->NextSiblingElement("gravity"); + auto *gravityElem = convertedElem->NextSiblingElement("gravity"); ASSERT_NE(nullptr, gravityElem); EXPECT_STREQ(gravityElem->GetText(), "1 0 -9.8"); // Get the magnetic_field - TiXmlElement *magneticFieldElem = + tinyxml2::XMLElement *magneticFieldElem = convertedElem->NextSiblingElement("magnetic_field"); ASSERT_NE(nullptr, magneticFieldElem); EXPECT_STREQ(magneticFieldElem->GetText(), "1 2 3"); @@ -2075,52 +2088,53 @@ TEST(Converter, Pose_16_to_17) )"; - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); // Convert - TiXmlDocument convertXmlDoc; - convertXmlDoc.LoadFile(CONVERT_DOC_16_17); + tinyxml2::XMLDocument convertXmlDoc; + convertXmlDoc.LoadFile(CONVERT_DOC_16_17.c_str()); sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); // Check some basic elements - TiXmlElement *convertedElem = xmlDoc.FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "sdf"); + tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); + EXPECT_STREQ(convertedElem->Name(), "sdf"); convertedElem = convertedElem->FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "world"); + EXPECT_STREQ(convertedElem->Name(), "world"); convertedElem = convertedElem->FirstChildElement(); - EXPECT_EQ(convertedElem->ValueStr(), "model"); + EXPECT_STREQ(convertedElem->Name(), "model"); - TiXmlElement *modelPoseElem = convertedElem->FirstChildElement(); + tinyxml2::XMLElement *modelPoseElem = convertedElem->FirstChildElement(); ASSERT_NE(nullptr, modelPoseElem); - EXPECT_EQ("pose", modelPoseElem->ValueStr()); + EXPECT_STREQ("pose", modelPoseElem->Name()); // frame attribute should have been moved to relative_to EXPECT_EQ(nullptr, modelPoseElem->Attribute("frame")); EXPECT_NE(nullptr, modelPoseElem->Attribute("relative_to")); EXPECT_STREQ("world", modelPoseElem->Attribute("relative_to")); - TiXmlElement *parentLinkElem = modelPoseElem->NextSiblingElement(); + tinyxml2::XMLElement *parentLinkElem = modelPoseElem->NextSiblingElement(); ASSERT_NE(nullptr, parentLinkElem); - EXPECT_EQ("link", parentLinkElem->ValueStr()); + EXPECT_STREQ("link", parentLinkElem->Name()); EXPECT_EQ(nullptr, parentLinkElem->FirstChildElement()); - TiXmlElement *childLinkElem = parentLinkElem->NextSiblingElement(); + tinyxml2::XMLElement *childLinkElem = parentLinkElem->NextSiblingElement(); ASSERT_NE(nullptr, childLinkElem); - EXPECT_EQ("link", childLinkElem->ValueStr()); - TiXmlElement *childLinkPoseElem = childLinkElem->FirstChildElement(); + EXPECT_STREQ("link", childLinkElem->Name()); + tinyxml2::XMLElement *childLinkPoseElem = childLinkElem->FirstChildElement(); ASSERT_NE(nullptr, childLinkPoseElem); - EXPECT_EQ("pose", childLinkPoseElem->ValueStr()); + EXPECT_STREQ("pose", childLinkPoseElem->Name()); // frame attribute should have been moved to relative_to EXPECT_EQ(nullptr, childLinkPoseElem->Attribute("frame")); EXPECT_NE(nullptr, childLinkPoseElem->Attribute("relative_to")); EXPECT_STREQ("joint", childLinkPoseElem->Attribute("relative_to")); - TiXmlElement *jointLinkElem = childLinkElem->NextSiblingElement(); + tinyxml2::XMLElement *jointLinkElem = childLinkElem->NextSiblingElement(); ASSERT_NE(nullptr, jointLinkElem); - EXPECT_EQ("joint", jointLinkElem->ValueStr()); - TiXmlElement *jointLinkPoseElem = jointLinkElem->FirstChildElement("pose"); + EXPECT_STREQ("joint", jointLinkElem->Name()); + tinyxml2::XMLElement *jointLinkPoseElem = + jointLinkElem->FirstChildElement("pose"); ASSERT_NE(nullptr, jointLinkPoseElem); - EXPECT_EQ("pose", jointLinkPoseElem->ValueStr()); + EXPECT_STREQ("pose", jointLinkPoseElem->Name()); // frame attribute should have been moved to relative_to EXPECT_EQ(nullptr, jointLinkPoseElem->Attribute("frame")); EXPECT_NE(nullptr, jointLinkPoseElem->Attribute("relative_to")); diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 0b878e070..c04de1592 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -14,12 +14,13 @@ * limitations under the License. * */ +#include #include "sdf/Cylinder.hh" using namespace sdf; // Private data class -class sdf::CylinderPrivate +class sdf::Cylinder::Implementation { // A cylinder with a length of 1 meter and radius if 0.5 meters. public: ignition::math::Cylinderd cylinder{1.0, 0.5}; @@ -30,44 +31,10 @@ class sdf::CylinderPrivate ///////////////////////////////////////////////// Cylinder::Cylinder() - : dataPtr(new CylinderPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Cylinder::~Cylinder() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Cylinder::Cylinder(const Cylinder &_cylinder) - : dataPtr(new CylinderPrivate) -{ - this->dataPtr->cylinder = _cylinder.dataPtr->cylinder; - this->dataPtr->sdf = _cylinder.dataPtr->sdf; -} - -////////////////////////////////////////////////// -Cylinder::Cylinder(Cylinder &&_cylinder) noexcept - : dataPtr(std::exchange(_cylinder.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Cylinder &Cylinder::operator=(const Cylinder &_cylinder) -{ - return *this = Cylinder(_cylinder); -} - -///////////////////////////////////////////////// -Cylinder &Cylinder::operator=(Cylinder &&_cylinder) -{ - std::swap(this->dataPtr, _cylinder.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Cylinder::Load(ElementPtr _sdf) { @@ -93,45 +60,35 @@ Errors Cylinder::Load(ElementPtr _sdf) return errors; } - if (_sdf->HasElement("radius")) { std::pair pair = _sdf->Get("radius", this->dataPtr->cylinder.Radius()); if (!pair.second) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid data for a geometry. " - "Using a radius of 1."}); + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a radius of " + << this->dataPtr->cylinder.Radius() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); } this->dataPtr->cylinder.SetRadius(pair.first); } - else - { - errors.push_back({ErrorCode::ELEMENT_MISSING, - "Cylinder geometry is missing a child element. " - "Using a radius of 1."}); - } - if (_sdf->HasElement("length")) { std::pair pair = _sdf->Get("length", this->dataPtr->cylinder.Length()); if (!pair.second) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid data for a geometry. " - "Using a length of 1."}); + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a length of " + << this->dataPtr->cylinder.Length() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); } this->dataPtr->cylinder.SetLength(pair.first); } - else - { - errors.push_back({ErrorCode::ELEMENT_MISSING, - "Cylinder geometry is missing a child element. " - "Using a length of 1."}); - } return errors; } diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index ddc74d064..741dc77d4 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -138,10 +138,12 @@ TEST(DOMCylinder, Load) sdf->SetName("cylinder"); errors = cylinder.Load(sdf); ASSERT_EQ(2u, errors.size()); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); - EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[1].Code()); - EXPECT_NE(std::string::npos, errors[1].Message().find("missing a ")); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[1].Code()); + EXPECT_NE(std::string::npos, errors[1].Message().find("Invalid ")) + << errors[1].Message(); EXPECT_NE(nullptr, cylinder.Element()); // Add a radius element @@ -156,8 +158,9 @@ TEST(DOMCylinder, Load) sdf->SetName("cylinder"); errors = cylinder.Load(sdf); ASSERT_EQ(1u, errors.size()); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); - EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); } ///////////////////////////////////////////////// diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc new file mode 100644 index 000000000..694b9a1e1 --- /dev/null +++ b/src/Ellipsoid.cc @@ -0,0 +1,115 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include "sdf/Ellipsoid.hh" + +using namespace sdf; + +// Private data class +class sdf::Ellipsoid::Implementation +{ + /// \brief An ellipsoid with all three radii of 1 meter + public: ignition::math::Ellipsoidd ellipsoid{ignition::math::Vector3d::One}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Ellipsoid::Ellipsoid() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Ellipsoid::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a ellipsoid, but the provided SDF " + "element is null."}); + return errors; + } + + // We need a ellipsoid child element + if (_sdf->GetName() != "ellipsoid") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a ellipsoid geometry, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("radii")) + { + std::pair pair = + _sdf->Get( + "radii", this->dataPtr->ellipsoid.Radii()); + + if (!pair.second) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "Invalid data for a geometry. " + "Using a radii of 1, 1, 1 "}); + } + this->dataPtr->ellipsoid.SetRadii(pair.first); + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Ellipsoid geometry is missing a child element. " + "Using a radii of 1, 1, 1."}); + } + + return errors; +} + +////////////////////////////////////////////////// +ignition::math::Vector3d Ellipsoid::Radii() const +{ + return this->dataPtr->ellipsoid.Radii(); +} + +////////////////////////////////////////////////// +void Ellipsoid::SetRadii(const ignition::math::Vector3d &_radii) +{ + this->dataPtr->ellipsoid.SetRadii(_radii); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Ellipsoid::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +const ignition::math::Ellipsoidd &Ellipsoid::Shape() const +{ + return this->dataPtr->ellipsoid; +} + +///////////////////////////////////////////////// +ignition::math::Ellipsoidd &Ellipsoid::Shape() +{ + return this->dataPtr->ellipsoid; +} diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc new file mode 100644 index 000000000..8a4bf4138 --- /dev/null +++ b/src/Ellipsoid_TEST.cc @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/Ellipsoid.hh" + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Construction) +{ + sdf::Ellipsoid ellipsoid; + EXPECT_EQ(nullptr, ellipsoid.Element()); + // A default ellipsoid has all three radii set to 1 + EXPECT_DOUBLE_EQ(IGN_PI * 4. / 3., ellipsoid.Shape().Volume()); + EXPECT_EQ(ignition::math::Vector3d::One, ellipsoid.Shape().Radii()); + + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + EXPECT_EQ(expectedRadii, ellipsoid.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, MoveConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2(std::move(ellipsoid)); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2(ellipsoid); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyAssignmentOperator) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2; + ellipsoid2 = ellipsoid; + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, MoveAssignmentConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2; + ellipsoid2 = std::move(ellipsoid); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyAssignmentAfterMove) +{ + sdf::Ellipsoid ellipsoid1; + const ignition::math::Vector3d expectedRadii1(1.0, 2.0, 3.0); + ellipsoid1.SetRadii(expectedRadii1); + + sdf::Ellipsoid ellipsoid2; + const ignition::math::Vector3d expectedRadii2(10.0, 20.0, 30.0); + ellipsoid2.SetRadii(expectedRadii2); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Ellipsoid tmp = std::move(ellipsoid1); + ellipsoid1 = ellipsoid2; + ellipsoid2 = tmp; + + EXPECT_EQ(expectedRadii1, ellipsoid2.Shape().Radii()); + EXPECT_EQ(expectedRadii2, ellipsoid1.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Load) +{ + sdf::Ellipsoid ellipsoid; + sdf::Errors errors; + + // Null element name + errors = ellipsoid.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, ellipsoid.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = ellipsoid.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, ellipsoid.Element()); + + // Missing element + sdf->SetName("ellipsoid"); + errors = ellipsoid.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")) + << errors[0].Message(); + EXPECT_NE(nullptr, ellipsoid.Element()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Shape) +{ + sdf::Ellipsoid ellipsoid; + EXPECT_EQ(ignition::math::Vector3d::One, ellipsoid.Radii()); + + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.Shape().SetRadii(expectedRadii); + EXPECT_EQ(expectedRadii, ellipsoid.Radii()); +} diff --git a/src/Exception.cc b/src/Exception.cc index bdc4b47f5..9a452fe10 100644 --- a/src/Exception.cc +++ b/src/Exception.cc @@ -18,21 +18,32 @@ #include #include -#include "ExceptionPrivate.hh" #include "sdf/Console.hh" #include "sdf/Exception.hh" using namespace sdf; +class sdf::Exception::Implementation +{ + /// \brief The error function + public: std::string file; + + /// \brief Line the error occured on + public: std::int64_t line; + + /// \brief The error string + public: std::string str; +}; + ////////////////////////////////////////////////// Exception::Exception() - : dataPtr(new ExceptionPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } ////////////////////////////////////////////////// Exception::Exception(const char *_file, std::int64_t _line, std::string _msg) - : dataPtr(new ExceptionPrivate) + : Exception() { this->dataPtr->file = _file; this->dataPtr->line = _line; @@ -40,30 +51,6 @@ Exception::Exception(const char *_file, std::int64_t _line, std::string _msg) this->Print(); } -////////////////////////////////////////////////// -Exception::~Exception() = default; - -////////////////////////////////////////////////// -Exception::Exception(const Exception &_e) - : dataPtr(new ExceptionPrivate) -{ - this->dataPtr->file = _e.dataPtr->file; - this->dataPtr->line = _e.dataPtr->line; - this->dataPtr->str = _e.dataPtr->str; -} - -////////////////////////////////////////////////// -Exception::Exception(Exception &&_exception) noexcept = default; - -////////////////////////////////////////////////// -Exception &Exception::operator=(const Exception &_exception) -{ - return *this = Exception(_exception); -} - -///////////////////////////////////////////////// -Exception &Exception::operator=(Exception &&_exception) = default; - ////////////////////////////////////////////////// void Exception::Print() const { diff --git a/src/Filesystem.cc b/src/Filesystem.cc index 2e1f206af..9df1aa550 100644 --- a/src/Filesystem.cc +++ b/src/Filesystem.cc @@ -55,6 +55,11 @@ #include "sdf/Filesystem.hh" +/* PATH_MAX is undefined on GNU Hurd */ +#ifndef PATH_MAX +#define PATH_MAX 4096 +#endif + namespace sdf { inline namespace SDF_VERSION_NAMESPACE { @@ -62,7 +67,7 @@ namespace filesystem { /// \internal /// \brief Private data for the DirIter class. -class DirIterPrivate +class DirIter::Implementation { /// \def current /// \brief The current directory item. @@ -144,7 +149,7 @@ std::string current_path() } ////////////////////////////////////////////////// -DirIter::DirIter(const std::string &_in) : dataPtr(new DirIterPrivate) +DirIter::DirIter(const std::string &_in) : DirIter() { this->dataPtr->dirname = _in; @@ -428,7 +433,7 @@ std::string current_path() } ////////////////////////////////////////////////// -DirIter::DirIter(const std::string &_in) : dataPtr(new DirIterPrivate) +DirIter::DirIter(const std::string &_in) : DirIter() { // use a form of search Sebastian Martel reports will work with Win98 this->dataPtr->dirname = _in; @@ -536,7 +541,7 @@ std::string basename(const std::string &_path) } ////////////////////////////////////////////////// -DirIter::DirIter() : dataPtr(new DirIterPrivate) +DirIter::DirIter() : dataPtr(ignition::utils::MakeUniqueImpl()) { this->dataPtr->current = ""; diff --git a/src/Filesystem_TEST.cc b/src/Filesystem_TEST.cc index a0b0ea87c..e96cc0198 100644 --- a/src/Filesystem_TEST.cc +++ b/src/Filesystem_TEST.cc @@ -25,6 +25,11 @@ #include #include +/* PATH_MAX is undefined on GNU Hurd */ +#ifndef PATH_MAX +#define PATH_MAX 4096 +#endif + ///////////////////////////////////////////////// bool create_and_switch_to_temp_dir(std::string &_new_temp_path) { diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc new file mode 100644 index 000000000..b09560454 --- /dev/null +++ b/src/ForceTorque.cc @@ -0,0 +1,154 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include +#include "sdf/ForceTorque.hh" + +using namespace sdf; + +/// \brief Private force torque data. +class sdf::ForceTorque::Implementation +{ + /// \brief Name of the reference frame for the wrench values. + public: ForceTorqueFrame frame = ForceTorqueFrame::CHILD; + + /// \brief The direction of the measured wrench values. + public: ForceTorqueMeasureDirection measure_direction = + ForceTorqueMeasureDirection::CHILD_TO_PARENT; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +////////////////////////////////////////////////// +ForceTorque::ForceTorque() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +////////////////////////////////////////////////// +Errors ForceTorque::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that the provided SDF element is a element. + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "force_torque") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a force torque sensor, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("frame")) + { + std::string frame = _sdf->Get("frame", "child").first; + + if (frame == "parent") + { + this->dataPtr->frame = ForceTorqueFrame::PARENT; + } + else if (frame == "child") + { + this->dataPtr->frame = ForceTorqueFrame::CHILD; + } + else if (frame == "sensor") + { + this->dataPtr->frame = ForceTorqueFrame::SENSOR; + } + else + { + this->dataPtr->frame = ForceTorqueFrame::INVALID; + errors.push_back({ErrorCode::ELEMENT_INVALID, + "ForceTorque element 'frame' is invalid with a value of [" + frame + + "]. Refer to the SDF documentation for the list of valid frames"}); + } + } + + if (_sdf->HasElement("measure_direction")) + { + std::string direction = + _sdf->Get("measure_direction", "child_to_parent").first; + + if (direction == "parent_to_child") + { + this->dataPtr->measure_direction = + ForceTorqueMeasureDirection::PARENT_TO_CHILD; + } + else if (direction == "child_to_parent") + { + this->dataPtr->measure_direction = + ForceTorqueMeasureDirection::CHILD_TO_PARENT; + } + else + { + this->dataPtr->measure_direction = ForceTorqueMeasureDirection::INVALID; + errors.push_back({ErrorCode::ELEMENT_INVALID, + "ForceTorque element 'measure_direction' is invalid with a value " + "of [" + direction + "]. Refer to the SDF documentation for the " + "list of valid frames"}); + } + } + + return errors; +} + +////////////////////////////////////////////////// +sdf::ElementPtr ForceTorque::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +bool ForceTorque::operator!=(const ForceTorque &_ft) const +{ + return !(*this == _ft); +} + +////////////////////////////////////////////////// +bool ForceTorque::operator==(const ForceTorque &_ft) const +{ + return this->dataPtr->frame == _ft.dataPtr->frame && + this->dataPtr->measure_direction == _ft.dataPtr->measure_direction; +} + +////////////////////////////////////////////////// +ForceTorqueFrame ForceTorque::Frame() const +{ + return this->dataPtr->frame; +} + +////////////////////////////////////////////////// +void ForceTorque::SetFrame(ForceTorqueFrame _frame) +{ + this->dataPtr->frame = _frame; +} + +////////////////////////////////////////////////// +ForceTorqueMeasureDirection ForceTorque::MeasureDirection() const +{ + return this->dataPtr->measure_direction; +} + +////////////////////////////////////////////////// +void ForceTorque::SetMeasureDirection( + ForceTorqueMeasureDirection _direction) +{ + this->dataPtr->measure_direction = _direction; +} diff --git a/src/ForceTorque_TEST.cc b/src/ForceTorque_TEST.cc new file mode 100644 index 000000000..57bff24f8 --- /dev/null +++ b/src/ForceTorque_TEST.cc @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/ForceTorque.hh" + +///////////////////////////////////////////////// +TEST(DOMForceTorque, Construction) +{ + sdf::ForceTorque ft; + + EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::CHILD); + ft.SetFrame(sdf::ForceTorqueFrame::PARENT); + EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::PARENT); + + EXPECT_EQ(ft.MeasureDirection(), + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT); + ft.SetMeasureDirection(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD); + EXPECT_EQ(ft.MeasureDirection(), + sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD); + + // Copy constructor + sdf::ForceTorque ft2(ft); + EXPECT_EQ(ft2, ft); + + // Copy operator + sdf::ForceTorque ft3; + ft3 = ft; + EXPECT_EQ(ft3, ft); + + // Move constructor + sdf::ForceTorque ft4(std::move(ft)); + EXPECT_EQ(ft4, ft2); + ft = ft4; + EXPECT_EQ(ft, ft2); + + // Move operator + sdf::ForceTorque ft5; + ft5 = std::move(ft2); + EXPECT_EQ(ft5, ft3); + ft2 = ft5; + EXPECT_EQ(ft2, ft3); + + // Inequality + sdf::ForceTorque ft6; + EXPECT_NE(ft6, ft3); +} + +///////////////////////////////////////////////// +TEST(DOMForceTorque, Load) +{ + sdf::ElementPtr sdf(std::make_shared()); + + sdf::ForceTorque ft; + sdf::Errors errors = ft.Load(sdf); + EXPECT_FALSE(errors.empty()); + EXPECT_TRUE(errors[0].Message().find("is not a ") + != std::string::npos) << errors[0].Message(); + + EXPECT_NE(nullptr, ft.Element()); + EXPECT_EQ(sdf.get(), ft.Element().get()); + + // The ForceTorque::Load function is tested more thouroughly in the + // link_dom.cc integration test. +} diff --git a/src/Frame.cc b/src/Frame.cc index c6a5737df..67834f641 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -20,11 +20,12 @@ #include "sdf/Error.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; -class sdf::FramePrivate +class sdf::Frame::Implementation { /// \brief Name of the frame. public: std::string name = ""; @@ -41,62 +42,23 @@ class sdf::FramePrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Name of graph source. - std::string graphSourceName = ""; + /// \brief The context name of the scoped Pose Relative-To graph + std::string graphScopeContextName = ""; - /// \brief Weak pointer to model's or worlds's Frame Attached-To Graph. - public: std::weak_ptr frameAttachedToGraph; + /// \brief Scoped Frame Attached-To graph at the parent model or world scope. + public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Weak pointer to model's or world's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. + /// TODO (addisu) Make const sdf::PoseRelativeToGraph + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// Frame::Frame() - : dataPtr(new FramePrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Frame::Frame(Frame &&_frame) -{ - this->dataPtr = _frame.dataPtr; - _frame.dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Frame::~Frame() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Frame::Frame(const Frame &_frame) - : dataPtr(new FramePrivate) -{ - *this->dataPtr = *_frame.dataPtr; -} - -////////////////////////////////////////////////// -Frame &Frame::operator=(const Frame &_frame) -{ - if (!this->dataPtr) - { - this->dataPtr = new FramePrivate; - } - *this->dataPtr = *_frame.dataPtr; - return *this; -} - -////////////////////////////////////////////////// -Frame &Frame::operator=(Frame &&_frame) -{ - this->dataPtr = _frame.dataPtr; - _frame.dataPtr = nullptr; - return *this; -} - ///////////////////////////////////////////////// Errors Frame::Load(ElementPtr _sdf) { @@ -152,7 +114,7 @@ const std::string &Frame::Name() const } ///////////////////////////////////////////////// -void Frame::SetName(const std::string &_name) const +void Frame::SetName(const std::string &_name) { this->dataPtr->name = _name; } @@ -195,20 +157,20 @@ void Frame::SetPoseRelativeTo(const std::string &_frame) ///////////////////////////////////////////////// void Frame::SetFrameAttachedToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->frameAttachedToGraph = _graph; } ///////////////////////////////////////////////// void Frame::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (graph) { - this->dataPtr->graphSourceName = graph->sourceName; + this->dataPtr->graphScopeContextName = graph.ScopeContextName(); } } @@ -217,7 +179,7 @@ Errors Frame::ResolveAttachedToBody(std::string &_body) const { Errors errors; - auto graph = this->dataPtr->frameAttachedToGraph.lock(); + auto graph = this->dataPtr->frameAttachedToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -226,7 +188,7 @@ Errors Frame::ResolveAttachedToBody(std::string &_body) const } std::string body; - errors = resolveFrameAttachedToBody(body, *graph, this->dataPtr->name); + errors = resolveFrameAttachedToBody(body, graph, this->dataPtr->name); if (errors.empty()) { _body = body; @@ -243,9 +205,10 @@ sdf::SemanticPose Frame::SemanticPose() const relativeTo = this->dataPtr->attachedTo; } return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, relativeTo, - this->dataPtr->graphSourceName, + this->dataPtr->graphScopeContextName, this->dataPtr->poseRelativeToGraph); } diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 6f606e3a4..136d36854 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -14,7 +14,10 @@ * limitations under the License. * */ +#include #include +#include +#include #include "sdf/Element.hh" #include "sdf/Error.hh" @@ -26,11 +29,23 @@ #include "sdf/World.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" namespace sdf { inline namespace SDF_VERSION_NAMESPACE { +// Helpful functions when debugging in gdb +void printGraph(const ScopedGraph &_graph) +{ + std::stringstream ss; + std::cout << _graph.Graph() << std::endl; +} +void printGraph(const ScopedGraph &_graph) +{ + std::cout << _graph.Graph() << std::endl; +} + // The following two functions were originally submitted to ign-math, // but were not accepted as they were not generic enough. // For now, they will be kept here. @@ -47,21 +62,19 @@ inline namespace SDF_VERSION_NAMESPACE { /// \return A source vertex paired with a vector of the edges leading the /// source to the starting vertex, or a NullVertex paired with an empty /// vector if a cycle or vertex with multiple incoming edges are detected. -template -std::pair &, - std::vector< ignition::math::graph::DirectedEdge > > -FindSourceVertex( - const ignition::math::graph::DirectedGraph &_graph, - const ignition::math::graph::VertexId _id, - Errors &_errors) +template +std::pair::Vertex &, + std::vector::Edge>> +FindSourceVertex(const ScopedGraph &_graph, + const ignition::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = ignition::math::graph::DirectedEdge; - using Vertex = ignition::math::graph::Vertex; + using DirectedEdge = typename ScopedGraph::Edge; + using Vertex = typename ScopedGraph::Vertex; using VertexId = ignition::math::graph::VertexId; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; - std::reference_wrapper vertex(_graph.VertexFromId(_id)); + std::reference_wrapper vertex(_graph.Graph().VertexFromId(_id)); if (!vertex.get().Valid()) { _errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, @@ -70,10 +83,16 @@ FindSourceVertex( return PairType(Vertex::NullVertex, EdgesType()); } + if (_id == _graph.ScopeVertexId()) + { + // This is the source. + return PairType(vertex, EdgesType()); + } + std::set visited; visited.insert(vertex.get().Id()); - auto incidentsTo = _graph.IncidentsTo(vertex); + auto incidentsTo = _graph.Graph().IncidentsTo(vertex); while (!incidentsTo.empty()) { if (incidentsTo.size() != 1) @@ -84,7 +103,7 @@ FindSourceVertex( return PairType(Vertex::NullVertex, EdgesType()); } auto const &edge = incidentsTo.begin()->second; - vertex = _graph.VertexFromId(edge.get().Vertices().first); + vertex = _graph.Graph().VertexFromId(edge.get().Vertices().first); edges.push_back(edge); if (visited.count(vertex.get().Id())) { @@ -93,8 +112,18 @@ FindSourceVertex( vertex.get().Name() + "]."}); return PairType(Vertex::NullVertex, EdgesType()); } + if (vertex.get().Id() == _graph.ScopeVertexId()) + { + // This is the source. + break; + } visited.insert(vertex.get().Id()); - incidentsTo = _graph.IncidentsTo(vertex); + incidentsTo = _graph.Graph().IncidentsTo(vertex); + } + if (vertex.get().Id() != _graph.ScopeVertexId()) + { + // Error, the root vertex is not the same as the the source + return PairType(Vertex::NullVertex, EdgesType()); } return PairType(vertex, edges); @@ -111,21 +140,21 @@ FindSourceVertex( /// \return A sink vertex paired with a vector of the edges leading the /// sink to the starting vertex, or a NullVertex paired with an empty /// vector if a cycle or vertex with multiple outgoing edges are detected. -template -std::pair &, - std::vector< ignition::math::graph::DirectedEdge > > +template +std::pair::Vertex &, + std::vector::Edge>> FindSinkVertex( - const ignition::math::graph::DirectedGraph &_graph, + const ScopedGraph &_graph, const ignition::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = ignition::math::graph::DirectedEdge; - using Vertex = ignition::math::graph::Vertex; + using DirectedEdge = typename ScopedGraph::Edge; + using Vertex = typename ScopedGraph::Vertex; using VertexId = ignition::math::graph::VertexId; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; - std::reference_wrapper vertex(_graph.VertexFromId(_id)); + std::reference_wrapper vertex(_graph.Graph().VertexFromId(_id)); if (!vertex.get().Valid()) { _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, @@ -137,7 +166,7 @@ FindSinkVertex( std::set visited; visited.insert(vertex.get().Id()); - auto incidentsFrom = _graph.IncidentsFrom(vertex); + auto incidentsFrom = _graph.Graph().IncidentsFrom(vertex); while (!incidentsFrom.empty()) { if (incidentsFrom.size() != 1) @@ -148,7 +177,7 @@ FindSinkVertex( return PairType(Vertex::NullVertex, EdgesType()); } auto const &edge = incidentsFrom.begin()->second; - vertex = _graph.VertexFromId(edge.get().Vertices().second); + vertex = _graph.Graph().VertexFromId(edge.get().Vertices().second); edges.push_back(edge); if (visited.count(vertex.get().Id())) { @@ -158,25 +187,97 @@ FindSinkVertex( return PairType(Vertex::NullVertex, EdgesType()); } visited.insert(vertex.get().Id()); - incidentsFrom = _graph.IncidentsFrom(vertex); + incidentsFrom = _graph.Graph().IncidentsFrom(vertex); } return PairType(vertex, edges); } ///////////////////////////////////////////////// -Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const Model *_model) +std::pair + modelCanonicalLinkAndRelativeName(const Model *_model) { - Errors errors; + if (nullptr == _model) + { + return std::make_pair(nullptr, ""); + } + return _model->CanonicalLinkAndRelativeName(); +} - // add implicit model frame vertex first - const std::string scopeName = "__model__"; - _out.scopeName = scopeName; - auto modelFrameId = - _out.graph.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); - _out.map[scopeName] = modelFrameId; +///////////////////////////////////////////////// +/// \brief Resolve the pose of a model taking into account the placement frame +/// attribute. This function is used to calculate the pose of the edge between a +/// model and its parent. +/// +/// Since this function calls sdf::resolvePoseRelativeToRoot to initially +/// resolve the pose of the placement frame, the passed in graph must already +/// have an edge between the input model and its parent frame. This edge will +/// later be updated with the pose calculated by this function. +/// +/// Notation used in this function: +/// Pf - The placement frame inside the model +/// R - The `relative_to` frame of the placement frame's //pose element. +/// M - The input model's implicit frame (__model__) +/// X_RPf - The pose of frame Pf relative to frame R +/// X_RM - The pose of the frame M relative to frame R +/// X_MPf - The pose of the frame Pf relative to frame M +/// +/// Example: +/// +/// +/// +/// +/// {X_RPf} +/// +/// +/// +/// +/// +/// When the placement_frame is specified in an SDFormat file, //model/pose +/// becomes the pose of the placement frame (X_RPf) instead of the pose of the +/// model frame (X_RM). However, when the model is inserted into a pose graph of +/// the parent scope, only the pose (X_RM) of the __model__ frame can be used. +/// i.e, X_RPf cannot be represented in the PoseRelativeTo graph. +/// Thus, the X_RM has to be calculated such that X_RPf = X_RM * X_MPf. +// +/// \param[in] _model Input model whose pose to resolve relative to its parent +/// \param[in] _graph The PoseRelativeTo graph with the same scope as the +/// frame referenced by the placement frame attribute of the input model. i.e, +/// the input model's graph. +/// \param[out] _resolvedPose The resolved pose (X_RM). If an error was +/// encountered during `sdf::resolvePoseRelativeToRoot`, _resolvedPose will not +/// be modified. +static Errors resolveModelPoseWithPlacementFrame(const sdf::Model &_model, + const sdf::ScopedGraph &_graph, + ignition::math::Pose3d &_resolvedPose) +{ + sdf::Errors errors; + // Alias for better pose notation + ignition::math::Pose3d &X_RM = _resolvedPose; + const ignition::math::Pose3d &X_RPf = _model.RawPose(); + + // If the model has a placement frame, calculate the necessary pose to use + if (!_model.PlacementFrameName().empty()) + { + ignition::math::Pose3d X_MPf; + + errors = sdf::resolvePoseRelativeToRoot( + X_MPf, _graph, _model.PlacementFrameName()); + if (errors.empty()) + { + X_RM = X_RPf * X_MPf.Inverse(); + } + } + + return errors; +} + +///////////////////////////////////////////////// +Errors buildFrameAttachedToGraph( + ScopedGraph &_out, const Model *_model, bool _root) +{ + Errors errors; if (!_model) { @@ -190,37 +291,52 @@ Errors buildFrameAttachedToGraph( "Invalid model element in sdf::Model."}); return errors; } - else if (_model->LinkCount() < 1) + else if (_model->LinkCount() == 0 && _model->ModelCount() == 0 && + !_model->Static()) { errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, "A model must have at least one link."}); return errors; } - // identify canonical link - const sdf::Link *canonicalLink = nullptr; - if (_model->CanonicalLinkName().empty()) - { - canonicalLink = _model->LinkByIndex(0); - } - else - { - canonicalLink = _model->LinkByName(_model->CanonicalLinkName()); - } - if (nullptr == canonicalLink) + auto frameType = + _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; + + const std::string scopeContextName = "__model__"; + + auto rootId = ignition::math::graph::kNullId; + if (_root) { - // return early - errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID, - "canonical_link with name[" + _model->CanonicalLinkName() + - "] not found in model with name[" + _model->Name() + "]."}); - return errors; + // The __root__ vertex identifies the scope that contains a root level + // model. In the PoseRelativeTo graph, this vertex is connected to the model + // with an edge that holds the value of //model/pose. However, in the + // FrameAttachedTo graph, the vertex is disconnected because nothing is + // attached to it. Since only links and static models are allowed to be + // disconnected in this graph, the STATIC_MODEL was chosen as its frame + // type. A different frame type could potentially be used, but that adds + // more complexity to the validateFrameAttachedToGraph code. + _out = _out.AddScopeVertex( + "", "__root__", scopeContextName, sdf::FrameType::STATIC_MODEL); + rootId = _out.ScopeVertexId(); } + const auto modelId = _out.AddVertex(_model->Name(), frameType).Id(); + + auto outModel = _out.AddScopeVertex( + _model->Name(), scopeContextName, scopeContextName, frameType); + const auto modelFrameId = outModel.ScopeVertexId(); + + // Add an aliasing edge from the model vertex to the + // corresponding vertex in the FrameAttachedTo graph of the child model, + auto &edge = outModel.AddEdge({modelId, modelFrameId}, true); + // Set the edge weight to 0 to indicate that this is an aliasing edge. + edge.SetWeight(0); + // add link vertices for (uint64_t l = 0; l < _model->LinkCount(); ++l) { auto link = _model->LinkByIndex(l); - if (_out.map.count(link->Name()) > 0) + if (outModel.Count(link->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Link with non-unique name [" + link->Name() + @@ -228,22 +344,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto linkId = - _out.graph.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); - _out.map[link->Name()] = linkId; - - // add edge from implicit model frame vertex to canonical link - if (link == canonicalLink) - { - _out.graph.AddEdge({modelFrameId, linkId}, true); - } + outModel.AddVertex(link->Name(), sdf::FrameType::LINK); } - // add joint vertices and edges to child link + // add joint vertices for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - if (_out.map.count(joint->Name()) > 0) + if (outModel.Count(joint->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Joint with non-unique name [" + joint->Name() + @@ -251,28 +359,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto jointId = - _out.graph.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); - _out.map[joint->Name()] = jointId; - - auto childLink = _model->LinkByName(joint->ChildLinkName()); - if (nullptr == childLink) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "Child link with name[" + joint->ChildLinkName() + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); - continue; - } - auto childLinkId = _out.map.at(childLink->Name()); - _out.graph.AddEdge({jointId, childLinkId}, true); + outModel.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } // add frame vertices for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (outModel.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -280,33 +374,65 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + outModel.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); + } + + // add nested model vertices + for (uint64_t m = 0; m < _model->ModelCount(); ++m) + { + auto nestedModel = _model->ModelByIndex(m); + if (outModel.Count(nestedModel->Name()) > 0) + { + errors.push_back({ErrorCode::DUPLICATE_NAME, + "Nested model with non-unique name [" + nestedModel->Name() + + "] detected in model with name [" + _model->Name() + + "]."}); + continue; + } + auto nestedErrors = buildFrameAttachedToGraph(outModel, nestedModel, false); + errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); + } + + // add edges from joint to child frames + for (uint64_t j = 0; j < _model->JointCount(); ++j) + { + auto joint = _model->JointByIndex(j); + auto jointId = outModel.VertexIdByName(joint->Name()); + auto childFrameName = joint->ChildLinkName(); + if (outModel.Count(childFrameName) != 1) + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "Child frame with name[" + childFrameName + + "] specified by joint with name[" + joint->Name() + + "] not found in model with name[" + _model->Name() + "]."}); + continue; + } + auto childFrameId = outModel.VertexIdByName(childFrameName); + outModel.AddEdge({jointId, childFrameId}, true); } // add frame edges for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - auto frameId = _out.map.at(frame->Name()); + auto frameId = outModel.VertexIdByName(frame->Name()); // look for vertex in graph that matches attached_to value std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) { - // if the attached-to name is empty, use the scope name - attachedTo = scopeName; + // if the attached-to name is empty, use the scope context name + attachedTo = scopeContextName; } - if (_out.map.count(attachedTo) != 1) + if (outModel.Count(attachedTo) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "attached_to name[" + attachedTo + "] specified by frame with name[" + frame->Name() + - "] does not match a link, joint, or frame name " + "] does not match a nested model, link, joint, or frame name " "in model with name[" + _model->Name() + "]."}); continue; } - auto attachedToId = _out.map[attachedTo]; + auto attachedToId = outModel.VertexIdByName(attachedTo); bool edgeData = true; if (frame->Name() == frame->AttachedTo()) { @@ -318,7 +444,49 @@ Errors buildFrameAttachedToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({frameId, attachedToId}, edgeData); + outModel.AddEdge({frameId, attachedToId}, edgeData); + } + + // identify canonical link, which may be nested + const auto[canonicalLink, canonicalLinkName] = + modelCanonicalLinkAndRelativeName(_model); + if (!_model->Static()) + { + if (nullptr == canonicalLink) + { + if (canonicalLinkName.empty()) + { + if (_model->ModelCount() == 0) + { + errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, + "A model must have at least one link."}); + } + else + { + // The canonical link was not found, but the model could have a + // descendant that has a static model, so simply create an edge to the + // first model and let the attached_to frame resolution take care of + // finding the canonical link + auto firstChildModelId = + outModel.VertexIdByName(_model->ModelByIndex(0)->Name()); + outModel.AddEdge({modelFrameId, firstChildModelId }, true); + } + } + else + { + errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID, + "canonical_link with name[" + canonicalLinkName + + "] not found in model with name[" + _model->Name() + "]."}); + } + // return early + return errors; + } + else + { + // Add an edge from the implicit model frame to the canonical link found. + auto linkId = outModel.VertexIdByName(canonicalLinkName); + outModel.AddEdge({modelFrameId, linkId}, true); + } } return errors; @@ -326,18 +494,10 @@ Errors buildFrameAttachedToGraph( ///////////////////////////////////////////////// Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const World *_world) + ScopedGraph &_out, const World *_world) { Errors errors; - // add implicit world frame vertex first - const std::string scopeName = "world"; - _out.scopeName = scopeName; - auto worldFrameId = - _out.graph.AddVertex(scopeName, sdf::FrameType::WORLD).Id(); - _out.map[scopeName] = worldFrameId; - - if (!_world) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -351,11 +511,16 @@ Errors buildFrameAttachedToGraph( return errors; } + // add implicit world frame vertex first + const std::string scopeContextName = "world"; + _out = _out.AddScopeVertex( + "", scopeContextName, scopeContextName, sdf::FrameType::WORLD); + // add model vertices - for (uint64_t l = 0; l < _world->ModelCount(); ++l) + for (uint64_t m = 0; m < _world->ModelCount(); ++m) { - auto model = _world->ModelByIndex(l); - if (_out.map.count(model->Name()) > 0) + auto model = _world->ModelByIndex(m); + if (_out.Count(model->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Model with non-unique name [" + model->Name() + @@ -363,16 +528,15 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto modelId = - _out.graph.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); - _out.map[model->Name()] = modelId; + auto modelErrors = buildFrameAttachedToGraph(_out, model, false); + errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); } // add frame vertices for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (_out.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -380,31 +544,29 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); } // add frame edges for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - auto frameId = _out.map.at(frame->Name()); + auto frameId = _out.VertexIdByName(frame->Name()); // look for vertex in graph that matches attached_to value std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) { - // if the attached-to name is empty, use the scope name - attachedTo = scopeName; - if (_out.map.count(scopeName) != 1) + // if the attached-to name is empty, use the scope context name + attachedTo = scopeContextName; + if (_out.Count(scopeContextName) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + - scopeName + "] not found in map."}); + scopeContextName + "] not found in map."}); continue; } } - if (_out.map.count(attachedTo) != 1) + if (_out.Count(attachedTo) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "attached_to name[" + attachedTo + @@ -413,7 +575,7 @@ Errors buildFrameAttachedToGraph( "in world with name[" + _world->Name() + "]."}); continue; } - auto attachedToId = _out.map[attachedTo]; + auto attachedToId = _out.VertexIdByName(attachedTo); bool edgeData = true; if (frame->Name() == frame->AttachedTo()) { @@ -425,7 +587,7 @@ Errors buildFrameAttachedToGraph( "], causing a graph cycle " "in world with name[" + _world->Name() + "]."}); } - _out.graph.AddEdge({frameId, attachedToId}, edgeData); + _out.AddEdge({frameId, attachedToId}, edgeData); } return errors; @@ -433,7 +595,7 @@ Errors buildFrameAttachedToGraph( ///////////////////////////////////////////////// Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const Model *_model) + ScopedGraph &_out, const Model *_model, bool _root) { Errors errors; @@ -450,18 +612,32 @@ Errors buildPoseRelativeToGraph( return errors; } - // add implicit model frame vertex first - const std::string sourceName = "__model__"; - _out.sourceName = sourceName; - auto modelFrameId = - _out.graph.AddVertex(sourceName, sdf::FrameType::MODEL).Id(); - _out.map[sourceName] = modelFrameId; + const std::string scopeContextName = "__model__"; + auto rootId = ignition::math::graph::kNullId; + // add the model frame vertex first + if (_root) + { + _out = _out.AddScopeVertex( + "", "__root__", scopeContextName, sdf::FrameType::MODEL); + rootId = _out.ScopeVertexId(); + } + auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); + auto outModel = _out.AddScopeVertex(_model->Name(), scopeContextName, + scopeContextName, sdf::FrameType::MODEL); + auto modelFrameId = outModel.ScopeVertexId(); + + // Add an aliasing edge from the model vertex to the + // corresponding vertex in the PoseRelativeTo graph of the child model, + // i.e, to the ::__model__ vertex. + auto &edge = _out.AddEdge({modelId, modelFrameId}, {}); + // Set the edge weight to 0 to indicate that this is an aliasing edge. + edge.SetWeight(0); // add link vertices and default edge if relative_to is empty for (uint64_t l = 0; l < _model->LinkCount(); ++l) { auto link = _model->LinkByIndex(l); - if (_out.map.count(link->Name()) > 0) + if (outModel.Count(link->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Link with non-unique name [" + link->Name() + @@ -470,21 +646,20 @@ Errors buildPoseRelativeToGraph( continue; } auto linkId = - _out.graph.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); - _out.map[link->Name()] = linkId; + outModel.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); if (link->PoseRelativeTo().empty()) { // relative_to is empty, so add edge from implicit model frame to link - _out.graph.AddEdge({modelFrameId, linkId}, link->RawPose()); + outModel.AddEdge({modelFrameId, linkId}, link->RawPose()); } } - // add joint vertices and default edge if relative_to is empty + // add joint vertices for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - if (_out.map.count(joint->Name()) > 0) + if (outModel.Count(joint->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Joint with non-unique name [" + joint->Name() + @@ -492,25 +667,7 @@ Errors buildPoseRelativeToGraph( "]."}); continue; } - auto jointId = - _out.graph.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); - _out.map[joint->Name()] = jointId; - - if (joint->PoseRelativeTo().empty()) - { - // relative_to is empty, so add edge from joint to child link - auto childLink = _model->LinkByName(joint->ChildLinkName()); - if (nullptr == childLink) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "Child link with name[" + joint->ChildLinkName() + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); - continue; - } - auto childLinkId = _out.map.at(childLink->Name()); - _out.graph.AddEdge({childLinkId, jointId}, joint->RawPose()); - } + outModel.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } // add frame vertices and default edge if both @@ -518,7 +675,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (outModel.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -527,16 +684,32 @@ Errors buildPoseRelativeToGraph( continue; } auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + outModel.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) { // add edge from implicit model frame to frame - _out.graph.AddEdge({modelFrameId, frameId}, frame->RawPose()); + outModel.AddEdge({modelFrameId, frameId}, frame->RawPose()); } } + // add nested model vertices and default edge if relative_to is empty + for (uint64_t m = 0; m < _model->ModelCount(); ++m) + { + auto nestedModel = _model->ModelByIndex(m); + if (outModel.Count(nestedModel->Name()) > 0) + { + errors.push_back({ErrorCode::DUPLICATE_NAME, + "Nested model with non-unique name [" + nestedModel->Name() + + "] detected in model with name [" + _model->Name() + + "]."}); + continue; + } + + auto nestedErrors = buildPoseRelativeToGraph(outModel, nestedModel, false); + errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); + } + // now that all vertices have been added to the graph, // add the edges that reference other vertices @@ -545,25 +718,25 @@ Errors buildPoseRelativeToGraph( auto link = _model->LinkByIndex(l); // check if we've already added a default edge - const std::string relativeTo = link->PoseRelativeTo(); + const std::string &relativeTo = link->PoseRelativeTo(); if (relativeTo.empty()) { continue; } - auto linkId = _out.map.at(link->Name()); + auto linkId = outModel.VertexIdByName(link->Name()); // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + "] specified by link with name[" + link->Name() + - "] does not match a link, joint, or frame name " + "] does not match a nested model, link, joint, or frame name " "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = outModel.VertexIdByName(relativeTo); if (link->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -572,33 +745,33 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, linkId}, link->RawPose()); + outModel.AddEdge({relativeToId, linkId}, link->RawPose()); } for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - // check if we've already added a default edge - const std::string relativeTo = joint->PoseRelativeTo(); - if (joint->PoseRelativeTo().empty()) + std::string relativeTo = joint->PoseRelativeTo(); + if (relativeTo.empty()) { - continue; + // since nothing else was specified, use the joint's child frame + relativeTo = joint->ChildLinkName(); } - auto jointId = _out.map.at(joint->Name()); + auto jointId = outModel.VertexIdByName(joint->Name()); // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + "] specified by joint with name[" + joint->Name() + - "] does not match a link, joint, or frame name " + "] does not match a nested model, link, joint, or frame name " "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = outModel.VertexIdByName(relativeTo); if (joint->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -607,7 +780,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, jointId}, joint->RawPose()); + outModel.AddEdge({relativeToId, jointId}, joint->RawPose()); } for (uint64_t f = 0; f < _model->FrameCount(); ++f) @@ -620,7 +793,7 @@ Errors buildPoseRelativeToGraph( continue; } - auto frameId = _out.map.at(frame->Name()); + auto frameId = outModel.VertexIdByName(frame->Name()); std::string relativeTo; std::string typeForErrorMsg; ErrorCode errorCode; @@ -638,16 +811,16 @@ Errors buildPoseRelativeToGraph( } // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({errorCode, typeForErrorMsg + " name[" + relativeTo + "] specified by frame with name[" + frame->Name() + - "] does not match a link, joint, or frame name " + "] does not match a nested model, link, joint, or frame name " "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = outModel.VertexIdByName(relativeTo); if (frame->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -656,15 +829,71 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose()); + outModel.AddEdge({relativeToId, frameId}, frame->RawPose()); + } + + for (uint64_t m = 0; m < _model->ModelCount(); ++m) + { + auto nestedModel = _model->ModelByIndex(m); + + auto nestedModelId = outModel.VertexIdByName(nestedModel->Name()); + // if relative_to is empty, add edge from implicit model frame to + // nestedModel + auto relativeToId = modelFrameId; + + const std::string &relativeTo = nestedModel->PoseRelativeTo(); + if (!relativeTo.empty()) + { + // look for vertex in graph that matches relative_to value + if (outModel.Count(relativeTo) != 1) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "relative_to name[" + relativeTo + + "] specified by nested model with name[" + nestedModel->Name() + + "] does not match a nested model, link, joint, or frame name " + "in model with name[" + _model->Name() + "]."}); + continue; + } + + relativeToId = outModel.VertexIdByName(relativeTo); + if (nestedModel->Name() == relativeTo) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, + "relative_to name[" + relativeTo + + "] is identical to nested model name[" + nestedModel->Name() + + "], causing a graph cycle " + "in model with name[" + _model->Name() + "]."}); + } + } + + ignition::math::Pose3d resolvedModelPose = nestedModel->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame(*nestedModel, + outModel.ChildModelScope(nestedModel->Name()), resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + + outModel.AddEdge({relativeToId, nestedModelId}, resolvedModelPose); } + if (_root) + { + // We have to add this edge now with an identity pose to be able to call + // resolveModelPoseWithPlacementFrame, which in turn calls + // sdf::resolvePoseRelativeToRoot. We will later update the edge after the + // pose is calculated. + auto rootToModel = outModel.AddEdge({rootId, modelId}, {}); + ignition::math::Pose3d resolvedModelPose = _model->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( + *_model, outModel, resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + + outModel.UpdateEdge(rootToModel, resolvedModelPose); + } return errors; } ///////////////////////////////////////////////// Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const World *_world) + ScopedGraph &_out, const World *_world) { Errors errors; @@ -680,19 +909,23 @@ Errors buildPoseRelativeToGraph( "Invalid world element in sdf::World."}); return errors; } - // add implicit world frame vertex first - const std::string sourceName = "world"; - _out.sourceName = sourceName; - auto worldFrameId = - _out.graph.AddVertex(sourceName, sdf::FrameType::WORLD).Id(); - _out.map[sourceName] = worldFrameId; + const std::string scopeContextName = "world"; + _out = _out.AddScopeVertex( + "", "__root__", scopeContextName, sdf::FrameType::WORLD); + auto rootId = _out.ScopeVertexId(); + + _out = _out.AddScopeVertex( + "", scopeContextName, scopeContextName, sdf::FrameType::WORLD); + auto worldFrameId = _out.ScopeVertexId(); + + _out.AddEdge({rootId, worldFrameId}, {}); // add model vertices and default edge if relative_to is empty for (uint64_t m = 0; m < _world->ModelCount(); ++m) { auto model = _world->ModelByIndex(m); - if (_out.map.count(model->Name()) > 0) + if (_out.Count(model->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Model with non-unique name [" + model->Name() + @@ -700,15 +933,9 @@ Errors buildPoseRelativeToGraph( "]."}); continue; } - auto modelId = - _out.graph.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); - _out.map[model->Name()] = modelId; - if (model->PoseRelativeTo().empty()) - { - // relative_to is empty, so add edge from implicit world frame to model - _out.graph.AddEdge({worldFrameId, modelId}, model->RawPose()); - } + auto modelErrors = buildPoseRelativeToGraph(_out , model, false); + errors.insert(errors.end(), modelErrors .begin(), modelErrors .end()); } // add frame vertices and default edge if both @@ -716,7 +943,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (_out.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -725,13 +952,12 @@ Errors buildPoseRelativeToGraph( continue; } auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) { // add edge from implicit world frame to frame - _out.graph.AddEdge({worldFrameId, frameId}, frame->RawPose()); + _out.AddEdge({worldFrameId, frameId}, frame->RawPose()); } } @@ -742,35 +968,43 @@ Errors buildPoseRelativeToGraph( { auto model = _world->ModelByIndex(m); + auto modelId = _out.VertexIdByName(model->Name()); + // if relative_to is empty, add edge from implicit model frame to + // world + auto relativeToId = worldFrameId; + // check if we've already added a default edge - const std::string relativeTo = model->PoseRelativeTo(); - if (relativeTo.empty()) + const std::string &relativeTo = model->PoseRelativeTo(); + if (!relativeTo.empty()) { - continue; + // look for vertex in graph that matches relative_to value + if (_out.Count(relativeTo) != 1) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "relative_to name[" + relativeTo + + "] specified by model with name[" + model->Name() + + "] does not match a model or frame name " + "in world with name[" + _world->Name() + "]."}); + continue; + } + + relativeToId = _out.VertexIdByName(relativeTo); + if (model->Name() == relativeTo) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, + "relative_to name[" + relativeTo + + "] is identical to model name[" + model->Name() + + "], causing a graph cycle " + "in world with name[" + _world->Name() + "]."}); + } } - auto modelId = _out.map.at(model->Name()); + ignition::math::Pose3d resolvedModelPose = model->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame(*model, + _out.ChildModelScope(model->Name()), resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by model with name[" + model->Name() + - "] does not match a model or frame name " - "in world with name[" + _world->Name() + "]."}); - continue; - } - auto relativeToId = _out.map[relativeTo]; - if (model->Name() == relativeTo) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to model name[" + model->Name() + - "], causing a graph cycle " - "in world with name[" + _world->Name() + "]."}); - } - _out.graph.AddEdge({relativeToId, modelId}, model->RawPose()); + _out.AddEdge({relativeToId, modelId}, resolvedModelPose); } for (uint64_t f = 0; f < _world->FrameCount(); ++f) @@ -783,7 +1017,7 @@ Errors buildPoseRelativeToGraph( continue; } - auto frameId = _out.map.at(frame->Name()); + auto frameId = _out.VertexIdByName(frame->Name()); std::string relativeTo; std::string typeForErrorMsg; ErrorCode errorCode; @@ -801,7 +1035,7 @@ Errors buildPoseRelativeToGraph( } // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (_out.Count(relativeTo) != 1) { errors.push_back({errorCode, typeForErrorMsg + " name[" + relativeTo + @@ -810,7 +1044,7 @@ Errors buildPoseRelativeToGraph( "in world with name[" + _world->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = _out.VertexIdByName(relativeTo); if (frame->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -819,54 +1053,51 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in world with name[" + _world->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose()); + _out.AddEdge({relativeToId, frameId}, frame->RawPose()); } return errors; } ///////////////////////////////////////////////// -Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) +Errors validateFrameAttachedToGraph( + const ScopedGraph &_in) { Errors errors; - // Expect scopeName to be either "__model__" or "world" - if (_in.scopeName != "__model__" && _in.scopeName != "world") + // Expect ScopeContextName to be either "__model__" or "world" + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + _in.scopeName + "] " - " does not match __model__ or world."}); + "FrameAttachedToGraph error: scope context name[" + + _in.ScopeContextName() + "] does not match __model__ or world."}); return errors; } - // Expect one vertex with name "__model__" and FrameType MODEL - // or with name "world" and FrameType WORLD - auto scopeVertices = _in.graph.Vertices(_in.scopeName); - if (scopeVertices.empty()) + // Expect the scope vertex to be valid and check that the scope context + // matches the frame type. + auto scopeVertex = _in.ScopeVertex(); + if (!scopeVertex.Valid()) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + - _in.scopeName + "] not found in graph."}); - return errors; - } - else if (scopeVertices.size() > 1) - { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error, " - "more than one vertex with scope name " + _in.scopeName}); + _in.ScopeContextName() + "] not found in graph."}); return errors; } - auto scopeVertex = scopeVertices.begin()->second.get(); sdf::FrameType scopeFrameType = scopeVertex.Data(); - if (_in.scopeName == "__model__" && scopeFrameType != sdf::FrameType::MODEL) + if (_in.ScopeContextName() == "__model__" && + scopeFrameType != sdf::FrameType::MODEL && + scopeFrameType != sdf::FrameType::STATIC_MODEL) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "scope vertex with name __model__ should have FrameType MODEL."}); + "scope vertex with name __model__ should have FrameType MODEL or " + "STATIC_MODEL."}); return errors; } - else if (_in.scopeName == "world" && + else if (_in.ScopeContextName() == "world" && scopeFrameType != sdf::FrameType::WORLD) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, @@ -876,33 +1107,54 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } // Check number of outgoing edges for each vertex - auto vertices = _in.graph.Vertices(); + auto vertices = _in.Graph().Vertices(); for (auto vertexPair : vertices) { + const std::string vertexName = _in.VertexLocalName(vertexPair.second.get()); // Vertex names should not be empty - if (vertexPair.second.get().Name().empty()) + if (vertexName.empty()) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "vertex with empty name detected."}); } + auto outDegree = _in.Graph().OutDegree(vertexPair.first); - auto outDegree = _in.graph.OutDegree(vertexPair.first); if (outDegree > 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "too many outgoing edges at a vertex with name [" + - vertexPair.second.get().Name() + "]."}); + vertexName + "]."}); } - else if (sdf::FrameType::MODEL == scopeFrameType) + else if (vertexName == "__root__" ) + { + if (sdf::FrameType::STATIC_MODEL != scopeFrameType && + sdf::FrameType::WORLD != scopeFrameType) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error," + " __root__ should have FrameType STATIC_MODEL or WORLD."}); + } + else if (outDegree != 0) + { + std::string graphType = + scopeFrameType == sdf::FrameType::WORLD ? "WORLD" : "MODEL"; + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error," + " __root__ should have no outgoing edges in " + + graphType + " attached_to graph."}); + } + } + else if (sdf::FrameType::MODEL == scopeFrameType || + sdf::FrameType::STATIC_MODEL == scopeFrameType) { switch (vertexPair.second.get().Data()) { case sdf::FrameType::WORLD: errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "vertex with name [" + vertexPair.second.get().Name() + "]" + + "vertex with name [" + vertexName + "]" + "should not have type WORLD in MODEL attached_to graph."}); break; case sdf::FrameType::LINK: @@ -911,18 +1163,39 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] should have no outgoing edges " "in MODEL attached_to graph."}); } break; + case sdf::FrameType::STATIC_MODEL: + if (outDegree != 0) + { + auto edges = _in.Graph().IncidentsFrom(vertexPair.first); + // Filter out alias edges (edge with a weight of 0) + auto outDegreeNoAliases = std::count_if( + edges.begin(), edges.end(), [](const auto &_edge) + { + return _edge.second.get().Weight() >= 1.0; + }); + if (outDegreeNoAliases) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "STATIC_MODEL vertex with name [" + + vertexName + + "] should have no outgoing edges " + "in MODEL attached_to graph."}); + } + } + break; default: if (outDegree == 0) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "Non-LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] is disconnected; it should have 1 outgoing edge " + "in MODEL attached_to graph."}); } @@ -931,7 +1204,7 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "Non-LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(outDegree) + " outgoing edges; it should only have 1 " "outgoing edge in MODEL attached_to graph."}); @@ -944,29 +1217,57 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) // scopeFrameType must be sdf::FrameType::WORLD switch (vertexPair.second.get().Data()) { - case sdf::FrameType::JOINT: - case sdf::FrameType::LINK: - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error, " - "No JOINT or LINK vertex should be in WORLD attached_to graph."}); - break; - case sdf::FrameType::MODEL: case sdf::FrameType::WORLD: if (outDegree != 0) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "MODEL and WORLD vertices should have no outgoing edges " + "WORLD vertices should have no outgoing edges " + "in WORLD attached_to graph."}); + } + break; + case sdf::FrameType::LINK: + if (outDegree != 0) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "LINK vertex with name [" + + vertexName + + "] should have no outgoing edges " "in WORLD attached_to graph."}); } break; + case sdf::FrameType::STATIC_MODEL: + if (outDegree != 0) + { + auto edges = _in.Graph().IncidentsFrom(vertexPair.first); + // Filter out alias edges (edge with a weight of 0) + auto outDegreeNoAliases = std::count_if( + edges.begin(), edges.end(), [](const auto &_edge) + { + return _edge.second.get().Weight() >= 1.0; + }); + if (outDegreeNoAliases) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "STATIC_MODEL vertex with name [" + + vertexName + + "] should have no outgoing edges " + "in WORLD attached_to graph."}); + } + } + break; default: if (outDegree != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "FRAME vertices in WORLD attached_to graph should have " - "1 outgoing edge."}); + "Non-LINK vertex with name [" + + vertexName + + "] has " + std::to_string(outDegree) + + " outgoing edges; it should only have 1 " + "outgoing edge in WORLD attached_to graph."}); } break; } @@ -974,10 +1275,10 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } // check graph for cycles by finding sink from each vertex - for (auto const &namePair : _in.map) + for (auto const &name : _in.VertexNames()) { std::string resolvedBody; - Errors e = resolveFrameAttachedToBody(resolvedBody, _in, namePair.first); + Errors e = resolveFrameAttachedToBody(resolvedBody, _in, name); errors.insert(errors.end(), e.begin(), e.end()); } @@ -985,47 +1286,42 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } ///////////////////////////////////////////////// -Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) +Errors validatePoseRelativeToGraph( + const ScopedGraph &_in) { Errors errors; - // Expect sourceName to be either "__model__" or "world" - if (_in.sourceName != "__model__" && _in.sourceName != "world") + // Expect scopeContextName to be either "__model__" or "world" + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error: source vertex name " + _in.sourceName + - " does not match __model__ or world."}); + "PoseRelativeToGraph error: scope context name " + + _in.ScopeContextName() + " does not match __model__ or world."}); return errors; } - // Expect one vertex with name "__model__" and FrameType MODEL - // or with name "world" and FrameType WORLD - auto sourceVertices = _in.graph.Vertices(_in.sourceName); - if (sourceVertices.empty()) + // Expect the scope vertex (which is the source vertex for this graph) to be + // valid and check that the scope context matches the frame type. + auto sourceVertex = _in.ScopeVertex(); + if (!sourceVertex.Valid()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: source frame[" + - _in.sourceName + "] not found in graph."}); - return errors; - } - else if (sourceVertices.size() > 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error, " - "more than one vertex with source name " + _in.sourceName}); + _in.ScopeContextName() + "] not found in graph."}); return errors; } - auto sourceVertex = sourceVertices.begin()->second.get(); sdf::FrameType sourceFrameType = sourceVertex.Data(); - if (_in.sourceName == "__model__" && sourceFrameType != sdf::FrameType::MODEL) + if (_in.ScopeContextName() == "__model__" && + sourceFrameType != sdf::FrameType::MODEL) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "source vertex with name __model__ should have FrameType MODEL."}); return errors; } - else if (_in.sourceName == "world" && + else if (_in.ScopeContextName() == "world" && sourceFrameType != sdf::FrameType::WORLD) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -1035,24 +1331,46 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) } // Check number of incoming edges for each vertex - auto vertices = _in.graph.Vertices(); + auto vertices = _in.Graph().Vertices(); for (auto vertexPair : vertices) { + const std::string vertexName = _in.VertexLocalName(vertexPair.second.get()); // Vertex names should not be empty - if (vertexPair.second.get().Name().empty()) + if (vertexName.empty()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "vertex with empty name detected."}); } - auto inDegree = _in.graph.InDegree(vertexPair.first); + std::size_t inDegree = _in.Graph().InDegree(vertexPair.first); + if (inDegree > 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "too many incoming edges at a vertex with name [" + - vertexPair.second.get().Name() + "]."}); + vertexName + "]."}); + } + else if (vertexName == "__root__" ) + { + if (sdf::FrameType::MODEL != sourceFrameType && + sdf::FrameType::STATIC_MODEL != sourceFrameType && + sdf::FrameType::WORLD != sourceFrameType) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph error," + " __root__ should have FrameType MODEL, STATIC_MODEL or WORLD."}); + } + else if (inDegree != 0) + { + std::string graphType = + sourceFrameType == sdf::FrameType::WORLD ? "WORLD" : "MODEL"; + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph error," + " __root__ vertex should have no incoming edges in " + + graphType + " relative_to graph."}); + } } else if (sdf::FrameType::MODEL == sourceFrameType) { @@ -1061,27 +1379,31 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) case sdf::FrameType::WORLD: errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "vertex with name [" + vertexPair.second.get().Name() + "]" + + "vertex with name [" + vertexName + "]" + "should not have type WORLD in MODEL relative_to graph."}); break; case sdf::FrameType::MODEL: - if (inDegree != 0) + if (_in.ScopeVertexId() == vertexPair.first) { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error, " - "MODEL vertex with name [" + - vertexPair.second.get().Name() + - "] should have no incoming edges " - "in MODEL relative_to graph."}); + if (inDegree != 0) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph error, " + "MODEL vertex with name [__model__" + "] should have no incoming edges " + "in MODEL relative_to graph."}); + } + break; } - break; + // fall through to default case for nested models + [[fallthrough]]; default: if (inDegree == 0) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "Non-MODEL vertex with name [" + - vertexPair.second.get().Name() + + "Vertex with name [" + + vertexName + "] is disconnected; it should have 1 incoming edge " + "in MODEL relative_to graph."}); } @@ -1090,7 +1412,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "Non-MODEL vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(inDegree) + " incoming edges; it should only have 1 " "incoming edge in MODEL relative_to graph."}); @@ -1103,18 +1425,12 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) // sourceFrameType must be sdf::FrameType::WORLD switch (vertexPair.second.get().Data()) { - case sdf::FrameType::JOINT: - case sdf::FrameType::LINK: - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error, " - "No JOINT or LINK vertex should be in WORLD relative_to graph."}); - break; case sdf::FrameType::WORLD: - if (inDegree != 0) + if (inDegree != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "WORLD vertices should have no incoming edges " + "WORLD vertices should have 1 incoming edge " "in WORLD relative_to graph."}); } break; @@ -1124,7 +1440,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "MODEL / FRAME vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] is disconnected; it should have 1 incoming edge " + "in WORLD relative_to graph."}); } @@ -1133,7 +1449,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "MODEL / FRAME vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(inDegree) + " incoming edges; it should only have 1 " "incoming edge in WORLD relative_to graph."}); @@ -1144,10 +1460,12 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) } // check graph for cycles by resolving pose of each vertex relative to root - for (auto const &namePair : _in.map) + for (auto const &name : _in.VertexNames()) { + if (name == "__root__") + continue; ignition::math::Pose3d pose; - Errors e = resolvePoseRelativeToRoot(pose, _in, namePair.first); + Errors e = resolvePoseRelativeToRoot(pose, _in, name); errors.insert(errors.end(), e.begin(), e.end()); } @@ -1157,29 +1475,30 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) ///////////////////////////////////////////////// Errors resolveFrameAttachedToBody( std::string &_attachedToBody, - const FrameAttachedToGraph &_in, + const ScopedGraph &_in, const std::string &_vertexName) { Errors errors; - if (_in.scopeName != "__model__" && _in.scopeName != "world") + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + _in.scopeName + "] " - " does not match __model__ or world."}); + "FrameAttachedToGraph error: scope context name[" + + _in.ScopeContextName() + "] does not match __model__ or world."}); return errors; } - if (_in.map.count(_vertexName) != 1) + if (_in.Count(_vertexName) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "FrameAttachedToGraph unable to find unique frame with name [" + _vertexName + "] in graph."}); return errors; } - auto vertexId = _in.map.at(_vertexName); + auto vertexId = _in.VertexIdByName(_vertexName); - auto sinkVertexEdges = FindSinkVertex(_in.graph, vertexId, errors); + auto sinkVertexEdges = FindSinkVertex(_in, vertexId, errors); auto sinkVertex = sinkVertexEdges.first; if (!errors.empty()) @@ -1195,27 +1514,42 @@ Errors resolveFrameAttachedToBody( return errors; } - if (_in.scopeName == "world" && + if (_in.ScopeContextName() == "world" && !(sinkVertex.Data() == FrameType::WORLD || - sinkVertex.Data() == FrameType::MODEL)) + sinkVertex.Data() == FrameType::STATIC_MODEL || + sinkVertex.Data() == FrameType::LINK)) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "Graph has world scope but sink vertex named [" + - sinkVertex.Name() + "] does not have FrameType WORLD or MODEL " - "when starting from vertex with name [" + _vertexName + "]."}); + "Graph has world scope but sink vertex named [" + sinkVertex.Name() + + "] does not have FrameType WORLD, LINK or STATIC_MODEL " + "when starting from vertex with name [" + + _vertexName + "]."}); return errors; } - if (_in.scopeName == "__model__" && sinkVertex.Data() != FrameType::LINK) + if (_in.ScopeContextName() == "__model__") { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "Graph has __model__ scope but sink vertex named [" + - sinkVertex.Name() + "] does not have FrameType LINK " - "when starting from vertex with name [" + _vertexName + "]."}); - return errors; + if (sinkVertex.Data() == FrameType::MODEL && + sinkVertex.Name() == "__model__") + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "Graph with __model__ scope has sink vertex named [__model__] " + "when starting from vertex with name [" + _vertexName + "], " + "which is not permitted."}); + return errors; + } + else if (sinkVertex.Data() != FrameType::LINK && + sinkVertex.Data() != FrameType::STATIC_MODEL) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "Graph has __model__ scope but sink vertex named [" + + sinkVertex.Name() + "] does not have FrameType LINK OR STATIC_MODEL " + "when starting from vertex with name [" + _vertexName + "]."}); + return errors; + } } - _attachedToBody = sinkVertex.Name(); + _attachedToBody = _in.VertexLocalName(sinkVertex); return errors; } @@ -1223,21 +1557,31 @@ Errors resolveFrameAttachedToBody( ///////////////////////////////////////////////// Errors resolvePoseRelativeToRoot( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_vertexName) { Errors errors; - if (_graph.map.count(_vertexName) != 1) + if (_graph.Count(_vertexName) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "PoseRelativeToGraph unable to find unique frame with name [" + _vertexName + "] in graph."}); return errors; } - auto vertexId = _graph.map.at(_vertexName); - auto incomingVertexEdges = FindSourceVertex(_graph.graph, vertexId, errors); + return resolvePoseRelativeToRoot( + _pose, _graph, _graph.VertexIdByName(_vertexName)); +} +///////////////////////////////////////////////// +Errors resolvePoseRelativeToRoot( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_vertexId) +{ + Errors errors; + + auto incomingVertexEdges = FindSourceVertex(_graph, _vertexId, errors); if (!errors.empty()) { @@ -1247,16 +1591,17 @@ Errors resolvePoseRelativeToRoot( { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph unable to find path to source vertex " - "when starting from vertex with name [" + _vertexName + "]."}); + "when starting from vertex with id [" + + std::to_string(_vertexId) + "]."}); return errors; } - else if (incomingVertexEdges.first.Name() != _graph.sourceName) + else if (incomingVertexEdges.first.Id() != _graph.ScopeVertex().Id()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph frame with name [" + _vertexName + "] " - "is disconnected; its source vertex has name [" + - incomingVertexEdges.first.Name() + - "], but its source name should be " + _graph.sourceName + "."}); + "PoseRelativeToGraph frame with name [" + std::to_string(_vertexId) + + "] is disconnected; its source vertex has name [" + + incomingVertexEdges.first.Name() + "], but its source name should " + "be " + _graph.ScopeContextName() + "."}); return errors; } @@ -1274,25 +1619,56 @@ Errors resolvePoseRelativeToRoot( return errors; } +///////////////////////////////////////////////// +Errors resolvePose(ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_frameVertexId, + const ignition::math::graph::VertexId &_resolveToVertexId) +{ + Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameVertexId); + + // If the resolveTo is empty, we're resolving to the Root, so we're done + if (_resolveToVertexId != ignition::math::graph::kNullId) + { + ignition::math::Pose3d poseR; + Errors errorsR = + resolvePoseRelativeToRoot(poseR, _graph, _resolveToVertexId); + errors.insert(errors.end(), errorsR.begin(), errorsR.end()); + + if (errors.empty()) + { + _pose = poseR.Inverse() * _pose; + } + } + + return errors; +} + ///////////////////////////////////////////////// Errors resolvePose( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_frameName, const std::string &_resolveTo) { - Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameName); - - ignition::math::Pose3d poseR; - Errors errorsR = resolvePoseRelativeToRoot(poseR, _graph, _resolveTo); - errors.insert(errors.end(), errorsR.begin(), errorsR.end()); - - if (errors.empty()) + Errors errors; + if (_graph.Count(_frameName) != 1) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "PoseRelativeToGraph unable to find unique frame with name [" + + _frameName + "] in graph."}); + return errors; + } + if (_graph.Count(_resolveTo) != 1) { - _pose = poseR.Inverse() * _pose; + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "PoseRelativeToGraph unable to find unique frame with name [" + + _resolveTo + "] in graph."}); + return errors; } - return errors; + return resolvePose(_pose, _graph, _graph.VertexIdByName(_frameName), + _graph.VertexIdByName(_resolveTo)); } } } diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 8de5bb040..416580a10 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -18,6 +18,7 @@ #define SDF_FRAMESEMANTICS_HH_ #include +#include #include #include @@ -32,6 +33,11 @@ /// The Frame Semantics Utilities construct and operate on graphs representing /// the kinematics, frame attached_to, and pose relative_to relationships /// defined within models and world. +/// +/// Note that all graphs should only contain relative names (e.g. "my_link"), +/// not absolute names ("top_model::nested_model::my_link"). +/// Graphs inside nested models (currently via directly nested models in +/// //model or //world elements) will be explicitly separate graphs. namespace sdf { // Inline bracket to help doxygen filtering. @@ -40,6 +46,7 @@ namespace sdf // Forward declaration. class Model; class World; + template class ScopedGraph; /// \enum FrameType /// \brief The set of frame types. INVALID indicates that frame type has @@ -60,6 +67,9 @@ namespace sdf /// \brief An explicit frame. FRAME = 4, + + /// \brief An implicit static model frame. + STATIC_MODEL = 5, }; /// \brief Data structure for frame attached_to graphs for Model or World. @@ -103,42 +113,44 @@ namespace sdf /// \param[out] _out Graph object to write. /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. - Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const Model *_model); + Errors buildFrameAttachedToGraph(ScopedGraph &_out, + const Model *_model, bool _root = true); /// \brief Build a FrameAttachedToGraph for a world. /// \param[out] _out Graph object to write. /// \param[in] _world World from which to build attached_to graph. /// \return Errors. Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const World *_world); + ScopedGraph &_out, const World *_world); /// \brief Build a PoseRelativeToGraph for a model. /// \param[out] _out Graph object to write. /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. - Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const Model *_model); + Errors buildPoseRelativeToGraph(ScopedGraph &_out, + const Model *_model, bool _root = true); /// \brief Build a PoseRelativeToGraph for a world. /// \param[out] _out Graph object to write. /// \param[in] _world World from which to build attached_to graph. /// \return Errors. Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const World *_world); + ScopedGraph &_out, const World *_world); /// \brief Confirm that FrameAttachedToGraph is valid by checking the number /// of outbound edges for each vertex and checking for graph cycles. /// \param[in] _in Graph object to validate. /// \return Errors. - Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in); + Errors validateFrameAttachedToGraph( + const ScopedGraph &_in); /// \brief Confirm that PoseRelativeToGraph is valid by checking the number /// of outbound edges for each vertex and checking for graph cycles. /// \param[in] _in Graph object to validate. /// \return Errors. - Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in); + Errors validatePoseRelativeToGraph( + const ScopedGraph &_in); /// \brief Resolve the attached-to body for a given frame. Following the /// edges of the frame attached-to graph from a given frame must lead @@ -152,7 +164,7 @@ namespace sdf /// a link or world frame. Errors resolveFrameAttachedToBody( std::string &_attachedToBody, - const FrameAttachedToGraph &_in, + const ScopedGraph &_in, const std::string &_vertexName); /// \brief Resolve pose of a vertex relative to its outgoing ancestor @@ -163,9 +175,14 @@ namespace sdf /// \return Errors. Errors resolvePoseRelativeToRoot( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_vertexName); + Errors resolvePoseRelativeToRoot( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_vertexId); + /// \brief Resolve pose of a frame relative to named frame. /// \param[out] _pose Pose object to write. /// \param[in] _graph PoseRelativeToGraph to read from. @@ -175,9 +192,15 @@ namespace sdf /// \return Errors. Errors resolvePose( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_frameName, const std::string &_resolveTo); + + Errors resolvePose( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_frameVertexId, + const ignition::math::graph::VertexId &_resolveToVertexId); } } #endif diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index e119b589b..415cdb662 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -32,6 +32,7 @@ #include "sdf/sdf_config.h" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "test_config.h" ///////////////////////////////////////////////// @@ -43,29 +44,32 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); - sdf::FrameAttachedToGraph graph; - auto errors = sdf::buildFrameAttachedToGraph(graph, model); - EXPECT_TRUE(errors.empty()); + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, model); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - EXPECT_EQ(6u, graph.map.size()); - EXPECT_EQ(6u, graph.graph.Vertices().size()); - EXPECT_EQ(5u, graph.graph.Edges().size()); + graph = graph.ChildModelScope(model->Name()); + EXPECT_EQ(8u, graph.Map().size()); + EXPECT_EQ(8u, graph.Graph().Vertices().size()); + EXPECT_EQ(6u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("__model__")); - EXPECT_EQ(1u, graph.map.count("L")); - EXPECT_EQ(1u, graph.map.count("F00")); - EXPECT_EQ(1u, graph.map.count("F0")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(0u, graph.map.count("invalid")); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("L")); + EXPECT_EQ(1u, graph.Count("F00")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(0u, graph.Count("invalid")); std::string resolvedBody; EXPECT_TRUE( @@ -108,29 +112,33 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first world const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(world); - sdf::FrameAttachedToGraph graph; - auto errors = sdf::buildFrameAttachedToGraph(graph, world); + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, world); EXPECT_TRUE(errors.empty()); - EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); + errors = sdf::validateFrameAttachedToGraph(graph); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - EXPECT_EQ(6u, graph.map.size()); - EXPECT_EQ(6u, graph.graph.Vertices().size()); - EXPECT_EQ(4u, graph.graph.Edges().size()); + EXPECT_EQ(9u, graph.Map().size()); + EXPECT_EQ(9u, graph.Graph().Vertices().size()); + EXPECT_EQ(7u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("world")); - EXPECT_EQ(1u, graph.map.count("world_frame")); - EXPECT_EQ(1u, graph.map.count("F0")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(1u, graph.map.count("M1")); - EXPECT_EQ(0u, graph.map.count("invalid")); + EXPECT_EQ(1u, graph.Count("world")); + EXPECT_EQ(1u, graph.Count("world_frame")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(0u, graph.Count("invalid")); std::string resolvedBody; EXPECT_TRUE( @@ -148,10 +156,10 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) EXPECT_EQ("world", resolvedBody); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); - EXPECT_EQ("M1", resolvedBody); + EXPECT_EQ("M1::L", resolvedBody); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); - EXPECT_EQ("M1", resolvedBody); + EXPECT_EQ("M1::L", resolvedBody); // Try to resolve invalid frame name errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); @@ -168,19 +176,22 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) ASSERT_NE(nullptr, world); const sdf::Model *model = world->ModelByIndex(0); - sdf::FrameAttachedToGraph modelGraph; + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); errors = sdf::buildFrameAttachedToGraph(modelGraph, model); EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); - EXPECT_EQ(3u, modelGraph.map.size()); - EXPECT_EQ(3u, modelGraph.graph.Vertices().size()); - EXPECT_EQ(2u, modelGraph.graph.Edges().size()); + modelGraph = modelGraph.ChildModelScope(model->Name()); + + EXPECT_EQ(5u, modelGraph.Map().size()); + EXPECT_EQ(5u, modelGraph.Graph().Vertices().size()); + EXPECT_EQ(3u, modelGraph.Graph().Edges().size()); - EXPECT_EQ(1u, modelGraph.map.count("L")); - EXPECT_EQ(1u, modelGraph.map.count("__model__")); - EXPECT_EQ(1u, modelGraph.map.count("F0")); - EXPECT_EQ(0u, modelGraph.map.count("invalid")); + EXPECT_EQ(1u, modelGraph.Count("L")); + EXPECT_EQ(1u, modelGraph.Count("__model__")); + EXPECT_EQ(1u, modelGraph.Count("F0")); + EXPECT_EQ(0u, modelGraph.Count("invalid")); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "L").empty()); @@ -206,25 +217,27 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) EXPECT_TRUE(root.Load(testFile).empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, model); EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validatePoseRelativeToGraph(graph).empty()); + graph = graph.ChildModelScope(model->Name()); - EXPECT_EQ(8u, graph.map.size()); - EXPECT_EQ(8u, graph.graph.Vertices().size()); - EXPECT_EQ(7u, graph.graph.Edges().size()); + EXPECT_EQ(10u, graph.Map().size()); + EXPECT_EQ(10u, graph.Graph().Vertices().size()); + EXPECT_EQ(9u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("__model__")); - EXPECT_EQ(1u, graph.map.count("P")); - EXPECT_EQ(1u, graph.map.count("C")); - EXPECT_EQ(1u, graph.map.count("J")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(1u, graph.map.count("F3")); - EXPECT_EQ(1u, graph.map.count("F4")); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("P")); + EXPECT_EQ(1u, graph.Count("C")); + EXPECT_EQ(1u, graph.Count("J")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); // Test resolvePoseRelativeToRoot for each frame. ignition::math::Pose3d pose; @@ -286,3 +299,334 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) "PoseRelativeToGraph unable to find unique frame with name [" "invalid] in graph.")); } + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, buildFrameAttachedToGraph_Model) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_nested_frame_attached_to.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + // Get the first model + const sdf::Model *model = root.Model(); + + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, model); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); + EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); + + graph = graph.ChildModelScope(model->Name()); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("L")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(1u, graph.Count("M1::__model__")); + EXPECT_EQ(1u, graph.Count("M1::L")); + EXPECT_EQ(1u, graph.Count("M1::M2")); + EXPECT_EQ(1u, graph.Count("M1::M2::L")); + EXPECT_EQ(1u, graph.Count("M1::F")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); + EXPECT_EQ(0u, graph.Count("invalid")); + + std::string resolvedBody; + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "__model__").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "L").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::__model__") + .empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::L").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::L").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::F").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F0").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F3").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F4").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + + // Try to resolve invalid frame name + errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); + for (auto &e : errors) + std::cerr << e.Message() << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "FrameAttachedToGraph unable to find unique frame with name [" + "invalid] in graph.")); +} + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, buildFrameAttachedToGraph_World) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_nested_frame_attached_to.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + // Get the first world + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(world); + + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, world); + EXPECT_TRUE(errors.empty()); + errors = sdf::validateFrameAttachedToGraph(graph); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); + + EXPECT_EQ(1u, graph.Count("world")); + EXPECT_EQ(1u, graph.Count("world_frame")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(1u, graph.Count("M1::__model__")); + EXPECT_EQ(1u, graph.Count("M1::L")); + EXPECT_EQ(1u, graph.Count("M1::M2")); + EXPECT_EQ(1u, graph.Count("M1::M2::__model__")); + EXPECT_EQ(1u, graph.Count("M1::M2::L")); + EXPECT_EQ(1u, graph.Count("M1::F0")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); + EXPECT_EQ(1u, graph.Count("F5")); + EXPECT_EQ(1u, graph.Count("F6")); + EXPECT_EQ(0u, graph.Count("invalid")); + + std::string resolvedBody; + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "world").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody( + resolvedBody, graph, "world_frame").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::__model__") + .empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::L").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::__model__") + .empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::L").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::F0").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F0").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F3").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F4").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F5").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F6").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + + // Try to resolve invalid frame name + errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); + for (auto &e : errors) + std::cerr << e.Message() << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "FrameAttachedToGraph unable to find unique frame with name [" + "invalid] in graph.")); + + // Create graph from embedded model + ASSERT_NE(nullptr, world); + const sdf::Model *model = world->ModelByIndex(0); + + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); + errors = sdf::buildFrameAttachedToGraph(modelGraph, model); + EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); + + modelGraph = modelGraph.ChildModelScope(model->Name()); + + EXPECT_EQ(1u, modelGraph.Count("__model__")); + EXPECT_EQ(1u, modelGraph.Count("L")); + EXPECT_EQ(1u, modelGraph.Count("M2")); + EXPECT_EQ(1u, modelGraph.Count("M2::__model__")); + EXPECT_EQ(1u, modelGraph.Count("M2::L")); + EXPECT_EQ(1u, modelGraph.Count("F0")); + EXPECT_EQ(0u, modelGraph.Count("invalid")); + // The following Count expectations are 0 because the frames are out of scope. + EXPECT_EQ(0u, modelGraph.Count("world")); + EXPECT_EQ(0u, modelGraph.Count("world_frame")); + EXPECT_EQ(0u, modelGraph.Count("F1")); + EXPECT_EQ(0u, modelGraph.Count("F2")); + EXPECT_EQ(0u, modelGraph.Count("F3")); + EXPECT_EQ(0u, modelGraph.Count("F4")); + EXPECT_EQ(0u, modelGraph.Count("F5")); + EXPECT_EQ(0u, modelGraph.Count("F6")); + + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody( + resolvedBody, modelGraph, "__model__").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "L").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2").empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2::__model__") + .empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2::L").empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F0").empty()); + EXPECT_EQ("M2::L", resolvedBody); + + errors = sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "world"); + ASSERT_FALSE(errors.empty()); + + EXPECT_EQ( + "FrameAttachedToGraph unable to find unique frame with name " + "[world] in graph.", + errors[0].Message()); + + errors = + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "world_frame"); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ( + "FrameAttachedToGraph unable to find unique frame with name " + "[world_frame] in graph.", + errors[0].Message()); + + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F1").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F2").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F3").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F4").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F5").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F6").empty()); +} + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, ModelWithoutLinksWithNestedStaticModel) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_nested_static_model.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); + errors = sdf::buildFrameAttachedToGraph(modelGraph, model); + EXPECT_TRUE(errors.empty()) << errors; + errors = sdf::validateFrameAttachedToGraph(modelGraph); + EXPECT_TRUE(errors.empty()) << errors; + + std::string resolvedBody; + errors = sdf::resolveFrameAttachedToBody( + resolvedBody, modelGraph, "model_nested_static_model"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ("model_nested_static_model::child_model::static_model::__model__", + resolvedBody); +} + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, InvalidAttachedToScope) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_frame_invalid_attached_to_scope.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + ASSERT_FALSE(errors.empty()); + const std::string expectedMsg = + "attached_to name[M2::M1::L1] specified by frame with name[F] " + "does not match a model or frame name in world with " + "name[world_frame_invalid_attached_to_scope]."; + EXPECT_EQ(expectedMsg, errors[0].Message()); + + EXPECT_FALSE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_FALSE(sdf::checkFrameAttachedToNames(&root)); +} diff --git a/src/Geometry.cc b/src/Geometry.cc index 96eba0f0a..59cacd0f0 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -14,35 +14,52 @@ * limitations under the License. * */ + +#include + #include "sdf/Geometry.hh" #include "sdf/Box.hh" +#include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" +#include "sdf/Ellipsoid.hh" +#include "sdf/Heightmap.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" +#include "Utils.hh" + using namespace sdf; // Private data class -class sdf::GeometryPrivate +class sdf::Geometry::Implementation { // \brief The geometry type. public: GeometryType type = GeometryType::EMPTY; - /// \brief Pointer to a box. - public: std::unique_ptr box; + /// \brief Optional box. + public: std::optional box; + + /// \brief Optional capsule. + public: std::optional capsule; + + /// \brief Optional cylinder. + public: std::optional cylinder; - /// \brief Pointer to a cylinder. - public: std::unique_ptr cylinder; + /// \brief Optional ellipsoid + public: std::optional ellipsoid; - /// \brief Pointer to a plane. - public: std::unique_ptr plane; + /// \brief Optional plane. + public: std::optional plane; - /// \brief Pointer to a sphere. - public: std::unique_ptr sphere; + /// \brief Optional sphere. + public: std::optional sphere; - /// \brief Pointer to a mesh. - public: std::unique_ptr mesh; + /// \brief Optional mesh. + public: std::optional mesh; + + /// \brief Optional heightmap. + public: std::optional heightmap; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -50,71 +67,8 @@ class sdf::GeometryPrivate ///////////////////////////////////////////////// Geometry::Geometry() - : dataPtr(new GeometryPrivate) -{ -} - -///////////////////////////////////////////////// -Geometry::~Geometry() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Geometry::Geometry(const Geometry &_geometry) - : dataPtr(new GeometryPrivate) -{ - this->dataPtr->type = _geometry.dataPtr->type; - - if (_geometry.dataPtr->box) - { - this->dataPtr->box = std::make_unique(*_geometry.dataPtr->box); - } - - if (_geometry.dataPtr->cylinder) - { - this->dataPtr->cylinder = std::make_unique( - *_geometry.dataPtr->cylinder); - } - - if (_geometry.dataPtr->plane) - { - this->dataPtr->plane = std::make_unique( - *_geometry.dataPtr->plane); - } - - if (_geometry.dataPtr->sphere) - { - this->dataPtr->sphere = std::make_unique( - *_geometry.dataPtr->sphere); - } - - if (_geometry.dataPtr->mesh) - { - this->dataPtr->mesh = std::make_unique(*_geometry.dataPtr->mesh); - } - - this->dataPtr->sdf = _geometry.dataPtr->sdf; -} - -////////////////////////////////////////////////// -Geometry::Geometry(Geometry &&_geometry) noexcept - : dataPtr(std::exchange(_geometry.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Geometry &Geometry::operator=(const Geometry &_geometry) + : dataPtr(ignition::utils::MakeImpl()) { - return *this = Geometry(_geometry); -} - -////////////////////////////////////////////////// -Geometry &Geometry::operator=(Geometry &&_geometry) -{ - std::swap(this->dataPtr, _geometry.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -146,38 +100,59 @@ Errors Geometry::Load(ElementPtr _sdf) if (_sdf->HasElement("box")) { this->dataPtr->type = GeometryType::BOX; - this->dataPtr->box.reset(new Box()); + this->dataPtr->box.emplace(); Errors err = this->dataPtr->box->Load(_sdf->GetElement("box")); errors.insert(errors.end(), err.begin(), err.end()); } + else if (_sdf->HasElement("capsule")) + { + this->dataPtr->type = GeometryType::CAPSULE; + this->dataPtr->capsule.emplace(); + Errors err = this->dataPtr->capsule->Load(_sdf->GetElement("capsule")); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (_sdf->HasElement("cylinder")) { this->dataPtr->type = GeometryType::CYLINDER; - this->dataPtr->cylinder.reset(new Cylinder()); + this->dataPtr->cylinder.emplace(); Errors err = this->dataPtr->cylinder->Load(_sdf->GetElement("cylinder")); errors.insert(errors.end(), err.begin(), err.end()); } + else if (_sdf->HasElement("ellipsoid")) + { + this->dataPtr->type = GeometryType::ELLIPSOID; + this->dataPtr->ellipsoid.emplace(); + Errors err = this->dataPtr->ellipsoid->Load(_sdf->GetElement("ellipsoid")); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (_sdf->HasElement("plane")) { this->dataPtr->type = GeometryType::PLANE; - this->dataPtr->plane.reset(new Plane()); + this->dataPtr->plane.emplace(); Errors err = this->dataPtr->plane->Load(_sdf->GetElement("plane")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("sphere")) { this->dataPtr->type = GeometryType::SPHERE; - this->dataPtr->sphere.reset(new Sphere()); + this->dataPtr->sphere.emplace(); Errors err = this->dataPtr->sphere->Load(_sdf->GetElement("sphere")); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("mesh")) { this->dataPtr->type = GeometryType::MESH; - this->dataPtr->mesh.reset(new Mesh()); + this->dataPtr->mesh.emplace(); Errors err = this->dataPtr->mesh->Load(_sdf->GetElement("mesh")); errors.insert(errors.end(), err.begin(), err.end()); } + else if (_sdf->HasElement("heightmap")) + { + this->dataPtr->type = GeometryType::HEIGHTMAP; + this->dataPtr->heightmap.emplace(); + Errors err = this->dataPtr->heightmap->Load(_sdf->GetElement("heightmap")); + errors.insert(errors.end(), err.begin(), err.end()); + } return errors; } @@ -197,61 +172,97 @@ void Geometry::SetType(const GeometryType _type) ///////////////////////////////////////////////// const Box *Geometry::BoxShape() const { - return this->dataPtr->box.get(); + return optionalToPointer(this->dataPtr->box); } ///////////////////////////////////////////////// void Geometry::SetBoxShape(const Box &_box) { - this->dataPtr->box = std::make_unique(_box); + this->dataPtr->box = _box; } ///////////////////////////////////////////////// const Sphere *Geometry::SphereShape() const { - return this->dataPtr->sphere.get(); + return optionalToPointer(this->dataPtr->sphere); } ///////////////////////////////////////////////// void Geometry::SetSphereShape(const Sphere &_sphere) { - this->dataPtr->sphere = std::make_unique(_sphere); + this->dataPtr->sphere = _sphere; +} + +///////////////////////////////////////////////// +const Capsule *Geometry::CapsuleShape() const +{ + return optionalToPointer(this->dataPtr->capsule); +} + +///////////////////////////////////////////////// +void Geometry::SetCapsuleShape(const Capsule &_capsule) +{ + this->dataPtr->capsule = _capsule; } ///////////////////////////////////////////////// const Cylinder *Geometry::CylinderShape() const { - return this->dataPtr->cylinder.get(); + return optionalToPointer(this->dataPtr->cylinder); } ///////////////////////////////////////////////// void Geometry::SetCylinderShape(const Cylinder &_cylinder) { - this->dataPtr->cylinder = std::make_unique(_cylinder); + this->dataPtr->cylinder = _cylinder; +} + +///////////////////////////////////////////////// +const Ellipsoid *Geometry::EllipsoidShape() const +{ + return optionalToPointer(this->dataPtr->ellipsoid); +} + +///////////////////////////////////////////////// +void Geometry::SetEllipsoidShape(const Ellipsoid &_ellipsoid) +{ + this->dataPtr->ellipsoid = _ellipsoid; } ///////////////////////////////////////////////// const Plane *Geometry::PlaneShape() const { - return this->dataPtr->plane.get(); + return optionalToPointer(this->dataPtr->plane); } ///////////////////////////////////////////////// void Geometry::SetPlaneShape(const Plane &_plane) { - this->dataPtr->plane = std::make_unique(_plane); + this->dataPtr->plane = _plane; } ///////////////////////////////////////////////// const Mesh *Geometry::MeshShape() const { - return this->dataPtr->mesh.get(); + return optionalToPointer(this->dataPtr->mesh); } ///////////////////////////////////////////////// void Geometry::SetMeshShape(const Mesh &_mesh) { - this->dataPtr->mesh = std::make_unique(_mesh); + this->dataPtr->mesh = _mesh; +} + +///////////////////////////////////////////////// +const Heightmap *Geometry::HeightmapShape() const +{ + return optionalToPointer(this->dataPtr->heightmap); +} + +///////////////////////////////////////////////// +void Geometry::SetHeightmapShape(const Heightmap &_heightmap) +{ + this->dataPtr->heightmap = _heightmap; } ///////////////////////////////////////////////// diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 77f19fc55..6aaab1a2a 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -17,7 +17,9 @@ #include #include "sdf/Box.hh" +#include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" +#include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" @@ -33,9 +35,15 @@ TEST(DOMGeometry, Construction) geom.SetType(sdf::GeometryType::BOX); EXPECT_EQ(sdf::GeometryType::BOX, geom.Type()); + geom.SetType(sdf::GeometryType::CAPSULE); + EXPECT_EQ(sdf::GeometryType::CAPSULE, geom.Type()); + geom.SetType(sdf::GeometryType::CYLINDER); EXPECT_EQ(sdf::GeometryType::CYLINDER, geom.Type()); + geom.SetType(sdf::GeometryType::ELLIPSOID); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, geom.Type()); + geom.SetType(sdf::GeometryType::PLANE); EXPECT_EQ(sdf::GeometryType::PLANE, geom.Type()); @@ -164,6 +172,23 @@ TEST(DOMGeometry, Sphere) EXPECT_DOUBLE_EQ(0.123, geom.SphereShape()->Radius()); } +///////////////////////////////////////////////// +TEST(DOMGeometry, Capsule) +{ + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::CAPSULE); + + sdf::Capsule capsuleShape; + capsuleShape.SetRadius(0.123); + capsuleShape.SetLength(4.56); + geom.SetCapsuleShape(capsuleShape); + + EXPECT_EQ(sdf::GeometryType::CAPSULE, geom.Type()); + EXPECT_NE(nullptr, geom.CapsuleShape()); + EXPECT_DOUBLE_EQ(0.123, geom.CapsuleShape()->Radius()); + EXPECT_DOUBLE_EQ(4.56, geom.CapsuleShape()->Length()); +} + ///////////////////////////////////////////////// TEST(DOMGeometry, Cylinder) { @@ -181,6 +206,22 @@ TEST(DOMGeometry, Cylinder) EXPECT_DOUBLE_EQ(4.56, geom.CylinderShape()->Length()); } +///////////////////////////////////////////////// +TEST(DOMGeometry, Ellipsoid) +{ + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::ELLIPSOID); + + sdf::Ellipsoid ellipsoidShape; + const ignition::math::Vector3d expectedRadii(1, 2, 3); + ellipsoidShape.SetRadii(expectedRadii); + geom.SetEllipsoidShape(ellipsoidShape); + + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, geom.Type()); + EXPECT_NE(nullptr, geom.EllipsoidShape()); + EXPECT_EQ(expectedRadii, geom.EllipsoidShape()->Radii()); +} + ///////////////////////////////////////////////// TEST(DOMGeometry, Mesh) { diff --git a/src/Gui.cc b/src/Gui.cc index 26ac5555a..f78973c22 100644 --- a/src/Gui.cc +++ b/src/Gui.cc @@ -20,7 +20,7 @@ using namespace sdf; /// \brief Gui private data. -class sdf::GuiPrivate +class sdf::Gui::Implementation { /// \brief True if the GUI should be fullscreen. public: bool fullscreen = false; @@ -31,42 +31,10 @@ class sdf::GuiPrivate ///////////////////////////////////////////////// Gui::Gui() - : dataPtr(new GuiPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Gui::~Gui() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Gui::Gui(const Gui &_gui) - : dataPtr(new GuiPrivate(*_gui.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Gui::Gui(Gui &&_gui) noexcept - : dataPtr(std::exchange(_gui.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Gui &Gui::operator=(const Gui &_gui) -{ - return *this = Gui(_gui); -} - -///////////////////////////////////////////////// -Gui &Gui::operator=(Gui &&_gui) -{ - std::swap(this->dataPtr, _gui.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Gui::Load(ElementPtr _sdf) { @@ -98,7 +66,7 @@ bool Gui::Fullscreen() const } ///////////////////////////////////////////////// -void Gui::SetFullscreen(const bool _fullscreen) const +void Gui::SetFullscreen(const bool _fullscreen) { this->dataPtr->fullscreen = _fullscreen; } diff --git a/src/Heightmap.cc b/src/Heightmap.cc new file mode 100644 index 000000000..8d018730f --- /dev/null +++ b/src/Heightmap.cc @@ -0,0 +1,461 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include "Utils.hh" +#include "sdf/Heightmap.hh" + +using namespace sdf; + +// Private data class +class sdf::HeightmapTexture::Implementation +{ + /// \brief URI of the diffuse map. + public: std::string diffuse{""}; + + /// \brief URI of the normal map. + public: std::string normal{""}; + + /// \brief Texture size in meters. + public: double size{10.0}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; +}; + +// Private data class +class sdf::HeightmapBlend::Implementation +{ + /// \brief Minimum height + public: double minHeight{0.0}; + + /// \brief Fade distance + public: double fadeDistance{0.0}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; +}; + +// Private data class +class sdf::Heightmap::Implementation +{ + /// \brief URI to 2d grayscale map. + public: std::string uri{""}; + + /// \brief The path to the file where this heightmap was defined. + public: std::string filePath{""}; + + /// \brief The heightmap's size. + public: ignition::math::Vector3d size{1, 1, 1}; + + /// \brief Position offset. + public: ignition::math::Vector3d position{0, 0, 0}; + + /// \brief Whether to use terrain paging. + public: bool useTerrainPaging{false}; + + /// \brief Sampling per datum. + public: unsigned int sampling{1u}; + + /// \brief Textures in order + public: std::vector textures; + + /// \brief Blends in order + public: std::vector blends; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; +}; + +///////////////////////////////////////////////// +HeightmapTexture::HeightmapTexture() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors HeightmapTexture::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a heightmap texture, but the provided SDF element " + "is null."}); + return errors; + } + + // We need a heightmap element + if (_sdf->GetName() != "texture") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a heightmap texture, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("size")) + { + this->dataPtr->size = _sdf->Get("size", this->dataPtr->size).first; + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Heightmap texture is missing a child element."}); + } + + if (_sdf->HasElement("diffuse")) + { + this->dataPtr->diffuse = _sdf->Get("diffuse", + this->dataPtr->diffuse).first; + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Heightmap texture is missing a child element."}); + } + + if (_sdf->HasElement("normal")) + { + this->dataPtr->normal = _sdf->Get("normal", + this->dataPtr->normal).first; + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Heightmap texture is missing a child element."}); + } + + return errors; +} + +///////////////////////////////////////////////// +sdf::ElementPtr HeightmapTexture::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +double HeightmapTexture::Size() const +{ + return this->dataPtr->size; +} + +////////////////////////////////////////////////// +void HeightmapTexture::SetSize(double _size) +{ + this->dataPtr->size = _size; +} + +////////////////////////////////////////////////// +std::string HeightmapTexture::Diffuse() const +{ + return this->dataPtr->diffuse; +} + +////////////////////////////////////////////////// +void HeightmapTexture::SetDiffuse(const std::string &_diffuse) +{ + this->dataPtr->diffuse = _diffuse; +} + +////////////////////////////////////////////////// +std::string HeightmapTexture::Normal() const +{ + return this->dataPtr->normal; +} + +////////////////////////////////////////////////// +void HeightmapTexture::SetNormal(const std::string &_normal) +{ + this->dataPtr->normal = _normal; +} + +///////////////////////////////////////////////// +HeightmapBlend::HeightmapBlend() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors HeightmapBlend::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a heightmap blend, but the provided SDF element " + "is null."}); + return errors; + } + + // We need a heightmap element + if (_sdf->GetName() != "blend") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a heightmap blend, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("min_height")) + { + this->dataPtr->minHeight = _sdf->Get("min_height", + this->dataPtr->minHeight).first; + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Heightmap blend is missing a child element."}); + } + + if (_sdf->HasElement("fade_dist")) + { + this->dataPtr->fadeDistance = _sdf->Get("fade_dist", + this->dataPtr->fadeDistance).first; + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Heightmap blend is missing a child element."}); + } + + return errors; +} + +///////////////////////////////////////////////// +sdf::ElementPtr HeightmapBlend::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +double HeightmapBlend::MinHeight() const +{ + return this->dataPtr->minHeight; +} + +////////////////////////////////////////////////// +void HeightmapBlend::SetMinHeight(double _minHeight) +{ + this->dataPtr->minHeight = _minHeight; +} + +////////////////////////////////////////////////// +double HeightmapBlend::FadeDistance() const +{ + return this->dataPtr->fadeDistance; +} + +////////////////////////////////////////////////// +void HeightmapBlend::SetFadeDistance(double _fadeDistance) +{ + this->dataPtr->fadeDistance = _fadeDistance; +} + +///////////////////////////////////////////////// +Heightmap::Heightmap() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Heightmap::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a heightmap, but the provided SDF element is null."}); + return errors; + } + + this->dataPtr->filePath = _sdf->FilePath(); + + // We need a heightmap element + if (_sdf->GetName() != "heightmap") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a heightmap geometry, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("uri")) + { + this->dataPtr->uri = _sdf->Get("uri", "").first; + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Heightmap geometry is missing a child element."}); + } + + this->dataPtr->size = _sdf->Get("size", + this->dataPtr->size).first; + + this->dataPtr->position = _sdf->Get("pos", + this->dataPtr->position).first; + + this->dataPtr->useTerrainPaging = _sdf->Get("use_terrain_paging", + this->dataPtr->useTerrainPaging).first; + + this->dataPtr->sampling = _sdf->Get("sampling", + this->dataPtr->sampling).first; + + Errors textureLoadErrors = loadRepeated(_sdf, + "texture", this->dataPtr->textures); + errors.insert(errors.end(), textureLoadErrors.begin(), + textureLoadErrors.end()); + + Errors blendLoadErrors = loadRepeated(_sdf, + "blend", this->dataPtr->blends); + errors.insert(errors.end(), blendLoadErrors.begin(), blendLoadErrors.end()); + + return errors; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Heightmap::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +std::string Heightmap::Uri() const +{ + return this->dataPtr->uri; +} + +////////////////////////////////////////////////// +void Heightmap::SetUri(const std::string &_uri) +{ + this->dataPtr->uri = _uri; +} + +////////////////////////////////////////////////// +const std::string &Heightmap::FilePath() const +{ + return this->dataPtr->filePath; +} + +////////////////////////////////////////////////// +void Heightmap::SetFilePath(const std::string &_filePath) +{ + this->dataPtr->filePath = _filePath; +} + +////////////////////////////////////////////////// +ignition::math::Vector3d Heightmap::Size() const +{ + return this->dataPtr->size; +} + +////////////////////////////////////////////////// +void Heightmap::SetSize(const ignition::math::Vector3d &_size) +{ + this->dataPtr->size = _size; +} + +////////////////////////////////////////////////// +ignition::math::Vector3d Heightmap::Position() const +{ + return this->dataPtr->position; +} + +////////////////////////////////////////////////// +void Heightmap::SetPosition(const ignition::math::Vector3d &_position) +{ + this->dataPtr->position = _position; +} + +////////////////////////////////////////////////// +bool Heightmap::UseTerrainPaging() const +{ + return this->dataPtr->useTerrainPaging; +} + +////////////////////////////////////////////////// +void Heightmap::SetUseTerrainPaging(bool _useTerrainPaging) +{ + this->dataPtr->useTerrainPaging = _useTerrainPaging; +} + +////////////////////////////////////////////////// +unsigned int Heightmap::Sampling() const +{ + return this->dataPtr->sampling; +} + +////////////////////////////////////////////////// +void Heightmap::SetSampling(unsigned int _sampling) +{ + this->dataPtr->sampling = _sampling; +} + +///////////////////////////////////////////////// +uint64_t Heightmap::TextureCount() const +{ + return this->dataPtr->textures.size(); +} + +///////////////////////////////////////////////// +const HeightmapTexture *Heightmap::TextureByIndex(uint64_t _index) const +{ + if (_index < this->dataPtr->textures.size()) + return &this->dataPtr->textures[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +void Heightmap::AddTexture(const HeightmapTexture &_texture) +{ + this->dataPtr->textures.push_back(_texture); +} + +///////////////////////////////////////////////// +uint64_t Heightmap::BlendCount() const +{ + return this->dataPtr->blends.size(); +} + +///////////////////////////////////////////////// +const HeightmapBlend *Heightmap::BlendByIndex(uint64_t _index) const +{ + if (_index < this->dataPtr->blends.size()) + return &this->dataPtr->blends[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +void Heightmap::AddBlend(const HeightmapBlend &_blend) +{ + this->dataPtr->blends.push_back(_blend); +} diff --git a/src/Heightmap_TEST.cc b/src/Heightmap_TEST.cc new file mode 100644 index 000000000..4cd1b1dc9 --- /dev/null +++ b/src/Heightmap_TEST.cc @@ -0,0 +1,408 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/Heightmap.hh" + +///////////////////////////////////////////////// +TEST(DOMHeightmap, Construction) +{ + sdf::Heightmap heightmap; + EXPECT_EQ(nullptr, heightmap.Element()); + + EXPECT_EQ(std::string(), heightmap.FilePath()); + EXPECT_EQ(std::string(), heightmap.Uri()); + EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), heightmap.Size()); + EXPECT_EQ(ignition::math::Vector3d::Zero, heightmap.Position()); + EXPECT_FALSE(heightmap.UseTerrainPaging()); + EXPECT_EQ(1u, heightmap.Sampling()); + EXPECT_EQ(0u, heightmap.TextureCount()); + EXPECT_EQ(0u, heightmap.BlendCount()); + EXPECT_EQ(nullptr, heightmap.TextureByIndex(0u)); + EXPECT_EQ(nullptr, heightmap.BlendByIndex(0u)); + + sdf::HeightmapTexture heightmapTexture; + EXPECT_EQ(nullptr, heightmapTexture.Element()); + + EXPECT_DOUBLE_EQ(10.0, heightmapTexture.Size()); + EXPECT_TRUE(heightmapTexture.Diffuse().empty()); + EXPECT_TRUE(heightmapTexture.Normal().empty()); + + sdf::HeightmapBlend heightmapBlend; + EXPECT_EQ(nullptr, heightmapBlend.Element()); + + EXPECT_DOUBLE_EQ(0.0, heightmapBlend.MinHeight()); + EXPECT_DOUBLE_EQ(0.0, heightmapBlend.FadeDistance()); +} + +////////////////////////////////////////////////// +TEST(DOMHeightmap, MoveConstructor) +{ + sdf::Heightmap heightmap; + heightmap.SetUri("banana"); + heightmap.SetFilePath("/pear"); + heightmap.SetSize({0.1, 0.2, 0.3}); + heightmap.SetPosition({0.5, 0.6, 0.7}); + heightmap.SetUseTerrainPaging(true); + heightmap.SetSampling(123u); + + sdf::Heightmap heightmap2(std::move(heightmap)); + EXPECT_EQ("banana", heightmap2.Uri()); + EXPECT_EQ("/pear", heightmap2.FilePath()); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); + EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); + EXPECT_TRUE(heightmap2.UseTerrainPaging()); + EXPECT_EQ(123u, heightmap2.Sampling()); + + sdf::HeightmapTexture heightmapTexture; + heightmapTexture.SetSize(123.456); + heightmapTexture.SetDiffuse("diffuse"); + heightmapTexture.SetNormal("normal"); + + sdf::HeightmapTexture heightmapTexture2(std::move(heightmapTexture)); + EXPECT_DOUBLE_EQ(123.456, heightmapTexture2.Size()); + EXPECT_EQ("diffuse", heightmapTexture2.Diffuse()); + EXPECT_EQ("normal", heightmapTexture2.Normal()); + + sdf::HeightmapBlend heightmapBlend; + heightmapBlend.SetMinHeight(123.456); + heightmapBlend.SetFadeDistance(456.123); + + sdf::HeightmapBlend heightmapBlend2(std::move(heightmapBlend)); + EXPECT_DOUBLE_EQ(123.456, heightmapBlend2.MinHeight()); + EXPECT_DOUBLE_EQ(456.123, heightmapBlend2.FadeDistance()); +} + +///////////////////////////////////////////////// +TEST(DOMHeightmap, CopyConstructor) +{ + sdf::Heightmap heightmap; + heightmap.SetUri("banana"); + heightmap.SetFilePath("/pear"); + heightmap.SetSize({0.1, 0.2, 0.3}); + heightmap.SetPosition({0.5, 0.6, 0.7}); + heightmap.SetUseTerrainPaging(true); + heightmap.SetSampling(123u); + + sdf::Heightmap heightmap2(heightmap); + EXPECT_EQ("banana", heightmap2.Uri()); + EXPECT_EQ("/pear", heightmap2.FilePath()); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); + EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); + EXPECT_TRUE(heightmap2.UseTerrainPaging()); + EXPECT_EQ(123u, heightmap2.Sampling()); + + sdf::HeightmapTexture heightmapTexture; + heightmapTexture.SetSize(123.456); + heightmapTexture.SetDiffuse("diffuse"); + heightmapTexture.SetNormal("normal"); + + sdf::HeightmapTexture heightmapTexture2(heightmapTexture); + EXPECT_DOUBLE_EQ(123.456, heightmapTexture2.Size()); + EXPECT_EQ("diffuse", heightmapTexture2.Diffuse()); + EXPECT_EQ("normal", heightmapTexture2.Normal()); + + sdf::HeightmapBlend heightmapBlend; + heightmapBlend.SetMinHeight(123.456); + heightmapBlend.SetFadeDistance(456.123); + + sdf::HeightmapBlend heightmapBlend2(heightmapBlend); + EXPECT_DOUBLE_EQ(123.456, heightmapBlend2.MinHeight()); + EXPECT_DOUBLE_EQ(456.123, heightmapBlend2.FadeDistance()); +} + +///////////////////////////////////////////////// +TEST(DOMHeightmap, CopyAssignmentOperator) +{ + sdf::Heightmap heightmap; + heightmap.SetUri("banana"); + heightmap.SetFilePath("/pear"); + heightmap.SetSize({0.1, 0.2, 0.3}); + heightmap.SetPosition({0.5, 0.6, 0.7}); + heightmap.SetUseTerrainPaging(true); + heightmap.SetSampling(123u); + + sdf::Heightmap heightmap2; + heightmap2 = heightmap; + EXPECT_EQ("banana", heightmap2.Uri()); + EXPECT_EQ("/pear", heightmap2.FilePath()); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); + EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); + EXPECT_TRUE(heightmap2.UseTerrainPaging()); + EXPECT_EQ(123u, heightmap2.Sampling()); + + sdf::HeightmapTexture heightmapTexture; + heightmapTexture.SetSize(123.456); + heightmapTexture.SetDiffuse("diffuse"); + heightmapTexture.SetNormal("normal"); + + sdf::HeightmapTexture heightmapTexture2; + heightmapTexture2 = heightmapTexture; + EXPECT_DOUBLE_EQ(123.456, heightmapTexture2.Size()); + EXPECT_EQ("diffuse", heightmapTexture2.Diffuse()); + EXPECT_EQ("normal", heightmapTexture2.Normal()); + + sdf::HeightmapBlend heightmapBlend; + heightmapBlend.SetMinHeight(123.456); + heightmapBlend.SetFadeDistance(456.123); + + sdf::HeightmapBlend heightmapBlend2; + heightmapBlend2 = heightmapBlend; + EXPECT_DOUBLE_EQ(123.456, heightmapBlend2.MinHeight()); + EXPECT_DOUBLE_EQ(456.123, heightmapBlend2.FadeDistance()); +} + +///////////////////////////////////////////////// +TEST(DOMHeightmap, MoveAssignmentOperator) +{ + sdf::Heightmap heightmap; + heightmap.SetUri("banana"); + heightmap.SetFilePath("/pear"); + heightmap.SetSize({0.1, 0.2, 0.3}); + heightmap.SetPosition({0.5, 0.6, 0.7}); + heightmap.SetUseTerrainPaging(true); + heightmap.SetSampling(123u); + + sdf::Heightmap heightmap2; + heightmap2 = std::move(heightmap); + EXPECT_EQ("banana", heightmap2.Uri()); + EXPECT_EQ("/pear", heightmap2.FilePath()); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); + EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); + EXPECT_TRUE(heightmap2.UseTerrainPaging()); + EXPECT_EQ(123u, heightmap2.Sampling()); + + sdf::HeightmapTexture heightmapTexture; + heightmapTexture.SetSize(123.456); + heightmapTexture.SetDiffuse("diffuse"); + heightmapTexture.SetNormal("normal"); + + sdf::HeightmapTexture heightmapTexture2; + heightmapTexture2 = std::move(heightmapTexture); + EXPECT_DOUBLE_EQ(123.456, heightmapTexture2.Size()); + EXPECT_EQ("diffuse", heightmapTexture2.Diffuse()); + EXPECT_EQ("normal", heightmapTexture2.Normal()); + + sdf::HeightmapBlend heightmapBlend; + heightmapBlend.SetMinHeight(123.456); + heightmapBlend.SetFadeDistance(456.123); + + sdf::HeightmapBlend heightmapBlend2; + heightmapBlend2 = std::move(heightmapBlend); + EXPECT_DOUBLE_EQ(123.456, heightmapBlend2.MinHeight()); + EXPECT_DOUBLE_EQ(456.123, heightmapBlend2.FadeDistance()); +} + +///////////////////////////////////////////////// +TEST(DOMHeightmap, CopyAssignmentAfterMove) +{ + sdf::Heightmap heightmap1; + heightmap1.SetUri("banana"); + + sdf::Heightmap heightmap2; + heightmap2.SetUri("watermelon"); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Heightmap tmp = std::move(heightmap1); + heightmap1 = heightmap2; + heightmap2 = tmp; + + EXPECT_EQ("watermelon", heightmap1.Uri()); + EXPECT_EQ("banana", heightmap2.Uri()); + + sdf::HeightmapTexture heightmapTexture1; + heightmapTexture1.SetSize(123.456); + + sdf::HeightmapTexture heightmapTexture2; + heightmapTexture2.SetSize(456.123); + + sdf::HeightmapTexture tmpTexture = std::move(heightmapTexture1); + heightmapTexture1 = heightmapTexture2; + heightmapTexture2 = tmpTexture; + + EXPECT_DOUBLE_EQ(456.123, heightmapTexture1.Size()); + EXPECT_DOUBLE_EQ(123.456, heightmapTexture2.Size()); + + sdf::HeightmapBlend heightmapBlend1; + heightmapBlend1.SetMinHeight(123.456); + + sdf::HeightmapBlend heightmapBlend2; + heightmapBlend2.SetMinHeight(456.123); + + sdf::HeightmapBlend tmpBlend = std::move(heightmapBlend1); + heightmapBlend1 = heightmapBlend2; + heightmapBlend2 = tmpBlend; + + EXPECT_DOUBLE_EQ(456.123, heightmapBlend1.MinHeight()); + EXPECT_DOUBLE_EQ(123.456, heightmapBlend2.MinHeight()); +} + +///////////////////////////////////////////////// +TEST(DOMHeightmap, Set) +{ + sdf::HeightmapTexture heightmapTexture; + EXPECT_EQ(nullptr, heightmapTexture.Element()); + + EXPECT_DOUBLE_EQ(10.0, heightmapTexture.Size()); + heightmapTexture.SetSize(21.05); + EXPECT_DOUBLE_EQ(21.05, heightmapTexture.Size()); + + EXPECT_TRUE(heightmapTexture.Diffuse().empty()); + heightmapTexture.SetDiffuse("diffuse"); + EXPECT_EQ("diffuse", heightmapTexture.Diffuse()); + + EXPECT_TRUE(heightmapTexture.Normal().empty()); + heightmapTexture.SetNormal("normal"); + EXPECT_EQ("normal", heightmapTexture.Normal()); + + sdf::HeightmapBlend heightmapBlend; + EXPECT_EQ(nullptr, heightmapBlend.Element()); + + EXPECT_DOUBLE_EQ(0.0, heightmapBlend.MinHeight()); + heightmapBlend.SetMinHeight(21.05); + EXPECT_DOUBLE_EQ(21.05, heightmapBlend.MinHeight()); + + EXPECT_DOUBLE_EQ(0.0, heightmapBlend.FadeDistance()); + heightmapBlend.SetFadeDistance(21.05); + EXPECT_DOUBLE_EQ(21.05, heightmapBlend.FadeDistance()); + + sdf::Heightmap heightmap; + EXPECT_EQ(nullptr, heightmap.Element()); + + EXPECT_EQ(std::string(), heightmap.Uri()); + heightmap.SetUri("http://myuri.com"); + EXPECT_EQ("http://myuri.com", heightmap.Uri()); + + EXPECT_EQ(std::string(), heightmap.FilePath()); + heightmap.SetFilePath("/mypath"); + EXPECT_EQ("/mypath", heightmap.FilePath()); + + EXPECT_EQ(ignition::math::Vector3d::One, heightmap.Size()); + heightmap.SetSize(ignition::math::Vector3d(0.2, 1.4, 7.8)); + EXPECT_EQ(ignition::math::Vector3d(0.2, 1.4, 7.8), heightmap.Size()); + + EXPECT_EQ(ignition::math::Vector3d::Zero, heightmap.Position()); + heightmap.SetPosition(ignition::math::Vector3d(0.2, 1.4, 7.8)); + EXPECT_EQ(ignition::math::Vector3d(0.2, 1.4, 7.8), heightmap.Position()); + + EXPECT_FALSE(heightmap.UseTerrainPaging()); + heightmap.SetUseTerrainPaging(true); + EXPECT_TRUE(heightmap.UseTerrainPaging()); + + EXPECT_EQ(1u, heightmap.Sampling()); + heightmap.SetSampling(12u); + EXPECT_EQ(12u, heightmap.Sampling()); + + EXPECT_EQ(0u, heightmap.TextureCount()); + heightmap.AddTexture(heightmapTexture); + EXPECT_EQ(1u, heightmap.TextureCount()); + auto heightmapTexture2 = heightmap.TextureByIndex(0); + EXPECT_DOUBLE_EQ(heightmapTexture2->Size(), heightmapTexture.Size()); + EXPECT_EQ(heightmapTexture2->Diffuse(), heightmapTexture.Diffuse()); + EXPECT_EQ(heightmapTexture2->Normal(), heightmapTexture.Normal()); + + EXPECT_EQ(0u, heightmap.BlendCount()); + heightmap.AddBlend(heightmapBlend); + EXPECT_EQ(1u, heightmap.BlendCount()); + auto heightmapBlend2 = heightmap.BlendByIndex(0); + EXPECT_DOUBLE_EQ(heightmapBlend2->MinHeight(), heightmapBlend.MinHeight()); + EXPECT_DOUBLE_EQ(heightmapBlend2->FadeDistance(), + heightmapBlend.FadeDistance()); +} + +///////////////////////////////////////////////// +TEST(DOMHeightmap, LoadErrors) +{ + sdf::Heightmap heightmap; + sdf::Errors errors; + + // Null element name + errors = heightmap.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, heightmap.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = heightmap.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, heightmap.Element()); + + // Missing element + sdf->SetName("heightmap"); + errors = heightmap.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")); + EXPECT_NE(nullptr, heightmap.Element()); + + // Texture + sdf::HeightmapTexture heightmapTexture; + + // Null element name + errors = heightmapTexture.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, heightmapTexture.Element()); + + // Bad element name + sdf->SetName("bad"); + errors = heightmapTexture.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, heightmapTexture.Element()); + + // Missing element + sdf->SetName("texture"); + errors = heightmapTexture.Load(sdf); + ASSERT_EQ(3u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")); + EXPECT_NE(std::string::npos, errors[1].Message().find("missing a ")); + EXPECT_NE(std::string::npos, errors[2].Message().find("missing a ")); + EXPECT_NE(nullptr, heightmapTexture.Element()); + + // Blend + sdf::HeightmapBlend heightmapBlend; + + // Null element name + errors = heightmapBlend.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, heightmapBlend.Element()); + + // Bad element name + sdf->SetName("bad"); + errors = heightmapBlend.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, heightmapBlend.Element()); + + // Missing element + sdf->SetName("blend"); + errors = heightmapBlend.Load(sdf); + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "missing a ")); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "missing a ")); + EXPECT_NE(nullptr, heightmapBlend.Element()); +} diff --git a/src/Imu.cc b/src/Imu.cc index 70b33b9e7..a99ae753c 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -21,7 +21,7 @@ using namespace sdf; /// \brief Private imu data. -class sdf::ImuPrivate +class sdf::Imu::Implementation { /// \brief Noise values related to the body-frame linear acceleration on the /// X-axis. @@ -68,42 +68,10 @@ class sdf::ImuPrivate ////////////////////////////////////////////////// Imu::Imu() - : dataPtr(new ImuPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -////////////////////////////////////////////////// -Imu::~Imu() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Imu::Imu(const Imu &_imu) - : dataPtr(new ImuPrivate(*_imu.dataPtr)) -{ -} - -////////////////////////////////////////////////// -Imu::Imu(Imu &&_imu) noexcept - : dataPtr(std::exchange(_imu.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Imu &Imu::operator=(const Imu &_imu) -{ - return *this = Imu(_imu); -} - -////////////////////////////////////////////////// -Imu &Imu::operator=(Imu &&_imu) noexcept -{ - std::swap(this->dataPtr, _imu.dataPtr); - return *this; -} - ////////////////////////////////////////////////// Errors Imu::Load(ElementPtr _sdf) { @@ -318,7 +286,7 @@ void Imu::SetAngularVelocityZNoise(const Noise &_noise) } ////////////////////////////////////////////////// -ignition::math::Vector3d &Imu::GravityDirX() const +const ignition::math::Vector3d &Imu::GravityDirX() const { return this->dataPtr->gravityDirX; } @@ -330,13 +298,13 @@ const std::string &Imu::GravityDirXParentFrame() const } ////////////////////////////////////////////////// -void Imu::SetGravityDirXParentFrame(const std::string &_frame) const +void Imu::SetGravityDirXParentFrame(const std::string &_frame) { this->dataPtr->gravityDirXParentFrame = _frame; } ////////////////////////////////////////////////// -void Imu::SetGravityDirX(const ignition::math::Vector3d &_grav) const +void Imu::SetGravityDirX(const ignition::math::Vector3d &_grav) { this->dataPtr->gravityDirX = _grav; } @@ -360,8 +328,7 @@ const ignition::math::Vector3d &Imu::CustomRpy() const } ////////////////////////////////////////////////// -void Imu::SetCustomRpy( - const ignition::math::Vector3d &_rpy) const +void Imu::SetCustomRpy(const ignition::math::Vector3d &_rpy) { this->dataPtr->customRpy = _rpy; } @@ -373,7 +340,7 @@ const std::string &Imu::CustomRpyParentFrame() const } ////////////////////////////////////////////////// -void Imu::SetCustomRpyParentFrame(const std::string &_frame) const +void Imu::SetCustomRpyParentFrame(const std::string &_frame) { this->dataPtr->customRpyParentFrame = _frame; } diff --git a/src/Joint.cc b/src/Joint.cc index 51018fff3..227cbf1ba 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -24,27 +24,14 @@ #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" #include "sdf/Types.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; -class sdf::JointPrivate +class sdf::Joint::Implementation { - public: JointPrivate() - { - // Initialize here because windows does not support list initialization - // at member initialization (ie ... axis = {{nullptr, nullpter}};). - this->axis[0] = nullptr; - this->axis[1] = nullptr; - } - - /// \brief Copy constructor - /// \param[in] _jointPrivate JointPrivate to copy. - public: explicit JointPrivate(const JointPrivate &_jointPrivate); - - // Delete copy assignment so it is not accidentally used - public: JointPrivate &operator=(const JointPrivate &_) = delete; - /// \brief Name of the joint. public: std::string name = ""; @@ -68,72 +55,22 @@ class sdf::JointPrivate /// \brief Joint axis // cppcheck-suppress - public: std::array, 2> axis; + public: std::array, 2> axis; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; -}; + /// \brief Scoped Frame Attached-To graph at the parent model scope + public: sdf::ScopedGraph frameAttachedToGraph; -///////////////////////////////////////////////// -JointPrivate::JointPrivate(const JointPrivate &_jointPrivate) - : name(_jointPrivate.name), - parentLinkName(_jointPrivate.parentLinkName), - childLinkName(_jointPrivate.childLinkName), - type(_jointPrivate.type), - pose(_jointPrivate.pose), - poseRelativeTo(_jointPrivate.poseRelativeTo), - threadPitch(_jointPrivate.threadPitch), - sdf(_jointPrivate.sdf), - poseRelativeToGraph(_jointPrivate.poseRelativeToGraph) -{ - for (std::size_t i = 0; i < _jointPrivate.axis.size(); ++i) - { - if (_jointPrivate.axis[i]) - { - this->axis[i] = std::make_unique(*_jointPrivate.axis[i]); - } - } -} + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; +}; ///////////////////////////////////////////////// Joint::Joint() - : dataPtr(new JointPrivate) -{ -} - -///////////////////////////////////////////////// -Joint::~Joint() + : dataPtr(ignition::utils::MakeImpl()) { - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Joint::Joint(const Joint &_joint) - : dataPtr(new JointPrivate(*_joint.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Joint::Joint(Joint &&_joint) noexcept - : dataPtr(std::exchange(_joint.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Joint &Joint::operator=(const Joint &_joint) -{ - return *this = Joint(_joint); -} - -///////////////////////////////////////////////// -Joint &Joint::operator=(Joint &&_joint) -{ - std::swap(this->dataPtr, _joint.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -177,6 +114,12 @@ Errors Joint::Load(ElementPtr _sdf) if (parentPair.second) { this->dataPtr->parentLinkName = parentPair.first; + if (!isValidFrameReference(this->dataPtr->parentLinkName)) + { + errors.push_back({ErrorCode::RESERVED_NAME, + "The supplied joint parent name [" + this->dataPtr->parentLinkName + + "] is not valid."}); + } } else { @@ -189,6 +132,12 @@ Errors Joint::Load(ElementPtr _sdf) if (childPair.second) { this->dataPtr->childLinkName = childPair.first; + if (!isValidFrameReference(this->dataPtr->childLinkName)) + { + errors.push_back({ErrorCode::RESERVED_NAME, + "The supplied joint child name [" + this->dataPtr->childLinkName + + "] is not valid."}); + } } else { @@ -214,14 +163,14 @@ Errors Joint::Load(ElementPtr _sdf) if (_sdf->HasElement("axis")) { - this->dataPtr->axis[0].reset(new JointAxis()); + this->dataPtr->axis[0].emplace(); Errors axisErrors = this->dataPtr->axis[0]->Load(_sdf->GetElement("axis")); errors.insert(errors.end(), axisErrors.begin(), axisErrors.end()); } if (_sdf->HasElement("axis2")) { - this->dataPtr->axis[1].reset(new JointAxis()); + this->dataPtr->axis[1].emplace(); Errors axisErrors = this->dataPtr->axis[1]->Load(_sdf->GetElement("axis2")); errors.insert(errors.end(), axisErrors.begin(), axisErrors.end()); } @@ -320,14 +269,13 @@ void Joint::SetChildLinkName(const std::string &_name) ///////////////////////////////////////////////// const JointAxis *Joint::Axis(const unsigned int _index) const { - return this->dataPtr->axis[std::min(_index, 1u)].get(); + return optionalToPointer(this->dataPtr->axis[std::min(_index, 1u)]); } ///////////////////////////////////////////////// void Joint::SetAxis(const unsigned int _index, const JointAxis &_axis) { - this->dataPtr->axis[std::min(_index, 1u)] = - std::make_unique(_axis); + this->dataPtr->axis[std::min(_index, 1u)] = _axis; } ///////////////////////////////////////////////// @@ -354,9 +302,16 @@ void Joint::SetPoseRelativeTo(const std::string &_frame) this->dataPtr->poseRelativeTo = _frame; } +///////////////////////////////////////////////// +void Joint::SetFrameAttachedToGraph( + sdf::ScopedGraph _graph) +{ + this->dataPtr->frameAttachedToGraph = _graph; +} + ///////////////////////////////////////////////// void Joint::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; @@ -370,10 +325,63 @@ void Joint::SetPoseRelativeToGraph( } } +///////////////////////////////////////////////// +Errors Joint::ResolveChildLink(std::string &_link) const +{ + Errors errors; + + auto graph = this->dataPtr->frameAttachedToGraph; + if (!graph) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "Frame has invalid pointer to FrameAttachedToGraph."}); + return errors; + } + + std::string link; + errors = resolveFrameAttachedToBody(link, graph, this->ChildLinkName()); + if (errors.empty()) + { + _link = link; + } + return errors; +} + +///////////////////////////////////////////////// +Errors Joint::ResolveParentLink(std::string &_link) const +{ + Errors errors; + + // special case for world, return without resolving since it's not in a + // model's FrameAttachedToGraph + if ("world" == this->ParentLinkName()) + { + _link = "world"; + return errors; + } + + auto graph = this->dataPtr->frameAttachedToGraph; + if (!graph) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "Frame has invalid pointer to FrameAttachedToGraph."}); + return errors; + } + + std::string link; + errors = resolveFrameAttachedToBody(link, graph, this->ParentLinkName()); + if (errors.empty()) + { + _link = link; + } + return errors; +} + ///////////////////////////////////////////////// sdf::SemanticPose Joint::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, this->ChildLinkName(), diff --git a/src/JointAxis.cc b/src/JointAxis.cc index bedc16b18..a6eee2fac 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -14,15 +14,22 @@ * limitations under the License. * */ +#include +#include + #include #include + +#include "sdf/Assert.hh" #include "sdf/Error.hh" #include "sdf/JointAxis.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" +#include "Utils.hh" using namespace sdf; -class sdf::JointAxisPrivate +class sdf::JointAxis::Implementation { /// \brief Default joint position for this joint axis. public: double initialPosition = 0.0; @@ -80,48 +87,16 @@ class sdf::JointAxisPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// JointAxis::JointAxis() - : dataPtr(new JointAxisPrivate) -{ -} - -///////////////////////////////////////////////// -JointAxis::~JointAxis() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -JointAxis::JointAxis(const JointAxis &_jointAxis) - : dataPtr(new JointAxisPrivate(*_jointAxis.dataPtr)) -{ -} - -///////////////////////////////////////////////// -JointAxis::JointAxis(JointAxis &&_jointAxis) noexcept - : dataPtr(std::exchange(_jointAxis.dataPtr, nullptr)) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -JointAxis &JointAxis::operator=(const JointAxis &_jointAxis) -{ - return *this = JointAxis(_jointAxis); -} - -///////////////////////////////////////////////// -JointAxis &JointAxis::operator=(JointAxis &&_jointAxis) -{ - std::swap(this->dataPtr, _jointAxis.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors JointAxis::Load(ElementPtr _sdf) { @@ -136,8 +111,9 @@ Errors JointAxis::Load(ElementPtr _sdf) // Read the xyz values. if (_sdf->HasElement("xyz")) { - this->dataPtr->xyz = _sdf->Get("xyz", - ignition::math::Vector3d::UnitZ).first; + using ignition::math::Vector3d; + auto errs = this->SetXyz(_sdf->Get("xyz", Vector3d::UnitZ).first); + std::copy(errs.begin(), errs.end(), std::back_inserter(errors)); auto e = _sdf->GetElement("xyz"); if (e->HasAttribute("expressed_in")) { @@ -209,20 +185,16 @@ ignition::math::Vector3d JointAxis::Xyz() const } ///////////////////////////////////////////////// -void JointAxis::SetXyz(const ignition::math::Vector3d &_xyz) +sdf::Errors JointAxis::SetXyz(const ignition::math::Vector3d &_xyz) { + if (sdf::equal(_xyz.Length(), 0.0)) + { + return {Error(ErrorCode::ELEMENT_INVALID, + "The norm of the xyz vector cannot be zero")}; + } this->dataPtr->xyz = _xyz; -} - -///////////////////////////////////////////////// -bool JointAxis::UseParentModelFrame() const -{ - return this->dataPtr->useParentModelFrame; -} -///////////////////////////////////////////////// -void JointAxis::SetUseParentModelFrame(const bool _parentModelFrame) -{ - this->dataPtr->useParentModelFrame = _parentModelFrame; + this->dataPtr->xyz.Normalize(); + return sdf::Errors(); } ///////////////////////////////////////////////// @@ -291,7 +263,7 @@ double JointAxis::Upper() const } ///////////////////////////////////////////////// -void JointAxis::SetUpper(const double _upper) const +void JointAxis::SetUpper(const double _upper) { this->dataPtr->upper = _upper; } @@ -299,7 +271,7 @@ void JointAxis::SetUpper(const double _upper) const ///////////////////////////////////////////////// double JointAxis::Effort() const { - return this->dataPtr->effort; + return infiniteIfNegative(this->dataPtr->effort); } ///////////////////////////////////////////////// @@ -311,11 +283,11 @@ void JointAxis::SetEffort(double _effort) ///////////////////////////////////////////////// double JointAxis::MaxVelocity() const { - return this->dataPtr->maxVelocity; + return infiniteIfNegative(this->dataPtr->maxVelocity); } ///////////////////////////////////////////////// -void JointAxis::SetMaxVelocity(const double _velocity) const +void JointAxis::SetMaxVelocity(const double _velocity) { this->dataPtr->maxVelocity = _velocity; } @@ -327,7 +299,7 @@ double JointAxis::Stiffness() const } ///////////////////////////////////////////////// -void JointAxis::SetStiffness(const double _stiffness) const +void JointAxis::SetStiffness(const double _stiffness) { this->dataPtr->stiffness = _stiffness; } @@ -339,7 +311,7 @@ double JointAxis::Dissipation() const } ///////////////////////////////////////////////// -void JointAxis::SetDissipation(const double _dissipation) const +void JointAxis::SetDissipation(const double _dissipation) { this->dataPtr->dissipation = _dissipation; } @@ -364,7 +336,7 @@ void JointAxis::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void JointAxis::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } @@ -375,7 +347,7 @@ Errors JointAxis::ResolveXyz( const std::string &_resolveTo) const { Errors errors; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -404,7 +376,7 @@ Errors JointAxis::ResolveXyz( } ignition::math::Pose3d pose; - errors = resolvePose(pose, *graph, axisExpressedIn, resolveTo); + errors = resolvePose(pose, graph, axisExpressedIn, resolveTo); if (errors.empty()) { diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index 893d06eb4..770061080 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -34,8 +34,8 @@ TEST(DOMJointAxis, Construction) EXPECT_DOUBLE_EQ(0.0, axis.SpringStiffness()); EXPECT_DOUBLE_EQ(-1e16, axis.Lower()); EXPECT_DOUBLE_EQ(1e16, axis.Upper()); - EXPECT_DOUBLE_EQ(-1, axis.Effort()); - EXPECT_DOUBLE_EQ(-1, axis.MaxVelocity()); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.Effort()); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.MaxVelocity()); EXPECT_DOUBLE_EQ(1e8, axis.Stiffness()); EXPECT_DOUBLE_EQ(1.0, axis.Dissipation()); @@ -44,8 +44,11 @@ TEST(DOMJointAxis, Construction) EXPECT_DOUBLE_EQ(1.2, axis.InitialPosition()); SDF_SUPPRESS_DEPRECATED_END - axis.SetXyz(ignition::math::Vector3d(0, 1, 0)); - EXPECT_EQ(ignition::math::Vector3d::UnitY, axis.Xyz()); + { + sdf::Errors errors = axis.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(errors.empty()); + EXPECT_EQ(ignition::math::Vector3d::UnitY, axis.Xyz()); + } axis.SetXyzExpressedIn("__model__"); EXPECT_EQ("__model__", axis.XyzExpressedIn()); @@ -89,7 +92,7 @@ TEST(DOMJointAxis, Construction) TEST(DOMJointAxis, CopyConstructor) { sdf::JointAxis jointAxis; - jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); sdf::JointAxis jointAxisCopy(jointAxis); EXPECT_EQ(jointAxis.Xyz(), jointAxisCopy.Xyz()); @@ -99,7 +102,7 @@ TEST(DOMJointAxis, CopyConstructor) TEST(DOMJointAxis, AssignmentOperator) { sdf::JointAxis jointAxis; - jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); sdf::JointAxis jointAxisCopy; jointAxisCopy = jointAxis; @@ -111,7 +114,7 @@ TEST(DOMJointAxis, MoveConstructor) { ignition::math::Vector3d axis{0, 1, 0}; sdf::JointAxis jointAxis; - jointAxis.SetXyz(axis); + EXPECT_TRUE(jointAxis.SetXyz(axis).empty()); sdf::JointAxis jointAxisMoved(std::move(jointAxis)); EXPECT_EQ(axis, jointAxisMoved.Xyz()); @@ -122,7 +125,7 @@ TEST(DOMJointAxis, MoveAssignmentOperator) { ignition::math::Vector3d axis{0, 1, 0}; sdf::JointAxis jointAxis; - jointAxis.SetXyz(axis); + EXPECT_TRUE(jointAxis.SetXyz(axis).empty()); sdf::JointAxis jointAxisMoved; jointAxisMoved = std::move(jointAxis); @@ -134,11 +137,11 @@ TEST(DOMJointAxis, CopyAssignmentAfterMove) { ignition::math::Vector3d axis1{0, 1, 0}; sdf::JointAxis jointAxis1; - jointAxis1.SetXyz(axis1); + EXPECT_TRUE(jointAxis1.SetXyz(axis1).empty()); ignition::math::Vector3d axis2{1, 0, 0}; sdf::JointAxis jointAxis2; - jointAxis2.SetXyz(axis2); + EXPECT_TRUE(jointAxis2.SetXyz(axis2).empty()); // This is similar to what std::swap does except it uses std::move for each // assignment @@ -149,3 +152,14 @@ TEST(DOMJointAxis, CopyAssignmentAfterMove) EXPECT_EQ(axis2, jointAxis1.Xyz()); EXPECT_EQ(axis1, jointAxis2.Xyz()); } + +///////////////////////////////////////////////// +TEST(DOMJointAxis, ZeroNormVectorReturnsError) +{ + sdf::JointAxis axis; + EXPECT_TRUE(axis.SetXyz({1.0, 0, 0}).empty()); + + sdf::Errors errors = axis.SetXyz(ignition::math::Vector3d::Zero); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(errors[0].Message(), "The norm of the xyz vector cannot be zero"); +} diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index 1f1fc6100..26a5f7359 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -64,6 +64,12 @@ TEST(DOMJoint, Construction) joint.SetChildLinkName("child"); EXPECT_EQ("child", joint.ChildLinkName()); + std::string body; + EXPECT_FALSE(joint.ResolveChildLink(body).empty()); + EXPECT_TRUE(body.empty()); + EXPECT_FALSE(joint.ResolveParentLink(body).empty()); + EXPECT_TRUE(body.empty()); + joint.SetType(sdf::JointType::BALL); EXPECT_EQ(sdf::JointType::BALL, joint.Type()); joint.SetType(sdf::JointType::CONTINUOUS); @@ -84,10 +90,10 @@ TEST(DOMJoint, Construction) EXPECT_EQ(nullptr, joint.Axis(0)); EXPECT_EQ(nullptr, joint.Axis(1)); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty()); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); joint.SetAxis(1, axis1); ASSERT_TRUE(nullptr != joint.Axis(0)); ASSERT_TRUE(nullptr != joint.Axis(1)); @@ -106,10 +112,10 @@ TEST(DOMJoint, MoveConstructor) sdf::Joint joint; joint.SetName("test_joint"); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty()); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); joint.SetAxis(1, axis1); sdf::Joint joint2(std::move(joint)); @@ -127,10 +133,10 @@ TEST(DOMJoint, CopyConstructor) sdf::Joint joint; joint.SetName("test_joint"); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty()); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); joint.SetAxis(1, axis1); sdf::Joint joint2(joint); @@ -154,10 +160,10 @@ TEST(DOMJoint, MoveAssignment) sdf::Joint joint; joint.SetName("test_joint"); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty()); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); joint.SetAxis(1, axis1); sdf::Joint joint2; @@ -176,10 +182,10 @@ TEST(DOMJoint, CopyAssignment) sdf::Joint joint; joint.SetName("test_joint"); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty()); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); joint.SetAxis(1, axis1); sdf::Joint joint2; @@ -204,19 +210,19 @@ TEST(DOMJoint, CopyAssignmentAfterMove) sdf::Joint joint1; joint1.SetName("test_joint1"); sdf::JointAxis joint1Axis; - joint1Axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + EXPECT_TRUE(joint1Axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty()); joint1.SetAxis(0, joint1Axis); sdf::JointAxis joint1Axis1; - joint1Axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + EXPECT_TRUE(joint1Axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty()); joint1.SetAxis(1, joint1Axis1); sdf::Joint joint2; joint2.SetName("test_joint2"); sdf::JointAxis joint2Axis; - joint2Axis.SetXyz(ignition::math::Vector3d(0, 0, 1)); + EXPECT_TRUE(joint2Axis.SetXyz(ignition::math::Vector3d(0, 0, 1)).empty()); joint2.SetAxis(0, joint2Axis); sdf::JointAxis joint2Axis1; - joint2Axis1.SetXyz(ignition::math::Vector3d(-1, 0, 0)); + EXPECT_TRUE(joint2Axis1.SetXyz(ignition::math::Vector3d(-1, 0, 0)).empty()); joint2.SetAxis(1, joint2Axis1); // This is similar to what std::swap does except it uses std::move for each diff --git a/src/Lidar.cc b/src/Lidar.cc index 103cccdea..e3b54600b 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -20,7 +20,7 @@ using namespace sdf; using namespace ignition; /// \brief Private lidar data. -class sdf::LidarPrivate +class sdf::Lidar::Implementation { /// \brief Number of rays horizontally per laser sweep public: unsigned int horizontalScanSamples{640}; @@ -64,42 +64,10 @@ class sdf::LidarPrivate ////////////////////////////////////////////////// Lidar::Lidar() - : dataPtr(new LidarPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -////////////////////////////////////////////////// -Lidar::~Lidar() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Lidar::Lidar(const Lidar &_lidar) - : dataPtr(new LidarPrivate(*_lidar.dataPtr)) -{ -} - -////////////////////////////////////////////////// -Lidar::Lidar(Lidar &&_lidar) noexcept - : dataPtr(std::exchange(_lidar.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Lidar &Lidar::operator=(const Lidar &_lidar) -{ - return *this = Lidar(_lidar); -} - -////////////////////////////////////////////////// -Lidar &Lidar::operator=(Lidar &&_lidar) noexcept -{ - std::swap(this->dataPtr, _lidar.dataPtr); - return * this; -} - ////////////////////////////////////////////////// /// \brief Load the lidar based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root diff --git a/src/Light.cc b/src/Light.cc index 1c65922fd..ddcf8bdbc 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -18,12 +18,14 @@ #include #include "sdf/Error.hh" #include "sdf/Light.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; /// \brief Light private data. -class sdf::LightPrivate +class sdf::Light::Implementation { /// \brief Name of the light. public: std::string name = ""; @@ -43,8 +45,8 @@ class sdf::LightPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief True if the light should cast shadows. public: bool castShadows = false; @@ -82,64 +84,10 @@ class sdf::LightPrivate ///////////////////////////////////////////////// Light::Light() - : dataPtr(new LightPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Light::~Light() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Light::Light(const Light &_light) - : dataPtr(new LightPrivate) -{ - this->CopyFrom(_light); -} - -///////////////////////////////////////////////// -Light::Light(Light &&_light) noexcept - : dataPtr(std::exchange(_light.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Light &Light::operator=(const Light &_light) -{ - return *this = Light(_light); -} - -////////////////////////////////////////////////// -Light &Light::operator=(Light &&_light) -{ - std::swap(this->dataPtr, _light.dataPtr); - return *this; -} - -////////////////////////////////////////////////// -void Light::CopyFrom(const Light &_light) -{ - this->dataPtr->name= _light.dataPtr->name; - this->dataPtr->pose = _light.dataPtr->pose; - this->dataPtr->poseRelativeTo = _light.dataPtr->poseRelativeTo; - this->dataPtr->type = _light.dataPtr->type; - this->dataPtr->sdf = _light.dataPtr->sdf; - this->dataPtr->castShadows = _light.dataPtr->castShadows; - this->dataPtr->attenuationRange = _light.dataPtr->attenuationRange; - this->dataPtr->linearAttenuation = _light.dataPtr->linearAttenuation; - this->dataPtr->constantAttenuation = _light.dataPtr->constantAttenuation; - this->dataPtr->quadraticAttenuation = _light.dataPtr->quadraticAttenuation; - this->dataPtr->direction = _light.dataPtr->direction; - this->dataPtr->diffuse = _light.dataPtr->diffuse; - this->dataPtr->specular = _light.dataPtr->specular; - this->dataPtr->spotInnerAngle = _light.dataPtr->spotInnerAngle; - this->dataPtr->spotOuterAngle = _light.dataPtr->spotOuterAngle; - this->dataPtr->spotFalloff = _light.dataPtr->spotFalloff; -} - ///////////////////////////////////////////////// Errors Light::Load(ElementPtr _sdf) { @@ -280,7 +228,7 @@ std::string Light::Name() const } ///////////////////////////////////////////////// -void Light::SetName(const std::string &_name) const +void Light::SetName(const std::string &_name) { this->dataPtr->name = _name; } @@ -317,7 +265,7 @@ void Light::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Light::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } @@ -357,7 +305,7 @@ ignition::math::Color Light::Diffuse() const } ///////////////////////////////////////////////// -void Light::SetDiffuse(const ignition::math::Color &_color) const +void Light::SetDiffuse(const ignition::math::Color &_color) { this->dataPtr->diffuse = _color; } @@ -369,7 +317,7 @@ ignition::math::Color Light::Specular() const } ///////////////////////////////////////////////// -void Light::SetSpecular(const ignition::math::Color &_color) const +void Light::SetSpecular(const ignition::math::Color &_color) { this->dataPtr->specular = _color; } diff --git a/src/Link.cc b/src/Link.cc index 9ec72fc60..ed0c57c60 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -28,11 +28,14 @@ #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" + +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; -class sdf::LinkPrivate +class sdf::Link::Implementation { /// \brief Name of the link. public: std::string name = ""; @@ -66,46 +69,14 @@ class sdf::LinkPrivate /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// Link::Link() - : dataPtr(new LinkPrivate) -{ -} - -///////////////////////////////////////////////// -Link::~Link() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Link::Link(const Link &_link) - : dataPtr(new LinkPrivate(*_link.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Link::Link(Link &&_link) noexcept - : dataPtr(std::exchange(_link.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Link &Link::operator=(const Link &_link) -{ - return *this = Link(_link); -} - -///////////////////////////////////////////////// -Link &Link::operator=(Link &&_link) + : dataPtr(ignition::utils::MakeImpl()) { - std::swap(this->dataPtr, _link.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -217,7 +188,7 @@ std::string Link::Name() const } ///////////////////////////////////////////////// -void Link::SetName(const std::string &_name) const +void Link::SetName(const std::string &_name) { this->dataPtr->name = _name; } @@ -374,8 +345,7 @@ void Link::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Link::SetPoseRelativeToGraph( - std::weak_ptr _graph) +void Link::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; @@ -406,6 +376,7 @@ void Link::SetPoseRelativeToGraph( sdf::SemanticPose Link::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, "__model__", diff --git a/src/Magnetometer.cc b/src/Magnetometer.cc index 4ecaf8bb0..796a6898f 100644 --- a/src/Magnetometer.cc +++ b/src/Magnetometer.cc @@ -21,7 +21,7 @@ using namespace sdf; /// \brief Private magnetometer data. -class sdf::MagnetometerPrivate +class sdf::Magnetometer::Implementation { /// \brief The magnetometer noise values. public: std::array noise; @@ -32,42 +32,10 @@ class sdf::MagnetometerPrivate ////////////////////////////////////////////////// Magnetometer::Magnetometer() - : dataPtr(new MagnetometerPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -////////////////////////////////////////////////// -Magnetometer::~Magnetometer() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Magnetometer::Magnetometer(const Magnetometer &_magnetometer) - : dataPtr(new MagnetometerPrivate(*_magnetometer.dataPtr)) -{ -} - -////////////////////////////////////////////////// -Magnetometer::Magnetometer(Magnetometer &&_magnetometer) noexcept - : dataPtr(std::exchange(_magnetometer.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Magnetometer &Magnetometer::operator=(const Magnetometer &_magnetometer) -{ - return *this = Magnetometer(_magnetometer); -} - -////////////////////////////////////////////////// -Magnetometer &Magnetometer::operator=(Magnetometer &&_magnetometer) -{ - std::swap(this->dataPtr, _magnetometer.dataPtr); - return *this; -} - ////////////////////////////////////////////////// Errors Magnetometer::Load(ElementPtr _sdf) { diff --git a/src/Material.cc b/src/Material.cc index dd465a942..d4459801b 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -15,6 +15,7 @@ * */ #include +#include #include #include @@ -25,7 +26,7 @@ using namespace sdf; -class sdf::MaterialPrivate +class sdf::Material::Implementation { /// \brief Script URI public: std::string scriptUri = ""; @@ -42,6 +43,9 @@ class sdf::MaterialPrivate /// \brief Lighting enabled? public: bool lighting = true; + /// \brief Double sided material + public: bool doubleSided = false; + /// \brief Ambient color public: ignition::math::Color ambient {0, 0, 0, 1}; @@ -54,61 +58,23 @@ class sdf::MaterialPrivate /// \brief Emissive color public: ignition::math::Color emissive {0, 0, 0, 1}; + /// \brief Render order + public: float renderOrder = 0; + /// \brief Physically Based Rendering (PBR) properties - public: std::unique_ptr pbr; + public: std::optional pbr; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; + + /// \brief The path to the file where this material was defined. + public: std::string filePath = ""; }; ///////////////////////////////////////////////// Material::Material() - : dataPtr(new MaterialPrivate) -{ -} - -///////////////////////////////////////////////// -Material::~Material() + : dataPtr(ignition::utils::MakeImpl()) { - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Material::Material(const Material &_material) - : dataPtr(new MaterialPrivate) -{ - this->dataPtr->scriptUri = _material.dataPtr->scriptUri; - this->dataPtr->scriptName = _material.dataPtr->scriptName; - this->dataPtr->shader = _material.dataPtr->shader; - this->dataPtr->normalMap = _material.dataPtr->normalMap; - this->dataPtr->lighting = _material.dataPtr->lighting; - this->dataPtr->ambient = _material.dataPtr->ambient; - this->dataPtr->diffuse = _material.dataPtr->diffuse; - this->dataPtr->specular = _material.dataPtr->specular; - this->dataPtr->emissive = _material.dataPtr->emissive; - this->dataPtr->sdf = _material.dataPtr->sdf; - if (_material.dataPtr->pbr) - this->dataPtr->pbr = std::make_unique(*_material.dataPtr->pbr); -} - -///////////////////////////////////////////////// -Material::Material(Material &&_material) noexcept - : dataPtr(std::exchange(_material.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Material &Material::operator=(const Material &_material) -{ - return *this = Material(_material); -} - -///////////////////////////////////////////////// -Material &Material::operator=(Material &&_material) -{ - std::swap(this->dataPtr, _material.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -118,6 +84,8 @@ Errors Material::Load(sdf::ElementPtr _sdf) this->dataPtr->sdf = _sdf; + this->dataPtr->filePath = _sdf->FilePath(); + // Check that the provided SDF element is a // This is an error that cannot be recovered, so return an error. if (_sdf->GetName() != "material") @@ -170,8 +138,12 @@ Errors Material::Load(sdf::ElementPtr _sdf) this->dataPtr->shader = ShaderType::VERTEX; else if (typePair.first == "normal_map_objectspace") this->dataPtr->shader = ShaderType::NORMAL_MAP_OBJECTSPACE; + else if (typePair.first == "normal_map_object_space") + this->dataPtr->shader = ShaderType::NORMAL_MAP_OBJECTSPACE; else if (typePair.first == "normal_map_tangentspace") this->dataPtr->shader = ShaderType::NORMAL_MAP_TANGENTSPACE; + else if (typePair.first == "normal_map_tangent_space") + this->dataPtr->shader = ShaderType::NORMAL_MAP_TANGENTSPACE; else { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -193,6 +165,9 @@ Errors Material::Load(sdf::ElementPtr _sdf) } } + this->dataPtr->renderOrder = _sdf->Get("render_order", + this->dataPtr->renderOrder).first; + this->dataPtr->ambient = _sdf->Get("ambient", this->dataPtr->ambient).first; @@ -205,10 +180,16 @@ Errors Material::Load(sdf::ElementPtr _sdf) this->dataPtr->emissive = _sdf->Get("emissive", this->dataPtr->emissive).first; + this->dataPtr->lighting = _sdf->Get("lighting", + this->dataPtr->lighting).first; + + this->dataPtr->doubleSided = _sdf->Get("double_sided", + this->dataPtr->doubleSided).first; + // load pbr param if (_sdf->HasElement("pbr")) { - this->dataPtr->pbr.reset(new sdf::Pbr()); + this->dataPtr->pbr.emplace(); Errors pbrErrors = this->dataPtr->pbr->Load(_sdf->GetElement("pbr")); errors.insert(errors.end(), pbrErrors.begin(), pbrErrors.end()); } @@ -223,7 +204,7 @@ ignition::math::Color Material::Ambient() const } ////////////////////////////////////////////////// -void Material::SetAmbient(const ignition::math::Color &_color) const +void Material::SetAmbient(const ignition::math::Color &_color) { this->dataPtr->ambient = _color; } @@ -235,7 +216,7 @@ ignition::math::Color Material::Diffuse() const } ////////////////////////////////////////////////// -void Material::SetDiffuse(const ignition::math::Color &_color) const +void Material::SetDiffuse(const ignition::math::Color &_color) { this->dataPtr->diffuse = _color; } @@ -247,7 +228,7 @@ ignition::math::Color Material::Specular() const } ////////////////////////////////////////////////// -void Material::SetSpecular(const ignition::math::Color &_color) const +void Material::SetSpecular(const ignition::math::Color &_color) { this->dataPtr->specular = _color; } @@ -259,11 +240,23 @@ ignition::math::Color Material::Emissive() const } ////////////////////////////////////////////////// -void Material::SetEmissive(const ignition::math::Color &_color) const +void Material::SetEmissive(const ignition::math::Color &_color) { this->dataPtr->emissive = _color; } +////////////////////////////////////////////////// +float Material::RenderOrder() const +{ + return this->dataPtr->renderOrder; +} + +////////////////////////////////////////////////// +void Material::SetRenderOrder(const float _renderOrder) +{ + this->dataPtr->renderOrder = _renderOrder; +} + ////////////////////////////////////////////////// bool Material::Lighting() const { @@ -276,6 +269,18 @@ void Material::SetLighting(const bool _lighting) this->dataPtr->lighting = _lighting; } +////////////////////////////////////////////////// +bool Material::DoubleSided() const +{ + return this->dataPtr->doubleSided; +} + +////////////////////////////////////////////////// +void Material::SetDoubleSided(const bool _doubleSided) +{ + this->dataPtr->doubleSided = _doubleSided; +} + ////////////////////////////////////////////////// sdf::ElementPtr Material::Element() const { @@ -333,11 +338,23 @@ void Material::SetNormalMap(const std::string &_map) ////////////////////////////////////////////////// void Material::SetPbrMaterial(const Pbr &_pbr) { - this->dataPtr->pbr.reset(new Pbr(_pbr)); + this->dataPtr->pbr = _pbr; +} + +////////////////////////////////////////////////// +const Pbr *Material::PbrMaterial() const +{ + return optionalToPointer(this->dataPtr->pbr); +} + +////////////////////////////////////////////////// +const std::string &Material::FilePath() const +{ + return this->dataPtr->filePath; } ////////////////////////////////////////////////// -Pbr *Material::PbrMaterial() const +void Material::SetFilePath(const std::string &_filePath) { - return this->dataPtr->pbr.get(); + this->dataPtr->filePath = _filePath; } diff --git a/src/Material_TEST.cc b/src/Material_TEST.cc index 579bfa6d9..115a56849 100644 --- a/src/Material_TEST.cc +++ b/src/Material_TEST.cc @@ -30,12 +30,15 @@ TEST(DOMMaterial, Construction) EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Specular()); EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Emissive()); EXPECT_TRUE(material.Lighting()); + EXPECT_FLOAT_EQ(0, material.RenderOrder()); + EXPECT_FALSE(material.DoubleSided()); EXPECT_EQ(nullptr, material.Element()); EXPECT_EQ("", material.ScriptUri()); EXPECT_EQ("", material.ScriptName()); EXPECT_EQ(sdf::ShaderType::PIXEL, material.Shader()); EXPECT_EQ("", material.NormalMap()); EXPECT_EQ(nullptr, material.PbrMaterial()); + EXPECT_EQ("", material.FilePath()); } ///////////////////////////////////////////////// @@ -47,10 +50,13 @@ TEST(DOMMaterial, MoveConstructor) material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); material.SetLighting(false); + material.SetRenderOrder(2); + material.SetDoubleSided(true); material.SetScriptUri("banana"); material.SetScriptName("orange"); material.SetShader(sdf::ShaderType::VERTEX); material.SetNormalMap("blueberry"); + material.SetFilePath("/tmp/path"); sdf::Material material2(std::move(material)); EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); @@ -60,11 +66,14 @@ TEST(DOMMaterial, MoveConstructor) EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material2.Emissive()); EXPECT_FALSE(material2.Lighting()); + EXPECT_TRUE(material2.DoubleSided()); + EXPECT_FLOAT_EQ(2.0, material2.RenderOrder()); EXPECT_EQ("banana", material2.ScriptUri()); EXPECT_EQ("orange", material2.ScriptName()); EXPECT_EQ(sdf::ShaderType::VERTEX, material2.Shader()); EXPECT_EQ("blueberry", material2.NormalMap()); EXPECT_EQ(nullptr, material2.PbrMaterial()); + EXPECT_EQ("/tmp/path", material2.FilePath()); } ///////////////////////////////////////////////// @@ -76,10 +85,13 @@ TEST(DOMMaterial, CopyConstructor) material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); material.SetLighting(false); + material.SetRenderOrder(4); + material.SetDoubleSided(true); material.SetScriptUri("banana"); material.SetScriptName("orange"); material.SetShader(sdf::ShaderType::VERTEX); material.SetNormalMap("blueberry"); + material.SetFilePath("/tmp/other"); sdf::Material material2(material); EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); @@ -89,11 +101,14 @@ TEST(DOMMaterial, CopyConstructor) EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material2.Emissive()); EXPECT_FALSE(material2.Lighting()); + EXPECT_TRUE(material2.DoubleSided()); + EXPECT_FLOAT_EQ(4, material2.RenderOrder()); EXPECT_EQ("banana", material2.ScriptUri()); EXPECT_EQ("orange", material2.ScriptName()); EXPECT_EQ(sdf::ShaderType::VERTEX, material2.Shader()); EXPECT_EQ("blueberry", material2.NormalMap()); EXPECT_EQ(nullptr, material2.PbrMaterial()); + EXPECT_EQ("/tmp/other", material2.FilePath()); } ///////////////////////////////////////////////// @@ -105,10 +120,13 @@ TEST(DOMMaterial, AssignmentOperator) material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); material.SetLighting(false); + material.SetRenderOrder(4); + material.SetDoubleSided(true); material.SetScriptUri("banana"); material.SetScriptName("orange"); material.SetShader(sdf::ShaderType::VERTEX); material.SetNormalMap("blueberry"); + material.SetFilePath("/tmp/another"); sdf::Material material2; material2 = material; @@ -119,11 +137,14 @@ TEST(DOMMaterial, AssignmentOperator) EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material2.Emissive()); EXPECT_FALSE(material2.Lighting()); + EXPECT_TRUE(material2.DoubleSided()); + EXPECT_FLOAT_EQ(4, material2.RenderOrder()); EXPECT_EQ("banana", material2.ScriptUri()); EXPECT_EQ("orange", material2.ScriptName()); EXPECT_EQ(sdf::ShaderType::VERTEX, material2.Shader()); EXPECT_EQ("blueberry", material2.NormalMap()); EXPECT_EQ(nullptr, material2.PbrMaterial()); + EXPECT_EQ("/tmp/another", material2.FilePath()); } ///////////////////////////////////////////////// @@ -135,6 +156,8 @@ TEST(DOMMaterial, MoveAssignmentOperator) material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); material.SetLighting(false); + material.SetRenderOrder(4); + material.SetDoubleSided(true); material.SetScriptUri("banana"); material.SetScriptName("orange"); material.SetShader(sdf::ShaderType::VERTEX); @@ -149,6 +172,8 @@ TEST(DOMMaterial, MoveAssignmentOperator) EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material2.Emissive()); EXPECT_FALSE(material2.Lighting()); + EXPECT_TRUE(material2.DoubleSided()); + EXPECT_FLOAT_EQ(4, material2.RenderOrder()); EXPECT_EQ("banana", material2.ScriptUri()); EXPECT_EQ("orange", material2.ScriptName()); EXPECT_EQ(sdf::ShaderType::VERTEX, material2.Shader()); @@ -199,6 +224,14 @@ TEST(DOMMaterial, Set) material.SetLighting(false); EXPECT_FALSE(material.Lighting()); + EXPECT_FLOAT_EQ(0, material.RenderOrder()); + material.SetRenderOrder(5); + EXPECT_FLOAT_EQ(5, material.RenderOrder()); + + EXPECT_FALSE(material.DoubleSided()); + material.SetDoubleSided(true); + EXPECT_TRUE(material.DoubleSided()); + EXPECT_EQ("", material.ScriptUri()); material.SetScriptUri("uri"); EXPECT_EQ("uri", material.ScriptUri()); @@ -215,6 +248,10 @@ TEST(DOMMaterial, Set) material.SetNormalMap("map"); EXPECT_EQ("map", material.NormalMap()); + EXPECT_EQ("", material.FilePath()); + material.SetFilePath("/my/path"); + EXPECT_EQ("/my/path", material.FilePath()); + // set pbr material sdf::Pbr pbr; sdf::PbrWorkflow workflow; @@ -232,12 +269,15 @@ TEST(DOMMaterial, Set) EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f), moved.Specular()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), moved.Emissive()); EXPECT_FALSE(moved.Lighting()); + EXPECT_FLOAT_EQ(5, moved.RenderOrder()); + EXPECT_TRUE(moved.DoubleSided()); EXPECT_EQ("uri", moved.ScriptUri()); EXPECT_EQ("name", moved.ScriptName()); EXPECT_EQ(sdf::ShaderType::VERTEX, moved.Shader()); EXPECT_EQ("map", moved.NormalMap()); EXPECT_EQ(workflow, *moved.PbrMaterial()->Workflow(sdf::PbrWorkflowType::METAL)); + EXPECT_EQ("/my/path", moved.FilePath()); } ///////////////////////////////////////////////// diff --git a/src/Mesh.cc b/src/Mesh.cc index 0e4c6d705..553623c3f 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -19,7 +19,7 @@ using namespace sdf; // Private data class -class sdf::MeshPrivate +class sdf::Mesh::Implementation { /// \brief The mesh's URI. public: std::string uri = ""; @@ -42,48 +42,10 @@ class sdf::MeshPrivate ///////////////////////////////////////////////// Mesh::Mesh() - : dataPtr(new MeshPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Mesh::~Mesh() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Mesh::Mesh(const Mesh &_mesh) - : dataPtr(new MeshPrivate) -{ - this->dataPtr->uri = _mesh.dataPtr->uri; - this->dataPtr->scale = _mesh.dataPtr->scale; - this->dataPtr->submesh = _mesh.dataPtr->submesh; - this->dataPtr->centerSubmesh = _mesh.dataPtr->centerSubmesh; - this->dataPtr->sdf = _mesh.dataPtr->sdf; - this->dataPtr->filePath = _mesh.dataPtr->filePath; -} - -////////////////////////////////////////////////// -Mesh::Mesh(Mesh &&_mesh) noexcept - : dataPtr(std::exchange(_mesh.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Mesh &Mesh::operator=(const Mesh &_mesh) -{ - return *this = Mesh(_mesh); -} - -///////////////////////////////////////////////// -Mesh &Mesh::operator=(Mesh &&_mesh) -{ - std::swap(this->dataPtr, _mesh.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Mesh::Load(ElementPtr _sdf) { diff --git a/src/Model.cc b/src/Model.cc index f60593a0e..7dac404ba 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -27,11 +27,12 @@ #include "sdf/Model.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; -class sdf::ModelPrivate +class sdf::Model::Implementation { /// \brief Name of the model. public: std::string name = ""; @@ -52,6 +53,9 @@ class sdf::ModelPrivate /// \brief Name of the canonical link. public: std::string canonicalLink = ""; + /// \brief Name of the placement frame + public: std::string placementFrameName = ""; + /// \brief Pose of the model public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; @@ -67,79 +71,26 @@ class sdf::ModelPrivate /// \brief The frames specified in this model. public: std::vector frames; + /// \brief The nested models specified in this model. + public: std::vector models; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Frame Attached-To Graph constructed during Load. - public: std::shared_ptr frameAttachedToGraph; + /// \brief Scoped Frame Attached-To graph at the parent model or world scope. + public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Pose Relative-To Graph constructed during Load. - public: std::shared_ptr poseGraph; + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. + public: sdf::ScopedGraph poseGraph; - /// \brief Pose Relative-To Graph in parent (world) scope. - public: std::weak_ptr parentPoseGraph; + /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). + public: std::string poseGraphScopeVertexName; }; ///////////////////////////////////////////////// Model::Model() - : dataPtr(new ModelPrivate) -{ -} - -///////////////////////////////////////////////// -Model::~Model() + : dataPtr(ignition::utils::MakeImpl()) { - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Model::Model(const Model &_model) - : dataPtr(new ModelPrivate(*_model.dataPtr)) -{ - if (_model.dataPtr->frameAttachedToGraph) - { - this->dataPtr->frameAttachedToGraph = - std::make_shared( - *_model.dataPtr->frameAttachedToGraph); - } - if (_model.dataPtr->poseGraph) - { - this->dataPtr->poseGraph = std::make_shared( - *_model.dataPtr->poseGraph); - } - for (auto &link : this->dataPtr->links) - { - link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &joint : this->dataPtr->joints) - { - joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } -} - -///////////////////////////////////////////////// -Model::Model(Model &&_model) noexcept - : dataPtr(std::exchange(_model.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Model &Model::operator=(const Model &_model) -{ - return *this = Model(_model); -} - -///////////////////////////////////////////////// -Model &Model::operator=(Model &&_model) -{ - std::swap(this->dataPtr, _model.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -185,6 +136,9 @@ Errors Model::Load(ElementPtr _sdf) } } + this->dataPtr->placementFrameName = _sdf->Get("placement_frame", + this->dataPtr->placementFrameName).first; + this->dataPtr->isStatic = _sdf->Get("static", false).first; this->dataPtr->selfCollide = _sdf->Get("self_collide", false).first; @@ -197,14 +151,6 @@ Errors Model::Load(ElementPtr _sdf) // Load the pose. Ignore the return value since the model pose is optional. loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); - // Nested models are not yet supported. - if (_sdf->HasElement("model")) - { - errors.push_back({ErrorCode::NESTED_MODELS_UNSUPPORTED, - "Nested models are not yet supported by DOM objects, " - "skipping model [" + this->dataPtr->name + "]."}); - } - if (!_sdf->HasUniqueChildNames()) { sdfwarn << "Non-unique names detected in XML children of model with name[" @@ -215,23 +161,63 @@ Errors Model::Load(ElementPtr _sdf) // name collisions std::unordered_set frameNames; + // Load nested models. + Errors nestedModelLoadErrors = loadUniqueRepeated(_sdf, "model", + this->dataPtr->models); + errors.insert(errors.end(), + nestedModelLoadErrors.begin(), + nestedModelLoadErrors.end()); + + // Nested models are loaded first, and loadUniqueRepeated ensures there are no + // duplicate names, so these names can be added to frameNames without + // checking uniqueness. + for (const auto &model : this->dataPtr->models) + { + frameNames.insert(model.Name()); + } + // Load all the links. Errors linkLoadErrors = loadUniqueRepeated(_sdf, "link", this->dataPtr->links); errors.insert(errors.end(), linkLoadErrors.begin(), linkLoadErrors.end()); - // Links are loaded first, and loadUniqueRepeated ensures there are no - // duplicate names, so these names can be added to frameNames without - // checking uniqueness. - for (const auto &link : this->dataPtr->links) + // Check links for name collisions and modify and warn if so. + for (auto &link : this->dataPtr->links) { - frameNames.insert(link.Name()); + std::string linkName = link.Name(); + if (frameNames.count(linkName) > 0) + { + // This link has a name collision + if (sdfVersion < ignition::math::SemanticVersion(1, 7)) + { + // This came from an old file, so try to workaround by renaming link + linkName += "_link"; + int i = 0; + while (frameNames.count(linkName) > 0) + { + linkName = link.Name() + "_link" + std::to_string(i++); + } + sdfwarn << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing link name to [" + << linkName << "].\n"; + link.SetName(linkName); + } + else + { + sdferr << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this link.\n"; + } + } + frameNames.insert(linkName); } - // If the model is not static: + // If the model is not static and has no nested models: // Require at least one link so the implicit model frame can be attached to // something. - if (!this->Static() && this->dataPtr->links.empty()) + if (!this->Static() && this->dataPtr->links.empty() && + this->dataPtr->models.empty()) { errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, "A model must have at least one link."}); @@ -311,51 +297,6 @@ Errors Model::Load(ElementPtr _sdf) frameNames.insert(frameName); } - // Build the graphs. - - // Build the FrameAttachedToGraph if the model is not static. - // Re-enable this when the buildFrameAttachedToGraph implementation handles - // static models. - if (!this->Static()) - { - this->dataPtr->frameAttachedToGraph - = std::make_shared(); - Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph, this); - errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), - frameAttachedToGraphErrors.end()); - Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph); - errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), - validateFrameAttachedGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - } - - // Build the PoseRelativeToGraph - this->dataPtr->poseGraph = std::make_shared(); - Errors poseGraphErrors = - buildPoseRelativeToGraph(*this->dataPtr->poseGraph, this); - errors.insert(errors.end(), poseGraphErrors.begin(), - poseGraphErrors.end()); - Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(*this->dataPtr->poseGraph); - errors.insert(errors.end(), validatePoseGraphErrors.begin(), - validatePoseGraphErrors.end()); - for (auto &link : this->dataPtr->links) - { - link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &joint : this->dataPtr->joints) - { - joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } return errors; } @@ -437,14 +378,7 @@ const Link *Model::LinkByIndex(const uint64_t _index) const ///////////////////////////////////////////////// bool Model::LinkNameExists(const std::string &_name) const { - for (auto const &l : this->dataPtr->links) - { - if (l.Name() == _name) - { - return true; - } - } - return false; + return nullptr != this->LinkByName(_name); } ///////////////////////////////////////////////// @@ -464,19 +398,28 @@ const Joint *Model::JointByIndex(const uint64_t _index) const ///////////////////////////////////////////////// bool Model::JointNameExists(const std::string &_name) const { - for (auto const &j : this->dataPtr->joints) - { - if (j.Name() == _name) - { - return true; - } - } - return false; + return nullptr != this->JointByName(_name); } ///////////////////////////////////////////////// const Joint *Model::JointByName(const std::string &_name) const { + auto index = _name.rfind("::"); + if (index != std::string::npos) + { + const Model *model = this->ModelByName(_name.substr(0, index)); + if (nullptr != model) + { + return model->JointByName(_name.substr(index + 2)); + } + + // The nested model name preceding the last "::" could not be found. + // For now, try to find a link that matches _name exactly. + // When "::" are reserved and not allowed in names, then uncomment + // the following line to return a nullptr. + // return nullptr; + } + for (auto const &j : this->dataPtr->joints) { if (j.Name() == _name) @@ -504,39 +447,120 @@ const Frame *Model::FrameByIndex(const uint64_t _index) const ///////////////////////////////////////////////// bool Model::FrameNameExists(const std::string &_name) const { + return nullptr != this->FrameByName(_name); +} + +///////////////////////////////////////////////// +const Frame *Model::FrameByName(const std::string &_name) const +{ + auto index = _name.rfind("::"); + if (index != std::string::npos) + { + const Model *model = this->ModelByName(_name.substr(0, index)); + if (nullptr != model) + { + return model->FrameByName(_name.substr(index + 2)); + } + + // The nested model name preceding the last "::" could not be found. + // For now, try to find a link that matches _name exactly. + // When "::" are reserved and not allowed in names, then uncomment + // the following line to return a nullptr. + // return nullptr; + } + for (auto const &f : this->dataPtr->frames) { if (f.Name() == _name) { - return true; + return &f; } } - return false; + return nullptr; } ///////////////////////////////////////////////// -const Frame *Model::FrameByName(const std::string &_name) const +uint64_t Model::ModelCount() const { - for (auto const &f : this->dataPtr->frames) + return this->dataPtr->models.size(); +} + +///////////////////////////////////////////////// +const Model *Model::ModelByIndex(const uint64_t _index) const +{ + if (_index < this->dataPtr->models.size()) + return &this->dataPtr->models[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +bool Model::ModelNameExists(const std::string &_name) const +{ + return nullptr != this->ModelByName(_name); +} + +///////////////////////////////////////////////// +const Model *Model::ModelByName(const std::string &_name) const +{ + auto index = _name.find("::"); + const std::string nextModelName = _name.substr(0, index); + const Model *nextModel = nullptr; + + for (auto const &m : this->dataPtr->models) { - if (f.Name() == _name) + if (m.Name() == nextModelName) { - return &f; + nextModel = &m; + break; } } - return nullptr; + + if (nullptr != nextModel && index != std::string::npos) + { + return nextModel->ModelByName(_name.substr(index + 2)); + } + return nextModel; } ///////////////////////////////////////////////// const Link *Model::CanonicalLink() const +{ + return this->CanonicalLinkAndRelativeName().first; +} + +///////////////////////////////////////////////// +std::pair Model::CanonicalLinkAndRelativeName() const { if (this->CanonicalLinkName().empty()) { - return this->LinkByIndex(0); + if (this->LinkCount() > 0) + { + auto firstLink = this->LinkByIndex(0); + return std::make_pair(firstLink, firstLink->Name()); + } + else if (this->ModelCount() > 0) + { + // Recursively choose the canonical link of the first nested model + // (depth first search). + auto firstModel = this->ModelByIndex(0); + auto canonicalLinkAndName = firstModel->CanonicalLinkAndRelativeName(); + // Prepend firstModelName if a valid link is found. + if (nullptr != canonicalLinkAndName.first) + { + canonicalLinkAndName.second = + firstModel->Name() + "::" + canonicalLinkAndName.second; + } + return canonicalLinkAndName; + } + else + { + return std::make_pair(nullptr, ""); + } } else { - return this->LinkByName(this->CanonicalLinkName()); + return std::make_pair(this->LinkByName(this->CanonicalLinkName()), + this->CanonicalLinkName()); } } @@ -552,6 +576,18 @@ void Model::SetCanonicalLinkName(const std::string &_canonicalLink) this->dataPtr->canonicalLink = _canonicalLink; } +///////////////////////////////////////////////// +const std::string &Model::PlacementFrameName() const +{ + return this->dataPtr->placementFrameName; +} + +///////////////////////////////////////////////// +void Model::SetPlacementFrameName(const std::string &_placementFrame) +{ + this->dataPtr->placementFrameName = _placementFrame; +} + ///////////////////////////////////////////////// const ignition::math::Pose3d &Model::RawPose() const { @@ -577,25 +613,84 @@ void Model::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Model::SetPoseRelativeToGraph( - std::weak_ptr _graph) +void Model::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { - this->dataPtr->parentPoseGraph = _graph; + this->dataPtr->poseGraph = _graph; + this->dataPtr->poseGraphScopeVertexName = + _graph.VertexLocalName(_graph.ScopeVertexId()); + + auto childPoseGraph = + this->dataPtr->poseGraph.ChildModelScope(this->Name()); + for (auto &model : this->dataPtr->models) + { + model.SetPoseRelativeToGraph(childPoseGraph); + } + for (auto &link : this->dataPtr->links) + { + link.SetPoseRelativeToGraph(childPoseGraph); + } + for (auto &joint : this->dataPtr->joints) + { + joint.SetPoseRelativeToGraph(childPoseGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetPoseRelativeToGraph(childPoseGraph); + } +} + +///////////////////////////////////////////////// +void Model::SetFrameAttachedToGraph( + sdf::ScopedGraph _graph) +{ + this->dataPtr->frameAttachedToGraph = _graph; + + auto childFrameAttachedToGraph = + this->dataPtr->frameAttachedToGraph.ChildModelScope(this->Name()); + for (auto &joint : this->dataPtr->joints) + { + joint.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } + for (auto &model : this->dataPtr->models) + { + model.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } } ///////////////////////////////////////////////// sdf::SemanticPose Model::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, - "world", - this->dataPtr->parentPoseGraph); + this->dataPtr->poseGraphScopeVertexName, + this->dataPtr->poseGraph); } ///////////////////////////////////////////////// const Link *Model::LinkByName(const std::string &_name) const { + auto index = _name.rfind("::"); + if (index != std::string::npos) + { + const Model *model = this->ModelByName(_name.substr(0, index)); + if (nullptr != model) + { + return model->LinkByName(_name.substr(index + 2)); + } + + // The nested model name preceding the last "::" could not be found. + // For now, try to find a link that matches _name exactly. + // When "::" are reserved and not allowed in names, then uncomment + // the following line to return a nullptr. + // return nullptr; + } + for (auto const &l : this->dataPtr->links) { if (l.Name() == _name) diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 2d5293e75..fed8a3109 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -22,6 +22,7 @@ #include "sdf/Model.hh" ///////////////////////////////////////////////// +/// Test default construction of sdf::Model. TEST(DOMModel, Construction) { sdf::Model model; @@ -47,23 +48,61 @@ TEST(DOMModel, Construction) model.SetEnableWind(true); EXPECT_TRUE(model.EnableWind()); + EXPECT_EQ(0u, model.ModelCount()); + EXPECT_EQ(nullptr, model.ModelByIndex(0)); + EXPECT_EQ(nullptr, model.ModelByIndex(1)); + EXPECT_EQ(nullptr, model.ModelByName("")); + EXPECT_EQ(nullptr, model.ModelByName("default")); + EXPECT_EQ(nullptr, model.ModelByName("a::b")); + EXPECT_EQ(nullptr, model.ModelByName("a::b::c")); + EXPECT_EQ(nullptr, model.ModelByName("::::")); + EXPECT_FALSE(model.ModelNameExists("")); + EXPECT_FALSE(model.ModelNameExists("default")); + EXPECT_FALSE(model.ModelNameExists("a::b")); + EXPECT_FALSE(model.ModelNameExists("a::b::c")); + EXPECT_FALSE(model.ModelNameExists("::::")); + EXPECT_EQ(0u, model.LinkCount()); EXPECT_EQ(nullptr, model.LinkByIndex(0)); EXPECT_EQ(nullptr, model.LinkByIndex(1)); + EXPECT_EQ(nullptr, model.LinkByName("")); + EXPECT_EQ(nullptr, model.LinkByName("default")); + EXPECT_EQ(nullptr, model.LinkByName("a::b")); + EXPECT_EQ(nullptr, model.LinkByName("a::b::c")); + EXPECT_EQ(nullptr, model.LinkByName("::::")); EXPECT_FALSE(model.LinkNameExists("")); EXPECT_FALSE(model.LinkNameExists("default")); + EXPECT_FALSE(model.LinkNameExists("a::b")); + EXPECT_FALSE(model.LinkNameExists("a::b::c")); + EXPECT_FALSE(model.LinkNameExists("::::")); EXPECT_EQ(0u, model.JointCount()); EXPECT_EQ(nullptr, model.JointByIndex(0)); EXPECT_EQ(nullptr, model.JointByIndex(1)); + EXPECT_EQ(nullptr, model.JointByName("")); + EXPECT_EQ(nullptr, model.JointByName("default")); + EXPECT_EQ(nullptr, model.JointByName("a::b")); + EXPECT_EQ(nullptr, model.JointByName("a::b::c")); + EXPECT_EQ(nullptr, model.JointByName("::::")); EXPECT_FALSE(model.JointNameExists("")); EXPECT_FALSE(model.JointNameExists("default")); + EXPECT_FALSE(model.JointNameExists("a::b")); + EXPECT_FALSE(model.JointNameExists("a::b::c")); + EXPECT_FALSE(model.JointNameExists("::::")); EXPECT_EQ(0u, model.FrameCount()); EXPECT_EQ(nullptr, model.FrameByIndex(0)); EXPECT_EQ(nullptr, model.FrameByIndex(1)); + EXPECT_EQ(nullptr, model.FrameByName("")); + EXPECT_EQ(nullptr, model.FrameByName("default")); + EXPECT_EQ(nullptr, model.FrameByName("a::b")); + EXPECT_EQ(nullptr, model.FrameByName("a::b::c")); + EXPECT_EQ(nullptr, model.FrameByName("::::")); EXPECT_FALSE(model.FrameNameExists("")); EXPECT_FALSE(model.FrameNameExists("default")); + EXPECT_FALSE(model.FrameNameExists("a::b")); + EXPECT_FALSE(model.FrameNameExists("a::b::c")); + EXPECT_FALSE(model.FrameNameExists("::::")); EXPECT_TRUE(model.CanonicalLinkName().empty()); EXPECT_EQ(nullptr, model.CanonicalLink()); @@ -71,6 +110,10 @@ TEST(DOMModel, Construction) EXPECT_EQ("link", model.CanonicalLinkName()); EXPECT_EQ(nullptr, model.CanonicalLink()); + EXPECT_TRUE(model.PlacementFrameName().empty()); + model.SetPlacementFrameName("test_frame"); + EXPECT_EQ("test_frame", model.PlacementFrameName()); + EXPECT_EQ(ignition::math::Pose3d::Zero, model.RawPose()); EXPECT_TRUE(model.PoseRelativeTo().empty()); { diff --git a/src/Noise.cc b/src/Noise.cc index 7b0c77ca3..e1309dae5 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -22,7 +22,7 @@ using namespace sdf; /// \brief Private noise data. -class sdf::NoisePrivate +class sdf::Noise::Implementation { /// \brief The noise type. public: NoiseType type = NoiseType::NONE; @@ -56,42 +56,10 @@ class sdf::NoisePrivate ////////////////////////////////////////////////// Noise::Noise() - : dataPtr(new NoisePrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -////////////////////////////////////////////////// -Noise::~Noise() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Noise::Noise(const Noise &_noise) - : dataPtr(new NoisePrivate(*_noise.dataPtr)) -{ -} - -////////////////////////////////////////////////// -Noise::Noise(Noise &&_noise) noexcept - : dataPtr(std::exchange(_noise.dataPtr, nullptr)) -{ -} - -////////////////////////////////////////////////// -Noise &Noise::operator=(const Noise &_noise) -{ - return *this = Noise(_noise); -} - -////////////////////////////////////////////////// -Noise &Noise::operator=(Noise &&_noise) -{ - std::swap(this->dataPtr, _noise.dataPtr); - return *this; -} - ////////////////////////////////////////////////// Errors Noise::Load(ElementPtr _sdf) { diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index 102490486..2af855e27 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -17,6 +17,7 @@ #include #include +#include #include @@ -25,7 +26,7 @@ #include "sdf/Exception.hh" #include "sdf/Param.hh" -bool check_double(std::string num) +bool check_double(const std::string &num) { const std::string name = "number"; const std::string type = "double"; @@ -100,6 +101,42 @@ TEST(SetFromString, Decimals) ASSERT_TRUE(check_double("0.2345")); } +//////////////////////////////////////////////////// +/// Test Inf +TEST(SetFromString, DoublePositiveInf) +{ + ASSERT_TRUE(std::numeric_limits::has_infinity); + std::vector positiveInfStrings{ + "inf", "Inf", "INF", "+inf", "+Inf", "+INF"}; + for (const auto &infString : positiveInfStrings) + { + sdf::Param doubleParam("key", "double", "0", false, "description"); + double value = 0.; + + EXPECT_TRUE(doubleParam.SetFromString(infString)); + doubleParam.Get(value); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), value); + } +} + +//////////////////////////////////////////////////// +/// Test -Inf +TEST(SetFromString, DoubleNegativeInf) +{ + ASSERT_TRUE(std::numeric_limits::is_iec559); + std::vector negativeInfStrings{ + "-inf", "-Inf", "-INF"}; + for (const auto &infString : negativeInfStrings) + { + sdf::Param doubleParam("key", "double", "0", false, "description"); + double value = 0.; + + EXPECT_TRUE(doubleParam.SetFromString(infString)); + doubleParam.Get(value); + EXPECT_DOUBLE_EQ(- std::numeric_limits::infinity(), value); + } +} + //////////////////////////////////////////////////// /// Test setting and reading hex int values. TEST(Param, HexInt) diff --git a/src/Pbr.cc b/src/Pbr.cc index dbfd06041..9ba0b3a0a 100644 --- a/src/Pbr.cc +++ b/src/Pbr.cc @@ -26,7 +26,7 @@ using namespace sdf; /// \brief Private data for PbrWorkflow class -class sdf::PbrWorkflowPrivate +class sdf::PbrWorkflow::Implementation { /// \brief Workflow type public: PbrWorkflowType type = PbrWorkflowType::NONE; @@ -55,6 +55,12 @@ class sdf::PbrWorkflowPrivate /// \brief Emissive map public: std::string emissiveMap = ""; + /// \brief Light map + public: std::string lightMapFilename; + + /// \brief Light map texture coordinate set + public: unsigned int lightMapUvSet = 0u; + /// \brief Roughness value (metal workflow only) public: double roughness = 0.5; @@ -76,7 +82,7 @@ class sdf::PbrWorkflowPrivate /// \brief Private data for Pbr class -class sdf::PbrPrivate +class sdf::Pbr::Implementation { /// \brief PBR workflows public: std::map workflows; @@ -87,43 +93,10 @@ class sdf::PbrPrivate ///////////////////////////////////////////////// PbrWorkflow::PbrWorkflow() - : dataPtr(new PbrWorkflowPrivate) -{ -} - -///////////////////////////////////////////////// -PbrWorkflow::~PbrWorkflow() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -PbrWorkflow::PbrWorkflow(const PbrWorkflow &_pbr) - : dataPtr(new PbrWorkflowPrivate) -{ - *this->dataPtr = *_pbr.dataPtr; -} - -///////////////////////////////////////////////// -PbrWorkflow::PbrWorkflow(PbrWorkflow &&_pbr) noexcept - : dataPtr(std::exchange(_pbr.dataPtr, nullptr)) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -PbrWorkflow &PbrWorkflow::operator=(const PbrWorkflow &_pbr) -{ - return *this = PbrWorkflow(_pbr); -} - -///////////////////////////////////////////////// -PbrWorkflow &PbrWorkflow::operator=(PbrWorkflow &&_pbr) -{ - std::swap(this->dataPtr, _pbr.dataPtr); - return *this; -} - ////////////////////////////////////////////////// bool PbrWorkflow::operator!=(const PbrWorkflow &_pbr) const { @@ -140,6 +113,7 @@ bool PbrWorkflow::operator==(const PbrWorkflow &_workflow) const && (this->dataPtr->glossinessMap == _workflow.dataPtr->glossinessMap) && (this->dataPtr->environmentMap == _workflow.dataPtr->environmentMap) && (this->dataPtr->emissiveMap == _workflow.dataPtr->emissiveMap) + && (this->dataPtr->lightMapFilename == _workflow.dataPtr->lightMapFilename) && (this->dataPtr->ambientOcclusionMap == _workflow.dataPtr->ambientOcclusionMap) && (ignition::math::equal( @@ -212,6 +186,14 @@ Errors PbrWorkflow::Load(sdf::ElementPtr _sdf) this->dataPtr->emissiveMap = _sdf->Get("emissive_map", this->dataPtr->emissiveMap).first; + if (_sdf->HasElement("light_map")) + { + sdf::ElementPtr lightMapElem = _sdf->GetElement("light_map"); + this->dataPtr->lightMapFilename = lightMapElem->Get(); + this->dataPtr->lightMapUvSet = lightMapElem->Get("uv_set", + this->dataPtr->lightMapUvSet).first; + } + return errors; } @@ -367,60 +349,46 @@ void PbrWorkflow::SetEmissiveMap(const std::string &_map) } ////////////////////////////////////////////////// -sdf::ElementPtr PbrWorkflow::Element() const +std::string PbrWorkflow::LightMap() const { - return this->dataPtr->sdf; + return this->dataPtr->lightMapFilename; } ////////////////////////////////////////////////// -PbrWorkflowType PbrWorkflow::Type() const +void PbrWorkflow::SetLightMap(const std::string &_map, unsigned int _uvSet) { - return this->dataPtr->type; + this->dataPtr->lightMapFilename = _map; + this->dataPtr->lightMapUvSet = _uvSet; } ////////////////////////////////////////////////// -void PbrWorkflow::SetType(PbrWorkflowType _type) +unsigned int PbrWorkflow::LightMapTexCoordSet() const { - this->dataPtr->type = _type; -} - -///////////////////////////////////////////////// -Pbr::Pbr() - : dataPtr(new PbrPrivate) -{ -} - -///////////////////////////////////////////////// -Pbr::~Pbr() -{ - delete this->dataPtr; - this->dataPtr = nullptr; + return this->dataPtr->lightMapUvSet; } ////////////////////////////////////////////////// -Pbr::Pbr(const Pbr &_pbr) - : dataPtr(new PbrPrivate) +sdf::ElementPtr PbrWorkflow::Element() const { - *this->dataPtr = *_pbr.dataPtr; + return this->dataPtr->sdf; } -///////////////////////////////////////////////// -Pbr::Pbr(Pbr &&_pbr) noexcept - : dataPtr(std::exchange(_pbr.dataPtr, nullptr)) +////////////////////////////////////////////////// +PbrWorkflowType PbrWorkflow::Type() const { + return this->dataPtr->type; } -///////////////////////////////////////////////// -Pbr &Pbr::operator=(const Pbr &_pbr) +////////////////////////////////////////////////// +void PbrWorkflow::SetType(PbrWorkflowType _type) { - return *this = Pbr(_pbr); + this->dataPtr->type = _type; } ///////////////////////////////////////////////// -Pbr &Pbr::operator=(Pbr &&_pbr) +Pbr::Pbr() + : dataPtr(ignition::utils::MakeImpl()) { - std::swap(this->dataPtr, _pbr.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -457,7 +425,7 @@ Errors Pbr::Load(sdf::ElementPtr _sdf) } ///////////////////////////////////////////////// -PbrWorkflow *Pbr::Workflow(PbrWorkflowType _type) const +const PbrWorkflow *Pbr::Workflow(PbrWorkflowType _type) const { auto it = this->dataPtr->workflows.find(_type); if (it != this->dataPtr->workflows.end()) diff --git a/src/Pbr_TEST.cc b/src/Pbr_TEST.cc index 39d3892b4..5b4c95da8 100644 --- a/src/Pbr_TEST.cc +++ b/src/Pbr_TEST.cc @@ -35,6 +35,8 @@ TEST(DOMPbr, Construction) EXPECT_EQ(std::string(), workflow.RoughnessMap()); EXPECT_EQ(std::string(), workflow.MetalnessMap()); EXPECT_EQ(std::string(), workflow.EmissiveMap()); + EXPECT_EQ(std::string(), workflow.LightMap()); + EXPECT_EQ(0u, workflow.LightMapTexCoordSet()); EXPECT_DOUBLE_EQ(0.5, workflow.Roughness()); EXPECT_DOUBLE_EQ(0.5, workflow.Metalness()); EXPECT_EQ(std::string(), workflow.SpecularMap()); @@ -70,6 +72,7 @@ TEST(DOMPbr, MoveConstructor) workflow.SetEnvironmentMap("metal_env_map.png"); workflow.SetAmbientOcclusionMap("metal_ambient_occlusion_map.png"); workflow.SetEmissiveMap("metal_emissive_map.png"); + workflow.SetLightMap("metal_light_map.png", 1u); workflow.SetRoughnessMap("roughness_map.png"); workflow.SetMetalnessMap("metalness_map.png"); workflow.SetRoughness(0.8); @@ -84,6 +87,8 @@ TEST(DOMPbr, MoveConstructor) EXPECT_EQ("metal_ambient_occlusion_map.png", workflow2.AmbientOcclusionMap()); EXPECT_EQ("metal_emissive_map.png", workflow2.EmissiveMap()); + EXPECT_EQ("metal_light_map.png", workflow2.LightMap()); + EXPECT_EQ(1u, workflow2.LightMapTexCoordSet()); EXPECT_EQ("roughness_map.png", workflow2.RoughnessMap()); EXPECT_EQ("metalness_map.png", workflow2.MetalnessMap()); EXPECT_DOUBLE_EQ(0.8, workflow2.Roughness()); @@ -104,6 +109,7 @@ TEST(DOMPbr, MoveConstructor) workflow.SetEnvironmentMap("specular_env_map.png"); workflow.SetAmbientOcclusionMap("specular_ambient_occlusion_map.png"); workflow.SetEmissiveMap("specular_emissive_map.png"); + workflow.SetLightMap("specular_light_map.png", 2u); workflow.SetGlossinessMap("glossiness_map.png"); workflow.SetSpecularMap("specular_map.png"); workflow.SetGlossiness(0.1); @@ -117,6 +123,8 @@ TEST(DOMPbr, MoveConstructor) EXPECT_EQ("specular_ambient_occlusion_map.png", workflow2.AmbientOcclusionMap()); EXPECT_EQ("specular_emissive_map.png", workflow2.EmissiveMap()); + EXPECT_EQ("specular_light_map.png", workflow2.LightMap()); + EXPECT_EQ(2u, workflow2.LightMapTexCoordSet()); EXPECT_EQ("specular_map.png", workflow2.SpecularMap()); EXPECT_EQ("glossiness_map.png", workflow2.GlossinessMap()); EXPECT_DOUBLE_EQ(0.1, workflow2.Glossiness()); @@ -155,6 +163,7 @@ TEST(DOMPbr, MoveAssignmentOperator) workflow.SetEnvironmentMap("metal_env_map.png"); workflow.SetAmbientOcclusionMap("metal_ambient_occlusion_map.png"); workflow.SetEmissiveMap("metal_emissive_map.png"); + workflow.SetLightMap("metal_light_map.png", 3u); workflow.SetRoughnessMap("roughness_map.png"); workflow.SetMetalnessMap("metalness_map.png"); workflow.SetRoughness(0.8); @@ -170,6 +179,8 @@ TEST(DOMPbr, MoveAssignmentOperator) EXPECT_EQ("metal_ambient_occlusion_map.png", workflow2.AmbientOcclusionMap()); EXPECT_EQ("metal_emissive_map.png", workflow2.EmissiveMap()); + EXPECT_EQ("metal_light_map.png", workflow2.LightMap()); + EXPECT_EQ(3u, workflow2.LightMapTexCoordSet()); EXPECT_EQ("roughness_map.png", workflow2.RoughnessMap()); EXPECT_EQ("metalness_map.png", workflow2.MetalnessMap()); EXPECT_DOUBLE_EQ(0.8, workflow2.Roughness()); @@ -190,6 +201,7 @@ TEST(DOMPbr, MoveAssignmentOperator) workflow.SetEnvironmentMap("specular_env_map.png"); workflow.SetAmbientOcclusionMap("specular_ambient_occlusion_map.png"); workflow.SetEmissiveMap("specular_emissive_map.png"); + workflow.SetLightMap("specular_light_map.png", 1u); workflow.SetGlossinessMap("glossiness_map.png"); workflow.SetSpecularMap("specular_map.png"); workflow.SetGlossiness(0.1); @@ -204,6 +216,8 @@ TEST(DOMPbr, MoveAssignmentOperator) EXPECT_EQ("specular_ambient_occlusion_map.png", workflow2.AmbientOcclusionMap()); EXPECT_EQ("specular_emissive_map.png", workflow2.EmissiveMap()); + EXPECT_EQ("specular_light_map.png", workflow2.LightMap()); + EXPECT_EQ(1u, workflow2.LightMapTexCoordSet()); EXPECT_EQ("specular_map.png", workflow2.SpecularMap()); EXPECT_EQ("glossiness_map.png", workflow2.GlossinessMap()); EXPECT_DOUBLE_EQ(0.1, workflow2.Glossiness()); @@ -241,6 +255,7 @@ TEST(DOMPbr, CopyConstructor) workflow.SetEnvironmentMap("metal_env_map.png"); workflow.SetAmbientOcclusionMap("metal_ambient_occlusion_map.png"); workflow.SetEmissiveMap("metal_emissive_map.png"); + workflow.SetLightMap("metal_light_map.png", 2u); workflow.SetRoughnessMap("roughness_map.png"); workflow.SetMetalnessMap("metalness_map.png"); workflow.SetRoughness(0.8); @@ -255,6 +270,8 @@ TEST(DOMPbr, CopyConstructor) EXPECT_EQ("metal_ambient_occlusion_map.png", workflow2.AmbientOcclusionMap()); EXPECT_EQ("metal_emissive_map.png", workflow2.EmissiveMap()); + EXPECT_EQ("metal_light_map.png", workflow2.LightMap()); + EXPECT_EQ(2u, workflow2.LightMapTexCoordSet()); EXPECT_EQ("roughness_map.png", workflow2.RoughnessMap()); EXPECT_EQ("metalness_map.png", workflow2.MetalnessMap()); EXPECT_DOUBLE_EQ(0.8, workflow2.Roughness()); @@ -274,6 +291,7 @@ TEST(DOMPbr, CopyConstructor) workflow.SetEnvironmentMap("specular_env_map.png"); workflow.SetAmbientOcclusionMap("specular_ambient_occlusion_map.png"); workflow.SetEmissiveMap("specular_emissive_map.png"); + workflow.SetLightMap("specular_light_map.png", 1u); workflow.SetGlossinessMap("glossiness_map.png"); workflow.SetSpecularMap("specular_map.png"); workflow.SetGlossiness(0.1); @@ -287,6 +305,8 @@ TEST(DOMPbr, CopyConstructor) EXPECT_EQ("specular_ambient_occlusion_map.png", workflow2.AmbientOcclusionMap()); EXPECT_EQ("specular_emissive_map.png", workflow2.EmissiveMap()); + EXPECT_EQ("specular_light_map.png", workflow2.LightMap()); + EXPECT_EQ(1u, workflow2.LightMapTexCoordSet()); EXPECT_EQ("specular_map.png", workflow2.SpecularMap()); EXPECT_EQ("glossiness_map.png", workflow2.GlossinessMap()); EXPECT_DOUBLE_EQ(0.1, workflow2.Glossiness()); @@ -325,6 +345,7 @@ TEST(DOMPbr, AssignmentOperator) workflow.SetEnvironmentMap("metal_env_map.png"); workflow.SetAmbientOcclusionMap("metal_ambient_occlusion_map.png"); workflow.SetEmissiveMap("metal_emissive_map.png"); + workflow.SetLightMap("metal_light_map.png", 1u); workflow.SetRoughnessMap("roughness_map.png"); workflow.SetMetalnessMap("metalness_map.png"); workflow.SetRoughness(0.8); @@ -339,6 +360,8 @@ TEST(DOMPbr, AssignmentOperator) EXPECT_EQ("metal_ambient_occlusion_map.png", workflow2.AmbientOcclusionMap()); EXPECT_EQ("metal_emissive_map.png", workflow2.EmissiveMap()); + EXPECT_EQ("metal_light_map.png", workflow2.LightMap()); + EXPECT_EQ(1u, workflow2.LightMapTexCoordSet()); EXPECT_EQ("roughness_map.png", workflow2.RoughnessMap()); EXPECT_EQ("metalness_map.png", workflow2.MetalnessMap()); EXPECT_DOUBLE_EQ(0.8, workflow2.Roughness()); @@ -358,6 +381,7 @@ TEST(DOMPbr, AssignmentOperator) workflow.SetEnvironmentMap("specular_env_map.png"); workflow.SetAmbientOcclusionMap("specular_ambient_occlusion_map.png"); workflow.SetEmissiveMap("specular_emissive_map.png"); + workflow.SetLightMap("specular_light_map.png", 2u); workflow.SetGlossinessMap("glossiness_map.png"); workflow.SetSpecularMap("specular_map.png"); workflow.SetGlossiness(0.1); @@ -370,6 +394,8 @@ TEST(DOMPbr, AssignmentOperator) EXPECT_EQ("specular_ambient_occlusion_map.png", workflow2.AmbientOcclusionMap()); EXPECT_EQ("specular_emissive_map.png", workflow2.EmissiveMap()); + EXPECT_EQ("specular_light_map.png", workflow2.LightMap()); + EXPECT_EQ(2u, workflow2.LightMapTexCoordSet()); EXPECT_EQ("specular_map.png", workflow2.SpecularMap()); EXPECT_EQ("glossiness_map.png", workflow2.GlossinessMap()); EXPECT_DOUBLE_EQ(0.1, workflow2.Glossiness()); @@ -443,6 +469,10 @@ TEST(DOMPbr, Set) workflow.SetEmissiveMap("metal_emissive_map.png"); EXPECT_EQ("metal_emissive_map.png", workflow.EmissiveMap()); + workflow.SetLightMap("metal_light_map.png", 1u); + EXPECT_EQ("metal_light_map.png", workflow.LightMap()); + EXPECT_EQ(1u, workflow.LightMapTexCoordSet()); + workflow.SetRoughnessMap("roughness_map.png"); EXPECT_EQ("roughness_map.png", workflow.RoughnessMap()); @@ -491,6 +521,10 @@ TEST(DOMPbr, Set) workflow.SetEmissiveMap("specular_emissive_map.png"); EXPECT_EQ("specular_emissive_map.png", workflow.EmissiveMap()); + workflow.SetLightMap("specular_light_map.png", 1u); + EXPECT_EQ("specular_light_map.png", workflow.LightMap()); + EXPECT_EQ(1u, workflow.LightMapTexCoordSet()); + workflow.SetGlossinessMap("glossiness_map.png"); EXPECT_EQ("glossiness_map.png", workflow.GlossinessMap()); diff --git a/src/Physics.cc b/src/Physics.cc index 04fbe4c91..480b85407 100644 --- a/src/Physics.cc +++ b/src/Physics.cc @@ -22,7 +22,7 @@ using namespace sdf; /// \brief Private data -class sdf::PhysicsPrivate +class sdf::Physics::Implementation { /// \brief Profile name public: std::string name {""}; @@ -45,42 +45,10 @@ class sdf::PhysicsPrivate ///////////////////////////////////////////////// Physics::Physics() - : dataPtr(new PhysicsPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Physics::~Physics() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Physics::Physics(const Physics &_physics) - : dataPtr(new PhysicsPrivate(*_physics.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Physics::Physics(Physics &&_physics) noexcept - : dataPtr(std::exchange(_physics.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Physics &Physics::operator=(const Physics &_physics) -{ - return *this = Physics(_physics); -} - -///////////////////////////////////////////////// -Physics &Physics::operator=(Physics &&_physics) -{ - std::swap(this->dataPtr, _physics.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Physics::Load(sdf::ElementPtr _sdf) { @@ -155,7 +123,7 @@ std::string Physics::Name() const } ///////////////////////////////////////////////// -void Physics::SetName(const std::string &_name) const +void Physics::SetName(const std::string &_name) { this->dataPtr->name = _name; } @@ -173,7 +141,7 @@ bool Physics::IsDefault() const } ///////////////////////////////////////////////// -void Physics::SetDefault(const bool _default) const +void Physics::SetDefault(const bool _default) { this->dataPtr->isDefault = _default; } diff --git a/src/Plane.cc b/src/Plane.cc index 157410b18..a24542080 100644 --- a/src/Plane.cc +++ b/src/Plane.cc @@ -21,7 +21,7 @@ using namespace sdf; // Private data class -class sdf::PlanePrivate +class sdf::Plane::Implementation { /// \brief A plane with a unit Z normal vector, size of 1x1 meters, and /// a zero offest. @@ -34,44 +34,10 @@ class sdf::PlanePrivate ///////////////////////////////////////////////// Plane::Plane() - : dataPtr(new PlanePrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Plane::~Plane() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Plane::Plane(const Plane &_plane) - : dataPtr(new PlanePrivate) -{ - this->dataPtr->plane = _plane.dataPtr->plane; - this->dataPtr->sdf = _plane.dataPtr->sdf; -} - -////////////////////////////////////////////////// -Plane::Plane(Plane &&_plane) noexcept - : dataPtr(std::exchange(_plane.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Plane &Plane::operator=(const Plane &_plane) -{ - return *this = Plane(_plane); -} - -///////////////////////////////////////////////// -Plane &Plane::operator=(Plane &&_plane) -{ - std::swap(this->dataPtr, _plane.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Plane::Load(ElementPtr _sdf) { diff --git a/src/Root.cc b/src/Root.cc index 916bd879d..131196979 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -15,6 +15,7 @@ * */ #include +#include #include #include @@ -26,12 +27,14 @@ #include "sdf/World.hh" #include "sdf/parser.hh" #include "sdf/sdf_config.h" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; /// \brief Private data for sdf::Root -class sdf::RootPrivate +class sdf::Root::Implementation { /// \brief Version string public: std::string version = ""; @@ -39,30 +42,89 @@ class sdf::RootPrivate /// \brief The worlds specified under the root SDF element public: std::vector worlds; + /// \brief A model, light or actor under the root SDF element + /// This variant does not currently retain ownership, and instead points to + /// the first model/light/actor if they exist. When the vectors below are + /// removed this should be changed from a variant of pointers to a variant of + /// objects + public: std::variant + modelLightOrActor; + /// \brief The models specified under the root SDF element - public: std::vector models; + /// Deprecated: to be removed in libsdformat12 + public: std::vector models; /// \brief The lights specified under the root SDF element - public: std::vector lights; + /// Deprecated: to be removed in libsdformat12 + public: std::vector lights; /// \brief The actors specified under the root SDF element - public: std::vector actors; + /// Deprecated: to be removed in libsdformat12 + public: std::vector actors; + + /// \brief Frame Attached-To Graphs constructed when loading Worlds. + public: std::vector> + worldFrameAttachedToGraphs; + + /// \brief Frame Attached-To Graphs constructed when loading Models. + /// Deprecated: to be removed in libsdformat12 and converted to a single graph + public: std::vector> + modelFrameAttachedToGraphs; + + /// \brief Pose Relative-To Graphs constructed when loading Worlds. + public: std::vector> + worldPoseRelativeToGraphs; + + /// \brief Pose Relative-To Graphs constructed when loading Models. + /// Deprecated: to be removed in libsdformat12 and converted to a single graph + public: std::vector> + modelPoseRelativeToGraphs; /// \brief The SDF element pointer generated during load. public: sdf::ElementPtr sdf; }; ///////////////////////////////////////////////// -Root::Root() - : dataPtr(new RootPrivate) +template +sdf::ScopedGraph addFrameAttachedToGraph( + std::vector> &_graphList, + const T &_domObj, sdf::Errors &_errors) { + auto &frameGraph = + _graphList.emplace_back(std::make_shared()); + + sdf::Errors buildErrors = + sdf::buildFrameAttachedToGraph(frameGraph, &_domObj); + _errors.insert(_errors.end(), buildErrors.begin(), buildErrors.end()); + + sdf::Errors validateErrors = sdf::validateFrameAttachedToGraph(frameGraph); + _errors.insert(_errors.end(), validateErrors.begin(), validateErrors.end()); + + return frameGraph; } ///////////////////////////////////////////////// -Root::~Root() +template +ScopedGraph addPoseRelativeToGraph( + std::vector> &_graphList, + const T &_domObj, Errors &_errors) +{ + auto &poseGraph = + _graphList.emplace_back(std::make_shared()); + + Errors buildErrors = buildPoseRelativeToGraph(poseGraph, &_domObj); + _errors.insert(_errors.end(), buildErrors.begin(), buildErrors.end()); + + Errors validateErrors = validatePoseRelativeToGraph(poseGraph); + _errors.insert(_errors.end(), validateErrors.begin(), validateErrors.end()); + + return poseGraph; +} + +///////////////////////////////////////////////// +Root::Root() + : dataPtr(ignition::utils::MakeUniqueImpl()) { - delete this->dataPtr; - this->dataPtr = nullptr; } ///////////////////////////////////////////////// @@ -98,7 +160,7 @@ Errors Root::LoadSdfString(const std::string &_sdf) if (!readString(_sdf, sdfParsed, errors)) { errors.push_back( - {ErrorCode::STRING_READ, "Unable to SDF string: " + _sdf}); + {ErrorCode::STRING_READ, "Unable to read SDF string: " + _sdf}); return errors; } @@ -151,6 +213,16 @@ Errors Root::Load(SDFPtr _sdf) World world; Errors worldErrors = world.Load(elem); + + // Build the graphs. + auto frameAttachedToGraph = addFrameAttachedToGraph( + this->dataPtr->worldFrameAttachedToGraphs, world, worldErrors); + world.SetFrameAttachedToGraph(frameAttachedToGraph); + + auto poseRelativeToGraph = addPoseRelativeToGraph( + this->dataPtr->worldPoseRelativeToGraphs, world, worldErrors); + world.SetPoseRelativeToGraph(poseRelativeToGraph); + // Attempt to load the world if (worldErrors.empty()) { @@ -161,10 +233,6 @@ Errors Root::Load(SDFPtr _sdf) "World with name[" + world.Name() + "] already exists." " Each world must have a unique name. Skipping this world."}); } - else - { - this->dataPtr->worlds.push_back(std::move(world)); - } } else { @@ -173,24 +241,71 @@ Errors Root::Load(SDFPtr _sdf) errors.push_back({ErrorCode::ELEMENT_INVALID, "Failed to load a world."}); } + + this->dataPtr->worlds.push_back(std::move(world)); elem = elem->GetNextElement("world"); } } // Load all the models. - Errors modelLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, - "model", this->dataPtr->models); + Errors modelLoadErrors = loadUniqueRepeated( + this->dataPtr->sdf, "model", this->dataPtr->models); errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end()); + if (!this->dataPtr->models.empty()) + { + this->dataPtr->modelLightOrActor = &this->dataPtr->models.front(); + } + + // Build the graphs. + for (sdf::Model &model : this->dataPtr->models) + { + auto frameAttachedToGraph = addFrameAttachedToGraph( + this->dataPtr->modelFrameAttachedToGraphs, model, errors); + + model.SetFrameAttachedToGraph(frameAttachedToGraph); + + auto poseRelativeToGraph = addPoseRelativeToGraph( + this->dataPtr->modelPoseRelativeToGraphs, model, errors); + model.SetPoseRelativeToGraph(poseRelativeToGraph); + } // Load all the lights. - Errors lightLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, + Errors lightLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, "light", this->dataPtr->lights); errors.insert(errors.end(), lightLoadErrors.begin(), lightLoadErrors.end()); + if (!this->dataPtr->lights.empty()) + { + if (std::holds_alternative( + this->dataPtr->modelLightOrActor)) + { + this->dataPtr->modelLightOrActor = &this->dataPtr->lights.front(); + } + else + { + sdfwarn << "Root object can only contain one of model, light or actor. " + << "This behavior was deprecated in libsdformat11 and will soon " + << "be removed"; + } + } // Load all the actors. - Errors actorLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, + Errors actorLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, "actor", this->dataPtr->actors); errors.insert(errors.end(), actorLoadErrors.begin(), actorLoadErrors.end()); + if (!this->dataPtr->actors.empty()) + { + if (std::holds_alternative( + this->dataPtr->modelLightOrActor)) + { + this->dataPtr->modelLightOrActor = &this->dataPtr->actors.front(); + } + else + { + sdfwarn << "Root object can only contain one of model, light or actor. " + << "This behavior was deprecated in libsdformat11 and will soon " + << "be removed"; + } + } return errors; } @@ -233,7 +348,6 @@ bool Root::WorldNameExists(const std::string &_name) const } return false; } - ///////////////////////////////////////////////// uint64_t Root::ModelCount() const { @@ -261,6 +375,17 @@ bool Root::ModelNameExists(const std::string &_name) const return false; } +///////////////////////////////////////////////// +const Model *Root::Model() const +{ + if (std::holds_alternative(this->dataPtr->modelLightOrActor)) + { + return std::get(this->dataPtr->modelLightOrActor); + } + return nullptr; +} + + ///////////////////////////////////////////////// uint64_t Root::LightCount() const { @@ -288,6 +413,16 @@ bool Root::LightNameExists(const std::string &_name) const return false; } +///////////////////////////////////////////////// +const Light *Root::Light() const +{ + if (std::holds_alternative(this->dataPtr->modelLightOrActor)) + { + return std::get(this->dataPtr->modelLightOrActor); + } + return nullptr; +} + ///////////////////////////////////////////////// uint64_t Root::ActorCount() const { @@ -315,6 +450,16 @@ bool Root::ActorNameExists(const std::string &_name) const return false; } +///////////////////////////////////////////////// +const Actor *Root::Actor() const +{ + if (std::holds_alternative(this->dataPtr->modelLightOrActor)) + { + return std::get(this->dataPtr->modelLightOrActor); + } + return nullptr; +} + ///////////////////////////////////////////////// sdf::ElementPtr Root::Element() const { diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 55fb747db..5f6478f12 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -23,6 +23,8 @@ #include "sdf/Link.hh" #include "sdf/Light.hh" #include "sdf/Model.hh" +#include "sdf/World.hh" +#include "sdf/Frame.hh" #include "sdf/Root.hh" ///////////////////////////////////////////////// @@ -37,6 +39,11 @@ TEST(DOMRoot, Construction) EXPECT_TRUE(root.WorldByIndex(0) == nullptr); EXPECT_TRUE(root.WorldByIndex(1) == nullptr); + EXPECT_EQ(nullptr, root.Model()); + EXPECT_EQ(nullptr, root.Light()); + EXPECT_EQ(nullptr, root.Actor()); + + SDF_SUPPRESS_DEPRECATED_BEGIN EXPECT_FALSE(root.ModelNameExists("default")); EXPECT_FALSE(root.ModelNameExists("")); EXPECT_EQ(0u, root.ModelCount()); @@ -54,13 +61,35 @@ TEST(DOMRoot, Construction) EXPECT_EQ(0u, root.ActorCount()); EXPECT_TRUE(root.ActorByIndex(0) == nullptr); EXPECT_TRUE(root.ActorByIndex(1) == nullptr); + SDF_SUPPRESS_DEPRECATED_END +} + +///////////////////////////////////////////////// +TEST(DOMRoot, MoveConstructor) +{ + sdf::Root root; + root.SetVersion("test_root"); + + sdf::Root root2(std::move(root)); + EXPECT_EQ("test_root", root2.Version()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, MoveAssignmentOperator) +{ + sdf::Root root; + root.SetVersion("test_root"); + + sdf::Root root2; + root2 = std::move(root); + EXPECT_EQ("test_root", root2.Version()); } ///////////////////////////////////////////////// -TEST(DOMRoot, StringParse) +TEST(DOMRoot, StringModelSdfParse) { std::string sdf = "" - " " + " " " " " " " " @@ -72,36 +101,14 @@ TEST(DOMRoot, StringParse) " " " " " " - " " - " -0.5 0.1 -0.9" - " " - " " - " 0 0 1.0 0 0 0" - " " - " /fake/path/to/mesh.dae" - " 1.0" - " " - " " - " /fake/path/to/mesh.dae" - " 0.055" - " true" - " " - " " - " " " "; sdf::Root root; sdf::Errors errors = root.LoadSdfString(sdf); EXPECT_TRUE(errors.empty()); - EXPECT_EQ(1u, root.ModelCount()); - EXPECT_EQ(1u, root.LightCount()); EXPECT_NE(nullptr, root.Element()); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_NE(nullptr, model->Element()); @@ -119,17 +126,72 @@ TEST(DOMRoot, StringParse) EXPECT_NE(nullptr, collision->Element()); EXPECT_EQ("box_col", collision->Name()); - EXPECT_TRUE(root.LightNameExists("sun")); - EXPECT_EQ(1u, root.LightCount()); - const sdf::Light *light = root.LightByIndex(0); + EXPECT_EQ(nullptr, root.Light()); + EXPECT_EQ(nullptr, root.Actor()); + EXPECT_EQ(0u, root.WorldCount()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, StringLightSdfParse) +{ + std::string sdf = "" + " " + " " + " -0.5 0.1 -0.9" + " " + " "; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdf); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Light *light = root.Light(); ASSERT_NE(nullptr, light); + EXPECT_EQ("sun", light->Name()); EXPECT_NE(nullptr, light->Element()); - EXPECT_TRUE(root.ActorNameExists("actor_test")); - EXPECT_EQ(1u, root.ActorCount()); - const sdf::Actor *actor = root.ActorByIndex(0); + EXPECT_EQ(nullptr, root.Model()); + EXPECT_EQ(nullptr, root.Actor()); + EXPECT_EQ(0u, root.WorldCount()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, StringActorSdfParse) +{ + std::string sdf = "" + " " + " " + " 0 0 1.0 0 0 0" + " " + " /fake/path/to/mesh.dae" + " 1.0" + " " + " " + " /fake/path/to/mesh.dae" + " 0.055" + " true" + " " + " " + " " + " "; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sdf); + EXPECT_TRUE(errors.empty()); + + const sdf::Actor *actor = root.Actor(); ASSERT_NE(nullptr, actor); + EXPECT_EQ("actor_test", actor->Name()); EXPECT_NE(nullptr, actor->Element()); + + EXPECT_EQ(nullptr, root.Model()); + EXPECT_EQ(nullptr, root.Light()); + EXPECT_EQ(0u, root.WorldCount()); } ///////////////////////////////////////////////// @@ -140,3 +202,91 @@ TEST(DOMRoot, Set) root.SetVersion(SDF_PROTOCOL_VERSION); EXPECT_STREQ(SDF_PROTOCOL_VERSION, root.Version().c_str()); } + +///////////////////////////////////////////////// +TEST(DOMRoot, FrameSemanticsOnMove) +{ + const std::string sdfString1 = R"( + + + + 0 1 0 0 0 0 + + + )"; + + const std::string sdfString2 = R"( + + + + 1 1 0 0 0 0 + + + )"; + + auto testFrame1 = [](const sdf::Root &_root) + { + auto *world = _root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *frame = world->FrameByIndex(0); + ASSERT_NE(nullptr, frame); + EXPECT_EQ("frame1", frame->Name()); + ignition::math::Pose3d pose; + sdf::Errors errors = frame->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, 1, 0, 0, 0, 0), pose); + }; + + auto testFrame2 = [](const sdf::Root &_root) + { + auto *world = _root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *frame = world->FrameByIndex(0); + ASSERT_NE(nullptr, frame); + EXPECT_EQ("frame2", frame->Name()); + ignition::math::Pose3d pose; + sdf::Errors errors = frame->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(1, 1, 0, 0, 0, 0), pose); + }; + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + testFrame1(root1); + } + + { + sdf::Root root2; + sdf::Errors errors = root2.LoadSdfString(sdfString2); + EXPECT_TRUE(errors.empty()) << errors; + testFrame2(root2); + } + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + + // then root1 is moved into root2 via the move constructor + sdf::Root root2(std::move(root1)); + testFrame1(root2); + } + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + sdf::Root root2; + errors = root2.LoadSdfString(sdfString2); + EXPECT_TRUE(errors.empty()) << errors; + + testFrame1(root1); + testFrame2(root2); + + // root1 is moved into root2 via the move assignment operator. + root2 = std::move(root1); + testFrame1(root2); + } +} diff --git a/src/SDFExtension.hh b/src/SDFExtension.hh index 4c5ce3381..53ca1df83 100644 --- a/src/SDFExtension.hh +++ b/src/SDFExtension.hh @@ -18,7 +18,7 @@ #ifndef SDFORMAT_SDFEXTENSION_HH_ #define SDFORMAT_SDFEXTENSION_HH_ -#include +#include #include #include @@ -34,6 +34,8 @@ namespace sdf // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // + using XMLDocumentPtr= std::shared_ptr; + using XMLElementPtr = std::shared_ptr; /// \internal /// \brief A class for holding sdf extension elements in urdf @@ -57,7 +59,7 @@ namespace sdf public: std::string material; /// \brief blobs of xml to be copied into the visual sdf element - public: std::vector > visual_blobs; + public: std::vector visual_blobs; /// \brief blobs of xml to be copied into the collision sdf element /// An example might be: @@ -84,7 +86,7 @@ namespace sdf /// /// where all the contents of `` element is copied into the /// resulting collision sdf. - public: std::vector > collision_blobs; + public: std::vector collision_blobs; // body, default off public: bool setStaticFlag; @@ -118,7 +120,7 @@ namespace sdf public: bool implicitSpringDamper; // blobs into body or robot - public: std::vector > blobs; + public: std::vector blobs; friend class URDF2SDF; }; diff --git a/src/Scene.cc b/src/Scene.cc index 9458d0dbb..6bcc0249d 100644 --- a/src/Scene.cc +++ b/src/Scene.cc @@ -20,7 +20,7 @@ using namespace sdf; /// \brief Scene private data. -class sdf::ScenePrivate +class sdf::Scene::Implementation { /// \brief True if grid should be enabled public: bool grid = true; @@ -39,46 +39,17 @@ class sdf::ScenePrivate public: ignition::math::Color background = ignition::math::Color(0.7f, 0.7f, .7f); + /// \brief Pointer to the sky properties. + public: std::optional sky; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; }; ///////////////////////////////////////////////// Scene::Scene() - : dataPtr(new ScenePrivate) -{ -} - -///////////////////////////////////////////////// -Scene::~Scene() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Scene::Scene(const Scene &_scene) - : dataPtr(new ScenePrivate(*_scene.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Scene::Scene(Scene &&_scene) noexcept - : dataPtr(std::exchange(_scene.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Scene &Scene::operator=(const Scene &_scene) -{ - return *this = Scene(_scene); -} - -///////////////////////////////////////////////// -Scene &Scene::operator=(Scene &&_scene) + : dataPtr(ignition::utils::MakeImpl()) { - std::swap(this->dataPtr, _scene.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -118,6 +89,14 @@ Errors Scene::Load(ElementPtr _sdf) this->dataPtr->originVisual = _sdf->Get("origin_visual", this->dataPtr->originVisual).first; + // load sky + if (_sdf->HasElement("sky")) + { + this->dataPtr->sky.emplace(); + Errors err = this->dataPtr->sky->Load(_sdf->GetElement("sky")); + errors.insert(errors.end(), err.begin(), err.end()); + } + return errors; } @@ -180,6 +159,18 @@ void Scene::SetOriginVisual(const bool _enabled) this->dataPtr->originVisual = _enabled; } +///////////////////////////////////////////////// +void Scene::SetSky(const sdf::Sky &_sky) +{ + this->dataPtr->sky = _sky; +} + +///////////////////////////////////////////////// +const sdf::Sky *Scene::Sky() const +{ + return optionalToPointer(this->dataPtr->sky); +} + ///////////////////////////////////////////////// sdf::ElementPtr Scene::Element() const { diff --git a/src/Scene_TEST.cc b/src/Scene_TEST.cc index e9a423970..0e10ece0d 100644 --- a/src/Scene_TEST.cc +++ b/src/Scene_TEST.cc @@ -27,6 +27,7 @@ TEST(DOMScene, Construction) EXPECT_TRUE(scene.Grid()); EXPECT_TRUE(scene.Shadows()); EXPECT_TRUE(scene.OriginVisual()); + EXPECT_EQ(nullptr, scene.Sky()); } ///////////////////////////////////////////////// @@ -41,6 +42,8 @@ TEST(DOMScene, CopyConstruction) scene.SetGrid(false); scene.SetShadows(false); scene.SetOriginVisual(false); + sdf::Sky sky; + scene.SetSky(sky); sdf::Scene scene2(scene); EXPECT_EQ(ignition::math::Color::Blue, scene2.Ambient()); @@ -48,6 +51,7 @@ TEST(DOMScene, CopyConstruction) EXPECT_FALSE(scene2.Grid()); EXPECT_FALSE(scene2.Shadows()); EXPECT_FALSE(scene2.OriginVisual()); + EXPECT_NE(nullptr, scene2.Sky()); EXPECT_NE(nullptr, scene2.Element()); EXPECT_EQ(scene.Element(), scene2.Element()); @@ -62,6 +66,8 @@ TEST(DOMScene, MoveConstruction) scene.SetGrid(false); scene.SetShadows(false); scene.SetOriginVisual(false); + sdf::Sky sky; + scene.SetSky(sky); sdf::Scene scene2(std::move(scene)); EXPECT_EQ(ignition::math::Color::Blue, scene2.Ambient()); @@ -69,6 +75,7 @@ TEST(DOMScene, MoveConstruction) EXPECT_FALSE(scene2.Grid()); EXPECT_FALSE(scene2.Shadows()); EXPECT_FALSE(scene2.OriginVisual()); + EXPECT_NE(nullptr, scene2.Sky()); } ///////////////////////////////////////////////// @@ -80,6 +87,8 @@ TEST(DOMScene, MoveAssignmentOperator) scene.SetGrid(false); scene.SetShadows(false); scene.SetOriginVisual(false); + sdf::Sky sky; + scene.SetSky(sky); sdf::Scene scene2; scene2 = std::move(scene); @@ -88,6 +97,7 @@ TEST(DOMScene, MoveAssignmentOperator) EXPECT_FALSE(scene2.Grid()); EXPECT_FALSE(scene2.Shadows()); EXPECT_FALSE(scene2.OriginVisual()); + EXPECT_NE(nullptr, scene2.Sky()); } ///////////////////////////////////////////////// @@ -99,6 +109,8 @@ TEST(DOMScene, AssignmentOperator) scene.SetGrid(false); scene.SetShadows(false); scene.SetOriginVisual(false); + sdf::Sky sky; + scene.SetSky(sky); sdf::Scene scene2; scene2 = scene; @@ -107,6 +119,7 @@ TEST(DOMScene, AssignmentOperator) EXPECT_FALSE(scene2.Grid()); EXPECT_FALSE(scene2.Shadows()); EXPECT_FALSE(scene2.OriginVisual()); + EXPECT_NE(nullptr, scene2.Sky()); } ///////////////////////////////////////////////// @@ -152,4 +165,8 @@ TEST(DOMScene, Set) EXPECT_TRUE(scene.OriginVisual()); scene.SetOriginVisual(false); EXPECT_FALSE(scene.OriginVisual()); + + sdf::Sky sky; + scene.SetSky(sky); + EXPECT_NE(nullptr, scene.Sky()); } diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh new file mode 100644 index 000000000..0c30ae588 --- /dev/null +++ b/src/ScopedGraph.hh @@ -0,0 +1,471 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef SDF_SCOPED_GRAPH_HH +#define SDF_SCOPED_GRAPH_HH + +#include +#include +#include +#include +#include + +#include +#include + +#include "sdf/sdf_config.h" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { + +/// \brief Data structure that holds information associated with each scope +struct ScopedGraphData +{ + /// \brief The vertex ID of the designated scope vertex + ignition::math::graph::VertexId scopeVertexId { + ignition::math::graph::kNullId}; + + /// \brief The prefix for all names under this scope + std::string prefix {}; + + /// \brief The context name of this scope. Either world or __model__ + std::string scopeContextName {}; +}; + +// Forward declarations for static_assert +struct PoseRelativeToGraph; +struct FrameAttachedToGraph; + +/// \brief The ScopedGraph allows manipulating FrameAttachedTo and +/// PoseRelativeTo graphs within a smaller scope such as the scope of a model +/// inside a world graph. +/// +/// Each valid instance of ScopedGraph has a pointer to either a +/// FrameAttachedToGraph or a PoseRelativeToGraph. In addition, each instance +/// contains the following: +/// - The prefix of the scope, which is empty for a world scope and the +/// name of a model for a model scope. +/// - The context name of the scope, which can be either "world" or "__model__". +/// - ID of the vertex on which the scope is anchored. For +/// PoseRelativeToGraph's, this vertex is the root of the sub tree represented +/// by the ScopedGraph instance. For FrameAttachedToGraph, this vertex only +/// represents the scope boundary and does not imply that it's the root of the +/// sub tree because these graphs can have multiple disconnected trees. +/// +/// When a vertex is added to the graph via ScopedGraph, the prefix is prepended +/// to the input name of the vertex. The name of the vertex with the prefix is +/// known as the absolute name of the vertex and the name of the vertex without +/// the prefix is known as the local name of the vertex. +/// +/// \remark ScopedGraph has pointer semantic. i.e, a ScopedGraph object and its +/// copy point to the same underlying data and modifying one will affect the +/// other. +/// +/// \tparam T Either FrameAttachedTo or PoseRelativeTo graph +template +class ScopedGraph +{ + static_assert( + std::is_same_v || + std::is_same_v, + "Template parameter has to be either sdf::PoseRelativeToGraph or " + "sdf::FrameAttachedToGraph"); + + /// \brief template for extracting the graph type from the template parameter. + /// \tparam G ignition::math::DirectedGraph type + /// \return Vertex and Edge types of G + public: template + struct GraphTypeExtracter + { + using Vertex = void; + using Edge = void; + }; + + /// \brief Specialization for GraphTypeExtracter on + /// ignition::math::DirectedGraph + public: template + struct GraphTypeExtracter> + { + using Vertex = V; + using Edge = E; + }; + + // Type aliases + public: using MathGraphType = typename T::GraphType; + public: using VertexId = ignition::math::graph::VertexId; + public: using VertexType = typename GraphTypeExtracter::Vertex; + public: using EdgeType = typename GraphTypeExtracter::Edge; + public: using Vertex = ignition::math::graph::Vertex; + public: using Edge = ignition::math::graph::DirectedEdge; + public: using MapType = typename T::MapType; + + /// \brief Default constructor. The constructed object is invalid as it + /// doesn't point to any graph. + public: ScopedGraph() = default; + + /// \brief Constructor. The constructed object holds a weak pointer to the + /// passed in graph. + /// \param[in] _graph A shared pointer to PoseRelativeTo or FrameAttachedTo + /// graph. + /// \remark The state of the ScopedGraph after construction is invalid because + /// member variables such as "ScopedGraphData::prefix" and + /// "ScopedGraphData::scopeName" will be empty. Therefore, it is imperative + /// that this constructor should only be used right before calling either + /// buildPoseRelativeToGraph or buildFrameAttachedToGraph. + public: explicit ScopedGraph(const std::shared_ptr &_graph); + + /// \brief Creates a scope anchored at an existing child model vertex. + /// \param[in] _name Name of child model vertex. The prefix of the current + /// scope will be prepended to this name before searching for the vertex that + /// matches the name. The new scope will have a new prefix formed by appending + /// _name to the existing prefix of the current scope. + /// \return A new child scope. + public: ScopedGraph ChildModelScope(const std::string &_name) const; + + /// \brief Checks if the scope points to a valid graph. + /// \return True if the scope points to a valid graph. + public: explicit operator bool() const; + + /// \brief Immutable reference to the underlying PoseRelativeTo::graph or + /// FrameAttachedTo::graph. + public: const MathGraphType &Graph() const; + + /// \brief Immutable reference to the underlying PoseRelativeTo::map or + /// FrameAttachedTo::map. + public: const MapType &Map() const; + + /// \brief Adds a scope vertex to the graph. This creates a new + /// scope by making a copy of the current scope with a new prefix and scope + /// type name. A new scope vertex is then added to the graph. + /// \param[in] _prefix The new prefix of the scope. + /// \param[in] _name The Name of the scope vertex to be added. The full name + /// of the vertex will be newPrefix::_name, where newPrefix is the prefix + /// obtained by adding _prefix to the prefix of the current scope. + /// \param[in] _scopeTypeName Name of scope type (either __model__ or world) + /// \param[in] _data Vertex data + public: ScopedGraph AddScopeVertex(const std::string &_prefix, + const std::string &_name, const std::string &_scopeTypeName, + const VertexType &_data); + + /// \brief Adds a vertex to the graph. + /// \param[in] _name The local name of the vertex. The absolute name of the + /// vertex will be _prefix::_name. + /// \param[in] _data Vertex data. + /// \return The newly created vertex. + public: Vertex &AddVertex(const std::string &_name, const VertexType &_data); + + /// \brief Adds an edge to the graph. + /// \param[in] _vertexPair Pair of vertex IDs between which the edge will be + /// created. + /// \param[in] _data Edge data. + /// \return The newly created edge. + public: Edge &AddEdge(const ignition::math::graph::VertexId_P &_vertexPair, + const EdgeType &_data); + + /// \brief Gets all the local names of the vertices in the current scope. + /// \return A list of vertex names in the current scope. + public: std::vector VertexNames() const; + + /// \brief Get the local name of a vertex. + /// \param[in] _id ID of the vertex. + /// \return The local name of the vertex. If the vertex was not found in the + /// graph, "__null__" will be returned. + public: std::string VertexLocalName(const VertexId &_id) const; + + /// \brief Get the local name of a vertex. + /// \param[in] _vertex Vertex object. + /// \return The local name of the vertex. If the vertex was not found in the + /// graph, "__null__" will be returned. + public: std::string VertexLocalName(const Vertex &_vertex) const; + + /// \brief Update the information contained by an edge + /// \remark Since this function has to remove the edge from the graph and add + /// a new one, the original edge will have a new ID. + /// \param[in] _edge The edge to update. + /// \param[in] _data The new data. + public: void UpdateEdge(Edge &_edge, const EdgeType &_data); + + /// \brief Count the number of vertices with a given local name in the scope. + /// \param[in] _name Local name query + /// \return Number of vertices that have the given local name in the scope. + public: size_t Count(const std::string &_name) const; + + /// \brief Get the vertex ID of a vertex with a give local name. + /// \param[in] _name Local name of the vertex + /// \return Vertex ID of the vertex if found in this scope of the graph. + /// Otherwise, kNullId is returned. + public: VertexId VertexIdByName(const std::string &_name) const; + + /// \brief Get the scope vertex. + /// \return Immutable reference to the scope vertex of this scope. + public: const Vertex &ScopeVertex() const; + + /// \brief Get the scope vertex ID. + /// \return The ID of the scope vertex of this scope. + public: VertexId ScopeVertexId() const; + + /// \brief Check if the graph on which this scope is based is the same as the + /// input graph. + /// \param[in] _graph Graph object to check. + /// \return True if this scope points to the same graph as the input. + public: bool PointsTo(const std::shared_ptr &_graph) const; + + /// \brief Set the context name of the scope. + /// \param[in] _name New context name. + public: void SetScopeContextName(const std::string &_name); + + /// \brief Get the current context name. + /// \return The current context name. + public: const std::string &ScopeContextName() const; + + /// \brief Add the current prefix to the given name. + /// \param[in] _name Input name. + /// \return The name with the prefix prepended. + public: std::string AddPrefix(const std::string &_name) const; + + /// \brief Find the current prefix in the given name. If the prefix is found, + /// remove it. + /// \param[in] _name Input name. + /// \return The name with the prefix removed and true, if the prefix is found. + /// Otherwise, the original name and false + public: std::pair FindAndRemovePrefix( + const std::string &_name) const; + + /// \brief Shared pointer to either a FrameAttachedToGraph or + /// PoseRelativeToGraph. + private: std::shared_ptr graphPtr; + + /// \brief Shared pointer to the scope's data. A shared_ptr is used because + /// this data has to be shared among many DOM objects. + private: std::shared_ptr dataPtr; +}; + +///////////////////////////////////////////////// +template +ScopedGraph::ScopedGraph(const std::shared_ptr &_graph) + : graphPtr(_graph) + , dataPtr(std::make_shared()) +{ +} + +///////////////////////////////////////////////// +template +ScopedGraph ScopedGraph::ChildModelScope(const std::string &_name) const +{ + auto newScopedGraph = *this; + newScopedGraph.dataPtr = std::make_shared(); + newScopedGraph.dataPtr->prefix = this->AddPrefix(_name); + newScopedGraph.dataPtr->scopeVertexId = + newScopedGraph.VertexIdByName("__model__"); + newScopedGraph.dataPtr->scopeContextName = "__model__"; + return newScopedGraph; +} + +///////////////////////////////////////////////// +template +ScopedGraph::operator bool() const +{ + return (this->graphPtr != nullptr) && (this->dataPtr != nullptr); +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::Graph() const -> const MathGraphType & +{ + return this->graphPtr->graph; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::Map() const -> const MapType & +{ + return this->graphPtr->map; +} + +///////////////////////////////////////////////// +template +ScopedGraph ScopedGraph::AddScopeVertex(const std::string &_prefix, + const std::string &_name, const std::string &_scopeTypeName, + const VertexType &_data) +{ + auto newScopedGraph = *this; + newScopedGraph.dataPtr = std::make_shared(); + newScopedGraph.dataPtr->prefix = this->AddPrefix(_prefix); + Vertex &vert = newScopedGraph.AddVertex(_name, _data); + newScopedGraph.dataPtr->scopeVertexId = vert.Id(); + newScopedGraph.SetScopeContextName(_scopeTypeName); + return newScopedGraph; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::AddVertex( + const std::string &_name, const VertexType &_data) -> Vertex & +{ + const std::string newName = this->AddPrefix(_name); + Vertex &vert = this->graphPtr->graph.AddVertex(newName, _data); + this->graphPtr->map[newName] = vert.Id(); + return vert; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::AddEdge( + const ignition::math::graph::VertexId_P &_vertexPair, const EdgeType &_data) + -> Edge & +{ + Edge &edge = this->graphPtr->graph.AddEdge(_vertexPair, _data); + return edge; +} + +///////////////////////////////////////////////// +template +std::vector ScopedGraph::VertexNames() const +{ + std::vector out; + for (const auto &namePair : this->Map()) + { + const auto &idNamePair = this->FindAndRemovePrefix(namePair.first); + if (idNamePair.second) + { + out.push_back(idNamePair.first); + } + } + return out; +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::VertexLocalName(const Vertex &_vert) const +{ + return this->FindAndRemovePrefix(_vert.Name()).first; +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::VertexLocalName(const VertexId &_id) const +{ + return this->VertexLocalName(this->Graph().VertexFromId(_id)); +} + +///////////////////////////////////////////////// +template +void ScopedGraph::UpdateEdge(Edge &_edge, const EdgeType &_data) +{ + // There's no API to update the data of an edge, so we remove the edge and + // insert a new one with the new pose. + auto tailVertexId = _edge.Tail(); + auto headVertexId = _edge.Head(); + auto &graph = this->graphPtr->graph; + graph.RemoveEdge(_edge.Id()); + _edge = graph.AddEdge({tailVertexId, headVertexId}, _data); +} + +///////////////////////////////////////////////// +template +const std::string &ScopedGraph::ScopeContextName() const +{ + return this->dataPtr->scopeContextName; +} + +///////////////////////////////////////////////// +template +void ScopedGraph::SetScopeContextName(const std::string &_name) +{ + this->dataPtr->scopeContextName = _name; +} + +///////////////////////////////////////////////// +template +std::size_t ScopedGraph::Count(const std::string &_name) const +{ + return this->graphPtr->map.count(this->AddPrefix(_name)); +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::VertexIdByName(const std::string &_name) const -> VertexId +{ + auto &map = this->Map(); + auto it = map.find(this->AddPrefix(_name)); + if (it != map.end()) + return it->second; + else + return ignition::math::graph::kNullId; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::ScopeVertex() const -> const Vertex & +{ + return this->graphPtr->graph.VertexFromId( + this->dataPtr->scopeVertexId); +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::ScopeVertexId() const -> VertexId +{ + return this->dataPtr->scopeVertexId; +} + +///////////////////////////////////////////////// +template +bool ScopedGraph::PointsTo(const std::shared_ptr &_graph) const +{ + return this->graphPtr == _graph; +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::AddPrefix(const std::string &_name) const +{ + if (this->dataPtr->prefix.empty()) + { + return _name; + } + else + { + return this->dataPtr->prefix + "::" + _name; + } +} + +///////////////////////////////////////////////// +template +std::pair +ScopedGraph::FindAndRemovePrefix(const std::string &_name) const +{ + if (this->dataPtr->prefix.empty()) + { + return std::make_pair(_name, true); + } + + // Convenient alias + const std::string &prefix = this->dataPtr->prefix; + if (_name.size() > prefix.size() + 2 && + (0 == _name.compare(0, prefix.size(), prefix))) + { + return std::make_pair(_name.substr(prefix.size() + 2), true); + } + return std::make_pair(_name, false); +} +} +} + +#endif /* SDF_SCOPED_GRAPH_HH */ diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index 01e7f904e..98154aed9 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -21,6 +21,7 @@ #include "sdf/Error.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" namespace sdf @@ -28,8 +29,10 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { /// \internal /// \brief Private data for the SemanticPose class. -class SemanticPosePrivate +class SemanticPose::Implementation { + public: std::string name = ""; + /// \brief Raw pose of the SemanticPose object. public: ignition::math::Pose3d rawPose = ignition::math::Pose3d::Zero; @@ -39,18 +42,21 @@ class SemanticPosePrivate /// \brief Name of the default frame to resolve to. public: std::string defaultResolveTo = ""; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. + /// TODO (addisu) Make this const + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// SemanticPose::SemanticPose( + const std::string &_name, const ignition::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, - const std::weak_ptr _graph) - : dataPtr(std::make_unique()) + const sdf::ScopedGraph &_graph) + : dataPtr(ignition::utils::MakeImpl()) { + this->dataPtr->name = _name; this->dataPtr->rawPose = _pose; this->dataPtr->relativeTo = _relativeTo; this->dataPtr->defaultResolveTo = _defaultResolveTo; @@ -58,30 +64,17 @@ SemanticPose::SemanticPose( } ///////////////////////////////////////////////// -SemanticPose::~SemanticPose() = default; - -///////////////////////////////////////////////// -SemanticPose::SemanticPose(const SemanticPose &_semanticPose) - : dataPtr(std::make_unique(*_semanticPose.dataPtr)) -{ -} - -///////////////////////////////////////////////// -SemanticPose::SemanticPose(SemanticPose &&_semanticPose) noexcept = default; - -///////////////////////////////////////////////// -SemanticPose &SemanticPose::operator=(SemanticPose &&_semanticPose) = default; - -///////////////////////////////////////////////// -SemanticPose &SemanticPose::operator=(const SemanticPose &_semanticPose) +SemanticPose::SemanticPose( + const ignition::math::Pose3d &_pose, + const std::string &_relativeTo, + const std::string &_defaultResolveTo, + const sdf::ScopedGraph &_graph) + : dataPtr(ignition::utils::MakeImpl()) { - if (!this->dataPtr) - { - // This would happen if this object is moved from. - this->dataPtr = std::make_unique(); - } - *this->dataPtr = *_semanticPose.dataPtr; - return *this; + this->dataPtr->rawPose = _pose; + this->dataPtr->relativeTo = _relativeTo; + this->dataPtr->defaultResolveTo = _defaultResolveTo; + this->dataPtr->poseRelativeToGraph = _graph; } ///////////////////////////////////////////////// @@ -103,10 +96,10 @@ Errors SemanticPose::Resolve( { Errors errors; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (!graph) { - errors.push_back({ErrorCode::ELEMENT_INVALID, + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "SemanticPose has invalid pointer to PoseRelativeToGraph."}); return errors; } @@ -124,8 +117,15 @@ Errors SemanticPose::Resolve( } ignition::math::Pose3d pose; - errors = resolvePose(pose, *graph, relativeTo, resolveTo); - pose *= this->RawPose(); + if (this->dataPtr->name.empty()) + { + errors = resolvePose(pose, graph, relativeTo, resolveTo); + pose *= this->RawPose(); + } + else + { + errors = resolvePose(pose, graph, this->dataPtr->name, resolveTo); + } if (errors.empty()) { diff --git a/src/Sensor.cc b/src/Sensor.cc index 1d3c467f3..fa849adfe 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -16,17 +16,21 @@ */ #include #include +#include #include #include #include "sdf/AirPressure.hh" #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" #include "sdf/Error.hh" +#include "sdf/ForceTorque.hh" #include "sdf/Imu.hh" #include "sdf/Magnetometer.hh" #include "sdf/Lidar.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -58,55 +62,8 @@ const std::vector sensorTypeStrs = "thermal_camera" }; -class sdf::SensorPrivate +class sdf::Sensor::Implementation { - /// \brief Default constructor - public: SensorPrivate() = default; - - /// \brief Copy constructor - public: explicit SensorPrivate(const SensorPrivate &_sensor) - : type(_sensor.type), - name(_sensor.name), - topic(_sensor.topic), - pose(_sensor.pose), - poseRelativeTo(_sensor.poseRelativeTo), - sdf(_sensor.sdf), - updateRate(_sensor.updateRate) - - { - if (_sensor.magnetometer) - { - this->magnetometer = std::make_unique( - *_sensor.magnetometer); - } - if (_sensor.altimeter) - { - this->altimeter = std::make_unique(*_sensor.altimeter); - } - if (_sensor.airPressure) - { - this->airPressure = std::make_unique( - *_sensor.airPressure); - } - if (_sensor.camera) - { - this->camera = std::make_unique(*_sensor.camera); - } - if (_sensor.imu) - { - this->imu = std::make_unique(*_sensor.imu); - } - if (_sensor.lidar) - { - this->lidar = std::make_unique(*_sensor.lidar); - } - // Developer note: If you add a new sensor type, make sure to also - // update the Sensor::operator== function. Please bump this text down as - // new sensors are added so that the next developer sees the message. - } - // Delete copy assignment so it is not accidentally used - public: SensorPrivate &operator=(const SensorPrivate &) = delete; - // \brief The sensor type. public: SensorType type = SensorType::NONE; @@ -128,26 +85,29 @@ class sdf::SensorPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; + + /// \brief Optional magnetometer. + public: std::optional magnetometer; - /// \brief Pointer to a magnetometer. - public: std::unique_ptr magnetometer; + /// \brief Optional altimeter. + public: std::optional altimeter; - /// \brief Pointer to an altimeter. - public: std::unique_ptr altimeter; + /// \brief Optional air pressure sensor. + public: std::optional airPressure; - /// \brief Pointer to an air pressure sensor. - public: std::unique_ptr airPressure; + /// \brief Optional camera. + public: std::optional camera; - /// \brief Pointer to a camera. - public: std::unique_ptr camera; + /// \brief Optional force torque sensor. + public: std::optional forceTorque; - /// \brief Pointer to an IMU. - public: std::unique_ptr imu; + /// \brief Optional IMU. + public: std::optional imu; - /// \brief Pointer to a lidar. - public: std::unique_ptr lidar; + /// \brief Optional lidar. + public: std::optional lidar; // Developer note: If you add a new sensor type, make sure to also // update the Sensor::operator== function. Please bump this text down as @@ -160,40 +120,8 @@ class sdf::SensorPrivate ///////////////////////////////////////////////// Sensor::Sensor() - : dataPtr(new SensorPrivate) -{ -} - -///////////////////////////////////////////////// -Sensor::~Sensor() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Sensor::Sensor(const Sensor &_sensor) - : dataPtr(new SensorPrivate(*_sensor.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Sensor::Sensor(Sensor &&_sensor) noexcept - : dataPtr(std::exchange(_sensor.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Sensor &Sensor::operator=(const Sensor &_sensor) -{ - return *this = Sensor(_sensor); -} - -///////////////////////////////////////////////// -Sensor &Sensor::operator=(Sensor &&_sensor) + : dataPtr(ignition::utils::MakeImpl()) { - std::swap(this->dataPtr, _sensor.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -219,6 +147,8 @@ bool Sensor::operator==(const Sensor &_sensor) const return *(this->dataPtr->magnetometer) == *(_sensor.dataPtr->magnetometer); case SensorType::AIR_PRESSURE: return *(this->dataPtr->airPressure) == *(_sensor.dataPtr->airPressure); + case SensorType::FORCE_TORQUE: + return *(this->dataPtr->forceTorque) == *(_sensor.dataPtr->forceTorque); case SensorType::IMU: return *(this->dataPtr->imu) == *(_sensor.dataPtr->imu); case SensorType::CAMERA: @@ -292,7 +222,7 @@ Errors Sensor::Load(ElementPtr _sdf) if (type == "air_pressure") { this->dataPtr->type = SensorType::AIR_PRESSURE; - this->dataPtr->airPressure.reset(new AirPressure()); + this->dataPtr->airPressure.emplace(); Errors err = this->dataPtr->airPressure->Load( _sdf->GetElement("air_pressure")); errors.insert(errors.end(), err.begin(), err.end()); @@ -300,14 +230,14 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "altimeter") { this->dataPtr->type = SensorType::ALTIMETER; - this->dataPtr->altimeter.reset(new Altimeter()); + this->dataPtr->altimeter.emplace(); Errors err = this->dataPtr->altimeter->Load(_sdf->GetElement("altimeter")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "camera") { this->dataPtr->type = SensorType::CAMERA; - this->dataPtr->camera.reset(new Camera()); + this->dataPtr->camera.emplace(); Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -318,27 +248,31 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "depth" || type == "depth_camera") { this->dataPtr->type = SensorType::DEPTH_CAMERA; - this->dataPtr->camera.reset(new Camera()); + this->dataPtr->camera.emplace(); Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "rgbd" || type == "rgbd_camera") { this->dataPtr->type = SensorType::RGBD_CAMERA; - this->dataPtr->camera.reset(new Camera()); + this->dataPtr->camera.emplace(); Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "thermal" || type == "thermal_camera") { this->dataPtr->type = SensorType::THERMAL_CAMERA; - this->dataPtr->camera.reset(new Camera()); + this->dataPtr->camera.emplace(); Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "force_torque") { this->dataPtr->type = SensorType::FORCE_TORQUE; + this->dataPtr->forceTorque.emplace(); + Errors err = this->dataPtr->forceTorque->Load( + _sdf->GetElement("force_torque")); + errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "gps") { @@ -347,7 +281,7 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "gpu_ray" || type == "gpu_lidar") { this->dataPtr->type = SensorType::GPU_LIDAR; - this->dataPtr->lidar.reset(new Lidar()); + this->dataPtr->lidar.emplace(); Errors err = this->dataPtr->lidar->Load( _sdf->GetElement(_sdf->HasElement("lidar") ? "lidar" : "ray")); errors.insert(errors.end(), err.begin(), err.end()); @@ -355,8 +289,7 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "imu") { this->dataPtr->type = SensorType::IMU; - this->dataPtr->type = SensorType::IMU; - this->dataPtr->imu.reset(new Imu()); + this->dataPtr->imu.emplace(); Errors err = this->dataPtr->imu->Load(_sdf->GetElement("imu")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -367,7 +300,7 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "magnetometer") { this->dataPtr->type = SensorType::MAGNETOMETER; - this->dataPtr->magnetometer.reset(new Magnetometer()); + this->dataPtr->magnetometer.emplace(); Errors err = this->dataPtr->magnetometer->Load( _sdf->GetElement("magnetometer")); errors.insert(errors.end(), err.begin(), err.end()); @@ -379,7 +312,7 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "ray" || type == "lidar") { this->dataPtr->type = SensorType::LIDAR; - this->dataPtr->lidar.reset(new Lidar()); + this->dataPtr->lidar.emplace(); Errors err = this->dataPtr->lidar->Load( _sdf->GetElement(_sdf->HasElement("lidar") ? "lidar" : "ray")); errors.insert(errors.end(), err.begin(), err.end()); @@ -474,7 +407,7 @@ void Sensor::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Sensor::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } @@ -524,49 +457,49 @@ bool Sensor::SetType(const std::string &_typeStr) ///////////////////////////////////////////////// const Magnetometer *Sensor::MagnetometerSensor() const { - return this->dataPtr->magnetometer.get(); + return optionalToPointer(this->dataPtr->magnetometer); } ///////////////////////////////////////////////// void Sensor::SetMagnetometerSensor(const Magnetometer &_mag) { - this->dataPtr->magnetometer = std::make_unique(_mag); + this->dataPtr->magnetometer = _mag; } ///////////////////////////////////////////////// const Altimeter *Sensor::AltimeterSensor() const { - return this->dataPtr->altimeter.get(); + return optionalToPointer(this->dataPtr->altimeter); } ///////////////////////////////////////////////// void Sensor::SetAltimeterSensor(const Altimeter &_alt) { - this->dataPtr->altimeter = std::make_unique(_alt); + this->dataPtr->altimeter = _alt; } ///////////////////////////////////////////////// const AirPressure *Sensor::AirPressureSensor() const { - return this->dataPtr->airPressure.get(); + return optionalToPointer(this->dataPtr->airPressure); } ///////////////////////////////////////////////// void Sensor::SetAirPressureSensor(const AirPressure &_air) { - this->dataPtr->airPressure = std::make_unique(_air); + this->dataPtr->airPressure = _air; } ///////////////////////////////////////////////// const Lidar *Sensor::LidarSensor() const { - return this->dataPtr->lidar.get(); + return optionalToPointer(this->dataPtr->lidar); } ///////////////////////////////////////////////// void Sensor::SetLidarSensor(const Lidar &_lidar) { - this->dataPtr->lidar = std::make_unique(_lidar); + this->dataPtr->lidar = _lidar; } ///////////////////////////////////////////////// @@ -593,23 +526,35 @@ std::string Sensor::TypeStr() const ///////////////////////////////////////////////// void Sensor::SetCameraSensor(const Camera &_cam) { - this->dataPtr->camera = std::make_unique(_cam); + this->dataPtr->camera = _cam; } ///////////////////////////////////////////////// const Camera *Sensor::CameraSensor() const { - return this->dataPtr->camera.get(); + return optionalToPointer(this->dataPtr->camera); +} + +///////////////////////////////////////////////// +void Sensor::SetForceTorqueSensor(const ForceTorque &_ft) +{ + this->dataPtr->forceTorque = _ft; +} + +///////////////////////////////////////////////// +const ForceTorque *Sensor::ForceTorqueSensor() const +{ + return optionalToPointer(this->dataPtr->forceTorque); } ///////////////////////////////////////////////// void Sensor::SetImuSensor(const Imu &_imu) { - this->dataPtr->imu = std::make_unique(_imu); + this->dataPtr->imu = _imu; } ///////////////////////////////////////////////// const Imu *Sensor::ImuSensor() const { - return this->dataPtr->imu.get(); + return optionalToPointer(this->dataPtr->imu); } diff --git a/src/Sky.cc b/src/Sky.cc new file mode 100644 index 000000000..2635ac1f8 --- /dev/null +++ b/src/Sky.cc @@ -0,0 +1,203 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include "sdf/Sky.hh" +#include "Utils.hh" + +using namespace sdf; + +/// \brief Sky private data. +class sdf::Sky::Implementation +{ + /// \brief Time of day + public: double time = 10.0; + + /// \brief Sunrise time + public: double sunrise = 6.0; + + /// \brief Sunset time + public: double sunset = 20.0; + + /// \brief Cloud speed + public: double cloudSpeed = 0.6; + + /// \brief Cloud direction. + public: ignition::math::Angle cloudDirection; + + /// \brief Cloud humidity + public: double cloudHumidity = 0.5; + + /// \brief Cloud mean size + public: double cloudMeanSize = 0.5; + + /// \brief Cloud ambient color + public: ignition::math::Color cloudAmbient = + ignition::math::Color(0.8f, 0.8f, 0.8f); + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Sky::Sky() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +double Sky::Time() const +{ + return this->dataPtr->time; +} + +///////////////////////////////////////////////// +void Sky::SetTime(double _time) +{ + this->dataPtr->time = _time; +} + +///////////////////////////////////////////////// +double Sky::Sunrise() const +{ + return this->dataPtr->sunrise; +} + +///////////////////////////////////////////////// +void Sky::SetSunrise(double _sunrise) +{ + this->dataPtr->sunrise = _sunrise; +} + +///////////////////////////////////////////////// +double Sky::Sunset() const +{ + return this->dataPtr->sunset; +} + +///////////////////////////////////////////////// +void Sky::SetSunset(double _sunset) +{ + this->dataPtr->sunset = _sunset; +} + +///////////////////////////////////////////////// +double Sky::CloudSpeed() const +{ + return this->dataPtr->cloudSpeed; +} + +///////////////////////////////////////////////// +void Sky::SetCloudSpeed(double _speed) +{ + this->dataPtr->cloudSpeed = _speed; +} + +///////////////////////////////////////////////// +ignition::math::Angle Sky::CloudDirection() const +{ + return this->dataPtr->cloudDirection; +} + +///////////////////////////////////////////////// +void Sky::SetCloudDirection(const ignition::math::Angle &_angle) +{ + this->dataPtr->cloudDirection = _angle; +} + +///////////////////////////////////////////////// +double Sky::CloudHumidity() const +{ + return this->dataPtr->cloudHumidity; +} + +///////////////////////////////////////////////// +void Sky::SetCloudHumidity(double _humidity) +{ + this->dataPtr->cloudHumidity = _humidity; +} + +///////////////////////////////////////////////// +double Sky::CloudMeanSize() const +{ + return this->dataPtr->cloudMeanSize; +} + +///////////////////////////////////////////////// +void Sky::SetCloudMeanSize(double _size) +{ + this->dataPtr->cloudMeanSize = _size; +} + +///////////////////////////////////////////////// +ignition::math::Color Sky::CloudAmbient() const +{ + return this->dataPtr->cloudAmbient; +} + +///////////////////////////////////////////////// +void Sky::SetCloudAmbient(const ignition::math::Color &_ambient) +{ + this->dataPtr->cloudAmbient = _ambient; +} + +///////////////////////////////////////////////// +Errors Sky::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that the provided SDF element is a element. + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "sky") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a Sky, but the provided SDF element is not a " + "."}); + return errors; + } + + this->dataPtr->time = _sdf->Get("time", this->dataPtr->time).first; + this->dataPtr->sunrise = + _sdf->Get("sunrise", this->dataPtr->sunrise).first; + this->dataPtr->sunset = + _sdf->Get("sunset", this->dataPtr->sunset).first; + + if ( _sdf->HasElement("clouds")) + { + sdf::ElementPtr cloudElem = _sdf->GetElement("clouds"); + this->dataPtr->cloudSpeed = + cloudElem->Get("speed", this->dataPtr->cloudSpeed).first; + this->dataPtr->cloudDirection = + cloudElem->Get("direction", + this->dataPtr->cloudDirection).first; + this->dataPtr->cloudHumidity = + cloudElem->Get("humidity", this->dataPtr->cloudHumidity).first; + this->dataPtr->cloudMeanSize = + cloudElem->Get("mean_size", this->dataPtr->cloudMeanSize).first; + this->dataPtr->cloudAmbient = + cloudElem->Get("ambient", + this->dataPtr->cloudAmbient).first; + } + + return errors; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Sky::Element() const +{ + return this->dataPtr->sdf; +} diff --git a/src/Sky_TEST.cc b/src/Sky_TEST.cc new file mode 100644 index 000000000..03461bc43 --- /dev/null +++ b/src/Sky_TEST.cc @@ -0,0 +1,188 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/Sky.hh" + +///////////////////////////////////////////////// +TEST(DOMSky, Construction) +{ + sdf::Sky sky; + EXPECT_DOUBLE_EQ(10.0, sky.Time()); + EXPECT_DOUBLE_EQ(6.0, sky.Sunrise()); + EXPECT_DOUBLE_EQ(20.0, sky.Sunset()); + EXPECT_DOUBLE_EQ(0.6, sky.CloudSpeed()); + EXPECT_EQ(ignition::math::Angle(), sky.CloudDirection()); + EXPECT_DOUBLE_EQ(0.5, sky.CloudHumidity()); + EXPECT_DOUBLE_EQ(0.5, sky.CloudMeanSize()); + EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f), + sky.CloudAmbient()); +} + +///////////////////////////////////////////////// +TEST(DOMSky, CopyConstruction) +{ + sdf::ElementPtr sdf(std::make_shared()); + + sdf::Sky sky; + sky.Load(sdf); + sky.SetTime(1.0); + sky.SetSunrise(5.0); + sky.SetSunset(15.0); + sky.SetCloudSpeed(0.3); + sky.SetCloudDirection(ignition::math::Angle(1.2)); + sky.SetCloudHumidity(0.9); + sky.SetCloudMeanSize(0.123); + sky.SetCloudAmbient(ignition::math::Color::Blue); + + sdf::Sky sky2(sky); + EXPECT_DOUBLE_EQ(1.0, sky2.Time()); + EXPECT_DOUBLE_EQ(5.0, sky2.Sunrise()); + EXPECT_DOUBLE_EQ(15.0, sky2.Sunset()); + EXPECT_DOUBLE_EQ(0.3, sky2.CloudSpeed()); + EXPECT_EQ(ignition::math::Angle(1.2), sky2.CloudDirection()); + EXPECT_DOUBLE_EQ(0.9, sky2.CloudHumidity()); + EXPECT_DOUBLE_EQ(0.123, sky2.CloudMeanSize()); + EXPECT_EQ(ignition::math::Color::Blue, sky2.CloudAmbient()); + + EXPECT_NE(nullptr, sky2.Element()); + EXPECT_EQ(sky.Element(), sky2.Element()); +} + +///////////////////////////////////////////////// +TEST(DOMSky, MoveConstruction) +{ + sdf::Sky sky; + sky.SetTime(1.0); + sky.SetSunrise(5.0); + sky.SetSunset(15.0); + sky.SetCloudSpeed(0.3); + sky.SetCloudDirection(ignition::math::Angle(1.2)); + sky.SetCloudHumidity(0.9); + sky.SetCloudMeanSize(0.123); + sky.SetCloudAmbient(ignition::math::Color::Blue); + + sdf::Sky sky2(std::move(sky)); + EXPECT_DOUBLE_EQ(1.0, sky2.Time()); + EXPECT_DOUBLE_EQ(5.0, sky2.Sunrise()); + EXPECT_DOUBLE_EQ(15.0, sky2.Sunset()); + EXPECT_DOUBLE_EQ(0.3, sky2.CloudSpeed()); + EXPECT_EQ(ignition::math::Angle(1.2), sky2.CloudDirection()); + EXPECT_DOUBLE_EQ(0.9, sky2.CloudHumidity()); + EXPECT_DOUBLE_EQ(0.123, sky2.CloudMeanSize()); + EXPECT_EQ(ignition::math::Color::Blue, sky2.CloudAmbient()); +} + +///////////////////////////////////////////////// +TEST(DOMSky, MoveAssignmentOperator) +{ + sdf::Sky sky; + sky.SetTime(1.0); + sky.SetSunrise(5.0); + sky.SetSunset(15.0); + sky.SetCloudSpeed(0.3); + sky.SetCloudDirection(ignition::math::Angle(1.2)); + sky.SetCloudHumidity(0.9); + sky.SetCloudMeanSize(0.123); + sky.SetCloudAmbient(ignition::math::Color::Blue); + + sdf::Sky sky2; + sky2 = std::move(sky); + EXPECT_DOUBLE_EQ(1.0, sky2.Time()); + EXPECT_DOUBLE_EQ(5.0, sky2.Sunrise()); + EXPECT_DOUBLE_EQ(15.0, sky2.Sunset()); + EXPECT_DOUBLE_EQ(0.3, sky2.CloudSpeed()); + EXPECT_EQ(ignition::math::Angle(1.2), sky2.CloudDirection()); + EXPECT_DOUBLE_EQ(0.9, sky2.CloudHumidity()); + EXPECT_DOUBLE_EQ(0.123, sky2.CloudMeanSize()); + EXPECT_EQ(ignition::math::Color::Blue, sky2.CloudAmbient()); +} + +///////////////////////////////////////////////// +TEST(DOMSky, AssignmentOperator) +{ + sdf::Sky sky; + sky.SetTime(1.0); + sky.SetSunrise(5.0); + sky.SetSunset(15.0); + sky.SetCloudSpeed(0.3); + sky.SetCloudDirection(ignition::math::Angle(1.2)); + sky.SetCloudHumidity(0.9); + sky.SetCloudMeanSize(0.123); + sky.SetCloudAmbient(ignition::math::Color::Blue); + + sdf::Sky sky2; + sky2 = sky; + EXPECT_DOUBLE_EQ(1.0, sky2.Time()); + EXPECT_DOUBLE_EQ(5.0, sky2.Sunrise()); + EXPECT_DOUBLE_EQ(15.0, sky2.Sunset()); + EXPECT_DOUBLE_EQ(0.3, sky2.CloudSpeed()); + EXPECT_EQ(ignition::math::Angle(1.2), sky2.CloudDirection()); + EXPECT_DOUBLE_EQ(0.9, sky2.CloudHumidity()); + EXPECT_DOUBLE_EQ(0.123, sky2.CloudMeanSize()); + EXPECT_EQ(ignition::math::Color::Blue, sky2.CloudAmbient()); +} + +///////////////////////////////////////////////// +TEST(DOMSky, CopyAssignmentAfterMove) +{ + sdf::Sky sky1; + sky1.SetTime(21.0); + + sdf::Sky sky2; + sky2.SetTime(1.0); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Sky tmp = std::move(sky1); + sky1 = sky2; + sky2 = tmp; + + EXPECT_DOUBLE_EQ(1.0, sky1.Time()); + EXPECT_DOUBLE_EQ(21.0, sky2.Time()); +} + +///////////////////////////////////////////////// +TEST(DOMSky, Set) +{ + sdf::Sky sky; + + sky.SetTime(1.0); + EXPECT_DOUBLE_EQ(1.0, sky.Time()); + + sky.SetSunrise(5.0); + EXPECT_DOUBLE_EQ(5.0, sky.Sunrise()); + + sky.SetSunset(15.0); + EXPECT_DOUBLE_EQ(15.0, sky.Sunset()); + + sky.SetCloudSpeed(0.3); + EXPECT_DOUBLE_EQ(0.3, sky.CloudSpeed()); + + sky.SetCloudDirection(ignition::math::Angle(1.2)); + EXPECT_EQ(ignition::math::Angle(1.2), sky.CloudDirection()); + + sky.SetCloudHumidity(0.9); + EXPECT_DOUBLE_EQ(0.9, sky.CloudHumidity()); + + sky.SetCloudMeanSize(0.123); + EXPECT_DOUBLE_EQ(0.123, sky.CloudMeanSize()); + + sky.SetCloudAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f)); + EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f), + sky.CloudAmbient()); +} diff --git a/src/Sphere.cc b/src/Sphere.cc index 2195dc724..e1c8ad3ed 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -19,7 +19,7 @@ using namespace sdf; // Private data class -class sdf::SpherePrivate +class sdf::Sphere::Implementation { /// \brief Representation of the sphere public: ignition::math::Sphered sphere{1.0}; @@ -30,44 +30,10 @@ class sdf::SpherePrivate ///////////////////////////////////////////////// Sphere::Sphere() - : dataPtr(new SpherePrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Sphere::~Sphere() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -////////////////////////////////////////////////// -Sphere::Sphere(const Sphere &_sphere) - : dataPtr(new SpherePrivate) -{ - this->dataPtr->sphere = _sphere.dataPtr->sphere; - this->dataPtr->sdf = _sphere.dataPtr->sdf; -} - -////////////////////////////////////////////////// -Sphere::Sphere(Sphere &&_sphere) noexcept - : dataPtr(std::exchange(_sphere.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Sphere &Sphere::operator=(const Sphere &_sphere) -{ - return *this = Sphere(_sphere); -} - -///////////////////////////////////////////////// -Sphere &Sphere::operator=(Sphere &&_sphere) -{ - std::swap(this->dataPtr, _sphere.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Sphere::Load(ElementPtr _sdf) { diff --git a/src/Surface.cc b/src/Surface.cc index 4aef9e6b3..7e7283906 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -23,7 +23,7 @@ using namespace sdf; -class sdf::ContactPrivate +class sdf::Contact::Implementation { // \brief The bitmask used to filter collisions. public: uint16_t collideBitmask = 0xff; @@ -32,10 +32,10 @@ class sdf::ContactPrivate public: sdf::ElementPtr sdf{nullptr}; }; -class sdf::SurfacePrivate +class sdf::Surface::Implementation { /// \brief The object storing contact parameters - public: Contact contact; + public: sdf::Contact contact; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf{nullptr}; @@ -43,42 +43,10 @@ class sdf::SurfacePrivate ///////////////////////////////////////////////// Contact::Contact() - : dataPtr(new ContactPrivate) + : dataPtr(ignition::utils::MakeImpl()) { } -///////////////////////////////////////////////// -Contact::~Contact() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Contact::Contact(const Contact &_contact) - : dataPtr(new ContactPrivate(*_contact.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Contact::Contact(Contact &&_contact) noexcept - : dataPtr(std::exchange(_contact.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Contact &Contact::operator=(const Contact &_contact) -{ - return *this = Contact(_contact); -} - -///////////////////////////////////////////////// -Contact &Contact::operator=(Contact &&_contact) -{ - std::swap(this->dataPtr, _contact.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors Contact::Load(ElementPtr _sdf) { @@ -133,40 +101,8 @@ void Contact::SetCollideBitmask(const uint16_t _bitmask) ///////////////////////////////////////////////// Surface::Surface() - : dataPtr(new SurfacePrivate) -{ -} - -///////////////////////////////////////////////// -Surface::~Surface() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Surface::Surface(const Surface &_surface) - : dataPtr(new SurfacePrivate(*_surface.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Surface::Surface(Surface &&_surface) noexcept - : dataPtr(std::exchange(_surface.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Surface &Surface::operator=(const Surface &_surface) -{ - return *this = Surface(_surface); -} - -///////////////////////////////////////////////// -Surface &Surface::operator=(Surface &&_surface) + : dataPtr(ignition::utils::MakeImpl()) { - std::swap(this->dataPtr, _surface.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -210,7 +146,7 @@ sdf::ElementPtr Surface::Element() const } ///////////////////////////////////////////////// -sdf::Contact *Surface::Contact() const +const sdf::Contact *Surface::Contact() const { return &this->dataPtr->contact; } diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index 51e33f59c..1e31247d9 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -31,7 +31,9 @@ TEST(DOMsurface, DefaultConstruction) TEST(DOMsurface, CopyOperator) { sdf::Surface surface1; - surface1.Contact()->SetCollideBitmask(0x12); + sdf::Contact contact; + contact.SetCollideBitmask(0x12); + surface1.SetContact(contact); sdf::Surface surface2(surface1); EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); @@ -41,7 +43,9 @@ TEST(DOMsurface, CopyOperator) TEST(DOMsurface, CopyAssignmentOperator) { sdf::Surface surface1; - surface1.Contact()->SetCollideBitmask(0x12); + sdf::Contact contact; + contact.SetCollideBitmask(0x12); + surface1.SetContact(contact); sdf::Surface surface2 = surface1; EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); @@ -53,8 +57,12 @@ TEST(DOMsurface, CopyAssignmentAfterMove) sdf::Surface surface1; sdf::Surface surface2; - surface1.Contact()->SetCollideBitmask(0x12); - surface2.Contact()->SetCollideBitmask(0x34); + sdf::Contact contact1; + contact1.SetCollideBitmask(0x12); + surface1.SetContact(contact1); + sdf::Contact contact2; + contact2.SetCollideBitmask(0x34); + surface2.SetContact(contact2); sdf::Surface tmp = std::move(surface1); surface1 = surface2; diff --git a/src/Types.cc b/src/Types.cc index a0cd1636e..278497796 100644 --- a/src/Types.cc +++ b/src/Types.cc @@ -59,13 +59,13 @@ std::string trim(const char *_in) ////////////////////////////////////////////////// std::string trim(const std::string &_in) { - const size_t strBegin = _in.find_first_not_of(" \t"); + const size_t strBegin = _in.find_first_not_of(" \t\n"); if (strBegin == std::string::npos) { return ""; } - const size_t strRange = _in.find_last_not_of(" \t") - strBegin + 1; + const size_t strRange = _in.find_last_not_of(" \t\n") - strBegin + 1; return _in.substr(strBegin, strRange); } @@ -78,5 +78,70 @@ std::string lowercase(const std::string &_in) out[i] = std::tolower(out[i], std::locale()); return out; } + +///////////////////////////////////////////////// +std::ostream &operator<<(std::ostream &_out, const sdf::Errors &_errs) +{ + for (const auto &e : _errs) + { + _out << e << std::endl; + } + return _out; +} + +// Split a given absolute name into the parent model name and the local name. +// If the give name is not scoped, this will return an empty string for the +// parent model name and the given name as the local name. +std::pair SplitName( + const std::string &_absoluteName) +{ + const auto pos = _absoluteName.rfind(kSdfScopeDelimiter); + if (pos != std::string::npos) + { + const std::string first = _absoluteName.substr(0, pos); + const std::string second = + _absoluteName.substr(pos + kSdfScopeDelimiter.size()); + return {first, second}; + } + return {"", _absoluteName}; +} + +static bool EndsWithDelimiter(const std::string &_s) +{ + if (_s.size() < kSdfScopeDelimiter.size()) + return false; + + const size_t startPosition = _s.size() - kSdfScopeDelimiter.size(); + return _s.compare( + startPosition, kSdfScopeDelimiter.size(), kSdfScopeDelimiter) == 0; +} + +static bool StartsWithDelimiter(const std::string &_s) +{ + if (_s.size() < kSdfScopeDelimiter.size()) + return false; + + return _s.compare(0, kSdfScopeDelimiter.size(), kSdfScopeDelimiter) == 0; +} + +// Join a scope name prefix with a local name using the scope delimeter +std::string JoinName( + const std::string &_scopeName, const std::string &_localName) +{ + if (_scopeName.empty()) + return _localName; + if (_localName.empty()) + return _scopeName; + + const bool scopeNameEndsWithDelimiter = EndsWithDelimiter(_scopeName); + const bool localNameStartsWithDelimiter = StartsWithDelimiter(_localName); + + if (scopeNameEndsWithDelimiter && localNameStartsWithDelimiter) + return _scopeName + _localName.substr(kSdfScopeDelimiter.size()); + else if (scopeNameEndsWithDelimiter || localNameStartsWithDelimiter) + return _scopeName + _localName; + else + return _scopeName + kSdfScopeDelimiter + _localName; +} } } diff --git a/src/Types_TEST.cc b/src/Types_TEST.cc index 73775cd8f..a486025d8 100644 --- a/src/Types_TEST.cc +++ b/src/Types_TEST.cc @@ -18,8 +18,10 @@ #include #include +#include #include +#include "sdf/Error.hh" #include "sdf/Types.hh" ///////////////////////////////////////////////// @@ -94,6 +96,107 @@ TEST(Types, trim_nothing) out = sdf::trim("\txyz\t"); EXPECT_EQ(out, "xyz"); + + out = sdf::trim("\n xyz \n"); + EXPECT_EQ(out, "xyz"); +} + +///////////////////////////////////////////////// +TEST(Types, ErrorsOutputStream) +{ + sdf::Errors errors; + errors.emplace_back(sdf::ErrorCode::FILE_READ, "Error reading file"); + errors.emplace_back(sdf::ErrorCode::DUPLICATE_NAME, "Found duplicate name"); + std::string expected = "Error Code "; + expected += + std::to_string(static_cast(sdf::ErrorCode::FILE_READ)); + expected += " Msg: Error reading file\nError Code "; + expected += + std::to_string(static_cast(sdf::ErrorCode::DUPLICATE_NAME)); + expected += " Msg: Found duplicate name\n"; + + std::stringstream output; + output << errors; + EXPECT_EQ(expected, output.str()); +} + +TEST(Types, SplitName) +{ + { + const auto[basePath, tipName] = sdf::SplitName("a::b"); + EXPECT_EQ(basePath, "a"); + EXPECT_EQ(tipName, "b"); + } + { + const auto[basePath, tipName] = sdf::SplitName("a::b::c"); + EXPECT_EQ(basePath, "a::b"); + EXPECT_EQ(tipName, "c"); + } + { + const auto[basePath, tipName] = sdf::SplitName("b"); + EXPECT_EQ(basePath, ""); + EXPECT_EQ(tipName, "b"); + } + { + const auto[basePath, tipName] = sdf::SplitName("a::b::"); + EXPECT_EQ(basePath, "a::b"); + EXPECT_EQ(tipName, ""); + } + { + const auto[basePath, tipName] = sdf::SplitName("::b"); + EXPECT_EQ(basePath, ""); + EXPECT_EQ(tipName, "b"); + } + { + const auto[basePath, tipName] = sdf::SplitName(""); + EXPECT_EQ(basePath, ""); + EXPECT_EQ(tipName, ""); + } + { + const auto[basePath, tipName] = sdf::SplitName("a::b::c::d"); + EXPECT_EQ(basePath, "a::b::c"); + EXPECT_EQ(tipName, "d"); + } +} + +TEST(Types, JoinName) +{ + { + const auto joinedName = sdf::JoinName("a", "b"); + EXPECT_EQ(joinedName, "a::b"); + } + { + const auto joinedName = sdf::JoinName("a::b", "c"); + EXPECT_EQ(joinedName, "a::b::c"); + } + { + const auto joinedName = sdf::JoinName("a", "b::c"); + EXPECT_EQ(joinedName, "a::b::c"); + } + { + const auto joinedName = sdf::JoinName("a::", "b"); + EXPECT_EQ(joinedName, "a::b"); + } + { + const auto joinedName = sdf::JoinName("a", "::b"); + EXPECT_EQ(joinedName, "a::b"); + } + { + const auto joinedName = sdf::JoinName("a::", "::b"); + EXPECT_EQ(joinedName, "a::b"); + } + { + const auto joinedName = sdf::JoinName("", "b"); + EXPECT_EQ(joinedName, "b"); + } + { + const auto joinedName = sdf::JoinName("a", ""); + EXPECT_EQ(joinedName, "a"); + } + { + const auto joinedName = sdf::JoinName("", ""); + EXPECT_EQ(joinedName, ""); + } } ///////////////////////////////////////////////// diff --git a/src/Utils.cc b/src/Utils.cc index 9ed658039..8e6613a2d 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -74,5 +74,20 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, // on the pose element value. return posePair.second; } + +///////////////////////////////////////////////// +double infiniteIfNegative(const double _value) +{ + if (_value < 0.0) + return std::numeric_limits::infinity(); + + return _value; +} + +///////////////////////////////////////////////// +bool isValidFrameReference(const std::string &_name) +{ + return "__root__" != _name; +} } } diff --git a/src/Utils.hh b/src/Utils.hh index 73740ed1d..ae9d945ee 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include "sdf/Error.hh" #include "sdf/Element.hh" @@ -37,6 +38,12 @@ namespace sdf /// \returns True if the name is a reserved name and thus invalid. bool isReservedName(const std::string &_name); + /// \brief Check if the passed string is a valid frame reference. + /// Currently it's only invalid if the name is __root__ + /// \param[in] _name Name to check. + /// \returns True if the name is a valid frame reference. + bool isValidFrameReference(const std::string &_name); + /// \brief Read the "name" attribute from an element. /// \param[in] _sdf SDF element pointer which contains the name. /// \param[out] _name String to hold the name value. @@ -55,6 +62,13 @@ namespace sdf bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, std::string &_frame); + /// \brief If the value is negative, convert it to positive infinity. + /// Otherwise, return the original value. + /// \param[in] _value The value to convert, if necessary. + /// \return Infinity if the input value is negative, otherwise the original + /// value. + double infiniteIfNegative(double _value); + /// \brief Load all objects of a specific sdf element type. No error /// is returned if an element is not present. This function assumes that /// an element has a "name" attribute that must be unique. @@ -65,7 +79,7 @@ namespace sdf /// exists. /// \return The vector of errors. An empty vector indicates no errors were /// experienced. - template + template sdf::Errors loadUniqueRepeated(sdf::ElementPtr _sdf, const std::string &_sdfName, std::vector &_objs) { @@ -128,9 +142,10 @@ namespace sdf /// vector, unless an error is encountered during load. /// \return The vector of errors. An empty vector indicates no errors were /// experienced. - template - sdf::Errors loadRepeated(sdf::ElementPtr _sdf, - const std::string &_sdfName, std::vector &_objs) + template + sdf::Errors loadRepeated(sdf::ElementPtr _sdf, const std::string &_sdfName, + std::vector &_objs, + const std::function &_beforeLoadFunc = {}) { Errors errors; @@ -142,6 +157,10 @@ namespace sdf while (elem) { Class obj; + if (_beforeLoadFunc) + { + _beforeLoadFunc(obj); + } // Load the model and capture the errors. Errors loadErrors = obj.Load(elem); @@ -163,6 +182,29 @@ namespace sdf return errors; } + + /// \brief Convenience function that returns a pointer to the value contained + /// in a std::optional. + /// \tparam T type of object contained in the std::optional + /// \param[in] _opt Input optional object. + /// \return A pointer to the value contained in the optional. A nullptr is + /// returned if the optional does not contain a value. + template + T *optionalToPointer(std::optional &_opt) + { + if (_opt) + return &_opt.value(); + return nullptr; + } + + /// \brief const overload of optionalToPointer(std::optional &_opt) + template + const T *optionalToPointer(const std::optional &_opt) + { + if (_opt) + return &_opt.value(); + return nullptr; + } } } #endif diff --git a/src/Visual.cc b/src/Visual.cc index 8d99b5f68..1c16f9e6f 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -21,22 +21,14 @@ #include "sdf/Types.hh" #include "sdf/Visual.hh" #include "sdf/Geometry.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; -class sdf::VisualPrivate +class sdf::Visual::Implementation { - /// \brief Default constructor - public: VisualPrivate() = default; - - /// \brief Copy constructor - /// \param[in] _visualPrivate Joint axis to move. - public: explicit VisualPrivate(const VisualPrivate &_visualPrivate); - - // Delete copy assignment so it is not accidentally used - public: VisualPrivate &operator=(const VisualPrivate &) = delete; - /// \brief Name of the visual. public: std::string name = ""; @@ -58,72 +50,23 @@ class sdf::VisualPrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Pointer to the visual's material properties. - public: std::unique_ptr material; + /// \brief The visual's material properties. + public: std::optional material; /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph at the parent model scope. + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief Visibility flags of a visual. Defaults to 0xFFFFFFFF public: uint32_t visibilityFlags = 4294967295u; }; -///////////////////////////////////////////////// -VisualPrivate::VisualPrivate(const VisualPrivate &_visualPrivate) - : name(_visualPrivate.name), - castShadows(_visualPrivate.castShadows), - transparency(_visualPrivate.transparency), - pose(_visualPrivate.pose), - poseRelativeTo(_visualPrivate.poseRelativeTo), - geom(_visualPrivate.geom), - sdf(_visualPrivate.sdf), - visibilityFlags(_visualPrivate.visibilityFlags) -{ - if (_visualPrivate.material) - { - this->material = std::make_unique(*(_visualPrivate.material)); - } -} - ///////////////////////////////////////////////// Visual::Visual() - : dataPtr(new VisualPrivate) -{ -} - -///////////////////////////////////////////////// -Visual::~Visual() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -Visual::Visual(const Visual &_visual) - : dataPtr(new VisualPrivate(*_visual.dataPtr)) -{ -} - -///////////////////////////////////////////////// -Visual::Visual(Visual &&_visual) noexcept - : dataPtr(std::exchange(_visual.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -Visual &Visual::operator=(const Visual &_visual) -{ - return *this = Visual(_visual); -} - -///////////////////////////////////////////////// -Visual &Visual::operator=(Visual &&_visual) + : dataPtr(ignition::utils::MakeImpl()) { - std::swap(this->dataPtr, _visual.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -173,7 +116,7 @@ Errors Visual::Load(ElementPtr _sdf) if (_sdf->HasElement("material")) { - this->dataPtr->material.reset(new sdf::Material()); + this->dataPtr->material.emplace(); Errors err = this->dataPtr->material->Load(_sdf->GetElement("material")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -203,7 +146,7 @@ std::string Visual::Name() const } ///////////////////////////////////////////////// -void Visual::SetName(const std::string &_name) const +void Visual::SetName(const std::string &_name) { this->dataPtr->name = _name; } @@ -276,7 +219,7 @@ void Visual::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Visual::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } @@ -298,15 +241,15 @@ sdf::ElementPtr Visual::Element() const } ///////////////////////////////////////////////// -sdf::Material *Visual::Material() const +const sdf::Material *Visual::Material() const { - return this->dataPtr->material.get(); + return optionalToPointer(this->dataPtr->material); } ///////////////////////////////////////////////// void Visual::SetMaterial(const sdf::Material &_material) { - this->dataPtr->material.reset(new sdf::Material(_material)); + this->dataPtr->material = _material; } ///////////////////////////////////////////////// diff --git a/src/World.cc b/src/World.cc index c21646155..f212ee008 100644 --- a/src/World.cc +++ b/src/World.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "sdf/Actor.hh" @@ -27,24 +28,15 @@ #include "sdf/Types.hh" #include "sdf/World.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; -class sdf::WorldPrivate +class sdf::World::Implementation { - /// \brief Default constructor - public: WorldPrivate() = default; - - /// \brief Copy constructor - /// \param[in] _worldPrivate Joint axis to move. - public: explicit WorldPrivate(const WorldPrivate &_worldPrivate); - - // Delete copy assignment so it is not accidentally used - public: WorldPrivate &operator=(const WorldPrivate &) = delete; - - /// \brief Pointer to an atmosphere model. - public: std::unique_ptr atmosphere; + /// \brief Optional atmosphere model. + public: std::optional atmosphere; /// \brief Audio device name public: std::string audioDevice = "default"; @@ -53,11 +45,11 @@ class sdf::WorldPrivate public: ignition::math::Vector3d gravity = ignition::math::Vector3d(0, 0, -9.80665); - /// \brief Pointer to Gui parameters. - public: std::unique_ptr gui; + /// \brief Optional Gui parameters. + public: std::optional gui; - /// \brief Pointer to Sene parameters. - public: std::unique_ptr scene; + /// \brief Optional Scene parameters. + public: std::optional scene; /// \brief The frames specified in this world. public: std::vector frames; @@ -88,105 +80,23 @@ class sdf::WorldPrivate public: ignition::math::Vector3d windLinearVelocity = ignition::math::Vector3d::Zero; - /// \brief Frame Attached-To Graph constructed during Load. - public: std::shared_ptr frameAttachedToGraph; + /// \brief Scoped Frame Attached-To graph that points to a graph owned + /// by this world. + public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Pose Relative-To Graph constructed during Load. - public: std::shared_ptr poseRelativeToGraph; + /// \brief Scoped Pose Relative-To graph that points to a graph owned by this + /// world. + public: sdf::ScopedGraph poseRelativeToGraph; }; -///////////////////////////////////////////////// -WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) - : audioDevice(_worldPrivate.audioDevice), - gravity(_worldPrivate.gravity), - frames(_worldPrivate.frames), - lights(_worldPrivate.lights), - actors(_worldPrivate.actors), - magneticField(_worldPrivate.magneticField), - models(_worldPrivate.models), - name(_worldPrivate.name), - physics(_worldPrivate.physics), - sdf(_worldPrivate.sdf), - windLinearVelocity(_worldPrivate.windLinearVelocity) -{ - if (_worldPrivate.atmosphere) - { - this->atmosphere = - std::make_unique(*(_worldPrivate.atmosphere)); - } - if (_worldPrivate.gui) - { - this->gui = std::make_unique(*(_worldPrivate.gui)); - } - if (_worldPrivate.frameAttachedToGraph) - { - this->frameAttachedToGraph = std::make_shared( - *(_worldPrivate.frameAttachedToGraph)); - } - if (_worldPrivate.poseRelativeToGraph) - { - this->poseRelativeToGraph = std::make_shared( - *(_worldPrivate.poseRelativeToGraph)); - } - if (_worldPrivate.scene) - { - this->scene = std::make_unique(*(_worldPrivate.scene)); - } -} ///////////////////////////////////////////////// World::World() - : dataPtr(new WorldPrivate) + : dataPtr(ignition::utils::MakeImpl()) { this->dataPtr->physics.emplace_back(Physics()); } -///////////////////////////////////////////////// -World::~World() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -World::World(const World &_world) - : dataPtr(new WorldPrivate(*_world.dataPtr)) -{ - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &light : this->dataPtr->lights) - { - light.SetXmlParentName("world"); - light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } -} - -///////////////////////////////////////////////// -World::World(World &&_world) noexcept - : dataPtr(std::exchange(_world.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -World &World::operator=(const World &_world) -{ - return *this = World(_world); -} - -///////////////////////////////////////////////// -World &World::operator=(World &&_world) -{ - std::swap(this->dataPtr, _world.dataPtr); - return *this; -} - ///////////////////////////////////////////////// Errors World::Load(sdf::ElementPtr _sdf) { @@ -239,7 +149,7 @@ Errors World::Load(sdf::ElementPtr _sdf) // Read the atmosphere element if (_sdf->HasElement("atmosphere")) { - this->dataPtr->atmosphere.reset(new sdf::Atmosphere()); + this->dataPtr->atmosphere.emplace(); Errors atmosphereLoadErrors = this->dataPtr->atmosphere->Load(_sdf->GetElement("atmosphere")); errors.insert(errors.end(), atmosphereLoadErrors.begin(), @@ -266,8 +176,8 @@ Errors World::Load(sdf::ElementPtr _sdf) std::unordered_set frameNames; // Load all the models. - Errors modelLoadErrors = loadUniqueRepeated(_sdf, "model", - this->dataPtr->models); + Errors modelLoadErrors = + loadUniqueRepeated(_sdf, "model", this->dataPtr->models); errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end()); // Models are loaded first, and loadUniqueRepeated ensures there are no @@ -327,7 +237,7 @@ Errors World::Load(sdf::ElementPtr _sdf) // Load the Gui if (_sdf->HasElement("gui")) { - this->dataPtr->gui.reset(new sdf::Gui()); + this->dataPtr->gui.emplace(); Errors guiLoadErrors = this->dataPtr->gui->Load(_sdf->GetElement("gui")); errors.insert(errors.end(), guiLoadErrors.begin(), guiLoadErrors.end()); } @@ -335,51 +245,12 @@ Errors World::Load(sdf::ElementPtr _sdf) // Load the Scene if (_sdf->HasElement("scene")) { - this->dataPtr->scene.reset(new sdf::Scene()); + this->dataPtr->scene.emplace(); Errors sceneLoadErrors = this->dataPtr->scene->Load(_sdf->GetElement("scene")); errors.insert(errors.end(), sceneLoadErrors.begin(), sceneLoadErrors.end()); } - // Build the graphs. - this->dataPtr->frameAttachedToGraph = - std::make_shared(); - Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph, this); - errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), - frameAttachedToGraphErrors.end()); - Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph); - errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), - validateFrameAttachedGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - - this->dataPtr->poseRelativeToGraph = std::make_shared(); - Errors poseRelativeToGraphErrors = - buildPoseRelativeToGraph(*this->dataPtr->poseRelativeToGraph, this); - errors.insert(errors.end(), poseRelativeToGraphErrors.begin(), - poseRelativeToGraphErrors.end()); - Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(*this->dataPtr->poseRelativeToGraph); - errors.insert(errors.end(), validatePoseGraphErrors.begin(), - validatePoseGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &light : this->dataPtr->lights) - { - light.SetXmlParentName("world"); - light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - return errors; } @@ -390,7 +261,7 @@ std::string World::Name() const } ///////////////////////////////////////////////// -void World::SetName(const std::string &_name) const +void World::SetName(const std::string &_name) { this->dataPtr->name = _name; } @@ -486,37 +357,37 @@ const Model *World::ModelByName(const std::string &_name) const ///////////////////////////////////////////////// const sdf::Atmosphere *World::Atmosphere() const { - return this->dataPtr->atmosphere.get(); + return optionalToPointer(this->dataPtr->atmosphere); } ///////////////////////////////////////////////// -void World::SetAtmosphere(const sdf::Atmosphere &_atmosphere) const +void World::SetAtmosphere(const sdf::Atmosphere &_atmosphere) { - this->dataPtr->atmosphere.reset(new sdf::Atmosphere(_atmosphere)); + this->dataPtr->atmosphere = _atmosphere; } ///////////////////////////////////////////////// -sdf::Gui *World::Gui() const +const sdf::Gui *World::Gui() const { - return this->dataPtr->gui.get(); + return optionalToPointer(this->dataPtr->gui); } ///////////////////////////////////////////////// void World::SetGui(const sdf::Gui &_gui) { - return this->dataPtr->gui.reset(new sdf::Gui(_gui)); + this->dataPtr->gui = _gui; } ///////////////////////////////////////////////// const sdf::Scene *World::Scene() const { - return this->dataPtr->scene.get(); + return optionalToPointer(this->dataPtr->scene); } ///////////////////////////////////////////////// void World::SetScene(const sdf::Scene &_scene) { - return this->dataPtr->scene.reset(new sdf::Scene(_scene)); + this->dataPtr->scene = _scene; } ///////////////////////////////////////////////// @@ -663,3 +534,39 @@ bool World::PhysicsNameExists(const std::string &_name) const return false; } + +///////////////////////////////////////////////// +void World::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) +{ + this->dataPtr->poseRelativeToGraph = _graph; + + for (auto &model : this->dataPtr->models) + { + model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } + for (auto &light : this->dataPtr->lights) + { + light.SetXmlParentName("world"); + light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } +} + +///////////////////////////////////////////////// +void World::SetFrameAttachedToGraph( + sdf::ScopedGraph _graph) +{ + this->dataPtr->frameAttachedToGraph = _graph; + + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } + for (auto &model : this->dataPtr->models) + { + model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } +} diff --git a/src/XmlUtils.cc b/src/XmlUtils.cc new file mode 100644 index 000000000..ff2d86b93 --- /dev/null +++ b/src/XmlUtils.cc @@ -0,0 +1,60 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include "XmlUtils.hh" + +#include "sdf/Console.hh" + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { + +///////////////////////////////////////////////// +tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, + const tinyxml2::XMLNode *_src) +{ + if (_src == nullptr) + { + sdferr << "Pointer to XML node _src is NULL\n"; + return nullptr; + } + + tinyxml2::XMLNode *copy = _src->ShallowClone(_doc); + if (copy == nullptr) + { + sdferr << "Failed to clone node " << _src->Value() << "\n"; + return nullptr; + } + + for (const tinyxml2::XMLNode *node = _src->FirstChild(); node != nullptr; + node = node->NextSibling()) + { + auto *childCopy = DeepClone(_doc, node); + if (childCopy == nullptr) + { + sdferr << "Failed to clone child " << node->Value() << "\n"; + return nullptr; + } + copy->InsertEndChild(childCopy); + } + + return copy; +} +} +} // namespace sdf + diff --git a/src/ExceptionPrivate.hh b/src/XmlUtils.hh similarity index 50% rename from src/ExceptionPrivate.hh rename to src/XmlUtils.hh index 216361024..82b08b2b9 100644 --- a/src/ExceptionPrivate.hh +++ b/src/XmlUtils.hh @@ -1,5 +1,5 @@ /* - * Copyright 2015 Open Source Robotics Foundation + * Copyright 2020 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -13,35 +13,33 @@ * See the License for the specific language governing permissions and * limitations under the License. * - */ +*/ +#ifndef SDFORMAT_XMLUTILS_HH +#define SDFORMAT_XMLUTILS_HH -#ifndef SDF_EXCEPTION_PRIVATE_HH_ -#define SDF_EXCEPTION_PRIVATE_HH_ - -#include #include -#include +#include + +#include "sdf/Error.hh" +#include "sdf/Element.hh" +#include "sdf/Types.hh" namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { - // - - /// \internal - /// \brief Private data for Exception - class ExceptionPrivate - { - /// \brief The error function - public: std::string file; - /// \brief Line the error occured on - public: std::int64_t line; - - /// \brief The error string - public: std::string str; - }; + /// \brief Perform a deep copy of an XML Node + /// + /// This copies an XMLNode _src and all of its decendants + /// into a specified XMLDocument. + /// + /// \param[in] _doc Document in which to place the copied node + /// \param[in] _src The node to deep copy + /// \returns The newly copied node upon success OR + /// nullptr if an error occurs. + tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, + const tinyxml2::XMLNode *_src); } } #endif - diff --git a/src/XmlUtils_TEST.cc b/src/XmlUtils_TEST.cc new file mode 100644 index 000000000..aef79262d --- /dev/null +++ b/src/XmlUtils_TEST.cc @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include + +#include "XmlUtils.hh" + +///////////////////////////////////////////////// +TEST(XMLUtils, DeepClone) +{ + tinyxml2::XMLDocument oldDoc; + tinyxml2::XMLDocument newDoc; + + std::string docXml = R"( + + Hello World + + )"; + + auto ret = oldDoc.Parse(docXml.c_str()); + ASSERT_EQ(tinyxml2::XML_SUCCESS, ret); + + auto root = oldDoc.FirstChild(); + auto newRoot = sdf::DeepClone(&newDoc, root); + + EXPECT_STREQ("document", newRoot->ToElement()->Name()); + + auto newChild = newRoot->FirstChild(); + EXPECT_STREQ("nodeA", newChild->ToElement()->Name()); + + auto newChildB = newChild->FirstChild(); + EXPECT_STREQ("nodeB", newChildB->ToElement()->Name()); + + auto childB_attr = newChildB->ToElement()->Attribute("attr"); + EXPECT_STREQ("true", childB_attr); + + auto childB_text = newChildB->ToElement()->GetText(); + EXPECT_STREQ("Hello World", childB_text); +} diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 4d354c3b9..52130611d 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -38,9 +38,11 @@ COMMANDS = { 'sdf' => "Utilities for SDF files.\n\n"\ " ign sdf [options]\n\n"\ "Options:\n\n"\ - " -k [ --check ] arg Check if an SDFormat file is valid.\n" + - " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + - " -p [ --print ] arg Print converted arg.\n" + + " -k [ --check ] arg Check if an SDFormat file is valid.\n" + + " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + + " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph. (WARNING: This is for advanced\n" + + " use only and the output may change without any promise of stability)\n" + + " -p [ --print ] arg Print converted arg.\n" + COMMON_OPTIONS } @@ -77,6 +79,10 @@ class Cmd 'Print converted arg') do |arg| options['print'] = arg end + opts.on('-g arg', '--graph type', String, + 'Print PoseRelativeTo or FrameAttachedTo graph') do |graph_type| + options['graph'] = {:type => graph_type} + end end begin opt_parser.parse!(args) @@ -156,6 +162,9 @@ class Cmd elsif options.key?('print') Importer.extern 'int cmdPrint(const char *)' exit(Importer.cmdPrint(File.expand_path(options['print']))) + elsif options.key?('graph') + Importer.extern 'int cmdGraph(const char *, const char *)' + exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1]))) else puts 'Command error: I do not have an implementation '\ 'for this command.' diff --git a/src/ign.cc b/src/ign.cc index 90262276b..de66011eb 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -24,6 +25,8 @@ #include "sdf/parser.hh" #include "sdf/system_util.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "ign.hh" ////////////////////////////////////////////////// @@ -157,3 +160,70 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path) return 0; } + +////////////////////////////////////////////////// +// cppcheck-suppress unusedFunction +extern "C" SDFORMAT_VISIBLE int cmdGraph( + const char *_graphType, const char *_path) +{ + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + + if (std::strcmp(_graphType, "pose") == 0) + { + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + if (root.WorldCount() > 0) + { + errors = sdf::buildPoseRelativeToGraph(graph, root.WorldByIndex(0)); + } + else if (root.Model() != nullptr) + { + errors = + sdf::buildPoseRelativeToGraph(graph, root.Model()); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + std::cout << graph.Graph() << std::endl; + } + else if (std::strcmp(_graphType, "frame") == 0) + { + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + if (root.WorldCount() > 0) + { + errors = sdf::buildFrameAttachedToGraph(graph, root.WorldByIndex(0)); + } + else if (root.Model() != nullptr) + { + errors = + sdf::buildFrameAttachedToGraph(graph, root.Model()); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + std::cout << graph.Graph() << std::endl; + } + else + { + std::cerr << R"(Only "pose" and "frame" graph types are supported)" + << std::endl; + } + + return 0; +} diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index b98c6e752..6f40ac989 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -20,6 +20,8 @@ #include #include +#include + #include "sdf/parser.hh" #include "sdf/SDFImpl.hh" #include "sdf/sdf_config.h" @@ -57,7 +59,7 @@ std::string custom_exec_str(std::string _cmd) } ///////////////////////////////////////////////// -TEST(check, SDF) +TEST(check, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { std::string pathBase = PROJECT_SOURCE_PATH; pathBase += "/test/sdf"; @@ -197,7 +199,7 @@ TEST(check, SDF) // Check joint_invalid_child.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: Child link with name[invalid] specified by " + EXPECT_NE(output.find("Error: Child frame with name[invalid] specified by " "joint with name[joint] not found in model with " "name[joint_invalid_child]."), std::string::npos) << output; @@ -210,12 +212,37 @@ TEST(check, SDF) // Check joint_invalid_parent.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: parent link with name[invalid] specified by " + EXPECT_NE(output.find("Error: parent frame with name[invalid] specified by " "joint with name[joint] not found in model with " "name[joint_invalid_parent]."), std::string::npos) << output; } + // Check an SDF file with a joint that names itself as the child frame. + { + std::string path = pathBase +"/joint_invalid_self_child.sdf"; + + // Check joint_invalid_self_child.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("Error: FrameAttachedToGraph cycle detected, already " + "visited vertex [joint_invalid_self_child::self]."), + std::string::npos) << output; + } + + // Check an SDF file with a joint that names itself as the parent frame. + { + std::string path = pathBase +"/joint_invalid_self_parent.sdf"; + + // Check joint_invalid_self_parent.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("Error: joint with name[self] in model with " + "name[joint_invalid_self_parent] must not specify " + "its own name as the parent frame."), + std::string::npos) << output; + } + // Check an SDF file with a joint with identical parent and child. { std::string path = pathBase +"/joint_invalid_parent_same_as_child.sdf"; @@ -229,6 +256,21 @@ TEST(check, SDF) std::string::npos) << output; } + // 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"; + + // Check joint_invalid_resolved_parent_same_as_child.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("specified parent frame [J1] and child frame [L2] " + "that both resolve to [L2], but they should resolve " + "to different values."), + std::string::npos) << output; + } + // Check an SDF file with the world specified as a child link. { std::string path = pathBase +"/joint_child_world.sdf"; @@ -252,6 +294,28 @@ TEST(check, SDF) EXPECT_EQ("Valid.\n", output) << output; } + // 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"; + + // Check joint_child_frame.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + + // 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"; + + // Check joint_parent_frame.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + // Check an SDF file with the second link specified as the canonical link. // This is a valid file. { @@ -275,6 +339,17 @@ TEST(check, SDF) std::string::npos) << output; } + // Check an SDF file with an invalid model without links. + { + std::string path = pathBase +"/model_without_links.sdf"; + + // Check model_without_links.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("Error: A model must have at least one link."), + std::string::npos) << output; + } + // Check an SDF file with a nested model. { std::string path = pathBase +"/nested_model.sdf"; @@ -282,8 +357,39 @@ TEST(check, SDF) // Check nested_model.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: Nested models are not yet supported by DOM " - "objects, skipping model [top_level_model]."), + EXPECT_EQ("Valid.\n", output) << output; + } + + // Check an SDF file with a model that has a nested canonical link. + { + std::string path = pathBase +"/nested_canonical_link.sdf"; + + // Check nested_canonical_link.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + + // Check an SDF file with a model that has a nested canonical link + // that is explicitly specified by //model/@canonical_link using :: + // syntax. + { + std::string path = pathBase +"/nested_explicit_canonical_link.sdf"; + + // Check nested_explicit_canonical_link.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + + // Check an SDF file with a model that a nested model without a link. + { + std::string path = pathBase +"/nested_without_links_invalid.sdf"; + + // Check nested_without_links_invalid.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("Error: A model must have at least one link."), std::string::npos) << output; } @@ -353,6 +459,17 @@ TEST(check, SDF) EXPECT_EQ("Valid.\n", output) << output; } + // 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"; + + // Check model_frame_attached_to_nested_model.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + // Check an SDF file with model frames with invalid attached_to attributes. { std::string path = pathBase +"/model_frame_invalid_attached_to.sdf"; @@ -361,8 +478,8 @@ TEST(check, SDF) std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE(output.find("Error: attached_to name[A] specified by frame with " - "name[F3] does not match a link, joint, or frame " - "name in model with " + "name[F3] does not match a nested model, link, " + "joint, or frame name in model with " "name[model_frame_invalid_attached_to]."), std::string::npos) << output; EXPECT_NE(output.find("Error: attached_to name[F4] is identical to frame " @@ -378,11 +495,13 @@ TEST(check, SDF) // Check model_frame_invalid_attached_to_cycle.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(std::string::npos, output.find( - "FrameAttachedToGraph cycle detected, already visited vertex [F1].")) + EXPECT_NE(std::string::npos, + output.find("FrameAttachedToGraph cycle detected, already visited " + "vertex [model_frame_invalid_attached_to_cycle::F1].")) << output; - EXPECT_NE(std::string::npos, output.find( - "FrameAttachedToGraph cycle detected, already visited vertex [F2].")) + EXPECT_NE(std::string::npos, + output.find("FrameAttachedToGraph cycle detected, already visited " + "vertex [model_frame_invalid_attached_to_cycle::F2].")) << output; } @@ -435,8 +554,8 @@ TEST(check, SDF) std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE(output.find("Error: relative_to name[A] specified by link with " - "name[L] does not match a link, joint, or frame " - "name in model with " + "name[L] does not match a nested model, link, " + "joint, or frame name in model with " "name[model_invalid_link_relative_to]."), std::string::npos) << output; EXPECT_NE(output.find("Error: relative_to name[self_cycle] is identical to " @@ -445,6 +564,29 @@ TEST(check, SDF) std::string::npos) << output; } + // 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"; + + // Check model_nested_model_relative_to.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + + // Check an SDF file with nested_models using nested links/frames as joint + // parent or child frames. + // This is a valid file. + { + std::string path = pathBase +"/joint_nested_parent_child.sdf"; + + // Check model_nested_model_relative_to.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + // Check an SDF file with joints using the relative_to attribute. // This is a valid file. { @@ -464,8 +606,8 @@ TEST(check, SDF) std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE(output.find("Error: relative_to name[A] specified by joint with " - "name[J] does not match a link, joint, or frame " - "name in model with " + "name[J] does not match a nested model, link, " + "joint, or frame name in model with " "name[model_invalid_joint_relative_to]."), std::string::npos) << output; EXPECT_NE(output.find("Error: relative_to name[Jcycle] is identical to " @@ -504,8 +646,8 @@ TEST(check, SDF) std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE(output.find("Error: relative_to name[A] specified by frame with " - "name[F] does not match a link, joint, or frame " - "name in model with " + "name[F] does not match a nested model, link, " + "joint, or frame name in model with " "name[model_invalid_frame_relative_to]."), std::string::npos) << output; EXPECT_NE(output.find("Error: relative_to name[cycle] is identical to " @@ -521,11 +663,13 @@ TEST(check, SDF) // Check model_invalid_frame_relative_to_cycle.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(std::string::npos, output.find( - "PoseRelativeToGraph cycle detected, already visited vertex [F1].")) + EXPECT_NE(std::string::npos, + output.find("PoseRelativeToGraph cycle detected, already visited " + "vertex [model_invalid_frame_relative_to_cycle::F1].")) << output; - EXPECT_NE(std::string::npos, output.find( - "PoseRelativeToGraph cycle detected, already visited vertex [F2].")) + EXPECT_NE(std::string::npos, + output.find("PoseRelativeToGraph cycle detected, already visited " + "vertex [model_invalid_frame_relative_to_cycle::F2].")) << output; } @@ -568,10 +712,130 @@ TEST(check, SDF) "name[world_frame_invalid_relative_to]."), std::string::npos) << output; } + // Check an SDF file with an invalid frame specified as the placement_frame + // attribute. + { + std::string path = pathBase + "/model_invalid_placement_frame.sdf"; + + // Check model_invalid_placement_frame.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE( + output.find("unable to find unique frame with name [link3] in graph"), + std::string::npos) + << output; + } + + // Check an SDF file with an valid nested model cross references + { + std::string path = pathBase + "/nested_model_cross_references.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + + // Check an SDF model file with an invalid usage of __root__ + { + std::string path = pathBase + "/model_invalid_root_reference.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [relative_to]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a" + " value of attribute [attached_to]"), + std::string::npos) + << output; + } + // Check an SDF world file with an invalid usage of __root__ + { + // Set SDF_PATH so that included models can be found + setenv("SDF_PATH", PROJECT_SOURCE_PATH "/test/integration/model", 1); + std::string path = pathBase + "/world_invalid_root_reference.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [relative_to]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [attached_to]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [placement_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [canonical_link]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [parent_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of element [placement_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find( + "Error: The supplied joint child name [__root__] is not valid"), + std::string::npos) + << output; + EXPECT_NE( + output.find( + "Error: The supplied joint parent name [__root__] is not valid"), + std::string::npos) + << output; + } + // Check an SDF world file with an valid usage of __root__ + { + std::string path = pathBase + "/world_valid_root_reference.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } } ///////////////////////////////////////////////// -TEST(check_model_sdf, SDF) +TEST(check_shapes_sdf, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + std::string pathBase = PROJECT_SOURCE_PATH; + pathBase += "/test/sdf"; + + { + std::string path = pathBase +"/shapes.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output); + } + + { + std::string path = pathBase +"/shapes_world.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output); + } +} + +///////////////////////////////////////////////// +TEST(check_model_sdf, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { std::string pathBase = PROJECT_SOURCE_PATH; pathBase += "/test/integration/model/box"; @@ -597,7 +861,7 @@ TEST(check_model_sdf, SDF) } ///////////////////////////////////////////////// -TEST(describe, SDF) +TEST(describe, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { // Get the description std::string output = @@ -609,7 +873,7 @@ TEST(describe, SDF) } ///////////////////////////////////////////////// -TEST(print, SDF) +TEST(print, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { std::string pathBase = PROJECT_SOURCE_PATH; pathBase += "/test/sdf"; @@ -638,6 +902,228 @@ TEST(print, SDF) } } +///////////////////////////////////////////////// +TEST(GraphCmd, IGN_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"; + + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g pose " + path + g_sdfVersion); + + std::stringstream expected; + expected << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"world (1)\"];\n" + << " 2 [label=\"M1 (2)\"];\n" + << " 3 [label=\"M1::__model__ (3)\"];\n" + << " 4 [label=\"M1::L1 (4)\"];\n" + << " 5 [label=\"M1::L2 (5)\"];\n" + << " 6 [label=\"M1::J1 (6)\"];\n" + << " 7 [label=\"M1::F1 (7)\"];\n" + << " 8 [label=\"M1::CM1 (8)\"];\n" + << " 9 [label=\"M1::CM1::__model__ (9)\"];\n" + << " 10 [label=\"M1::CM1::L (10)\"];\n" + << " 11 [label=\"F2 (11)\"];\n" + << " 12 [label=\"F3 (12)\"];\n" + << " 13 [label=\"F4 (13)\"];\n" + << " 14 [label=\"F5 (14)\"];\n" + << " 15 [label=\"F6 (15)\"];\n" + << " 16 [label=\"F7 (16)\"];\n" + << " 0 -> 1 [label=1];\n" + << " 2 -> 3 [label=0];\n" + << " 3 -> 4 [label=1];\n" + << " 3 -> 5 [label=1];\n" + << " 8 -> 9 [label=0];\n" + << " 9 -> 10 [label=1];\n" + << " 5 -> 6 [label=1];\n" + << " 5 -> 7 [label=1];\n" + << " 3 -> 8 [label=1];\n" + << " 1 -> 2 [label=1];\n" + << " 2 -> 11 [label=1];\n" + << " 4 -> 12 [label=1];\n" + << " 6 -> 13 [label=1];\n" + << " 7 -> 14 [label=1];\n" + << " 8 -> 15 [label=1];\n" + << " 10 -> 16 [label=1];\n" + << "}"; + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, IGN_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 std::string output = + custom_exec_str(g_ignCommand + " sdf -g pose " + path + g_sdfVersion); + + std::stringstream expected; + expected << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"parent_model (1)\"];\n" + << " 2 [label=\"parent_model::__model__ (2)\"];\n" + << " 3 [label=\"parent_model::L (3)\"];\n" + << " 4 [label=\"parent_model::M1 (4)\"];\n" + << " 5 [label=\"parent_model::M1::__model__ (5)\"];\n" + << " 6 [label=\"parent_model::M1::L1 (6)\"];\n" + << " 7 [label=\"parent_model::M1::L2 (7)\"];\n" + << " 8 [label=\"parent_model::M1::J1 (8)\"];\n" + << " 9 [label=\"parent_model::M1::F1 (9)\"];\n" + << " 10 [label=\"parent_model::M1::CM1 (10)\"];\n" + << " 11 [label=\"parent_model::M1::CM1::__model__ (11)\"];\n" + << " 12 [label=\"parent_model::M1::CM1::L (12)\"];\n" + << " 13 [label=\"parent_model::M2 (13)\"];\n" + << " 14 [label=\"parent_model::M2::__model__ (14)\"];\n" + << " 15 [label=\"parent_model::M2::L (15)\"];\n" + << " 16 [label=\"parent_model::M3 (16)\"];\n" + << " 17 [label=\"parent_model::M3::__model__ (17)\"];\n" + << " 18 [label=\"parent_model::M3::L (18)\"];\n" + << " 19 [label=\"parent_model::M4 (19)\"];\n" + << " 20 [label=\"parent_model::M4::__model__ (20)\"];\n" + << " 21 [label=\"parent_model::M4::L (21)\"];\n" + << " 22 [label=\"parent_model::M5 (22)\"];\n" + << " 23 [label=\"parent_model::M5::__model__ (23)\"];\n" + << " 24 [label=\"parent_model::M5::L (24)\"];\n" + << " 25 [label=\"parent_model::M6 (25)\"];\n" + << " 26 [label=\"parent_model::M6::__model__ (26)\"];\n" + << " 27 [label=\"parent_model::M6::L (27)\"];\n" + << " 28 [label=\"parent_model::M7 (28)\"];\n" + << " 29 [label=\"parent_model::M7::__model__ (29)\"];\n" + << " 30 [label=\"parent_model::M7::L (30)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 2 -> 3 [label=1];\n" + << " 4 -> 5 [label=0];\n" + << " 5 -> 6 [label=1];\n" + << " 5 -> 7 [label=1];\n" + << " 10 -> 11 [label=0];\n" + << " 11 -> 12 [label=1];\n" + << " 7 -> 8 [label=1];\n" + << " 7 -> 9 [label=1];\n" + << " 5 -> 10 [label=1];\n" + << " 13 -> 14 [label=0];\n" + << " 14 -> 15 [label=1];\n" + << " 16 -> 17 [label=0];\n" + << " 17 -> 18 [label=1];\n" + << " 19 -> 20 [label=0];\n" + << " 20 -> 21 [label=1];\n" + << " 22 -> 23 [label=0];\n" + << " 23 -> 24 [label=1];\n" + << " 25 -> 26 [label=0];\n" + << " 26 -> 27 [label=1];\n" + << " 28 -> 29 [label=0];\n" + << " 29 -> 30 [label=1];\n" + << " 2 -> 4 [label=1];\n" + << " 4 -> 13 [label=1];\n" + << " 6 -> 16 [label=1];\n" + << " 8 -> 19 [label=1];\n" + << " 9 -> 22 [label=1];\n" + << " 10 -> 25 [label=1];\n" + << " 12 -> 28 [label=1];\n" + << " 0 -> 1 [label=1];\n" + << "}"; + + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, IGN_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 std::string output = + custom_exec_str(g_ignCommand + " sdf -g frame " + path + g_sdfVersion); + + std::stringstream expected; + + expected << "digraph {\n" + << " 0 [label=\"world (0)\"];\n" + << " 1 [label=\"M1 (1)\"];\n" + << " 2 [label=\"M1::__model__ (2)\"];\n" + << " 3 [label=\"M1::L (3)\"];\n" + << " 4 [label=\"M1::J1 (4)\"];\n" + << " 5 [label=\"M1::F0 (5)\"];\n" + << " 6 [label=\"M1::M2 (6)\"];\n" + << " 7 [label=\"M1::M2::__model__ (7)\"];\n" + << " 8 [label=\"M1::M2::L (8)\"];\n" + << " 9 [label=\"world_frame (9)\"];\n" + << " 10 [label=\"F0 (10)\"];\n" + << " 11 [label=\"F1 (11)\"];\n" + << " 12 [label=\"F2 (12)\"];\n" + << " 13 [label=\"F3 (13)\"];\n" + << " 14 [label=\"F4 (14)\"];\n" + << " 15 [label=\"F5 (15)\"];\n" + << " 16 [label=\"F6 (16)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 6 -> 7 [label=0];\n" + << " 7 -> 8 [label=1];\n" + << " 4 -> 5 [label=1];\n" + << " 5 -> 8 [label=1];\n" + << " 2 -> 3 [label=1];\n" + << " 9 -> 0 [label=1];\n" + << " 10 -> 0 [label=1];\n" + << " 11 -> 1 [label=1];\n" + << " 12 -> 3 [label=1];\n" + << " 13 -> 6 [label=1];\n" + << " 14 -> 8 [label=1];\n" + << " 15 -> 5 [label=1];\n" + << " 16 -> 4 [label=1];\n" + << "}"; + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, IGN_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 std::string output = + custom_exec_str(g_ignCommand + " sdf -g frame " + path + g_sdfVersion); + + std::stringstream expected; + + expected + << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"nested_model_frame_attached_to (1)\"];\n" + << " 2 [label=\"nested_model_frame_attached_to::__model__ (2)\"];\n" + << " 3 [label=\"nested_model_frame_attached_to::L (3)\"];\n" + << " 4 [label=\"nested_model_frame_attached_to::F0 (4)\"];\n" + << " 5 [label=\"nested_model_frame_attached_to::F1 (5)\"];\n" + << " 6 [label=\"nested_model_frame_attached_to::F2 (6)\"];\n" + << " 7 [label=\"nested_model_frame_attached_to::F3 (7)\"];\n" + << " 8 [label=\"nested_model_frame_attached_to::F4 (8)\"];\n" + << " 9 [label=\"nested_model_frame_attached_to::M1 (9)\"];\n" + << " 10 [label=\"nested_model_frame_attached_to::M1::__model__ " + "(10)\"];\n" + << " 11 [label=\"nested_model_frame_attached_to::M1::L (11)\"];\n" + << " 12 [label=\"nested_model_frame_attached_to::M1::J1 (12)\"];\n" + << " 13 [label=\"nested_model_frame_attached_to::M1::F (13)\"];\n" + << " 14 [label=\"nested_model_frame_attached_to::M1::M2 (14)\"];\n" + << " 15 [label=\"nested_model_frame_attached_to::M1::M2::__model__ " + "(15)\"];\n" + << " 16 [label=\"nested_model_frame_attached_to::M1::M2::L (16)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 9 -> 10 [label=0];\n" + << " 14 -> 15 [label=0];\n" + << " 15 -> 16 [label=1];\n" + << " 12 -> 13 [label=1];\n" + << " 13 -> 14 [label=1];\n" + << " 10 -> 11 [label=1];\n" + << " 4 -> 9 [label=1];\n" + << " 5 -> 11 [label=1];\n" + << " 6 -> 5 [label=1];\n" + << " 7 -> 13 [label=1];\n" + << " 8 -> 12 [label=1];\n" + << " 2 -> 3 [label=1];\n" + << "}"; + + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/src/parser.cc b/src/parser.cc index 3e8279d91..d5d7c2441 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -37,6 +38,8 @@ #include "Converter.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" +#include "Utils.hh" #include "parser_private.hh" #include "parser_urdf.hh" @@ -78,28 +81,42 @@ bool readStringInternal( const bool _convert, Errors &_errors); +////////////////////////////////////////////////// +/// \brief Internal helper for creating XMLDocuments +/// +/// This creates an XMLDocument with whitespace collapse +/// on, which is not default behavior in tinyxml2. +/// This function is to consolidate locations it is used. +/// +/// There is a performance impact associated with collapsing whitespace. +/// +/// For more information on the behavior and performance implications, +/// consult the TinyXML2 documentation: https://leethomason.github.io/tinyxml2/ +inline auto makeSdfDoc() +{ + return tinyxml2::XMLDocument(true, tinyxml2::COLLAPSE_WHITESPACE); +} + ////////////////////////////////////////////////// template static inline bool _initFile(const std::string &_filename, TPtr _sdf) { - TiXmlDocument xmlDoc; - if (xmlDoc.LoadFile(_filename)) - { - return initDoc(&xmlDoc, _sdf); - } - else + auto xmlDoc = makeSdfDoc(); + if (tinyxml2::XML_SUCCESS != xmlDoc.LoadFile(_filename.c_str())) { - sdferr << "Unable to load file[" << _filename << "]\n"; + sdferr << "Unable to load file[" + << _filename << "]: " << xmlDoc.ErrorStr() << "\n"; + return false; } - return false; + return initDoc(&xmlDoc, _sdf); } ////////////////////////////////////////////////// bool init(SDFPtr _sdf) { std::string xmldata = SDF::EmbeddedSpec("root.sdf", false); - TiXmlDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); return initDoc(&xmlDoc, _sdf); } @@ -110,7 +127,7 @@ bool initFile(const std::string &_filename, SDFPtr _sdf) std::string xmldata = SDF::EmbeddedSpec(_filename, true); if (!xmldata.empty()) { - TiXmlDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); return initDoc(&xmlDoc, _sdf); } @@ -123,7 +140,7 @@ bool initFile(const std::string &_filename, ElementPtr _sdf) std::string xmldata = SDF::EmbeddedSpec(_filename, true); if (!xmldata.empty()) { - TiXmlDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); return initDoc(&xmlDoc, _sdf); } @@ -133,11 +150,10 @@ bool initFile(const std::string &_filename, ElementPtr _sdf) ////////////////////////////////////////////////// bool initString(const std::string &_xmlString, SDFPtr _sdf) { - TiXmlDocument xmlDoc; - xmlDoc.Parse(_xmlString.c_str()); - if (xmlDoc.Error()) + auto xmlDoc = makeSdfDoc(); + if (xmlDoc.Parse(_xmlString.c_str())) { - sdferr << "Failed to parse string as XML: " << xmlDoc.ErrorDesc() << '\n'; + sdferr << "Failed to parse string as XML: " << xmlDoc.ErrorStr() << '\n'; return false; } @@ -145,7 +161,7 @@ bool initString(const std::string &_xmlString, SDFPtr _sdf) } ////////////////////////////////////////////////// -inline TiXmlElement *_initDocGetElement(TiXmlDocument *_xmlDoc) +inline tinyxml2::XMLElement *_initDocGetElement(tinyxml2::XMLDocument *_xmlDoc) { if (!_xmlDoc) { @@ -153,7 +169,7 @@ inline TiXmlElement *_initDocGetElement(TiXmlDocument *_xmlDoc) return nullptr; } - TiXmlElement *element = _xmlDoc->FirstChildElement("element"); + tinyxml2::XMLElement *element = _xmlDoc->FirstChildElement("element"); if (!element) { sdferr << "Could not find the 'element' element in the xml file\n"; @@ -164,7 +180,7 @@ inline TiXmlElement *_initDocGetElement(TiXmlDocument *_xmlDoc) } ////////////////////////////////////////////////// -bool initDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf) +bool initDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf) { auto element = _initDocGetElement(_xmlDoc); if (!element) @@ -176,7 +192,7 @@ bool initDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf) } ////////////////////////////////////////////////// -bool initDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf) +bool initDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf) { auto element = _initDocGetElement(_xmlDoc); if (!element) @@ -188,7 +204,7 @@ bool initDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf) } ////////////////////////////////////////////////// -bool initXml(TiXmlElement *_xml, ElementPtr _sdf) +bool initXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf) { const char *refString = _xml->Attribute("ref"); if (refString) @@ -218,7 +234,7 @@ bool initXml(TiXmlElement *_xml, ElementPtr _sdf) bool required = std::string(requiredString) == "1" ? true : false; const char *elemDefaultValue = _xml->Attribute("default"); std::string description; - TiXmlElement *descChild = _xml->FirstChildElement("description"); + tinyxml2::XMLElement *descChild = _xml->FirstChildElement("description"); if (descChild && descChild->GetText()) { description = descChild->GetText(); @@ -243,10 +259,10 @@ bool initXml(TiXmlElement *_xml, ElementPtr _sdf) } // Get all attributes - for (TiXmlElement *child = _xml->FirstChildElement("attribute"); + for (tinyxml2::XMLElement *child = _xml->FirstChildElement("attribute"); child; child = child->NextSiblingElement("attribute")) { - TiXmlElement *descriptionChild = child->FirstChildElement("description"); + auto *descriptionChild = child->FirstChildElement("description"); const char *name = child->Attribute("name"); const char *type = child->Attribute("type"); const char *defaultValue = child->Attribute("default"); @@ -286,14 +302,14 @@ bool initXml(TiXmlElement *_xml, ElementPtr _sdf) } // Read the element description - TiXmlElement *descChild = _xml->FirstChildElement("description"); + tinyxml2::XMLElement *descChild = _xml->FirstChildElement("description"); if (descChild && descChild->GetText()) { _sdf->SetDescription(descChild->GetText()); } // Get all child elements - for (TiXmlElement *child = _xml->FirstChildElement("element"); + for (tinyxml2::XMLElement *child = _xml->FirstChildElement("element"); child; child = child->NextSiblingElement("element")) { const char *copyDataString = child->Attribute("copy_data"); @@ -312,7 +328,7 @@ bool initXml(TiXmlElement *_xml, ElementPtr _sdf) } // Get all include elements - for (TiXmlElement *child = _xml->FirstChildElement("include"); + for (tinyxml2::XMLElement *child = _xml->FirstChildElement("include"); child; child = child->NextSiblingElement("include")) { std::string filename = child->Attribute("filename"); @@ -322,7 +338,7 @@ bool initXml(TiXmlElement *_xml, ElementPtr _sdf) initFile(filename, element); // override description for include elements - TiXmlElement *description = child->FirstChildElement("description"); + tinyxml2::XMLElement *description = child->FirstChildElement("description"); if (description) { element->SetDescription(description->GetText()); @@ -393,7 +409,7 @@ bool readFileWithoutConversion( bool readFileInternal(const std::string &_filename, SDFPtr _sdf, const bool _convert, Errors &_errors) { - TiXmlDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); std::string filename = sdf::findFile(_filename, true, true); if (filename.empty()) @@ -413,10 +429,11 @@ bool readFileInternal(const std::string &_filename, SDFPtr _sdf, return false; } - if (!xmlDoc.LoadFile(filename)) + auto error_code = xmlDoc.LoadFile(filename.c_str()); + if (error_code) { sdferr << "Error parsing XML in file [" << filename << "]: " - << xmlDoc.ErrorDesc() << '\n'; + << xmlDoc.ErrorStr() << '\n'; return false; } @@ -428,7 +445,8 @@ bool readFileInternal(const std::string &_filename, SDFPtr _sdf, else if (URDF2SDF::IsURDF(filename)) { URDF2SDF u2g; - TiXmlDocument doc = u2g.InitModelFile(filename); + auto doc = makeSdfDoc(); + u2g.InitModelFile(filename, &doc); if (sdf::readDoc(&doc, _sdf, "urdf file", _convert, _errors)) { sdfdbg << "parse from urdf file [" << _filename << "].\n"; @@ -474,11 +492,11 @@ bool readStringWithoutConversion( bool readStringInternal(const std::string &_xmlString, SDFPtr _sdf, const bool _convert, Errors &_errors) { - TiXmlDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(_xmlString.c_str()); if (xmlDoc.Error()) { - sdferr << "Error parsing XML from string: " << xmlDoc.ErrorDesc() << '\n'; + sdferr << "Error parsing XML from string: " << xmlDoc.ErrorStr() << '\n'; return false; } if (readDoc(&xmlDoc, _sdf, "data-string", _convert, _errors)) @@ -488,7 +506,9 @@ bool readStringInternal(const std::string &_xmlString, SDFPtr _sdf, else { URDF2SDF u2g; - TiXmlDocument doc = u2g.InitModelString(_xmlString); + auto doc = makeSdfDoc(); + u2g.InitModelString(_xmlString, &doc); + if (sdf::readDoc(&doc, _sdf, "urdf string", _convert, _errors)) { sdfdbg << "Parsing from urdf.\n"; @@ -520,11 +540,11 @@ bool readString(const std::string &_xmlString, ElementPtr _sdf) ////////////////////////////////////////////////// bool readString(const std::string &_xmlString, ElementPtr _sdf, Errors &_errors) { - TiXmlDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(_xmlString.c_str()); if (xmlDoc.Error()) { - sdferr << "Error parsing XML from string: " << xmlDoc.ErrorDesc() << '\n'; + sdferr << "Error parsing XML from string: " << xmlDoc.ErrorStr() << '\n'; return false; } if (readDoc(&xmlDoc, _sdf, "data-string", true, _errors)) @@ -540,7 +560,7 @@ bool readString(const std::string &_xmlString, ElementPtr _sdf, Errors &_errors) } ////////////////////////////////////////////////// -bool readDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf, +bool readDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf, const std::string &_source, bool _convert, Errors &_errors) { if (!_xmlDoc) @@ -550,7 +570,7 @@ bool readDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf, } // check sdf version - TiXmlElement *sdfNode = _xmlDoc->FirstChildElement("sdf"); + tinyxml2::XMLElement *sdfNode = _xmlDoc->FirstChildElement("sdf"); if (!sdfNode) { return false; @@ -587,7 +607,7 @@ bool readDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf, } // parse new sdf xml - TiXmlElement *elemXml = _xmlDoc->FirstChildElement(_sdf->Root()->GetName()); + auto *elemXml = _xmlDoc->FirstChildElement(_sdf->Root()->GetName().c_str()); if (!readXml(elemXml, _sdf->Root(), _errors)) { _errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -613,7 +633,7 @@ bool readDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf, } ////////////////////////////////////////////////// -bool readDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf, +bool readDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf, const std::string &_source, bool _convert, Errors &_errors) { if (!_xmlDoc) @@ -623,7 +643,7 @@ bool readDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf, } // check sdf version - TiXmlElement *sdfNode = _xmlDoc->FirstChildElement("sdf"); + tinyxml2::XMLElement *sdfNode = _xmlDoc->FirstChildElement("sdf"); if (!sdfNode) { return false; @@ -648,11 +668,11 @@ bool readDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf, Converter::Convert(_xmlDoc, SDF::Version()); } - TiXmlElement *elemXml = sdfNode; + tinyxml2::XMLElement *elemXml = sdfNode; if (sdfNode->Value() != _sdf->GetName() && - sdfNode->FirstChildElement(_sdf->GetName())) + sdfNode->FirstChildElement(_sdf->GetName().c_str())) { - elemXml = sdfNode->FirstChildElement(_sdf->GetName()); + elemXml = sdfNode->FirstChildElement(_sdf->GetName().c_str()); } // parse new sdf xml @@ -680,18 +700,18 @@ bool readDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf, } ////////////////////////////////////////////////// -std::string getBestSupportedModelVersion(TiXmlElement *_modelXML, +std::string getBestSupportedModelVersion(tinyxml2::XMLElement *_modelXML, std::string &_modelFileName) { - TiXmlElement *sdfXML = _modelXML->FirstChildElement("sdf"); - TiXmlElement *nameSearch = _modelXML->FirstChildElement("name"); + tinyxml2::XMLElement *sdfXML = _modelXML->FirstChildElement("sdf"); + tinyxml2::XMLElement *nameSearch = _modelXML->FirstChildElement("name"); // If a match is not found, use the latest version of the element // that is not older than the SDF parser. ignition::math::SemanticVersion sdfParserVersion(SDF_VERSION); std::string bestVersionStr = "0.0"; - TiXmlElement *sdfSearch = sdfXML; + tinyxml2::XMLElement *sdfSearch = sdfXML; while (sdfSearch) { if (sdfSearch->Attribute("version")) @@ -757,7 +777,8 @@ std::string getModelFilePath(const std::string &_modelDirPath) if (!sdf::filesystem::exists(configFilePath)) { // We didn't find manifest.xml either, output an error and get out. - sdferr << "Could not find model.config or manifest.xml for the model\n"; + sdferr << "Could not find model.config or manifest.xml in [" + << _modelDirPath << "]\n"; return std::string(); } else @@ -769,16 +790,16 @@ std::string getModelFilePath(const std::string &_modelDirPath) } } - TiXmlDocument configFileDoc; - if (!configFileDoc.LoadFile(configFilePath)) + auto configFileDoc = makeSdfDoc(); + if (tinyxml2::XML_SUCCESS != configFileDoc.LoadFile(configFilePath.c_str())) { sdferr << "Error parsing XML in file [" << configFilePath << "]: " - << configFileDoc.ErrorDesc() << '\n'; + << configFileDoc.ErrorStr() << '\n'; return std::string(); } - TiXmlElement *modelXML = configFileDoc.FirstChildElement("model"); + tinyxml2::XMLElement *modelXML = configFileDoc.FirstChildElement("model"); if (!modelXML) { @@ -796,7 +817,7 @@ std::string getModelFilePath(const std::string &_modelDirPath) } ////////////////////////////////////////////////// -bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) +bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) { // Check if the element pointer is deprecated. if (_sdf->GetRequired() == "-1") @@ -836,7 +857,21 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) _sdf->Copy(refSDF); } - TiXmlAttribute *attribute = _xml->FirstAttribute(); + // A list of parent element-attributes pairs where a frame name is referenced + // in the attribute. This is used to check if the reference is invalid. + std::set> frameReferenceAttributes { + // //frame/[@attached_to] + {"frame", "attached_to"}, + // //pose/[@relative_to] + {"pose", "relative_to"}, + // //model/[@placement_frame] + {"model", "placement_frame"}, + // //model/[@canonical_link] + {"model", "canonical_link"}, + // //sensor/imu/orientation_reference_frame/custom_rpy/[@parent_frame] + {"custom_rpy", "parent_frame"}}; + + const tinyxml2::XMLAttribute *attribute = _xml->FirstAttribute(); unsigned int i = 0; @@ -845,11 +880,11 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) { // Avoid printing a warning message for missing attributes if a namespaced // attribute is found - if (std::strchr(attribute->Name(), ':') != NULL) + if (std::strchr(attribute->Name(), ':') != nullptr) { _sdf->AddAttribute(attribute->Name(), "string", "", 1, ""); _sdf->GetAttribute(attribute->Name())->SetFromString( - attribute->ValueStr()); + attribute->Value()); attribute = attribute->Next(); continue; } @@ -859,8 +894,20 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) ParamPtr p = _sdf->GetAttribute(i); if (p->GetKey() == attribute->Name()) { + if (frameReferenceAttributes.count( + std::make_pair(_sdf->GetName(), attribute->Name())) != 0) + { + if (!isValidFrameReference(attribute->Value())) + { + _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, + "'" + std::string(attribute->Value()) + + "' is reserved; it cannot be used as a value of " + "attribute [" + + p->GetKey() + "]"}); + } + } // Set the value of the SDF attribute - if (!p->SetFromString(attribute->ValueStr())) + if (!p->SetFromString(attribute->Value())) { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Unable to read attribute[" + p->GetKey() + "]"}); @@ -902,7 +949,7 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) std::string filename; // Iterate over all the child elements - TiXmlElement *elemXml = nullptr; + tinyxml2::XMLElement *elemXml = nullptr; for (elemXml = _xml->FirstChildElement(); elemXml; elemXml = elemXml->NextSiblingElement()) { @@ -931,16 +978,28 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) } else { - if (!sdf::filesystem::is_directory(modelPath)) + if (sdf::filesystem::is_directory(modelPath)) { - _errors.push_back({ErrorCode::DIRECTORY_NONEXISTANT, - "Directory doesn't exist[" + modelPath + "]"}); - continue; + // Get the model.config filename + filename = getModelFilePath(modelPath); + + if (filename.empty()) + { + _errors.push_back({ErrorCode::URI_LOOKUP, + "Unable to resolve uri[" + uri + "] to model path [" + + modelPath + "] since it does not contain a model.config " + + "file."}); + continue; + } + } + else + { + // This is a file path and since sdf::findFile returns an empty + // string if the file doesn't exist, we don't have to check for + // existence again here. + filename = modelPath; } } - - // Get the config.xml filename - filename = getModelFilePath(modelPath); } else { @@ -970,24 +1029,30 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) return false; } + // For now there is only a warning if there is more than one model, + // actor or light element, or two different types of those elements. For + // compatibility with old behavior, this chooses the first element + // in the preference order: model->actor->light sdf::ElementPtr topLevelElem; - bool isModel{false}; - bool isActor{false}; - if (includeSDF->Root()->HasElement("model")) + for (const auto & elementType : {"model", "actor", "light"}) { - topLevelElem = includeSDF->Root()->GetElement("model"); - isModel = true; - } - else if (includeSDF->Root()->HasElement("actor")) - { - topLevelElem = includeSDF->Root()->GetElement("actor"); - isActor = true; - } - else if (includeSDF->Root()->HasElement("light")) - { - topLevelElem = includeSDF->Root()->GetElement("light"); + if (includeSDF->Root()->HasElement(elementType)) + { + if (nullptr == topLevelElem) + { + topLevelElem = includeSDF->Root()->GetElement(elementType); + } + else + { + sdfwarn << "Found other top level element <" << elementType + << "> in addition to <" << topLevelElem->GetName() + << "> in include file. This is unsupported and in future " + << "versions of libsdformat will become an error"; + } + } } - else + + if (nullptr == topLevelElem) { _errors.push_back({ErrorCode::ELEMENT_MISSING, "Failed to find top level / / for " @@ -995,13 +1060,25 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) continue; } + const auto topLevelElementType = topLevelElem->GetName(); + // Check for more than one of the discovered top-level element type + if (nullptr != topLevelElem->GetNextElement(topLevelElementType)) + { + sdfwarn << "Found more than one of " << topLevelElem->GetName() + << " for . This is unsupported and in future " + << "versions of libsdformat will become an error"; + } + + bool isModel = topLevelElementType == "model"; + bool isActor = topLevelElementType == "actor"; + if (elemXml->FirstChildElement("name")) { topLevelElem->GetAttribute("name")->SetFromString( elemXml->FirstChildElement("name")->GetText()); } - TiXmlElement *poseElemXml = elemXml->FirstChildElement("pose"); + tinyxml2::XMLElement *poseElemXml = elemXml->FirstChildElement("pose"); if (poseElemXml) { sdf::ElementPtr poseElem = topLevelElem->GetElement("pose"); @@ -1032,9 +1109,33 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) elemXml->FirstChildElement("static")->GetText()); } + if (isModel && elemXml->FirstChildElement("placement_frame")) + { + if (nullptr == elemXml->FirstChildElement("pose")) + { + _errors.push_back({ErrorCode::MODEL_PLACEMENT_FRAME_INVALID, + " is required when specifying the placement_frame " + "element"}); + return false; + } + + const std::string placementFrameVal = + elemXml->FirstChildElement("placement_frame")->GetText(); + + if (!isValidFrameReference(placementFrameVal)) + { + _errors.push_back({ErrorCode::RESERVED_NAME, + "'" + placementFrameVal + + "' is reserved; it cannot be used as a value of " + "element [placement_frame]"}); + } + topLevelElem->GetAttribute("placement_frame") + ->SetFromString(placementFrameVal); + } + if (isModel || isActor) { - for (TiXmlElement *childElemXml = elemXml->FirstChildElement(); + for (auto *childElemXml = elemXml->FirstChildElement(); childElemXml; childElemXml = childElemXml->NextSiblingElement()) { if (std::string("plugin") == childElemXml->Value()) @@ -1052,21 +1153,14 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) } } - if (_sdf->GetName() == "model") - { - addNestedModel(_sdf, includeSDF->Root(), _errors); - } - else - { - includeSDF->Root()->GetFirstElement()->SetParent(_sdf); - _sdf->InsertElement(includeSDF->Root()->GetFirstElement()); - // TODO: This was used to store the included filename so that when - // a world is saved, the included model's SDF is not stored in the - // world file. This highlights the need to make model inclusion - // a core feature of SDF, and not a hack that that parser handles - // includeSDF->Root()->GetFirstElement()->SetInclude( - // elemXml->Attribute("filename")); - } + includeSDF->Root()->GetFirstElement()->SetParent(_sdf); + _sdf->InsertElement(includeSDF->Root()->GetFirstElement()); + // TODO: This was used to store the included filename so that when + // a world is saved, the included model's SDF is not stored in the + // world file. This highlights the need to make model inclusion + // a core feature of SDF, and not a hack that that parser handles + // includeSDF->Root()->GetFirstElement()->SetInclude( + // elemXml->Attribute("filename")); continue; } @@ -1096,7 +1190,8 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) } } - if (descCounter == _sdf->GetElementDescriptionCount()) + if (descCounter == _sdf->GetElementDescriptionCount() + && std::strchr(elemXml->Value(), ':') == nullptr) { sdfdbg << "XML Element[" << elemXml->Value() << "], child of element[" << _xml->Value() @@ -1141,34 +1236,16 @@ bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors) } ///////////////////////////////////////////////// -static void replace_all(std::string &_str, - const std::string &_from, - const std::string &_to) -{ - if (_from.empty()) - { - return; - } - size_t start_pos = 0; - while ((start_pos = _str.find(_from, start_pos)) != std::string::npos) - { - _str.replace(start_pos, _from.length(), _to); - // We need to advance our starting position beyond what we - // just replaced to deal with the case where the '_to' string - // happens to contain a piece of '_from'. - start_pos += _to.length(); - } -} - -///////////////////////////////////////////////// -void copyChildren(ElementPtr _sdf, TiXmlElement *_xml, const bool _onlyUnknown) +void copyChildren(ElementPtr _sdf, + tinyxml2::XMLElement *_xml, + const bool _onlyUnknown) { // Iterate over all the child elements - TiXmlElement *elemXml = nullptr; + tinyxml2::XMLElement *elemXml = nullptr; for (elemXml = _xml->FirstChildElement(); elemXml; elemXml = elemXml->NextSiblingElement()) { - std::string elem_name = elemXml->ValueStr(); + std::string elem_name = elemXml->Name(); if (_sdf->HasElementDescription(elem_name)) { @@ -1177,11 +1254,11 @@ void copyChildren(ElementPtr _sdf, TiXmlElement *_xml, const bool _onlyUnknown) sdf::ElementPtr element = _sdf->AddElement(elem_name); // FIXME: copy attributes - for (TiXmlAttribute *attribute = elemXml->FirstAttribute(); + for (const auto *attribute = elemXml->FirstAttribute(); attribute; attribute = attribute->Next()) { element->GetAttribute(attribute->Name())->SetFromString( - attribute->ValueStr()); + attribute->Value()); } // copy value @@ -1203,12 +1280,12 @@ void copyChildren(ElementPtr _sdf, TiXmlElement *_xml, const bool _onlyUnknown) element->AddValue("string", elemXml->GetText(), "1"); } - for (TiXmlAttribute *attribute = elemXml->FirstAttribute(); + for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute(); attribute; attribute = attribute->Next()) { element->AddAttribute(attribute->Name(), "string", "", 1, ""); element->GetAttribute(attribute->Name())->SetFromString( - attribute->ValueStr()); + attribute->Value()); } copyChildren(element, elemXml, _onlyUnknown); @@ -1217,141 +1294,6 @@ void copyChildren(ElementPtr _sdf, TiXmlElement *_xml, const bool _onlyUnknown) } } -///////////////////////////////////////////////// -void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF) -{ - Errors errors; - addNestedModel(_sdf, _includeSDF, errors); - for (const auto &e : errors) - { - sdferr << e << '\n'; - } -} - -///////////////////////////////////////////////// -void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors) -{ - ElementPtr modelPtr = _includeSDF->GetElement("model"); - ElementPtr elem = modelPtr->GetFirstElement(); - std::map replace; - - ignition::math::Pose3d modelPose = - modelPtr->Get("pose"); - - std::string modelName = modelPtr->Get("name"); - - // Inject a frame to represent the nested __model__ frame. - ElementPtr nestedModelFrame = _sdf->AddElement("frame"); - const std::string nestedModelFrameName = modelName + "::__model__"; - nestedModelFrame->GetAttribute("name")->Set(nestedModelFrameName); - - replace["__model__"] = nestedModelFrameName; - - std::string canonicalLinkName = ""; - if (modelPtr->GetAttribute("canonical_link")->GetSet()) - { - canonicalLinkName = modelPtr->GetAttribute("canonical_link")->GetAsString(); - } - else if (modelPtr->HasElement("link")) - { - canonicalLinkName = - modelPtr->GetElement("link")->GetAttribute("name")->GetAsString(); - } - nestedModelFrame->GetAttribute("attached_to") - ->Set(modelName + "::" + canonicalLinkName); - - ElementPtr nestedModelFramePose = nestedModelFrame->AddElement("pose"); - nestedModelFramePose->Set(modelPose); - - // Set the nestedModelFrame's //pose/@relative_to to the frame used in - // //include/pose/@relative_to. - std::string modelPoseRelativeTo = ""; - if (modelPtr->HasElement("pose")) - { - modelPoseRelativeTo = - modelPtr->GetElement("pose")->Get("relative_to"); - } - - // If empty, use "__model__", since leaving it empty would make it - // relative_to the canonical link frame specified in //frame/@attached_to. - if (modelPoseRelativeTo.empty()) - { - modelPoseRelativeTo = "__model__"; - } - - nestedModelFramePose->GetAttribute("relative_to")->Set(modelPoseRelativeTo); - - while (elem) - { - if ((elem->GetName() == "link") || - (elem->GetName() == "joint") || - (elem->GetName() == "frame")) - { - std::string elemName = elem->Get("name"); - std::string newName = modelName + "::" + elemName; - replace[elemName] = newName; - } - - if ((elem->GetName() == "link")) - { - // Add a pose element even if the element doesn't originally have one - auto elemPose = elem->GetElement("pose"); - - // If //pose/@relative_to is empty, explicitly set it to the name - // of the nested model frame. - auto relativeTo = elemPose->GetAttribute("relative_to"); - if (relativeTo->GetAsString().empty()) - { - relativeTo->Set(nestedModelFrameName); - } - - // If //pose/@relative_to is set, let the replacement step handle it. - } - else if (elem->GetName() == "frame") - { - // If //frame/@attached_to is empty, explicitly set it to the name - // of the nested model frame. - auto attachedTo = elem->GetAttribute("attached_to"); - if (attachedTo->GetAsString().empty()) - { - attachedTo->Set(nestedModelFrameName); - } - - // If //frame/@attached_to is set, let the replacement step handle it. - } - elem = elem->GetNextElement(); - } - - std::string str = _includeSDF->ToString(""); - for (std::map::iterator iter = replace.begin(); - iter != replace.end(); ++iter) - { - replace_all(str, std::string("\"") + iter->first + "\"", - std::string("\"") + iter->second + "\""); - replace_all(str, std::string("'") + iter->first + "'", - std::string("'") + iter->second + "'"); - replace_all(str, std::string(">") + iter->first + "<", - std::string(">") + iter->second + "<"); - } - - _includeSDF->ClearElements(); - readString(str, _includeSDF, _errors); - - elem = _includeSDF->GetElement("model")->GetFirstElement(); - ElementPtr nextElem; - while (elem) - { - nextElem = elem->GetNextElement(); - - if (elem->GetName() != "pose") - { - elem->SetParent(_sdf); - _sdf->InsertElement(elem); - } - elem = nextElem; - } -} - ///////////////////////////////////////////////// bool convertFile(const std::string &_filename, const std::string &_version, SDFPtr _sdf) @@ -1370,13 +1312,13 @@ bool convertFile(const std::string &_filename, const std::string &_version, return false; } - TiXmlDocument xmlDoc; - if (xmlDoc.LoadFile(filename)) + auto xmlDoc = makeSdfDoc(); + if (!xmlDoc.LoadFile(filename.c_str())) { // read initial sdf version std::string originalVersion; { - TiXmlElement *sdfNode = xmlDoc.FirstChildElement("sdf"); + tinyxml2::XMLElement *sdfNode = xmlDoc.FirstChildElement("sdf"); if (sdfNode && sdfNode->Attribute("version")) { originalVersion = sdfNode->Attribute("version"); @@ -1415,7 +1357,7 @@ bool convertString(const std::string &_sdfString, const std::string &_version, return false; } - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(_sdfString.c_str()); if (!xmlDoc.Error()) @@ -1423,7 +1365,7 @@ bool convertString(const std::string &_sdfString, const std::string &_version, // read initial sdf version std::string originalVersion; { - TiXmlElement *sdfNode = xmlDoc.FirstChildElement("sdf"); + tinyxml2::XMLElement *sdfNode = xmlDoc.FirstChildElement("sdf"); if (sdfNode && sdfNode->Attribute("version")) { originalVersion = sdfNode->Attribute("version"); @@ -1481,10 +1423,9 @@ bool checkCanonicalLinkNames(const sdf::Root *_root) return modelResult; }; - for (uint64_t m = 0; m < _root->ModelCount(); ++m) + if (_root->Model()) { - auto model = _root->ModelByIndex(m); - result = checkModelCanonicalLinkName(model) && result; + result = checkModelCanonicalLinkName(_root->Model()) && result; } for (uint64_t w = 0; w < _root->WorldCount(); ++w) @@ -1532,13 +1473,14 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) modelResult = false; } else if (!_model->LinkNameExists(attachedTo) && + !_model->ModelNameExists(attachedTo) && !_model->JointNameExists(attachedTo) && !_model->FrameNameExists(attachedTo)) { std::cerr << "Error: attached_to name[" << attachedTo << "] specified by frame with name[" << frame->Name() - << "] does not match a link, joint, or frame name " - << "in model with name[" << _model->Name() + << "] does not match a nested model, link, joint, " + << "or frame name in model with name[" << _model->Name() << "]." << std::endl; modelResult = false; @@ -1550,6 +1492,36 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) auto checkWorldFrameAttachedToNames = []( const sdf::World *_world) -> bool { + auto findNameInWorld = [](const sdf::World *_inWorld, + const std::string &_name) -> bool { + if (_inWorld->ModelNameExists(_name) || + _inWorld->FrameNameExists(_name)) + { + return true; + } + + const auto delimIndex = _name.find("::"); + if (delimIndex != std::string::npos && delimIndex + 2 < _name.size()) + { + std::string modelName = _name.substr(0, delimIndex); + std::string nameToCheck = _name.substr(delimIndex + 2); + const auto *model = _inWorld->ModelByName(modelName); + if (nullptr == model) + { + return false; + } + + if (model->LinkNameExists(nameToCheck) || + model->ModelNameExists(nameToCheck) || + model->JointNameExists(nameToCheck) || + model->FrameNameExists(nameToCheck)) + { + return true; + } + } + return false; + }; + bool worldResult = true; for (uint64_t f = 0; f < _world->FrameCount(); ++f) { @@ -1573,8 +1545,7 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) << std::endl; worldResult = false; } - else if (!_world->ModelNameExists(attachedTo) && - !_world->FrameNameExists(attachedTo)) + else if (!findNameInWorld(_world, attachedTo)) { std::cerr << "Error: attached_to name[" << attachedTo << "] specified by frame with name[" << frame->Name() @@ -1588,10 +1559,9 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) return worldResult; }; - for (uint64_t m = 0; m < _root->ModelCount(); ++m) + if (_root->Model()) { - auto model = _root->ModelByIndex(m); - result = checkModelFrameAttachedToNames(model) && result; + result = checkModelFrameAttachedToNames(_root->Model()) && result; } for (uint64_t w = 0; w < _root->WorldCount(); ++w) @@ -1672,7 +1642,8 @@ bool checkFrameAttachedToGraph(const sdf::Root *_root) const sdf::Model *_model) -> bool { bool modelResult = true; - sdf::FrameAttachedToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildFrameAttachedToGraph(graph, _model); if (!errors.empty()) { @@ -1700,7 +1671,8 @@ bool checkFrameAttachedToGraph(const sdf::Root *_root) const sdf::World *_world) -> bool { bool worldResult = true; - sdf::FrameAttachedToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildFrameAttachedToGraph(graph, _world); if (!errors.empty()) { @@ -1724,10 +1696,9 @@ bool checkFrameAttachedToGraph(const sdf::Root *_root) return worldResult; }; - for (uint64_t m = 0; m < _root->ModelCount(); ++m) + if (_root->Model()) { - auto model = _root->ModelByIndex(m); - result = checkModelFrameAttachedToGraph(model) && result; + result = checkModelFrameAttachedToGraph(_root->Model()) && result; } for (uint64_t w = 0; w < _root->WorldCount(); ++w) @@ -1753,7 +1724,8 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) const sdf::Model *_model) -> bool { bool modelResult = true; - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, _model); if (!errors.empty()) { @@ -1781,7 +1753,8 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) const sdf::World *_world) -> bool { bool worldResult = true; - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, _world); if (!errors.empty()) { @@ -1805,10 +1778,9 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) return worldResult; }; - for (uint64_t m = 0; m < _root->ModelCount(); ++m) + if (_root->Model()) { - auto model = _root->ModelByIndex(m); - result = checkModelPoseRelativeToGraph(model) && result; + result = checkModelPoseRelativeToGraph(_root->Model()) && result; } for (uint64_t w = 0; w < _root->WorldCount(); ++w) @@ -1839,9 +1811,11 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) auto joint = _model->JointByIndex(j); const std::string &parentName = joint->ParentLinkName(); - if (parentName != "world" && !_model->LinkNameExists(parentName)) + if (parentName != "world" && !_model->LinkNameExists(parentName) && + !_model->JointNameExists(parentName) && + !_model->FrameNameExists(parentName)) { - std::cerr << "Error: parent link with name[" << parentName + std::cerr << "Error: parent frame with name[" << parentName << "] specified by joint with name[" << joint->Name() << "] not found in model with name[" << _model->Name() << "]." @@ -1850,9 +1824,22 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) } const std::string &childName = joint->ChildLinkName(); - if (childName != "world" && !_model->LinkNameExists(childName)) + if (childName == "world") { - std::cerr << "Error: child link with name[" << childName + std::cerr << "Error: invalid child name[world" + << "] specified by joint with name[" << joint->Name() + << "] in model with name[" << _model->Name() + << "]." + << std::endl; + modelResult = false; + } + + if (!_model->LinkNameExists(childName) && + !_model->JointNameExists(childName) && + !_model->FrameNameExists(childName) && + !_model->ModelNameExists(childName)) + { + std::cerr << "Error: child frame with name[" << childName << "] specified by joint with name[" << joint->Name() << "] not found in model with name[" << _model->Name() << "]." @@ -1860,13 +1847,57 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) modelResult = false; } - if (childName == parentName) + if (childName == joint->Name()) + { + std::cerr << "Error: joint with name[" << joint->Name() + << "] in model with name[" << _model->Name() + << "] must not specify its own name as the child frame." + << std::endl; + modelResult = false; + } + + if (parentName == joint->Name()) + { + std::cerr << "Error: joint with name[" << joint->Name() + << "] in model with name[" << _model->Name() + << "] must not specify its own name as the parent frame." + << std::endl; + modelResult = false; + } + + // Check that parent and child frames resolve to different links + std::string resolvedChildName; + std::string resolvedParentName; + auto errors = joint->ResolveChildLink(resolvedChildName); + if (!errors.empty()) + { + std::cerr << "Error when attempting to resolve child link name:" + << std::endl; + for (auto error : errors) + { + std::cerr << error.Message() << std::endl; + } + modelResult = false; + } + errors = joint->ResolveParentLink(resolvedParentName); + if (!errors.empty()) + { + std::cerr << "Error when attempting to resolve parent link name:" + << std::endl; + for (auto error : errors) + { + std::cerr << error.Message() << std::endl; + } + modelResult = false; + } + if (resolvedChildName == resolvedParentName) { std::cerr << "Error: joint with name[" << joint->Name() << "] in model with name[" << _model->Name() - << "] must specify different link names for " - << "parent and child, while [" << childName - << "] was specified for both." + << "] specified parent frame [" << parentName + << "] and child frame [" << childName + << "] that both resolve to [" << resolvedChildName + << "], but they should resolve to different values." << std::endl; modelResult = false; } @@ -1874,10 +1905,9 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) return modelResult; }; - for (uint64_t m = 0; m < _root->ModelCount(); ++m) + if (_root->Model()) { - auto model = _root->ModelByIndex(m); - result = checkModelJointParentChildNames(model) && result; + result = checkModelJointParentChildNames(_root->Model()) && result; } for (uint64_t w = 0; w < _root->WorldCount(); ++w) diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index 85ebe6a1a..3c735a696 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -15,9 +15,14 @@ * */ +#include +#include +#include #include #include "sdf/parser.hh" #include "sdf/Element.hh" +#include "sdf/Console.hh" +#include "sdf/Filesystem.hh" #include "test_config.h" ///////////////////////////////////////////////// @@ -59,6 +64,39 @@ sdf::SDFPtr InitSDF() return sdf; } +///////////////////////////////////////////////// +/// Checks emitted warnings for custom/unknown elements in log file +TEST(Parser, CustomUnknownElements) +{ + std::string pathBase = PROJECT_SOURCE_PATH; + pathBase += "/test/sdf"; + const std::string path = pathBase +"/custom_and_unknown_elements.sdf"; + + sdf::SDFPtr sdf = InitSDF(); + EXPECT_TRUE(sdf::readFile(path, sdf)); + +#ifndef _WIN32 + char *homeDir = getenv("HOME"); +#else + char *homeDir; + size_t sz = 0; + _dupenv_s(&homeDir, &sz, "HOMEPATH"); +#endif + + std::string pathLog = + sdf::filesystem::append(homeDir, ".sdformat", "sdformat.log"); + + std::fstream fs; + fs.open(pathLog); + ASSERT_TRUE(fs.is_open()); + + std::stringstream fileStr; + fs >> fileStr.rdbuf(); + + EXPECT_NE(fileStr.str().find("XML Element[test_unknown]"), std::string::npos); + EXPECT_EQ(fileStr.str().find("XML Element[test:custom]"), std::string::npos); +} + ///////////////////////////////////////////////// TEST(Parser, ReusedSDFVersion) { @@ -109,240 +147,6 @@ TEST(Parser, readFileConversions) } } -///////////////////////////////////////////////// -TEST(Parser, addNestedModel) -{ - auto getIncludedModelSdfString = []( - const std::string &_version, - const std::string &_expressedIn = "", - const std::string &_canonicalLink = "") -> std::string - { - std::string modelAttributeString = ""; - if (!_canonicalLink.empty()) - { - modelAttributeString = " canonical_link='" + _canonicalLink + "'"; - } - std::ostringstream stream; - stream - << "" - << "" - << " 0 0 10 0 0 1.57" - << " " - << " " - << " " - << " parent" - << " child" - << " "; - if (_expressedIn.empty()) - { - stream << "1 0 0"; - } - else - { - stream << "1 0 0"; - } - stream - << " " - << " " - << "" - << ""; - return stream.str(); - }; - - auto checkNestedModel = []( - sdf::ElementPtr _elem, const std::string &_expressedIn = "", - const std::string &_canonicalLink = "included::parent") - { - EXPECT_TRUE(_elem->HasElement("frame")); - sdf::ElementPtr nestedModelFrame = _elem->GetElement("frame"); - EXPECT_EQ(nullptr, nestedModelFrame->GetNextElement("frame")); - - sdf::ElementPtr link1 = _elem->GetElement("link"); - sdf::ElementPtr link2 = link1->GetNextElement("link"); - EXPECT_EQ(nullptr, link2->GetNextElement("link")); - - EXPECT_TRUE(_elem->HasElement("joint")); - sdf::ElementPtr joint = _elem->GetElement("joint"); - EXPECT_EQ(nullptr, joint->GetNextElement("joint")); - - EXPECT_EQ( - "included::__model__", nestedModelFrame->Get("name")); - EXPECT_EQ("included::parent", link1->Get("name")); - EXPECT_EQ("included::child", link2->Get("name")); - EXPECT_EQ("included::joint", joint->Get("name")); - EXPECT_EQ("included::parent", joint->Get("parent")); - EXPECT_EQ("included::child", joint->Get("child")); - - EXPECT_EQ(_canonicalLink, - nestedModelFrame->Get("attached_to")); - using ignition::math::Pose3d; - const Pose3d pose(0, 0, 10, 0, 0, 1.57); - EXPECT_EQ(pose, nestedModelFrame->Get("pose")); - - EXPECT_FALSE(_elem->HasElement("pose")); - EXPECT_EQ("included::__model__", - link1->GetElement("pose")->Get("relative_to")); - EXPECT_EQ("included::__model__", - link2->GetElement("pose")->Get("relative_to")); - EXPECT_EQ(Pose3d::Zero, link1->Get("pose")); - EXPECT_EQ(Pose3d::Zero, link2->Get("pose")); - EXPECT_EQ(Pose3d::Zero, joint->Get("pose")); - - EXPECT_TRUE(joint->HasElement("axis")); - sdf::ElementPtr axis = joint->GetElement("axis"); - EXPECT_TRUE(axis->HasElement("xyz")); - sdf::ElementPtr xyz = axis->GetElement("xyz"); - - EXPECT_EQ(_expressedIn, xyz->Get("expressed_in")); - EXPECT_EQ( - ignition::math::Vector3d::UnitX, xyz->Get()); - }; - - // insert as 1.4, expect rotation of //joint/axis/xyz - { - const std::string version = "1.4"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version), sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - // Expect //joint/axis/xyz[@expressed_in] = "included::__model__" because - // it is the default behavior 1.4 - checkNestedModel(elem, "included::__model__"); - } - - // insert as 1.5, expect no change to //joint/axis/xyz - { - const std::string version = "1.5"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version), sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem); - } - - // insert as 1.7, expressed_in=__model__ - // expect rotation of //joint/axis/xyz - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "__model__"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem, "included::__model__"); - } - - // insert as 1.7, expressed_in=child - // expect no change to //joint/axis/xyz - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "child"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem, "included::child"); - - // test coverage for addNestedModel without returning Errors - sdf::ElementPtr elem2 = std::make_shared(); - sdf::initFile("model.sdf", elem2); - sdf::addNestedModel(elem2, sdf->Root()); - } - - // insert as 1.7, expressed_in=parent - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "parent"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_EQ(0u, errors.size()); - - checkNestedModel(elem, "included::parent"); - - // test coverage for addNestedModel without returning Errors - sdf::ElementPtr elem2 = std::make_shared(); - sdf::initFile("model.sdf", elem2); - sdf::addNestedModel(elem2, sdf->Root()); - } - - // insert as 1.7, canonicalLink = child - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "parent", "child"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_EQ(0u, errors.size()); - - checkNestedModel(elem, "included::parent", "included::child"); - } -} - ///////////////////////////////////////////////// TEST(Parser, NameUniqueness) { @@ -532,6 +336,25 @@ TEST(Parser, SyntaxErrorInValues) #endif } +TEST(Parser, PlacementFrameMissingPose) +{ + const std::string modelRootPath = sdf::filesystem::append( + PROJECT_SOURCE_PATH, "test", "integration", "model"); + + const std::string testModelPath = sdf::filesystem::append( + PROJECT_SOURCE_PATH, "test", "sdf", "placement_frame_missing_pose.sdf"); + + sdf::setFindCallback([&](const std::string &_file) + { + return sdf::filesystem::append(modelRootPath, _file); + }); + sdf::SDFPtr sdf = InitSDF(); + sdf::Errors errors; + EXPECT_FALSE(sdf::readFile(testModelPath, sdf, errors)); + ASSERT_GE(errors.size(), 0u); + EXPECT_EQ(sdf::ErrorCode::MODEL_PLACEMENT_FRAME_INVALID, errors[0].Code()); +} + ///////////////////////////////////////////////// /// Fixture for setting up stream redirection class ValueConstraintsFixture : public ::testing::Test @@ -639,6 +462,21 @@ TEST_F(ValueConstraintsFixture, ElementMinMaxValues) /// Main int main(int argc, char **argv) { + // temporarily set HOME to build directory +#ifndef _WIN32 + setenv("HOME", PROJECT_BINARY_DIR, 1); +#else + std::string buildDir = PROJECT_BINARY_DIR; + for (int i = 0; i < buildDir.size(); ++i) + { + if (buildDir[i] == '/') + buildDir[i] = '\\'; + } + std::string homePath = "HOMEPATH=" + buildDir; + _putenv(homePath.c_str()); +#endif + sdf::Console::Clear(); + ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/src/parser_private.hh b/src/parser_private.hh index 40dfd7393..70160004f 100644 --- a/src/parser_private.hh +++ b/src/parser_private.hh @@ -17,7 +17,7 @@ #ifndef SDF_PARSER_PRIVATE_HH_ #define SDF_PARSER_PRIVATE_HH_ -#include +#include #include @@ -38,41 +38,41 @@ namespace sdf /// model XML tag /// \param[out] _modelFileName file name of the best model file /// \return string with the best SDF version supported - static std::string getBestSupportedModelVersion(TiXmlElement *_modelXML, - std::string &_modelFileName); + static std::string getBestSupportedModelVersion( + tinyxml2::XMLElement *_modelXML, std::string &_modelFileName); - /// \brief Initialize the SDF interface using a TinyXML document. + /// \brief Initialize the SDF interface using a TinyXML2 document. /// /// This actually forwards to initXml after converting the inputs - /// \param[in] _xmlDoc TinyXML document containing the SDFormat description + /// \param[in] _xmlDoc TinyXML2 document containing the SDFormat description /// file that corresponds with the input SDFPtr /// \param[in] _sdf SDF interface to be initialized - static bool initDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf); + static bool initDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf); - /// \brief Initialize the SDF Element using a TinyXML document + /// \brief Initialize the SDF Element using a TinyXML2 document /// /// This actually forwards to initXml after converting the inputs - /// \param[in] _xmlDoc TinyXML document containing the SDFormat description + /// \param[in] _xmlDoc TinyXML2 document containing the SDFormat description /// file that corresponds with the input ElementPtr /// \param[in] _sdf SDF Element to be initialized - static bool initDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf); + static bool initDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf); /// \brief Initialize the SDF Element by parsing the SDFormat description in - /// the input TinyXML element. This is where SDFormat spec/description files + /// the input TinyXML2 element. This is where SDFormat spec/description files /// are parsed /// \remark For internal use only. Do not use this function. - /// \param[in] _xml TinyXML element containing the SDFormat description + /// \param[in] _xml TinyXML2 element containing the SDFormat description /// file that corresponds with the input ElementPtr /// \param[in] _sdf SDF ElementPtr to be initialized - static bool initXml(TiXmlElement *_xml, ElementPtr _sdf); + static bool initXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf); /// \brief Populate the SDF values from a TinyXML document - static bool readDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf, + static bool readDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf, const std::string &_source, bool _convert, Errors &_errors); /// \brief Populate the SDF values from a TinyXML document - static bool readDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf, + static bool readDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf, const std::string &_source, bool _convert, Errors &_errors); /// \brief Populate an SDF Element from the XML input. The XML input here is @@ -83,7 +83,9 @@ namespace sdf /// \param[in,out] _sdf SDF pointer to parse data into. /// \param[out] _errors Captures errors found during parsing. /// \return True on success, false on error. - static bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors); + static bool readXml(tinyxml2::XMLElement *_xml, + ElementPtr _sdf, + Errors &_errors); /// \brief Copy child XML elements into the _sdf element. /// \param[in] _sdf Parent Element. @@ -91,7 +93,7 @@ namespace sdf /// copied. /// \param[in] _onlyUnknown True to copy only elements that are NOT part of /// the SDF spec. Set this to false to copy everything. - static void copyChildren(ElementPtr _sdf, TiXmlElement *_xml, + static void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, const bool _onlyUnknown); } } diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 27d786125..85eebbaa9 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -33,6 +33,7 @@ #include "sdf/sdf.hh" +#include "XmlUtils.hh" #include "SDFExtension.hh" #include "parser_urdf.hh" @@ -40,7 +41,10 @@ using namespace sdf; namespace sdf { inline namespace SDF_VERSION_NAMESPACE { -typedef std::shared_ptr TiXmlElementPtr; + +using XMLDocumentPtr = std::shared_ptr; +using XMLElementPtr = std::shared_ptr; + typedef std::shared_ptr SDFExtensionPtr; typedef std::map > StringSDFExtensionPtrMap; @@ -63,23 +67,23 @@ const int g_outputDecimalPrecision = 16; /// \param[in] _key XML key where vector3 value might be /// \param[in] _scale scalar scale for the vector3 /// \return a urdf::Vector3 -urdf::Vector3 ParseVector3(TiXmlNode* _key, double _scale = 1.0); +urdf::Vector3 ParseVector3(tinyxml2::XMLNode *_key, double _scale = 1.0); urdf::Vector3 ParseVector3(const std::string &_str, double _scale = 1.0); /// insert extensions into collision geoms -void InsertSDFExtensionCollision(TiXmlElement *_elem, +void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem, const std::string &_linkName); /// insert extensions into model -void InsertSDFExtensionRobot(TiXmlElement *_elem); +void InsertSDFExtensionRobot(tinyxml2::XMLElement *_elem); /// insert extensions into visuals -void InsertSDFExtensionVisual(TiXmlElement *_elem, +void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem, const std::string &_linkName); /// insert extensions into joints -void InsertSDFExtensionJoint(TiXmlElement *_elem, +void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem, const std::string &_jointName); /// reduced fixed joints: check if a fixed joint should be lumped @@ -90,13 +94,13 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt); /// reduced fixed joints: apply transform reduction for ray sensors /// in extensions when doing fixed joint reduction void ReduceSDFExtensionSensorTransformReduction( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, ignition::math::Pose3d _reductionTransform); /// reduced fixed joints: apply transform reduction for projectors in /// extensions when doing fixed joint reduction void ReduceSDFExtensionProjectorTransformReduction( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, ignition::math::Pose3d _reductionTransform); @@ -117,65 +121,69 @@ void ReduceVisualsToParent(urdf::LinkSharedPtr _link); void ReduceInertialToParent(urdf::LinkSharedPtr /*_link*/); /// create SDF Collision block based on URDF -void CreateCollision(TiXmlElement* _elem, urdf::LinkConstSharedPtr _link, +void CreateCollision(tinyxml2::XMLElement* _elem, + urdf::LinkConstSharedPtr _link, urdf::CollisionSharedPtr _collision, const std::string &_oldLinkName = std::string("")); /// create SDF Visual block based on URDF -void CreateVisual(TiXmlElement *_elem, urdf::LinkConstSharedPtr _link, +void CreateVisual(tinyxml2::XMLElement *_elem, urdf::LinkConstSharedPtr _link, urdf::VisualSharedPtr _visual, const std::string &_oldLinkName = std::string("")); /// create SDF Joint block based on URDF -void CreateJoint(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, +void CreateJoint(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link, ignition::math::Pose3d &_currentTransform); /// insert extensions into links -void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName); +void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem, + const std::string &_linkName); /// create visual blocks from urdf visuals -void CreateVisuals(TiXmlElement* _elem, urdf::LinkConstSharedPtr _link); +void CreateVisuals(tinyxml2::XMLElement* _elem, urdf::LinkConstSharedPtr _link); /// create collision blocks from urdf collisions -void CreateCollisions(TiXmlElement* _elem, urdf::LinkConstSharedPtr _link); +void CreateCollisions(tinyxml2::XMLElement* _elem, + urdf::LinkConstSharedPtr _link); /// create SDF Inertial block based on URDF -void CreateInertial(TiXmlElement *_elem, urdf::LinkConstSharedPtr _link); +void CreateInertial(tinyxml2::XMLElement *_elem, + urdf::LinkConstSharedPtr _link); /// append transform (pose) to the end of the xml element -void AddTransform(TiXmlElement *_elem, +void AddTransform(tinyxml2::XMLElement *_elem, const ignition::math::Pose3d &_transform); /// create SDF from URDF link -void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, +void CreateSDF(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link, const ignition::math::Pose3d &_transform); /// create SDF Link block based on URDF -void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, +void CreateLink(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link, ignition::math::Pose3d &_currentTransform); /// reduced fixed joints: apply appropriate frame updates in joint /// inside urdf extensions when doing fixed joint reduction void ReduceSDFExtensionJointFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link); /// reduced fixed joints: apply appropriate frame updates in gripper /// inside urdf extensions when doing fixed joint reduction void ReduceSDFExtensionGripperFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link); /// reduced fixed joints: apply appropriate frame updates in projector /// inside urdf extensions when doing fixed joint reduction void ReduceSDFExtensionProjectorFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link); /// reduced fixed joints: apply appropriate frame updates in plugins /// inside urdf extensions when doing fixed joint reduction void ReduceSDFExtensionPluginFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link, const std::string &_pluginName, const std::string &_elementName, ignition::math::Pose3d _reductionTransform); @@ -183,7 +191,7 @@ void ReduceSDFExtensionPluginFrameReplace( /// reduced fixed joints: apply appropriate frame updates in urdf /// extensions when doing fixed joint reduction void ReduceSDFExtensionContactSensorFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link); /// \brief reduced fixed joints: apply appropriate updates to urdf @@ -204,13 +212,13 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge, urdf::LinkSharedPtr _link); /// get value from pair and return it as string -std::string GetKeyValueAsString(TiXmlElement* _elem); +std::string GetKeyValueAsString(tinyxml2::XMLElement* _elem); /// \brief append key value pair to the end of the xml element /// \param[in] _elem pointer to xml element /// \param[in] _key string containing key to add to xml element /// \param[in] _value string containing value for the key added -void AddKeyValue(TiXmlElement *_elem, const std::string &_key, +void AddKeyValue(tinyxml2::XMLElement *_elem, const std::string &_key, const std::string &_value); /// \brief convert values to string @@ -219,7 +227,8 @@ void AddKeyValue(TiXmlElement *_elem, const std::string &_key, /// \return a string std::string Values2str(unsigned int _count, const double *_values); -void CreateGeometry(TiXmlElement *_elem, urdf::GeometrySharedPtr _geometry); +void CreateGeometry(tinyxml2::XMLElement *_elem, + urdf::GeometrySharedPtr _geometry); ignition::math::Pose3d inverseTransformToParentFrame( ignition::math::Pose3d _transformInLinkFrame, @@ -250,13 +259,13 @@ urdf::Pose CopyPose(ignition::math::Pose3d _pose); //////////////////////////////////////////////////////////////////////////////// bool URDF2SDF::IsURDF(const std::string &_filename) { - TiXmlDocument xmlDoc; + tinyxml2::XMLDocument xmlDoc; - if (xmlDoc.LoadFile(_filename)) + if (tinyxml2::XML_SUCCESS == xmlDoc.LoadFile(_filename.c_str())) { - std::ostringstream stream; - stream << xmlDoc; - std::string urdfStr = stream.str(); + tinyxml2::XMLPrinter printer; + xmlDoc.Print(&printer); + std::string urdfStr = printer.CStr(); urdf::ModelInterfaceSharedPtr robotModel = urdf::parseURDF(urdfStr); return robotModel != nullptr; } @@ -299,11 +308,11 @@ urdf::Vector3 ParseVector3(const std::string &_str, double _scale) } ///////////////////////////////////////////////// -urdf::Vector3 ParseVector3(TiXmlNode *_key, double _scale) +urdf::Vector3 ParseVector3(tinyxml2::XMLNode *_key, double _scale) { if (_key != nullptr) { - TiXmlElement *key = _key->ToElement(); + tinyxml2::XMLElement *key = _key->ToElement(); if (key != nullptr) { return ParseVector3(GetKeyValueAsString(key), _scale); @@ -402,7 +411,7 @@ void ReduceVisualToParent(urdf::LinkSharedPtr _parentLink, //////////////////////////////////////////////////////////////////////////////// /// reduce fixed joints by lumping inertial, visual and // collision elements of the child link into the parent link -void ReduceFixedJoints(TiXmlElement *_root, urdf::LinkSharedPtr _link) +void ReduceFixedJoints(tinyxml2::XMLElement *_root, urdf::LinkSharedPtr _link) { // if child is attached to self by fixed _link first go up the tree, // check it's children recursively @@ -1125,10 +1134,10 @@ std::string Values2str(unsigned int _count, const int *_values) } //////////////////////////////////////////////////////////////////////////////// -void AddKeyValue(TiXmlElement *_elem, const std::string &_key, +void AddKeyValue(tinyxml2::XMLElement *_elem, const std::string &_key, const std::string &_value) { - TiXmlElement* childElem = _elem->FirstChildElement(_key); + tinyxml2::XMLElement *childElem = _elem->FirstChildElement(_key.c_str()); if (childElem) { std::string oldValue = GetKeyValueAsString(childElem); @@ -1145,17 +1154,19 @@ void AddKeyValue(TiXmlElement *_elem, const std::string &_key, << "> exists with [" << _value << "] due to fixed joint reduction.\n"; } - _elem->RemoveChild(childElem); // remove old _elem + _elem->DeleteChild(childElem); // remove old _elem } - TiXmlElement *ekey = new TiXmlElement(_key); - TiXmlText *textEkey = new TiXmlText(_value); + auto *doc = _elem->GetDocument(); + tinyxml2::XMLElement *ekey = doc->NewElement(_key.c_str()); + tinyxml2::XMLText *textEkey = doc->NewText(_value.c_str()); ekey->LinkEndChild(textEkey); _elem->LinkEndChild(ekey); } //////////////////////////////////////////////////////////////////////////////// -void AddTransform(TiXmlElement *_elem, const ignition::math::Pose3d &_transform) +void AddTransform(tinyxml2::XMLElement *_elem, + const ignition::math::Pose3d &_transform) { ignition::math::Vector3d e = _transform.Rot().Euler(); double cpose[6] = { _transform.Pos().X(), _transform.Pos().Y(), @@ -1166,7 +1177,7 @@ void AddTransform(TiXmlElement *_elem, const ignition::math::Pose3d &_transform) } //////////////////////////////////////////////////////////////////////////////// -std::string GetKeyValueAsString(TiXmlElement* _elem) +std::string GetKeyValueAsString(tinyxml2::XMLElement* _elem) { std::string valueStr; if (_elem->Attribute("value")) @@ -1174,20 +1185,25 @@ std::string GetKeyValueAsString(TiXmlElement* _elem) valueStr = _elem->Attribute("value"); } else if (_elem->FirstChild()) - /// @todo: FIXME: comment out check for now, different tinyxml - /// versions fails to compile: - // && _elem->FirstChild()->Type() == TiXmlNode::TINYXML_TEXT) { - valueStr = _elem->FirstChild()->ValueStr(); + // Check that this node is a XMLText + if (_elem->FirstChild()->ToText()) + { + valueStr = _elem->FirstChild()->Value(); + } + else + { + sdfwarn << "Attribute value string not set\n"; + } } - return valueStr; + return trim(valueStr); } ///////////////////////////////////////////////// -void ParseRobotOrigin(TiXmlDocument &_urdfXml) +void ParseRobotOrigin(tinyxml2::XMLDocument &_urdfXml) { - TiXmlElement *robotXml = _urdfXml.FirstChildElement("robot"); - TiXmlElement *originXml = robotXml->FirstChildElement("origin"); + tinyxml2::XMLElement *robotXml = _urdfXml.FirstChildElement("robot"); + tinyxml2::XMLElement *originXml = robotXml->FirstChildElement("origin"); if (originXml) { const char *xyzstr = originXml->Attribute("xyz"); @@ -1215,7 +1231,7 @@ void ParseRobotOrigin(TiXmlDocument &_urdfXml) } ///////////////////////////////////////////////// -void InsertRobotOrigin(TiXmlElement *_elem) +void InsertRobotOrigin(tinyxml2::XMLElement *_elem) { if (g_initialRobotPoseValid) { @@ -1230,14 +1246,14 @@ void InsertRobotOrigin(TiXmlElement *_elem) } //////////////////////////////////////////////////////////////////////////////// -void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) +void URDF2SDF::ParseSDFExtension(tinyxml2::XMLDocument &_urdfXml) { - TiXmlElement* robotXml = _urdfXml.FirstChildElement("robot"); + tinyxml2::XMLElement* robotXml = _urdfXml.FirstChildElement("robot"); // Get all SDF extension elements, put everything in // g_extensions map, containing a key string // (link/joint name) and values - for (TiXmlElement* sdfXml = robotXml->FirstChildElement("gazebo"); + for (tinyxml2::XMLElement* sdfXml = robotXml->FirstChildElement("gazebo"); sdfXml; sdfXml = sdfXml->NextSiblingElement("gazebo")) { const char* ref = sdfXml->Attribute("reference"); @@ -1264,7 +1280,7 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) SDFExtensionPtr sdf(new SDFExtension()); // begin parsing xml node - for (TiXmlElement *childElem = sdfXml->FirstChildElement(); + for (tinyxml2::XMLElement *childElem = sdfXml->FirstChildElement(); childElem; childElem = childElem->NextSiblingElement()) { sdf->oldLinkName = refStr; @@ -1275,12 +1291,12 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) // objects // material - if (childElem->ValueStr() == "material") + if (strcmp(childElem->Name(), "material") == 0) { sdf->material = GetKeyValueAsString(childElem); } - else if (childElem->ValueStr() == "collision" - || childElem->ValueStr() == "visual") + else if (strcmp(childElem->Name(), "collision") == 0 + || strcmp(childElem->Name(), "visual") == 0) { // anything inside of collision or visual tags: // @@ -1303,29 +1319,27 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) // // a place to store converted doc - for (TiXmlElement* e = childElem->FirstChildElement(); e; + for (tinyxml2::XMLElement* e = childElem->FirstChildElement(); e; e = e->NextSiblingElement()) { - TiXmlDocument xmlNewDoc; + tinyxml2::XMLPrinter printer; + e->Accept(&printer); - std::ostringstream origStream; - origStream << *e; - xmlNewDoc.Parse(origStream.str().c_str()); + XMLDocumentPtr xmlDocBlob(new tinyxml2::XMLDocument); + xmlDocBlob->Parse(printer.CStr()); // save all unknown stuff in a vector of blobs - TiXmlElementPtr blob( - new TiXmlElement(*xmlNewDoc.FirstChildElement())); - if (childElem->ValueStr() == "collision") + if (strcmp(childElem->Name(), "collision") == 0) { - sdf->collision_blobs.push_back(blob); + sdf->collision_blobs.push_back(xmlDocBlob); } else { - sdf->visual_blobs.push_back(blob); + sdf->visual_blobs.push_back(xmlDocBlob); } } } - else if (childElem->ValueStr() == "static") + else if (strcmp(childElem->Name(), "static") == 0) { std::string valueStr = GetKeyValueAsString(childElem); @@ -1340,7 +1354,7 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) sdf->setStaticFlag = false; } } - else if (childElem->ValueStr() == "turnGravityOff") + else if (strcmp(childElem->Name(), "turnGravityOff") == 0) { std::string valueStr = GetKeyValueAsString(childElem); @@ -1355,46 +1369,46 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) sdf->gravity = false; } } - else if (childElem->ValueStr() == "dampingFactor") + else if (strcmp(childElem->Name(), "dampingFactor") == 0) { sdf->isDampingFactor = true; sdf->dampingFactor = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "maxVel") + else if (strcmp(childElem->Name(), "maxVel") == 0) { sdf->isMaxVel = true; sdf->maxVel = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "minDepth") + else if (strcmp(childElem->Name(), "minDepth") == 0) { sdf->isMinDepth = true; sdf->minDepth = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "mu1") + else if (strcmp(childElem->Name(), "mu1") == 0) { sdf->isMu1 = true; sdf->mu1 = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "mu2") + else if (strcmp(childElem->Name(), "mu2") == 0) { sdf->isMu2 = true; sdf->mu2 = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "fdir1") + else if (strcmp(childElem->Name(), "fdir1") == 0) { sdf->fdir1 = GetKeyValueAsString(childElem); } - else if (childElem->ValueStr() == "kp") + else if (strcmp(childElem->Name(), "kp") == 0) { sdf->isKp = true; sdf->kp = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "kd") + else if (strcmp(childElem->Name(), "kd") == 0) { sdf->isKd = true; sdf->kd = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "selfCollide") + else if (strcmp(childElem->Name(), "selfCollide") == 0) { sdf->isSelfCollide = true; std::string valueStr = GetKeyValueAsString(childElem); @@ -1410,42 +1424,42 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) sdf->selfCollide = false; } } - else if (childElem->ValueStr() == "maxContacts") + else if (strcmp(childElem->Name(), "maxContacts") == 0) { sdf->isMaxContacts = true; sdf->maxContacts = std::stoi(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "laserRetro") + else if (strcmp(childElem->Name(), "laserRetro") == 0) { sdf->isLaserRetro = true; sdf->laserRetro = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "springReference") + else if (strcmp(childElem->Name(), "springReference") == 0) { sdf->isSpringReference = true; sdf->springReference = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "springStiffness") + else if (strcmp(childElem->Name(), "springStiffness") == 0) { sdf->isSpringStiffness = true; sdf->springStiffness = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "stopCfm") + else if (strcmp(childElem->Name(), "stopCfm") == 0) { sdf->isStopCfm = true; sdf->stopCfm = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "stopErp") + else if (strcmp(childElem->Name(), "stopErp") == 0) { sdf->isStopErp = true; sdf->stopErp = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "fudgeFactor") + else if (strcmp(childElem->Name(), "fudgeFactor") == 0) { sdf->isFudgeFactor = true; sdf->fudgeFactor = std::stod(GetKeyValueAsString(childElem)); } - else if (childElem->ValueStr() == "provideFeedback") + else if (strcmp(childElem->Name(), "provideFeedback") == 0) { sdf->isProvideFeedback = true; std::string valueStr = GetKeyValueAsString(childElem); @@ -1460,14 +1474,14 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) sdf->provideFeedback = false; } } - else if (childElem->ValueStr() == "canonicalBody") + else if (strcmp(childElem->Name(), "canonicalBody") == 0) { sdfdbg << "do nothing with canonicalBody\n"; } - else if (childElem->ValueStr() == "cfmDamping" || - childElem->ValueStr() == "implicitSpringDamper") + else if (strcmp(childElem->Name(), "cfmDamping") == 0 || + strcmp(childElem->Name(), "implicitSpringDamper") == 0) { - if (childElem->ValueStr() == "cfmDamping") + if (strcmp(childElem->Name(), "cfmDamping") == 0) { sdfwarn << "Note that cfmDamping is being deprecated by " << "implicitSpringDamper, please replace instances " @@ -1487,7 +1501,7 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) sdf->implicitSpringDamper = false; } } - else if (childElem->ValueStr() == "disableFixedJointLumping") + else if (strcmp(childElem->Name(), "disableFixedJointLumping") == 0) { std::string valueStr = GetKeyValueAsString(childElem); @@ -1497,7 +1511,7 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) g_fixedJointsTransformedInRevoluteJoints.insert(refStr); } } - else if (childElem->ValueStr() == "preserveFixedJoint") + else if (strcmp(childElem->Name(), "preserveFixedJoint") == 0) { std::string valueStr = GetKeyValueAsString(childElem); @@ -1510,17 +1524,16 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) else { // a place to store converted doc - TiXmlDocument xmlNewDoc; + XMLDocumentPtr xmlNewDoc(new tinyxml2::XMLDocument); + tinyxml2::XMLPrinter printer; + childElem->Accept(&printer); + xmlNewDoc->Parse(printer.CStr()); - std::ostringstream stream; - stream << *childElem; - sdfdbg << "extension [" << stream.str() << + sdfdbg << "extension [" << printer.CStr() << "] not converted from URDF, probably already in SDF format.\n"; - xmlNewDoc.Parse(stream.str().c_str()); // save all unknown stuff in a vector of blobs - TiXmlElementPtr blob(new TiXmlElement(*xmlNewDoc.FirstChildElement())); - sdf->blobs.push_back(blob); + sdf->blobs.push_back(xmlNewDoc); } } @@ -1542,8 +1555,27 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) } } +void CopyBlob(tinyxml2::XMLElement *_src, tinyxml2::XMLElement *_blob_parent) +{ + if (_blob_parent == nullptr) + { + sdferr << "blob parent is null\n"; + return; + } + + tinyxml2::XMLNode *clone = DeepClone(_blob_parent->GetDocument(), _src); + if (clone == nullptr) + { + sdferr << "Unable to deep copy blob\n"; + } + else + { + _blob_parent->LinkEndChild(clone); + } +} + //////////////////////////////////////////////////////////////////////////////// -void InsertSDFExtensionCollision(TiXmlElement *_elem, +void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem, const std::string &_linkName) { // loop through extensions for the whole model @@ -1561,11 +1593,11 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, // std::cerr << "working on g_extensions for link [" // << sdfIt->first << "]\n"; // if _elem already has a surface element, use it - TiXmlNode *surface = _elem->FirstChild("surface"); - TiXmlNode *friction = nullptr; - TiXmlNode *frictionOde = nullptr; - TiXmlNode *contact = nullptr; - TiXmlNode *contactOde = nullptr; + tinyxml2::XMLNode *surface = _elem->FirstChildElement("surface"); + tinyxml2::XMLNode *friction = nullptr; + tinyxml2::XMLNode *frictionOde = nullptr; + tinyxml2::XMLNode *contact = nullptr; + tinyxml2::XMLNode *contactOde = nullptr; // loop through all the gazebo extensions stored in sdfIt->second for (std::vector::iterator ge = sdfIt->second.begin(); @@ -1622,9 +1654,8 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, // explicitly specified fields (above). if (!(*ge)->collision_blobs.empty()) { - std::vector::iterator blob; - for (blob = (*ge)->collision_blobs.begin(); - blob != (*ge)->collision_blobs.end(); ++blob) + for (auto blob = (*ge)->collision_blobs.begin(); + blob != (*ge)->collision_blobs.end(); ++blob) { // find elements and assign pointers if they exist // for mu1, mu2, minDepth, maxVel, fdir1, kp, kd @@ -1632,14 +1663,7 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, // std::cerr << ">>>>> working on extension blob: [" // << (*blob)->Value() << "]\n"; - // print for debug - std::ostringstream origStream; - std::unique_ptr blobClone((*blob)->Clone()); - origStream << *blobClone; - // std::cerr << "collision extension [" - // << origStream.str() << "]\n"; - - if (strcmp((*blob)->Value(), "surface") == 0) + if (strcmp((*blob)->FirstChildElement()->Name(), "surface") == 0) { // blob is a , tread carefully otherwise // we end up with multiple copies of . @@ -1650,8 +1674,8 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, // do not exist, it simple, // just add it to the current collision // and it's done. - _elem->LinkEndChild((*blob)->Clone()); - surface = _elem->LastChild("surface"); + CopyBlob((*blob)->FirstChildElement(), _elem); + surface = _elem->LastChildElement("surface"); // std::cerr << " --- surface created " // << (void*)surface << "\n"; } @@ -1659,9 +1683,9 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, { // exist already, remove it and // overwrite with the blob. - _elem->RemoveChild(surface); - _elem->LinkEndChild((*blob)->Clone()); - surface = _elem->FirstChild("surface"); + _elem->DeleteChild(surface); + CopyBlob((*blob)->FirstChildElement(), _elem); + surface = _elem->FirstChildElement("surface"); // std::cerr << " --- surface exists, replace with blob.\n"; } @@ -1679,15 +1703,15 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, // "max_contacts" // Get contact[Ode] and friction[Ode] node pointers // if they exist. - contact = surface->FirstChild("contact"); + contact = surface->FirstChildElement("contact"); if (contact != nullptr) { - contactOde = contact->FirstChild("ode"); + contactOde = contact->FirstChildElement("ode"); } - friction = surface->FirstChild("friction"); + friction = surface->FirstChildElement("friction"); if (friction != nullptr) { - frictionOde = friction->FirstChild("ode"); + frictionOde = friction->FirstChildElement("ode"); } } else @@ -1695,7 +1719,7 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, // If the blob is not a , we don't have // to worry about backwards compatibility. // Simply add to master element. - _elem->LinkEndChild((*blob)->Clone()); + CopyBlob((*blob)->FirstChildElement(), _elem); } } } @@ -1717,9 +1741,10 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, // So there's no need for custom code for each property. // construct new elements if not in blobs + auto* doc = _elem->GetDocument(); if (surface == nullptr) { - surface = new TiXmlElement("surface"); + surface = doc->NewElement("surface"); if (!surface) { // Memory allocation error @@ -1732,9 +1757,9 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, // construct new elements if not in blobs if (contact == nullptr) { - if (surface->FirstChild("contact") == nullptr) + if (surface->FirstChildElement("contact") == nullptr) { - contact = new TiXmlElement("contact"); + contact = doc->NewElement("contact"); if (!contact) { // Memory allocation error @@ -1745,15 +1770,15 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, } else { - contact = surface->FirstChild("contact"); + contact = surface->FirstChildElement("contact"); } } if (contactOde == nullptr) { - if (contact->FirstChild("ode") == nullptr) + if (contact->FirstChildElement("ode") == nullptr) { - contactOde = new TiXmlElement("ode"); + contactOde = doc->NewElement("ode"); if (!contactOde) { // Memory allocation error @@ -1764,15 +1789,15 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, } else { - contactOde = contact->FirstChild("ode"); + contactOde = contact->FirstChildElement("ode"); } } if (friction == nullptr) { - if (surface->FirstChild("friction") == nullptr) + if (surface->FirstChildElement("friction") == nullptr) { - friction = new TiXmlElement("friction"); + friction = doc->NewElement("friction"); if (!friction) { // Memory allocation error @@ -1783,15 +1808,15 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, } else { - friction = surface->FirstChild("friction"); + friction = surface->FirstChildElement("friction"); } } if (frictionOde == nullptr) { - if (friction->FirstChild("ode") == nullptr) + if (friction->FirstChildElement("ode") == nullptr) { - frictionOde = new TiXmlElement("ode"); + frictionOde = doc->NewElement("ode"); if (!frictionOde) { // Memory allocation error @@ -1802,7 +1827,7 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, } else { - frictionOde = friction->FirstChild("ode"); + frictionOde = friction->FirstChildElement("ode"); } } @@ -1860,7 +1885,7 @@ void InsertSDFExtensionCollision(TiXmlElement *_elem, } //////////////////////////////////////////////////////////////////////////////// -void InsertSDFExtensionVisual(TiXmlElement *_elem, +void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem, const std::string &_linkName) { // loop through extensions for the whole model @@ -1878,8 +1903,8 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, // std::cerr << "working on g_extensions for link [" // << sdfIt->first << "]\n"; // if _elem already has a material element, use it - TiXmlNode *material = _elem->FirstChild("material"); - TiXmlElement *script = nullptr; + tinyxml2::XMLElement *material = _elem->FirstChildElement("material"); + tinyxml2::XMLElement *script = nullptr; // loop through all the gazebo extensions stored in sdfIt->second for (std::vector::iterator ge = sdfIt->second.begin(); @@ -1936,8 +1961,7 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, // explicitly specified fields (above). if (!(*ge)->visual_blobs.empty()) { - std::vector::iterator blob; - for (blob = (*ge)->visual_blobs.begin(); + for (auto blob = (*ge)->visual_blobs.begin(); blob != (*ge)->visual_blobs.end(); ++blob) { // find elements and assign pointers if they exist @@ -1952,7 +1976,7 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, // std::cerr << "visual extension [" // << origStream.str() << "]\n"; - if (strcmp((*blob)->Value(), "material") == 0) + if (strcmp((*blob)->FirstChildElement()->Name(), "material") == 0) { // blob is a , tread carefully otherwise // we end up with multiple copies of . @@ -1963,8 +1987,8 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, // do not exist, it simple, // just add it to the current visual // and it's done. - _elem->LinkEndChild((*blob)->Clone()); - material = _elem->LastChild("material"); + CopyBlob((*blob)->FirstChildElement(), _elem); + material = _elem->LastChildElement("material"); // std::cerr << " --- material created " // << (void*)material << "\n"; } @@ -1972,9 +1996,9 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, { // exist already, remove it and // overwrite with the blob. - _elem->RemoveChild(material); - _elem->LinkEndChild((*blob)->Clone()); - material = _elem->FirstChild("material"); + _elem->DeleteChild(material); + CopyBlob((*blob)->FirstChildElement(), _elem); + material = _elem->FirstChildElement("material"); // std::cerr << " --- material exists, replace with blob.\n"; } @@ -1993,7 +2017,7 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, // If the blob is not a , we don't have // to worry about backwards compatibility. // Simply add to master element. - _elem->LinkEndChild((*blob)->Clone()); + CopyBlob((*blob)->FirstChildElement(), _elem); } } } @@ -2014,7 +2038,7 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, // construct new elements if not in blobs if (material == nullptr) { - material = new TiXmlElement("material"); + material = _elem->GetDocument()->NewElement("material"); if (!material) { // Memory allocation error @@ -2028,7 +2052,7 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, { if (material->FirstChildElement("script") == nullptr) { - script = new TiXmlElement("script"); + script = _elem->GetDocument()->NewElement("script"); if (!script) { // Memory allocation error @@ -2039,7 +2063,7 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, } else { - script = material->FirstChildElement("script"); + script = material->FirstChildElement("script"); } } @@ -2055,7 +2079,8 @@ void InsertSDFExtensionVisual(TiXmlElement *_elem, } //////////////////////////////////////////////////////////////////////////////// -void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName) +void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem, + const std::string &_linkName) { for (StringSDFExtensionPtrMap::iterator sdfIt = g_extensions.begin(); @@ -2079,7 +2104,9 @@ void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName) } // damping factor - TiXmlElement *velocityDecay = new TiXmlElement("velocity_decay"); + + tinyxml2::XMLElement *velocityDecay = + _elem->GetDocument()->NewElement("velocity_decay"); if ((*ge)->isDampingFactor) { /// @todo separate linear and angular velocity decay @@ -2095,11 +2122,10 @@ void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName) AddKeyValue(_elem, "self_collide", (*ge)->selfCollide ? "1" : "0"); } // insert blobs into body - for (std::vector::iterator - blobIt = (*ge)->blobs.begin(); + for (auto blobIt = (*ge)->blobs.begin(); blobIt != (*ge)->blobs.end(); ++blobIt) { - _elem->LinkEndChild((*blobIt)->Clone()); + CopyBlob((*blobIt)->FirstChildElement(), _elem); } } } @@ -2107,9 +2133,10 @@ void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName) } //////////////////////////////////////////////////////////////////////////////// -void InsertSDFExtensionJoint(TiXmlElement *_elem, +void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem, const std::string &_jointName) { + auto* doc = _elem->GetDocument(); for (StringSDFExtensionPtrMap::iterator sdfIt = g_extensions.begin(); sdfIt != g_extensions.end(); ++sdfIt) @@ -2120,43 +2147,43 @@ void InsertSDFExtensionJoint(TiXmlElement *_elem, ge = sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge) { - TiXmlElement *physics = _elem->FirstChildElement("physics"); + tinyxml2::XMLElement *physics = _elem->FirstChildElement("physics"); bool newPhysics = false; if (physics == nullptr) { - physics = new TiXmlElement("physics"); + physics = doc->NewElement("physics"); newPhysics = true; } - TiXmlElement *physicsOde = physics->FirstChildElement("ode"); + tinyxml2::XMLElement *physicsOde = physics->FirstChildElement("ode"); bool newPhysicsOde = false; if (physicsOde == nullptr) { - physicsOde = new TiXmlElement("ode"); + physicsOde = doc->NewElement("ode"); newPhysicsOde = true; } - TiXmlElement *limit = physicsOde->FirstChildElement("limit"); + tinyxml2::XMLElement *limit = physicsOde->FirstChildElement("limit"); bool newLimit = false; if (limit == nullptr) { - limit = new TiXmlElement("limit"); + limit = doc->NewElement("limit"); newLimit = true; } - TiXmlElement *axis = _elem->FirstChildElement("axis"); + tinyxml2::XMLElement *axis = _elem->FirstChildElement("axis"); bool newAxis = false; if (axis == nullptr) { - axis = new TiXmlElement("axis"); + axis = doc->NewElement("axis"); newAxis = true; } - TiXmlElement *dynamics = axis->FirstChildElement("dynamics"); + tinyxml2::XMLElement *dynamics = axis->FirstChildElement("dynamics"); bool newDynamics = false; if (dynamics == nullptr) { - dynamics = new TiXmlElement("dynamics"); + dynamics = doc->NewElement("dynamics"); newDynamics = true; } @@ -2242,11 +2269,10 @@ void InsertSDFExtensionJoint(TiXmlElement *_elem, } // insert all additional blobs into joint - for (std::vector::iterator - blobIt = (*ge)->blobs.begin(); + for (auto blobIt = (*ge)->blobs.begin(); blobIt != (*ge)->blobs.end(); ++blobIt) { - _elem->LinkEndChild((*blobIt)->Clone()); + CopyBlob((*blobIt)->FirstChildElement(), _elem); } } } @@ -2254,7 +2280,7 @@ void InsertSDFExtensionJoint(TiXmlElement *_elem, } //////////////////////////////////////////////////////////////////////////////// -void InsertSDFExtensionRobot(TiXmlElement *_elem) +void InsertSDFExtensionRobot(tinyxml2::XMLElement *_elem) { for (StringSDFExtensionPtrMap::iterator sdfIt = g_extensions.begin(); @@ -2277,13 +2303,10 @@ void InsertSDFExtensionRobot(TiXmlElement *_elem) } // copy extension containing blobs and without reference - for (std::vector::iterator - blobIt = (*ge)->blobs.begin(); + for (auto blobIt = (*ge)->blobs.begin(); blobIt != (*ge)->blobs.end(); ++blobIt) { - std::ostringstream streamIn; - streamIn << *(*blobIt); - _elem->LinkEndChild((*blobIt)->Clone()); + CopyBlob((*blobIt)->FirstChildElement(), _elem); } } } @@ -2291,12 +2314,14 @@ void InsertSDFExtensionRobot(TiXmlElement *_elem) } //////////////////////////////////////////////////////////////////////////////// -void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry) +void CreateGeometry(tinyxml2::XMLElement* _elem, + urdf::GeometrySharedPtr _geometry) { - TiXmlElement *sdfGeometry = new TiXmlElement("geometry"); + auto* doc = _elem->GetDocument(); + tinyxml2::XMLElement *sdfGeometry = doc->NewElement("geometry"); std::string type; - TiXmlElement *geometryType = nullptr; + tinyxml2::XMLElement *geometryType = nullptr; switch (_geometry->type) { @@ -2310,7 +2335,7 @@ void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry) sizeVals[0] = box->dim.x; sizeVals[1] = box->dim.y; sizeVals[2] = box->dim.z; - geometryType = new TiXmlElement(type); + geometryType = doc->NewElement(type.c_str()); AddKeyValue(geometryType, "size", Values2str(sizeCount, sizeVals)); } break; @@ -2319,7 +2344,7 @@ void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry) { urdf::CylinderConstSharedPtr cylinder = urdf::dynamic_pointer_cast(_geometry); - geometryType = new TiXmlElement(type); + geometryType = doc->NewElement(type.c_str()); AddKeyValue(geometryType, "length", Values2str(1, &cylinder->length)); AddKeyValue(geometryType, "radius", Values2str(1, &cylinder->radius)); } @@ -2329,7 +2354,7 @@ void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry) { urdf::SphereConstSharedPtr sphere = urdf::dynamic_pointer_cast(_geometry); - geometryType = new TiXmlElement(type); + geometryType = doc->NewElement(type.c_str()); AddKeyValue(geometryType, "radius", Values2str(1, &sphere->radius)); } break; @@ -2338,7 +2363,7 @@ void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry) { urdf::MeshConstSharedPtr mesh = urdf::dynamic_pointer_cast(_geometry); - geometryType = new TiXmlElement(type); + geometryType = doc->NewElement(type.c_str()); AddKeyValue(geometryType, "scale", Vector32Str(mesh->scale)); // do something more to meshes { @@ -2497,7 +2522,8 @@ void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link) // find pointer to the existing extension with the new _link reference std::string parentLinkName = _link->getParent()->name; - auto parentExt = g_extensions.find(parentLinkName); + StringSDFExtensionPtrMap::iterator parentExt = + g_extensions.find(parentLinkName); // if none exist, create new extension with parentLinkName if (parentExt == g_extensions.end()) @@ -2547,16 +2573,15 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge, // and it needs to be reparented to // base_footprint_collision sdfdbg << " STRING REPLACE: instances of _link name [" - << linkName << "] with [" << parentLinkName << "]\n"; - for (std::vector::iterator blobIt = _ge->blobs.begin(); - blobIt != _ge->blobs.end(); ++blobIt) + << linkName << "] with [" << parentLinkName << "]\n"; + for (auto blobIt = _ge->blobs.begin(); + blobIt != _ge->blobs.end(); ++blobIt) { - std::ostringstream debugStreamIn; - debugStreamIn << *(*blobIt); - std::string debugBlob = debugStreamIn.str(); + tinyxml2::XMLPrinter debugStreamIn; + (*blobIt)->Print(&debugStreamIn); sdfdbg << " INITIAL STRING link [" << linkName << "]-->[" << parentLinkName << "]: [" - << debugBlob << "]\n"; + << debugStreamIn.CStr() << "]\n"; ReduceSDFExtensionContactSensorFrameReplace(blobIt, _link); ReduceSDFExtensionPluginFrameReplace(blobIt, _link, @@ -2568,17 +2593,14 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge, ReduceSDFExtensionProjectorFrameReplace(blobIt, _link); ReduceSDFExtensionGripperFrameReplace(blobIt, _link); ReduceSDFExtensionJointFrameReplace(blobIt, _link); - - std::ostringstream debugStreamOut; - debugStreamOut << *(*blobIt); } } //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge) { - for (std::vector::iterator blobIt = _ge->blobs.begin(); - blobIt != _ge->blobs.end(); ++blobIt) + for (auto blobIt = _ge->blobs.begin(); + blobIt != _ge->blobs.end(); ++blobIt) { /// @todo make sure we are not missing any additional transform reductions ReduceSDFExtensionSensorTransformReduction(blobIt, @@ -2604,13 +2626,12 @@ void URDF2SDF::ListSDFExtensions() sdfdbg << " PRINTING [" << static_cast((*ge)->blobs.size()) << "] BLOBS for extension [" << ++extCount << "] referencing [" << sdfIt->first << "]\n"; - for (std::vector::iterator - blobIt = (*ge)->blobs.begin(); + for (auto blobIt = (*ge)->blobs.begin(); blobIt != (*ge)->blobs.end(); ++blobIt) { - std::ostringstream streamIn; - streamIn << *(*blobIt); - sdfdbg << " BLOB: [" << streamIn.str() << "]\n"; + tinyxml2::XMLPrinter streamIn; + (*blobIt)->Print(&streamIn); + sdfdbg << " BLOB: [" << streamIn.CStr() << "]\n"; } } } @@ -2631,13 +2652,12 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference) for (std::vector::iterator ge = sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge) { - for (std::vector::iterator - blobIt = (*ge)->blobs.begin(); + for (auto blobIt = (*ge)->blobs.begin(); blobIt != (*ge)->blobs.end(); ++blobIt) { - std::ostringstream streamIn; - streamIn << *(*blobIt); - sdfdbg << " BLOB: [" << streamIn.str() << "]\n"; + tinyxml2::XMLPrinter streamIn; + (*blobIt)->Print(&streamIn); + sdfdbg << " BLOB: [" << streamIn.CStr() << "]\n"; } } } @@ -2645,7 +2665,7 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference) } //////////////////////////////////////////////////////////////////////////////// -void CreateSDF(TiXmlElement *_root, +void CreateSDF(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link, const ignition::math::Pose3d &_transform) { @@ -2731,22 +2751,24 @@ urdf::Pose CopyPose(ignition::math::Pose3d _pose) } //////////////////////////////////////////////////////////////////////////////// -void CreateLink(TiXmlElement *_root, +void CreateLink(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link, ignition::math::Pose3d &_currentTransform) { // create new body - TiXmlElement *elem = new TiXmlElement("link"); + tinyxml2::XMLElement *elem = _root->GetDocument()->NewElement("link"); // set body name - elem->SetAttribute("name", _link->name); + elem->SetAttribute("name", _link->name.c_str()); + // compute global transform + ignition::math::Pose3d localTransform; // this is the transform from parent link to current _link // this transform does not exist for the root link if (_link->parent_joint) { - TiXmlElement *pose = new TiXmlElement("pose"); - pose->SetAttribute("relative_to", _link->parent_joint->name); + tinyxml2::XMLElement *pose = _root->GetDocument()->NewElement("pose"); + pose->SetAttribute("relative_to", _link->parent_joint->name.c_str()); elem->LinkEndChild(pose); } else @@ -2780,7 +2802,7 @@ void CreateLink(TiXmlElement *_root, } //////////////////////////////////////////////////////////////////////////////// -void CreateCollisions(TiXmlElement* _elem, +void CreateCollisions(tinyxml2::XMLElement* _elem, urdf::LinkConstSharedPtr _link) { // loop through all collisions in @@ -2825,7 +2847,7 @@ void CreateCollisions(TiXmlElement* _elem, } //////////////////////////////////////////////////////////////////////////////// -void CreateVisuals(TiXmlElement* _elem, +void CreateVisuals(tinyxml2::XMLElement* _elem, urdf::LinkConstSharedPtr _link) { // loop through all visuals in @@ -2870,10 +2892,11 @@ void CreateVisuals(TiXmlElement* _elem, } //////////////////////////////////////////////////////////////////////////////// -void CreateInertial(TiXmlElement *_elem, +void CreateInertial(tinyxml2::XMLElement *_elem, urdf::LinkConstSharedPtr _link) { - TiXmlElement *inertial = new TiXmlElement("inertial"); + auto* doc = _elem->GetDocument(); + tinyxml2::XMLElement *inertial = doc->NewElement("inertial"); // set mass properties // check and print a warning message @@ -2889,7 +2912,7 @@ void CreateInertial(TiXmlElement *_elem, Values2str(1, &_link->inertial->mass)); // add inertia (ixx, ixy, ixz, iyy, iyz, izz) - TiXmlElement *inertia = new TiXmlElement("inertia"); + tinyxml2::XMLElement *inertia = doc->NewElement("inertia"); AddKeyValue(inertia, "ixx", Values2str(1, &_link->inertial->ixx)); AddKeyValue(inertia, "ixy", @@ -2908,7 +2931,7 @@ void CreateInertial(TiXmlElement *_elem, } //////////////////////////////////////////////////////////////////////////////// -void CreateJoint(TiXmlElement *_root, +void CreateJoint(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link, ignition::math::Pose3d &/*_currentTransform*/) { @@ -2964,16 +2987,17 @@ void CreateJoint(TiXmlElement *_root, if (!jtype.empty()) { - TiXmlElement *joint = new TiXmlElement("joint"); + auto* doc = _root->GetDocument(); + tinyxml2::XMLElement *joint = doc->NewElement("joint"); if (jtype == "fixed" && fixedJointConvertedToRevoluteJoint) { joint->SetAttribute("type", "revolute"); } else { - joint->SetAttribute("type", jtype); + joint->SetAttribute("type", jtype.c_str()); } - joint->SetAttribute("name", _link->parent_joint->name); + joint->SetAttribute("name", _link->parent_joint->name.c_str()); // Add joint pose relative to parent link AddTransform( joint, CopyPose(_link->parent_joint->parent_to_joint_origin_transform)); @@ -2983,14 +3007,14 @@ void CreateJoint(TiXmlElement *_root, { relativeToAttr = "__model__"; } - pose->SetAttribute("relative_to", relativeToAttr); + pose->SetAttribute("relative_to", relativeToAttr.c_str()); AddKeyValue(joint, "parent", _link->getParent()->name); AddKeyValue(joint, "child", _link->name); - TiXmlElement *jointAxis = new TiXmlElement("axis"); - TiXmlElement *jointAxisLimit = new TiXmlElement("limit"); - TiXmlElement *jointAxisDynamics = new TiXmlElement("dynamics"); + tinyxml2::XMLElement *jointAxis = doc->NewElement("axis"); + tinyxml2::XMLElement *jointAxisLimit = doc->NewElement("limit"); + tinyxml2::XMLElement *jointAxisDynamics = doc->NewElement("dynamics"); if (jtype == "fixed" && fixedJointConvertedToRevoluteJoint) { AddKeyValue(jointAxisLimit, "lower", "0"); @@ -3057,11 +3081,11 @@ void CreateJoint(TiXmlElement *_root, if (jtype == "fixed" && !fixedJointConvertedToRevoluteJoint) { - delete jointAxisLimit; + doc->DeleteNode(jointAxisLimit); jointAxisLimit = 0; - delete jointAxisDynamics; + doc->DeleteNode(jointAxisDynamics); jointAxisDynamics = 0; - delete jointAxis; + doc->DeleteNode(jointAxis); jointAxis = 0; } else @@ -3080,12 +3104,14 @@ void CreateJoint(TiXmlElement *_root, } //////////////////////////////////////////////////////////////////////////////// -void CreateCollision(TiXmlElement* _elem, urdf::LinkConstSharedPtr _link, +void CreateCollision(tinyxml2::XMLElement* _elem, + urdf::LinkConstSharedPtr _link, urdf::CollisionSharedPtr _collision, const std::string &_oldLinkName) { + auto* doc = _elem->GetDocument(); // begin create geometry node, skip if no collision specified - TiXmlElement *sdfCollision = new TiXmlElement("collision"); + tinyxml2::XMLElement *sdfCollision = doc->NewElement("collision"); // std::cerr << "CreateCollision link [" << _link->name // << "] old [" << _oldLinkName @@ -3096,12 +3122,12 @@ void CreateCollision(TiXmlElement* _elem, urdf::LinkConstSharedPtr _link, if (_oldLinkName.compare(0, _link->name.size(), _link->name) == 0 || _oldLinkName.empty()) { - sdfCollision->SetAttribute("name", _oldLinkName); + sdfCollision->SetAttribute("name", _oldLinkName.c_str()); } else { - sdfCollision->SetAttribute("name", _link->name - + g_lumpPrefix + _oldLinkName); + sdfCollision->SetAttribute("name", (_link->name + + g_lumpPrefix + _oldLinkName).c_str()); } // std::cerr << "collision [" << sdfCollision->Attribute("name") << "]\n"; @@ -3133,21 +3159,23 @@ void CreateCollision(TiXmlElement* _elem, urdf::LinkConstSharedPtr _link, } //////////////////////////////////////////////////////////////////////////////// -void CreateVisual(TiXmlElement *_elem, urdf::LinkConstSharedPtr _link, +void CreateVisual(tinyxml2::XMLElement *_elem, urdf::LinkConstSharedPtr _link, urdf::VisualSharedPtr _visual, const std::string &_oldLinkName) { + auto* doc = _elem->GetDocument(); // begin create sdf visual node - TiXmlElement *sdfVisual = new TiXmlElement("visual"); + tinyxml2::XMLElement *sdfVisual = doc->NewElement("visual"); // set its name if (_oldLinkName.compare(0, _link->name.size(), _link->name) == 0 || _oldLinkName.empty()) { - sdfVisual->SetAttribute("name", _oldLinkName); + sdfVisual->SetAttribute("name", _oldLinkName.c_str()); } else { - sdfVisual->SetAttribute("name", _link->name + g_lumpPrefix + _oldLinkName); + sdfVisual->SetAttribute("name", + (_link->name + g_lumpPrefix + _oldLinkName).c_str()); } // add the visualisation transfrom @@ -3177,30 +3205,38 @@ void CreateVisual(TiXmlElement *_elem, urdf::LinkConstSharedPtr _link, } //////////////////////////////////////////////////////////////////////////////// -TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr, - bool _enforceLimits) +void URDF2SDF::InitModelString(const std::string &_urdfStr, + tinyxml2::XMLDocument* _sdfXmlOut, + bool _enforceLimits) { g_enforceLimits = _enforceLimits; // Create a RobotModel from string urdf::ModelInterfaceSharedPtr robotModel = urdf::parseURDF(_urdfStr); - // an xml object to hold the xml result - TiXmlDocument sdfXmlOut; - if (!robotModel) { sdferr << "Unable to call parseURDF on robot model\n"; - return sdfXmlOut; + return; } + // create root element and define needed namespaces + tinyxml2::XMLElement *robot = _sdfXmlOut->NewElement("model"); + + // set model name to urdf robot name if not specified + robot->SetAttribute("name", robotModel->getName().c_str()); + // initialize transform for the model, urdf is recursive, // while sdf defines all links relative to model frame ignition::math::Pose3d transform; // parse sdf extension - TiXmlDocument urdfXml; - urdfXml.Parse(_urdfStr.c_str()); + tinyxml2::XMLDocument urdfXml; + if (urdfXml.Parse(_urdfStr.c_str())) + { + sdferr << "Unable to parse URDF string: " << urdfXml.ErrorStr() << "\n"; + return; + } g_extensions.clear(); g_fixedJointsTransformedInFixedJoints.clear(); g_fixedJointsTransformedInRevoluteJoints.clear(); @@ -3210,16 +3246,12 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr, ParseRobotOrigin(urdfXml); urdf::LinkConstSharedPtr rootLink = robotModel->getRoot(); - - // create root element and define needed namespaces - TiXmlElement *robot = new TiXmlElement("model"); - - TiXmlElement *sdf; + tinyxml2::XMLElement *sdf; try { // set model name to urdf robot name if not specified - robot->SetAttribute("name", robotModel->getName()); + robot->SetAttribute("name", robotModel->getName().c_str()); // Fixed Joint Reduction // if link connects to parent via fixed joint, lump down and remove link @@ -3255,7 +3287,7 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr, InsertRobotOrigin(robot); // Create new sdf - sdf = new TiXmlElement("sdf"); + sdf = _sdfXmlOut->NewElement("sdf"); try { @@ -3267,44 +3299,41 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr, } catch(...) { - delete sdf; throw; } } catch(...) { - delete robot; throw; } - sdfXmlOut.LinkEndChild(sdf); - - return sdfXmlOut; + _sdfXmlOut->LinkEndChild(sdf); } //////////////////////////////////////////////////////////////////////////////// -TiXmlDocument URDF2SDF::InitModelDoc(TiXmlDocument* _xmlDoc) +void URDF2SDF::InitModelDoc(const tinyxml2::XMLDocument *_xmlDoc, + tinyxml2::XMLDocument *_sdfXmlDoc) { - std::ostringstream stream; - stream << *_xmlDoc; - std::string urdfStr = stream.str(); - return InitModelString(urdfStr); + tinyxml2::XMLPrinter printer; + _xmlDoc->Print(&printer); + std::string urdfStr = printer.CStr(); + InitModelString(urdfStr, _sdfXmlDoc); } //////////////////////////////////////////////////////////////////////////////// -TiXmlDocument URDF2SDF::InitModelFile(const std::string &_filename) +void URDF2SDF::InitModelFile(const std::string &_filename, + tinyxml2::XMLDocument *_sdfXmlDoc) { - TiXmlDocument xmlDoc; - if (xmlDoc.LoadFile(_filename)) + tinyxml2::XMLDocument xmlDoc; + if (!xmlDoc.LoadFile(_filename.c_str())) { - return this->InitModelDoc(&xmlDoc); + this->InitModelDoc(&xmlDoc, _sdfXmlDoc); } else { - sdferr << "Unable to load file[" << _filename << "].\n"; + sdferr << "Unable to load file[" + << _filename << "]:" << xmlDoc.ErrorStr() << "\n"; } - - return xmlDoc; } //////////////////////////////////////////////////////////////////////////////// @@ -3322,31 +3351,31 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt) //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionSensorTransformReduction( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, ignition::math::Pose3d _reductionTransform) { // overwrite and if they exist - if ((*_blobIt)->ValueStr() == "sensor") + if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "sensor") == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform // debug print - // for (TiXmlNode* elIt = (*_blobIt)->FirstChild(); + // for (tinyxml2::XMLNode* elIt = (*_blobIt)->FirstChild(); // elIt; elIt = elIt->NextSibling()) // { - // std::ostringstream streamIn; - // streamIn << *elIt; - // sdfdbg << " " << streamIn << "\n"; + // tinyxml2::XMLPrinter streamIn; + // elIt->Accept(&streamIn); + // sdfdbg << " " << streamIn.CStr() << "\n"; // } { - TiXmlNode* oldPoseKey = (*_blobIt)->FirstChild("pose"); + tinyxml2::XMLNode *oldPoseKey = (*_blobIt)->FirstChildElement("pose"); /// @todo: FIXME: we should read xyz, rpy and aggregate it to /// reductionTransform instead of just throwing the info away. if (oldPoseKey) { - (*_blobIt)->RemoveChild(oldPoseKey); + (*_blobIt)->DeleteChild(oldPoseKey); } } @@ -3367,9 +3396,11 @@ void ReduceSDFExtensionSensorTransformReduction( poseStream << reductionXyz.x << " " << reductionXyz.y << " " << reductionXyz.z << " " << reductionRpy.x << " " << reductionRpy.y << " " << reductionRpy.z; - TiXmlText* poseTxt = new TiXmlText(poseStream.str()); - TiXmlElement* poseKey = new TiXmlElement("pose"); + auto* doc = (*_blobIt)->GetDocument(); + tinyxml2::XMLText *poseTxt = doc->NewText(poseStream.str().c_str()); + tinyxml2::XMLElement *poseKey = doc->NewElement("pose"); + poseKey->LinkEndChild(poseTxt); (*_blobIt)->LinkEndChild(poseKey); @@ -3378,16 +3409,16 @@ void ReduceSDFExtensionSensorTransformReduction( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionProjectorTransformReduction( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, ignition::math::Pose3d _reductionTransform) { // overwrite (xyz/rpy) if it exists - if ((*_blobIt)->ValueStr() == "projector") + if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "projector") == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform // - // for (TiXmlNode* elIt = (*_blobIt)->FirstChild(); + // for (tinyxml2::XMLNode* elIt = (*_blobIt)->FirstChildElement(); // elIt; elIt = elIt->NextSibling()) // { // std::ostringstream streamIn; @@ -3396,13 +3427,13 @@ void ReduceSDFExtensionProjectorTransformReduction( // } // should read ... and agregate reductionTransform - TiXmlNode* poseKey = (*_blobIt)->FirstChild("pose"); + tinyxml2::XMLNode *poseKey = (*_blobIt)->FirstChildElement("pose"); // read pose and save it // remove the tag for now if (poseKey) { - (*_blobIt)->RemoveChild(poseKey); + (*_blobIt)->DeleteChild(poseKey); } // convert reductionTransform to values @@ -3422,9 +3453,10 @@ void ReduceSDFExtensionProjectorTransformReduction( poseStream << reductionXyz.x << " " << reductionXyz.y << " " << reductionXyz.z << " " << reductionRpy.x << " " << reductionRpy.y << " " << reductionRpy.z; - TiXmlText* poseTxt = new TiXmlText(poseStream.str()); - poseKey = new TiXmlElement("pose"); + auto* doc = (*_blobIt)->GetDocument(); + tinyxml2::XMLText *poseTxt = doc->NewText(poseStream.str().c_str()); + poseKey = doc->NewElement("pose"); poseKey->LinkEndChild(poseTxt); (*_blobIt)->LinkEndChild(poseKey); @@ -3433,31 +3465,33 @@ void ReduceSDFExtensionProjectorTransformReduction( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionContactSensorFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; - if ((*_blobIt)->ValueStr() == "sensor") + if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "sensor") == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform - TiXmlNode* contact = (*_blobIt)->FirstChild("contact"); + tinyxml2::XMLNode *contact = (*_blobIt)->FirstChildElement("contact"); if (contact) { - TiXmlNode* collision = contact->FirstChild("collision"); + tinyxml2::XMLNode *collision = contact->FirstChildElement("collision"); if (collision) { if (GetKeyValueAsString(collision->ToElement()) == linkName + g_collisionExt) { - contact->RemoveChild(collision); - TiXmlElement* collisionNameKey = new TiXmlElement("collision"); + contact->DeleteChild(collision); + + auto* doc = contact->GetDocument(); + tinyxml2::XMLElement *collisionNameKey = doc->NewElement("collision"); std::ostringstream collisionNameStream; collisionNameStream << parentLinkName << g_collisionExt << "_" << linkName; - TiXmlText* collisionNameTxt = new TiXmlText( - collisionNameStream.str()); + tinyxml2::XMLText *collisionNameTxt = doc->NewText( + collisionNameStream.str().c_str()); collisionNameKey->LinkEndChild(collisionNameTxt); contact->LinkEndChild(collisionNameKey); } @@ -3471,49 +3505,53 @@ void ReduceSDFExtensionContactSensorFrameReplace( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionPluginFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link, const std::string &_pluginName, const std::string &_elementName, ignition::math::Pose3d _reductionTransform) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; - if ((*_blobIt)->ValueStr() == _pluginName) + if ((*_blobIt)->FirstChildElement()->Name() == _pluginName) { // replace element containing _link names to parent link names // find first instance of xyz and rpy, replace with reduction transform - TiXmlNode* elementNode = (*_blobIt)->FirstChild(_elementName); + tinyxml2::XMLNode *elementNode = + (*_blobIt)->FirstChildElement(_elementName.c_str()); if (elementNode) { if (GetKeyValueAsString(elementNode->ToElement()) == linkName) { - (*_blobIt)->RemoveChild(elementNode); - TiXmlElement* bodyNameKey = new TiXmlElement(_elementName); + (*_blobIt)->DeleteChild(elementNode); + auto* doc = elementNode->GetDocument(); + tinyxml2::XMLElement *bodyNameKey = + doc->NewElement(_elementName.c_str()); std::ostringstream bodyNameStream; bodyNameStream << parentLinkName; - TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str()); + tinyxml2::XMLText *bodyNameTxt = + doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); (*_blobIt)->LinkEndChild(bodyNameKey); /// @todo update transforms for this sdf plugin too // look for offset transforms, add reduction transform - TiXmlNode* xyzKey = (*_blobIt)->FirstChild("xyzOffset"); + tinyxml2::XMLNode *xyzKey = (*_blobIt)->FirstChildElement("xyzOffset"); if (xyzKey) { urdf::Vector3 v1 = ParseVector3(xyzKey); _reductionTransform.Pos() = ignition::math::Vector3d(v1.x, v1.y, v1.z); // remove xyzOffset and rpyOffset - (*_blobIt)->RemoveChild(xyzKey); + (*_blobIt)->DeleteChild(xyzKey); } - TiXmlNode* rpyKey = (*_blobIt)->FirstChild("rpyOffset"); + tinyxml2::XMLNode *rpyKey = (*_blobIt)->FirstChildElement("rpyOffset"); if (rpyKey) { urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0); _reductionTransform.Rot() = ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z); // remove xyzOffset and rpyOffset - (*_blobIt)->RemoveChild(rpyKey); + (*_blobIt)->DeleteChild(rpyKey); } // pass through the parent transform from fixed joint reduction @@ -3521,8 +3559,8 @@ void ReduceSDFExtensionPluginFrameReplace( _link->parent_joint->parent_to_joint_origin_transform); // create new offset xml blocks - xyzKey = new TiXmlElement("xyzOffset"); - rpyKey = new TiXmlElement("rpyOffset"); + xyzKey = doc->NewElement("xyzOffset"); + rpyKey = doc->NewElement("rpyOffset"); // create new offset xml blocks urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), @@ -3541,8 +3579,8 @@ void ReduceSDFExtensionPluginFrameReplace( rpyStream << reductionRpy.x << " " << reductionRpy.y << " " << reductionRpy.z; - TiXmlText* xyzTxt = new TiXmlText(xyzStream.str()); - TiXmlText* rpyTxt = new TiXmlText(rpyStream.str()); + tinyxml2::XMLText *xyzTxt = doc->NewText(xyzStream.str().c_str()); + tinyxml2::XMLText *rpyTxt = doc->NewText(rpyStream.str().c_str()); xyzKey->LinkEndChild(xyzTxt); rpyKey->LinkEndChild(rpyTxt); @@ -3556,7 +3594,7 @@ void ReduceSDFExtensionPluginFrameReplace( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionProjectorFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link) { std::string linkName = _link->name; @@ -3566,7 +3604,7 @@ void ReduceSDFExtensionProjectorFrameReplace( // projector plugins // update from MyLinkName/MyProjectorName // to NewLinkName/MyProjectorName - TiXmlNode* projectorElem = (*_blobIt)->FirstChild("projector"); + tinyxml2::XMLNode *projectorElem = (*_blobIt)->FirstChildElement("projector"); { if (projectorElem) { @@ -3589,11 +3627,13 @@ void ReduceSDFExtensionProjectorFrameReplace( projectorName = parentLinkName + "/" + projectorName.substr(pos+1, projectorName.size()); - (*_blobIt)->RemoveChild(projectorElem); - TiXmlElement *bodyNameKey = new TiXmlElement("projector"); + (*_blobIt)->DeleteChild(projectorElem); + auto* doc = projectorElem->GetDocument(); + tinyxml2::XMLElement *bodyNameKey = doc->NewElement("projector"); std::ostringstream bodyNameStream; bodyNameStream << projectorName; - TiXmlText *bodyNameTxt = new TiXmlText(bodyNameStream.str()); + tinyxml2::XMLText *bodyNameTxt = + doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); (*_blobIt)->LinkEndChild(bodyNameKey); } @@ -3604,38 +3644,44 @@ void ReduceSDFExtensionProjectorFrameReplace( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionGripperFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; - if ((*_blobIt)->ValueStr() == "gripper") + if (strcmp((*_blobIt)->FirstChildElement()->Name(), "gripper") == 0) { - TiXmlNode* gripperLink = (*_blobIt)->FirstChild("gripper_link"); + tinyxml2::XMLNode *gripperLink = + (*_blobIt)->FirstChildElement("gripper_link"); if (gripperLink) { if (GetKeyValueAsString(gripperLink->ToElement()) == linkName) { - (*_blobIt)->RemoveChild(gripperLink); - TiXmlElement* bodyNameKey = new TiXmlElement("gripper_link"); + (*_blobIt)->DeleteChild(gripperLink); + auto* doc = (*_blobIt)->GetDocument(); + tinyxml2::XMLElement *bodyNameKey = doc->NewElement("gripper_link"); std::ostringstream bodyNameStream; bodyNameStream << parentLinkName; - TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str()); + tinyxml2::XMLText *bodyNameTxt = + doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); (*_blobIt)->LinkEndChild(bodyNameKey); } } - TiXmlNode* palmLink = (*_blobIt)->FirstChild("palm_link"); + tinyxml2::XMLNode *palmLink = (*_blobIt)->FirstChildElement("palm_link"); if (palmLink) { if (GetKeyValueAsString(palmLink->ToElement()) == linkName) { - (*_blobIt)->RemoveChild(palmLink); - TiXmlElement* bodyNameKey = new TiXmlElement("palm_link"); + (*_blobIt)->DeleteChild(palmLink); + auto* doc = (*_blobIt)->GetDocument(); + tinyxml2::XMLElement *bodyNameKey = + doc->NewElement("palm_link"); std::ostringstream bodyNameStream; bodyNameStream << parentLinkName; - TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str()); + tinyxml2::XMLText *bodyNameTxt = + doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); (*_blobIt)->LinkEndChild(bodyNameKey); } @@ -3645,40 +3691,43 @@ void ReduceSDFExtensionGripperFrameReplace( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionJointFrameReplace( - std::vector::iterator _blobIt, + std::vector::iterator _blobIt, urdf::LinkSharedPtr _link) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; + auto* doc = (*_blobIt)->GetDocument(); - if ((*_blobIt)->ValueStr() == "joint") + if (strcmp((*_blobIt)->FirstChildElement()->Name(), "joint") == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform - TiXmlNode* parent = (*_blobIt)->FirstChild("parent"); + tinyxml2::XMLNode *parent = (*_blobIt)->FirstChildElement("parent"); if (parent) { if (GetKeyValueAsString(parent->ToElement()) == linkName) { - (*_blobIt)->RemoveChild(parent); - TiXmlElement* parentNameKey = new TiXmlElement("parent"); + (*_blobIt)->DeleteChild(parent); + tinyxml2::XMLElement *parentNameKey = doc->NewElement("parent"); std::ostringstream parentNameStream; parentNameStream << parentLinkName; - TiXmlText* parentNameTxt = new TiXmlText(parentNameStream.str()); + tinyxml2::XMLText *parentNameTxt = + doc->NewText(parentNameStream.str().c_str()); parentNameKey->LinkEndChild(parentNameTxt); (*_blobIt)->LinkEndChild(parentNameKey); } } - TiXmlNode* child = (*_blobIt)->FirstChild("child"); + tinyxml2::XMLNode *child = (*_blobIt)->FirstChildElement("child"); if (child) { if (GetKeyValueAsString(child->ToElement()) == linkName) { - (*_blobIt)->RemoveChild(child); - TiXmlElement* childNameKey = new TiXmlElement("child"); + (*_blobIt)->DeleteChild(child); + tinyxml2::XMLElement *childNameKey = doc->NewElement("child"); std::ostringstream childNameStream; childNameStream << parentLinkName; - TiXmlText* childNameTxt = new TiXmlText(childNameStream.str()); + tinyxml2::XMLText *childNameTxt = + doc->NewText(childNameStream.str().c_str()); childNameKey->LinkEndChild(childNameTxt); (*_blobIt)->LinkEndChild(childNameKey); } diff --git a/src/parser_urdf.hh b/src/parser_urdf.hh index 94dca1f07..8f703169c 100644 --- a/src/parser_urdf.hh +++ b/src/parser_urdf.hh @@ -17,7 +17,7 @@ #ifndef SDFORMAT_URDF2SDF_HH_ #define SDFORMAT_URDF2SDF_HH_ -#include +#include #include #include @@ -46,22 +46,25 @@ namespace sdf public: ~URDF2SDF(); /// \brief convert urdf xml document string to sdf xml document - /// \param[in] _xmlDoc a tinyxml document containing the urdf model - /// \return a tinyxml document containing sdf of the model - public: TiXmlDocument InitModelDoc(TiXmlDocument* _xmlDoc); + /// \param[in] _xmlDoc document containing the urdf model. + /// \param[inout] _sdfXmlDoc document to populate with the sdf model. + public: void InitModelDoc(const tinyxml2::XMLDocument* _xmlDoc, + tinyxml2::XMLDocument *_sdfXmlDoc); /// \brief convert urdf file to sdf xml document - /// \param[in] _urdfStr a string containing filename of the urdf model - /// \return a tinyxml document containing sdf of the model - public: TiXmlDocument InitModelFile(const std::string &_filename); + /// \param[in] _urdfStr a string containing filename of the urdf model. + /// \param[inout] _sdfXmlDoc document to populate with the sdf model. + public: void InitModelFile(const std::string &_filename, + tinyxml2::XMLDocument *_sdfXmlDoc); /// \brief convert urdf string to sdf xml document, with option to enforce /// limits. /// \param[in] _urdfStr a string containing model urdf + /// \param[inout] _sdfXmlDoc document to populate with the sdf model. /// \param[in] _enforceLimits option to enforce joint limits - /// \return a tinyxml document containing sdf of the model - public: TiXmlDocument InitModelString(const std::string &_urdfStr, - bool _enforceLimits = true); + public: void InitModelString(const std::string &_urdfStr, + tinyxml2::XMLDocument *_sdfXmlDoc, + bool _enforceLimits = true); /// \brief Return true if the filename is a URDF model. /// \param[in] _filename File to check. @@ -76,7 +79,7 @@ namespace sdf /// things that do not belong in urdf but should be mapped into sdf /// @todo: do this using sdf definitions, not hard coded stuff - private: void ParseSDFExtension(TiXmlDocument &_urdfXml); + private: void ParseSDFExtension(tinyxml2::XMLDocument &_urdfXml); }; } } diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index 19e194638..cf65e1845 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -36,10 +36,11 @@ std::string getMinimalUrdfTxt() std::string convertUrdfStrToSdfStr(const std::string &_urdf) { sdf::URDF2SDF parser_; - TiXmlDocument sdf_result = parser_.InitModelString(_urdf); - TiXmlPrinter printer; + tinyxml2::XMLDocument sdf_result; + parser_.InitModelString(_urdf, &sdf_result); + tinyxml2::XMLPrinter printer; sdf_result.Accept(&printer); - return printer.Str(); + return printer.CStr(); } ///////////////////////////////////////////////// @@ -54,9 +55,10 @@ TEST(URDFParser, InitModelDoc_EmptyDoc_NoThrow) { // Suppress deprecation for sdf::URDF2SDF ASSERT_NO_THROW( - TiXmlDocument doc = TiXmlDocument(); + tinyxml2::XMLDocument doc = tinyxml2::XMLDocument(); sdf::URDF2SDF parser_; - TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + tinyxml2::XMLDocument sdf_result; + parser_.InitModelDoc(&doc, &sdf_result); ); // NOLINT(whitespace/parens) } @@ -65,10 +67,11 @@ TEST(URDFParser, InitModelDoc_BasicModel_NoThrow) { // Suppress deprecation for sdf::URDF2SDF ASSERT_NO_THROW( - TiXmlDocument doc; + tinyxml2::XMLDocument doc; doc.Parse(getMinimalUrdfTxt().c_str()); sdf::URDF2SDF parser_; - TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + tinyxml2::XMLDocument sdf_result; + parser_.InitModelDoc(&doc, &sdf_result); ); // NOLINT(whitespace/parens) } @@ -76,12 +79,16 @@ TEST(URDFParser, InitModelDoc_BasicModel_NoThrow) TEST(URDFParser, ParseResults_BasicModel_ParseEqualToModel) { // URDF -> SDF - TiXmlDocument doc; + tinyxml2::XMLDocument doc; doc.Parse(getMinimalUrdfTxt().c_str()); sdf::URDF2SDF parser_; - TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); - std::string sdf_result_str; - sdf_result_str << sdf_result; + + tinyxml2::XMLDocument sdf_result; + parser_.InitModelDoc(&doc, &sdf_result); + + tinyxml2::XMLPrinter printer; + sdf_result.Print(&printer); + std::string sdf_result_str = printer.CStr(); // SDF -> SDF std::ostringstream stream; @@ -89,10 +96,12 @@ TEST(URDFParser, ParseResults_BasicModel_ParseEqualToModel) stream << "" << " " << ""; - TiXmlDocument sdf_doc; + tinyxml2::XMLDocument sdf_doc; sdf_doc.Parse(stream.str().c_str()); - std::string sdf_same_result_str; - sdf_same_result_str << sdf_doc; + + tinyxml2::XMLPrinter printer2; + sdf_doc.Print(&printer2); + std::string sdf_same_result_str = printer2.CStr(); ASSERT_EQ(sdf_same_result_str, sdf_result_str); } @@ -105,15 +114,16 @@ TEST(URDFParser, ParseRobotOriginXYZBlank) << " " << " " << ""; - TiXmlDocument doc; + tinyxml2::XMLDocument doc; doc.Parse(stream.str().c_str()); sdf::URDF2SDF parser_; - TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); - TiXmlElement *sdf = sdf_result.FirstChildElement("sdf"); + tinyxml2::XMLDocument sdf_result; + parser_.InitModelDoc(&doc, &sdf_result); + tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdf); - TiXmlElement *model = sdf->FirstChildElement("model"); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); ASSERT_NE(nullptr, model); - TiXmlElement *pose = model->FirstChildElement("pose"); + tinyxml2::XMLElement *pose = model->FirstChildElement("pose"); ASSERT_NE(nullptr, pose); } @@ -125,15 +135,16 @@ TEST(URDFParser, ParseRobotOriginRPYBlank) << " " << " " << ""; - TiXmlDocument doc; + tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); - TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); - TiXmlElement *sdf = sdf_result.FirstChildElement("sdf"); + tinyxml2::XMLDocument sdf_result; + parser_.InitModelDoc(&doc, &sdf_result); + tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdf); - TiXmlElement *model = sdf->FirstChildElement("model"); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); ASSERT_NE(nullptr, model); - TiXmlElement *pose = model->FirstChildElement("pose"); + tinyxml2::XMLElement *pose = model->FirstChildElement("pose"); ASSERT_NE(nullptr, pose); } @@ -158,10 +169,11 @@ TEST(URDFParser, ParseRobotMaterialBlank) << " 0.2" << " " << ""; - TiXmlDocument doc; + tinyxml2::XMLDocument doc; doc.Parse(stream.str().c_str()); sdf::URDF2SDF parser; - auto sdfXml = parser.InitModelDoc(&doc); + tinyxml2::XMLDocument sdfXml; + parser.InitModelDoc(&doc, &sdfXml); auto sdfElem = sdfXml.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdfElem); auto modelElem = sdfElem->FirstChildElement("model"); @@ -199,10 +211,13 @@ TEST(URDFParser, ParseRobotMaterialName) << " Gazebo/Orange" << " " << ""; - TiXmlDocument doc; + tinyxml2::XMLDocument doc; doc.Parse(stream.str().c_str()); sdf::URDF2SDF parser; - auto sdfXml = parser.InitModelDoc(&doc); + + tinyxml2::XMLDocument sdfXml; + parser.InitModelDoc(&doc, &sdfXml); + auto sdfElem = sdfXml.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdfElem); auto modelElem = sdfElem->FirstChildElement("model"); @@ -236,15 +251,16 @@ TEST(URDFParser, ParseRobotOriginInvalidXYZ) << " " << " " << ""; - TiXmlDocument doc; + tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); - TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); - TiXmlElement *sdf = sdf_result.FirstChildElement("sdf"); + tinyxml2::XMLDocument sdf_result; + parser_.InitModelDoc(&doc, &sdf_result); + tinyxml2::XMLElement *sdf = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, sdf); - TiXmlElement *model = sdf->FirstChildElement("model"); + tinyxml2::XMLElement *model = sdf->FirstChildElement("model"); ASSERT_NE(nullptr, model); - TiXmlElement *pose = model->FirstChildElement("pose"); + tinyxml2::XMLElement *pose = model->FirstChildElement("pose"); ASSERT_NE(nullptr, pose); } @@ -297,25 +313,25 @@ TEST(URDFParser, ParseGazeboLinkFactors) << " " << ""; - TiXmlDocument doc; + tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); - TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + tinyxml2::XMLDocument sdf_result; + parser_.InitModelDoc(&doc, &sdf_result); - TiXmlElement *tmp = sdf_result.FirstChildElement("sdf"); + tinyxml2::XMLElement *tmp = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, tmp); unsigned int i; for (i = 0; i < it->second.size() - 1; ++i) { - tmp = tmp->FirstChildElement(it->second[i]); + tmp = tmp->FirstChildElement(it->second[i].c_str()); ASSERT_NE(nullptr, tmp); } // For the last element, check that it is exactly what we expect - EXPECT_EQ(tmp->FirstChild()->ValueStr(), it->second[i]); - + EXPECT_EQ(tmp->FirstChild()->Value(), it->second[i]); parser_.ListSDFExtensions(); parser_.ListSDFExtensions("wheel_left_link"); } @@ -336,10 +352,11 @@ TEST(URDFParser, ParseGazeboInvalidDampingFactor) << " foo" << " " << ""; - TiXmlDocument doc; + tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); - ASSERT_THROW(TiXmlDocument sdf_result = parser_.InitModelDoc(&doc), + tinyxml2::XMLDocument sdf_result; + ASSERT_THROW(parser_.InitModelDoc(&doc, &sdf_result), std::invalid_argument); parser_.ListSDFExtensions(); @@ -408,24 +425,25 @@ TEST(URDFParser, ParseGazeboJointElements) << " " << ""; - TiXmlDocument doc; + tinyxml2::XMLDocument doc; sdf::URDF2SDF parser_; doc.Parse(stream.str().c_str()); - TiXmlDocument sdf_result = parser_.InitModelDoc(&doc); + tinyxml2::XMLDocument sdf_result; + parser_.InitModelDoc(&doc, &sdf_result); - TiXmlElement *tmp = sdf_result.FirstChildElement("sdf"); + tinyxml2::XMLElement *tmp = sdf_result.FirstChildElement("sdf"); ASSERT_NE(nullptr, tmp); unsigned int i; for (i = 0; i < it->second.size() - 1; ++i) { - tmp = tmp->FirstChildElement(it->second[i]); + tmp = tmp->FirstChildElement(it->second[i].c_str()); ASSERT_NE(nullptr, tmp); } // For the last element, check that it is exactly what we expect - EXPECT_EQ(tmp->FirstChild()->ValueStr(), it->second[i]); + EXPECT_STREQ(tmp->FirstChild()->Value(), it->second[i].c_str()); parser_.ListSDFExtensions(); parser_.ListSDFExtensions("head_j0"); @@ -713,7 +731,7 @@ TEST(URDFParser, CheckJointTransform) link1 - + 0 0 0 0 0 0 1 @@ -733,12 +751,12 @@ TEST(URDFParser, CheckJointTransform) link2 1 0 0 - - + + - + 0 0 0 0 0 0 1 @@ -758,12 +776,12 @@ TEST(URDFParser, CheckJointTransform) link3 1 0 0 - - + + - + 0 0 0 0 0 0 1 @@ -799,19 +817,20 @@ TEST(URDFParser, OutputPrecision) )"; sdf::URDF2SDF parser; - TiXmlDocument sdfResult = parser.InitModelString(str); + tinyxml2::XMLDocument sdfResult; + parser.InitModelString(str, &sdfResult); auto root = sdfResult.RootElement(); - auto model = root->FirstChild("model"); + auto model = root->FirstChildElement("model"); ASSERT_NE(nullptr, model); - auto link = model->FirstChild("link"); + auto link = model->FirstChildElement("link"); ASSERT_NE(nullptr, link); - auto inertial = link->FirstChild("inertial"); + auto inertial = link->FirstChildElement("inertial"); ASSERT_NE(nullptr, inertial); - auto pose = inertial->FirstChild("pose"); + auto pose = inertial->FirstChildElement("pose"); ASSERT_NE(nullptr, pose); ASSERT_NE(nullptr, pose->FirstChild()); - std::string poseTxt = pose->FirstChild()->ValueStr(); + std::string poseTxt = pose->FirstChild()->Value(); EXPECT_FALSE(poseTxt.empty()); std::string poseValues[6]; @@ -833,6 +852,82 @@ TEST(URDFParser, OutputPrecision) EXPECT_EQ("0", poseValues[5]); } +///////////////////////////////////////////////// +TEST(URDFParser, ParseWhitespace) +{ + std::string str = R"( + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + + + 100 + + + + 1000 + + + +)"; + tinyxml2::XMLDocument doc; + doc.Parse(str.c_str()); + + sdf::URDF2SDF parser; + tinyxml2::XMLDocument sdfXml; + parser.InitModelDoc(&doc, &sdfXml); + + auto root = sdfXml.RootElement(); + ASSERT_NE(nullptr, root); + auto modelElem = root->FirstChildElement("model"); + ASSERT_NE(nullptr, modelElem); + auto linkElem = modelElem->FirstChildElement("link"); + ASSERT_NE(nullptr, linkElem); + auto visualElem = linkElem->FirstChildElement("visual"); + ASSERT_NE(nullptr, visualElem); + auto collisionElem = linkElem->FirstChildElement("collision"); + ASSERT_NE(nullptr, collisionElem); + + auto materialElem = visualElem->FirstChildElement("material"); + ASSERT_NE(nullptr, materialElem); + auto scriptElem = materialElem->FirstChildElement("script"); + ASSERT_NE(nullptr, scriptElem); + auto nameElem = scriptElem->FirstChildElement("name"); + ASSERT_NE(nullptr, nameElem); + EXPECT_EQ("Gazebo/Orange", std::string(nameElem->GetText())); + + auto surfaceElem = collisionElem->FirstChildElement("surface"); + ASSERT_NE(nullptr, surfaceElem); + auto frictionElem = surfaceElem->FirstChildElement("friction"); + ASSERT_NE(nullptr, frictionElem); + auto odeElem = frictionElem->FirstChildElement("ode"); + ASSERT_NE(nullptr, odeElem); + auto muElem = odeElem->FirstChildElement("mu"); + ASSERT_NE(nullptr, muElem); + auto mu2Elem = odeElem->FirstChildElement("mu2"); + ASSERT_NE(nullptr, mu2Elem); + + EXPECT_EQ("100", std::string(muElem->GetText())); + EXPECT_EQ("1000", std::string(mu2Elem->GetText())); +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/src/urdf/urdf_parser/joint.cpp b/src/urdf/urdf_parser/joint.cpp index 7dcec6581..48765718d 100644 --- a/src/urdf/urdf_parser/joint.cpp +++ b/src/urdf/urdf_parser/joint.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software Ligcense Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -40,14 +40,14 @@ #include #include // #include -#include +#include #include namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml); +bool parsePose(Pose &pose, tinyxml2::XMLElement* xml); -bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) +bool parseJointDynamics(JointDynamics &jd, tinyxml2::XMLElement* config) { jd.clear(); @@ -102,7 +102,7 @@ bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) } } -bool parseJointLimits(JointLimits &jl, TiXmlElement* config) +bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config) { jl.clear(); @@ -193,7 +193,7 @@ bool parseJointLimits(JointLimits &jl, TiXmlElement* config) return true; } -bool parseJointSafety(JointSafety &js, TiXmlElement* config) +bool parseJointSafety(JointSafety &js, tinyxml2::XMLElement* config) { js.clear(); @@ -287,7 +287,7 @@ bool parseJointSafety(JointSafety &js, TiXmlElement* config) return true; } -bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) +bool parseJointCalibration(JointCalibration &jc, tinyxml2::XMLElement* config) { jc.clear(); @@ -338,7 +338,7 @@ bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) return true; } -bool parseJointMimic(JointMimic &jm, TiXmlElement* config) +bool parseJointMimic(JointMimic &jm, tinyxml2::XMLElement* config) { jm.clear(); @@ -351,13 +351,13 @@ bool parseJointMimic(JointMimic &jm, TiXmlElement* config) } else jm.joint_name = joint_name_str; - + // Get mimic multiplier const char* multiplier_str = config->Attribute("multiplier"); if (multiplier_str == NULL) { - jm.multiplier = 1; + jm.multiplier = 1; } else { @@ -375,7 +375,7 @@ bool parseJointMimic(JointMimic &jm, TiXmlElement* config) } } - + // Get mimic offset const char* offset_str = config->Attribute("offset"); if (offset_str == NULL) @@ -401,7 +401,7 @@ bool parseJointMimic(JointMimic &jm, TiXmlElement* config) return true; } -bool parseJoint(Joint &joint, TiXmlElement* config) +bool parseJoint(Joint &joint, tinyxml2::XMLElement* config) { joint.clear(); @@ -414,7 +414,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) joint.name = name; // Get transform from Parent Link to Joint Frame - TiXmlElement *origin_xml = config->FirstChildElement("origin"); + tinyxml2::XMLElement *origin_xml = config->FirstChildElement("origin"); if (!origin_xml) { joint.parent_to_joint_origin_transform.clear(); @@ -429,7 +429,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get Parent Link - TiXmlElement *parent_xml = config->FirstChildElement("parent"); + tinyxml2::XMLElement *parent_xml = config->FirstChildElement("parent"); if (parent_xml) { const char *pname = parent_xml->Attribute("link"); @@ -443,7 +443,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get Child Link - TiXmlElement *child_xml = config->FirstChildElement("child"); + tinyxml2::XMLElement *child_xml = config->FirstChildElement("child"); if (child_xml) { const char *pname = child_xml->Attribute("link"); @@ -462,7 +462,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) { return false; } - + std::string type_str = type_char; if (type_str == "planar") joint.type = Joint::PLANAR; @@ -485,7 +485,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) { // axis - TiXmlElement *axis_xml = config->FirstChildElement("axis"); + tinyxml2::XMLElement *axis_xml = config->FirstChildElement("axis"); if (!axis_xml){ joint.axis = Vector3(1.0, 0.0, 0.0); } @@ -503,7 +503,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get limit - TiXmlElement *limit_xml = config->FirstChildElement("limit"); + tinyxml2::XMLElement *limit_xml = config->FirstChildElement("limit"); if (limit_xml) { joint.limits.reset(new JointLimits()); @@ -523,7 +523,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get safety - TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); + tinyxml2::XMLElement *safety_xml = config->FirstChildElement("safety_controller"); if (safety_xml) { joint.safety.reset(new JointSafety()); @@ -535,7 +535,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get calibration - TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); + tinyxml2::XMLElement *calibration_xml = config->FirstChildElement("calibration"); if (calibration_xml) { joint.calibration.reset(new JointCalibration()); @@ -547,7 +547,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get Joint Mimic - TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); + tinyxml2::XMLElement *mimic_xml = config->FirstChildElement("mimic"); if (mimic_xml) { joint.mimic.reset(new JointMimic()); @@ -559,7 +559,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get Dynamics - TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + tinyxml2::XMLElement *prop_xml = config->FirstChildElement("dynamics"); if (prop_xml) { joint.dynamics.reset(new JointDynamics()); @@ -575,71 +575,71 @@ bool parseJoint(Joint &joint, TiXmlElement* config) /* exports */ -bool exportPose(Pose &pose, TiXmlElement* xml); +bool exportPose(Pose &pose, tinyxml2::XMLElement* xml); -bool exportJointDynamics(JointDynamics &jd, TiXmlElement* xml) +bool exportJointDynamics(JointDynamics &jd, tinyxml2::XMLElement* xml) { - TiXmlElement *dynamics_xml = new TiXmlElement("dynamics"); - dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping) ); - dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction) ); + tinyxml2::XMLElement *dynamics_xml = xml->GetDocument()->NewElement("dynamics"); + dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping).c_str() ); + dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction).c_str() ); xml->LinkEndChild(dynamics_xml); return true; } -bool exportJointLimits(JointLimits &jl, TiXmlElement* xml) +bool exportJointLimits(JointLimits &jl, tinyxml2::XMLElement* xml) { - TiXmlElement *limit_xml = new TiXmlElement("limit"); - limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort) ); - limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity) ); - limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower) ); - limit_xml->SetAttribute("upper", urdf_export_helpers::values2str(jl.upper) ); + tinyxml2::XMLElement *limit_xml = xml->GetDocument()->NewElement("limit"); + limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort).c_str() ); + limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity).c_str() ); + limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower).c_str() ); + limit_xml->SetAttribute("upper", urdf_export_helpers::values2str(jl.upper).c_str() ); xml->LinkEndChild(limit_xml); return true; } -bool exportJointSafety(JointSafety &js, TiXmlElement* xml) +bool exportJointSafety(JointSafety &js, tinyxml2::XMLElement* xml) { - TiXmlElement *safety_xml = new TiXmlElement("safety_controller"); - safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position) ); - safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity) ); - safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit) ); - safety_xml->SetAttribute("soft_upper_limit", urdf_export_helpers::values2str(js.soft_upper_limit) ); + tinyxml2::XMLElement *safety_xml = xml->GetDocument()->NewElement("safety_controller"); + safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position).c_str() ); + safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity).c_str() ); + safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit).c_str() ); + safety_xml->SetAttribute("soft_upper_limit", urdf_export_helpers::values2str(js.soft_upper_limit).c_str() ); xml->LinkEndChild(safety_xml); return true; } -bool exportJointCalibration(JointCalibration &jc, TiXmlElement* xml) +bool exportJointCalibration(JointCalibration &jc, tinyxml2::XMLElement* xml) { if (jc.falling || jc.rising) { - TiXmlElement *calibration_xml = new TiXmlElement("calibration"); + tinyxml2::XMLElement *calibration_xml = xml->GetDocument()->NewElement("calibration"); if (jc.falling) - calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling) ); + calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling).c_str() ); if (jc.rising) - calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising) ); + calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising).c_str() ); //calibration_xml->SetAttribute("reference_position", urdf_export_helpers::values2str(jc.reference_position) ); xml->LinkEndChild(calibration_xml); } return true; } -bool exportJointMimic(JointMimic &jm, TiXmlElement* xml) +bool exportJointMimic(JointMimic &jm, tinyxml2::XMLElement* xml) { if (!jm.joint_name.empty()) { - TiXmlElement *mimic_xml = new TiXmlElement("mimic"); - mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset) ); - mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier) ); - mimic_xml->SetAttribute("joint", jm.joint_name ); + tinyxml2::XMLElement *mimic_xml = xml->GetDocument()->NewElement("mimic"); + mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset).c_str() ); + mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier).c_str() ); + mimic_xml->SetAttribute("joint", jm.joint_name.c_str() ); xml->LinkEndChild(mimic_xml); } return true; } -bool exportJoint(Joint &joint, TiXmlElement* xml) +bool exportJoint(Joint &joint, tinyxml2::XMLElement* xml) { - TiXmlElement * joint_xml = new TiXmlElement("joint"); - joint_xml->SetAttribute("name", joint.name); + tinyxml2::XMLElement * joint_xml = xml->GetDocument()->NewElement("joint"); + joint_xml->SetAttribute("name", joint.name.c_str()); if (joint.type == urdf::Joint::PLANAR) joint_xml->SetAttribute("type", "planar"); else if (joint.type == urdf::Joint::FLOATING) @@ -658,18 +658,18 @@ bool exportJoint(Joint &joint, TiXmlElement* xml) exportPose(joint.parent_to_joint_origin_transform, joint_xml); // axis - TiXmlElement * axis_xml = new TiXmlElement("axis"); - axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis)); + tinyxml2::XMLElement * axis_xml = xml->GetDocument()->NewElement("axis"); + axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis).c_str()); joint_xml->LinkEndChild(axis_xml); - // parent - TiXmlElement * parent_xml = new TiXmlElement("parent"); - parent_xml->SetAttribute("link", joint.parent_link_name); + // parent + tinyxml2::XMLElement * parent_xml = xml->GetDocument()->NewElement("parent"); + parent_xml->SetAttribute("link", joint.parent_link_name.c_str()); joint_xml->LinkEndChild(parent_xml); // child - TiXmlElement * child_xml = new TiXmlElement("child"); - child_xml->SetAttribute("link", joint.child_link_name); + tinyxml2::XMLElement * child_xml = xml->GetDocument()->NewElement("child"); + child_xml->SetAttribute("link", joint.child_link_name.c_str()); joint_xml->LinkEndChild(child_xml); if (joint.dynamics) diff --git a/src/urdf/urdf_parser/link.cpp b/src/urdf/urdf_parser/link.cpp index 2860bb8e4..415c4ee5d 100644 --- a/src/urdf/urdf_parser/link.cpp +++ b/src/urdf/urdf_parser/link.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -42,14 +42,14 @@ #include #include #include -#include +#include // #include namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml); +bool parsePose(Pose &pose, tinyxml2::XMLElement* xml); -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) +bool parseMaterial(Material &material, tinyxml2::XMLElement *config, bool only_name_is_ok) { bool has_rgb = false; bool has_filename = false; @@ -60,11 +60,11 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o { return false; } - + material.name = config->Attribute("name"); // texture - TiXmlElement *t = config->FirstChildElement("texture"); + tinyxml2::XMLElement *t = config->FirstChildElement("texture"); if (t) { if (t->Attribute("filename")) @@ -75,7 +75,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o } // color - TiXmlElement *c = config->FirstChildElement("color"); + tinyxml2::XMLElement *c = config->FirstChildElement("color"); if (c) { if (c->Attribute("rgba")) { @@ -84,7 +84,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o material.color.init(c->Attribute("rgba")); has_rgb = true; } - catch (ParseError &e) { + catch (ParseError &e) { material.color.clear(); } } @@ -100,7 +100,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o } -bool parseSphere(Sphere &s, TiXmlElement *c) +bool parseSphere(Sphere &s, tinyxml2::XMLElement *c) { s.clear(); @@ -126,14 +126,14 @@ bool parseSphere(Sphere &s, TiXmlElement *c) stm << "radius [" << c->Attribute("radius") << "] is out of range: " << e.what(); return false; } - + return true; } -bool parseBox(Box &b, TiXmlElement *c) +bool parseBox(Box &b, tinyxml2::XMLElement *c) { b.clear(); - + b.type = Geometry::BOX; if (!c->Attribute("size")) { @@ -151,7 +151,7 @@ bool parseBox(Box &b, TiXmlElement *c) return true; } -bool parseCylinder(Cylinder &y, TiXmlElement *c) +bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c) { y.clear(); @@ -199,7 +199,7 @@ bool parseCylinder(Cylinder &y, TiXmlElement *c) } -bool parseMesh(Mesh &m, TiXmlElement *c) +bool parseMesh(Mesh &m, tinyxml2::XMLElement *c) { m.clear(); @@ -226,18 +226,18 @@ bool parseMesh(Mesh &m, TiXmlElement *c) return true; } -GeometrySharedPtr parseGeometry(TiXmlElement *g) +GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g) { GeometrySharedPtr geom; if (!g) return geom; - TiXmlElement *shape = g->FirstChildElement(); + tinyxml2::XMLElement *shape = g->FirstChildElement(); if (!shape) { return geom; } - std::string type_name = shape->ValueStr(); + std::string type_name = shape->Name(); if (type_name == "sphere") { Sphere *s = new Sphere(); @@ -264,29 +264,29 @@ GeometrySharedPtr parseGeometry(TiXmlElement *g) Mesh *m = new Mesh(); geom.reset(m); if (parseMesh(*m, shape)) - return geom; + return geom; } else { return geom; } - + return GeometrySharedPtr(); } -bool parseInertial(Inertial &i, TiXmlElement *config) +bool parseInertial(Inertial &i, tinyxml2::XMLElement *config) { i.clear(); // Origin - TiXmlElement *o = config->FirstChildElement("origin"); + tinyxml2::XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(i.origin, o)) return false; } - TiXmlElement *mass_xml = config->FirstChildElement("mass"); + tinyxml2::XMLElement *mass_xml = config->FirstChildElement("mass"); if (!mass_xml) { return false; @@ -315,7 +315,7 @@ bool parseInertial(Inertial &i, TiXmlElement *config) return false; } - TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); + tinyxml2::XMLElement *inertia_xml = config->FirstChildElement("inertia"); if (!inertia_xml) { return false; @@ -362,19 +362,19 @@ bool parseInertial(Inertial &i, TiXmlElement *config) return true; } -bool parseVisual(Visual &vis, TiXmlElement *config) +bool parseVisual(Visual &vis, tinyxml2::XMLElement *config) { vis.clear(); // Origin - TiXmlElement *o = config->FirstChildElement("origin"); + tinyxml2::XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(vis.origin, o)) return false; } // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); + tinyxml2::XMLElement *geom = config->FirstChildElement("geometry"); vis.geometry = parseGeometry(geom); if (!vis.geometry) return false; @@ -384,37 +384,37 @@ bool parseVisual(Visual &vis, TiXmlElement *config) vis.name = name_char; // Material - TiXmlElement *mat = config->FirstChildElement("material"); + tinyxml2::XMLElement *mat = config->FirstChildElement("material"); if (mat) { // get material name if (!mat->Attribute("name")) { return false; } vis.material_name = mat->Attribute("name"); - + // try to parse material element in place vis.material.reset(new Material()); if (!parseMaterial(*vis.material, mat, true)) { } } - + return true; } -bool parseCollision(Collision &col, TiXmlElement* config) -{ +bool parseCollision(Collision &col, tinyxml2::XMLElement* config) +{ col.clear(); // Origin - TiXmlElement *o = config->FirstChildElement("origin"); + tinyxml2::XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(col.origin, o)) return false; } - + // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); + tinyxml2::XMLElement *geom = config->FirstChildElement("geometry"); col.geometry = parseGeometry(geom); if (!col.geometry) return false; @@ -426,9 +426,9 @@ bool parseCollision(Collision &col, TiXmlElement* config) return true; } -bool parseLink(Link &link, TiXmlElement* config) +bool parseLink(Link &link, tinyxml2::XMLElement* config) { - + link.clear(); const char *name_char = config->Attribute("name"); @@ -439,7 +439,7 @@ bool parseLink(Link &link, TiXmlElement* config) link.name = std::string(name_char); // Inertial (optional) - TiXmlElement *i = config->FirstChildElement("inertial"); + tinyxml2::XMLElement *i = config->FirstChildElement("inertial"); if (i) { link.inertial.reset(new Inertial()); @@ -450,7 +450,7 @@ bool parseLink(Link &link, TiXmlElement* config) } // Multiple Visuals (optional) - for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) + for (tinyxml2::XMLElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) { VisualSharedPtr vis; @@ -470,14 +470,14 @@ bool parseLink(Link &link, TiXmlElement* config) // Assign the first visual to the .visual ptr, if it exists if (!link.visual_array.empty()) link.visual = link.visual_array[0]; - + // Multiple Collisions (optional) - for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) + for (tinyxml2::XMLElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) { CollisionSharedPtr col; col.reset(new Collision()); if (parseCollision(*col, col_xml)) - { + { link.collision_array.push_back(col); } else @@ -486,8 +486,8 @@ bool parseLink(Link &link, TiXmlElement* config) return false; } } - - // Collision (optional) + + // Collision (optional) // Assign the first collision to the .collision ptr, if it exists if (!link.collision_array.empty()) link.collision = link.collision_array[0]; @@ -496,67 +496,67 @@ bool parseLink(Link &link, TiXmlElement* config) } /* exports */ -bool exportPose(Pose &pose, TiXmlElement* xml); +bool exportPose(Pose &pose, tinyxml2::XMLElement* xml); -bool exportMaterial(Material &material, TiXmlElement *xml) +bool exportMaterial(Material &material, tinyxml2::XMLElement *xml) { - TiXmlElement *material_xml = new TiXmlElement("material"); - material_xml->SetAttribute("name", material.name); + auto *material_xml = xml->GetDocument()->NewElement("material"); + material_xml->SetAttribute("name", material.name.c_str()); - TiXmlElement* texture = new TiXmlElement("texture"); + auto *texture= xml->GetDocument()->NewElement("texture"); if (!material.texture_filename.empty()) - texture->SetAttribute("filename", material.texture_filename); + texture->SetAttribute("filename", material.texture_filename.c_str()); material_xml->LinkEndChild(texture); - TiXmlElement* color = new TiXmlElement("color"); - color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color)); + auto *color = xml->GetDocument()->NewElement("color"); + color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color).c_str()); material_xml->LinkEndChild(color); xml->LinkEndChild(material_xml); return true; } -bool exportSphere(Sphere &s, TiXmlElement *xml) +bool exportSphere(Sphere &s, tinyxml2::XMLElement *xml) { // e.g. add - TiXmlElement *sphere_xml = new TiXmlElement("sphere"); - sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius)); + auto *sphere_xml = xml->GetDocument()->NewElement("sphere"); + sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius).c_str()); xml->LinkEndChild(sphere_xml); return true; } -bool exportBox(Box &b, TiXmlElement *xml) +bool exportBox(Box &b, tinyxml2::XMLElement *xml) { // e.g. add - TiXmlElement *box_xml = new TiXmlElement("box"); - box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim)); + auto *box_xml = xml->GetDocument()->NewElement("box"); + box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim).c_str()); xml->LinkEndChild(box_xml); return true; } -bool exportCylinder(Cylinder &y, TiXmlElement *xml) +bool exportCylinder(Cylinder &y, tinyxml2::XMLElement *xml) { // e.g. add - TiXmlElement *cylinder_xml = new TiXmlElement("cylinder"); - cylinder_xml->SetAttribute("radius", urdf_export_helpers::values2str(y.radius)); - cylinder_xml->SetAttribute("length", urdf_export_helpers::values2str(y.length)); + auto *cylinder_xml = xml->GetDocument()->NewElement("cylinder"); + cylinder_xml->SetAttribute("radius", urdf_export_helpers::values2str(y.radius).c_str()); + cylinder_xml->SetAttribute("length", urdf_export_helpers::values2str(y.length).c_str()); xml->LinkEndChild(cylinder_xml); return true; } -bool exportMesh(Mesh &m, TiXmlElement *xml) +bool exportMesh(Mesh &m, tinyxml2::XMLElement *xml) { // e.g. add - TiXmlElement *mesh_xml = new TiXmlElement("mesh"); + auto *mesh_xml = xml->GetDocument()->NewElement("mesh"); if (!m.filename.empty()) - mesh_xml->SetAttribute("filename", m.filename); - mesh_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale)); + mesh_xml->SetAttribute("filename", m.filename.c_str()); + mesh_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale).c_str()); xml->LinkEndChild(mesh_xml); return true; } -bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) +bool exportGeometry(GeometrySharedPtr &geom, tinyxml2::XMLElement *xml) { - TiXmlElement *geometry_xml = new TiXmlElement("geometry"); + auto *geometry_xml= xml->GetDocument()->NewElement("geometry"); if (urdf::dynamic_pointer_cast(geom)) { exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); @@ -585,36 +585,36 @@ bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) return true; } -bool exportInertial(Inertial &i, TiXmlElement *xml) +bool exportInertial(Inertial &i, tinyxml2::XMLElement *xml) { // adds // // // // - TiXmlElement *inertial_xml = new TiXmlElement("inertial"); + auto *inertial_xml = xml->GetDocument()->NewElement("inertial"); + auto *mass_xml = xml->GetDocument()->NewElement("mass"); - TiXmlElement *mass_xml = new TiXmlElement("mass"); - mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass)); + mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass).c_str()); inertial_xml->LinkEndChild(mass_xml); exportPose(i.origin, inertial_xml); - TiXmlElement *inertia_xml = new TiXmlElement("inertia"); - inertia_xml->SetAttribute("ixx", urdf_export_helpers::values2str(i.ixx)); - inertia_xml->SetAttribute("ixy", urdf_export_helpers::values2str(i.ixy)); - inertia_xml->SetAttribute("ixz", urdf_export_helpers::values2str(i.ixz)); - inertia_xml->SetAttribute("iyy", urdf_export_helpers::values2str(i.iyy)); - inertia_xml->SetAttribute("iyz", urdf_export_helpers::values2str(i.iyz)); - inertia_xml->SetAttribute("izz", urdf_export_helpers::values2str(i.izz)); + auto *inertia_xml = xml->GetDocument()->NewElement("inertia"); + inertia_xml->SetAttribute("ixx", urdf_export_helpers::values2str(i.ixx).c_str()); + inertia_xml->SetAttribute("ixy", urdf_export_helpers::values2str(i.ixy).c_str()); + inertia_xml->SetAttribute("ixz", urdf_export_helpers::values2str(i.ixz).c_str()); + inertia_xml->SetAttribute("iyy", urdf_export_helpers::values2str(i.iyy).c_str()); + inertia_xml->SetAttribute("iyz", urdf_export_helpers::values2str(i.iyz).c_str()); + inertia_xml->SetAttribute("izz", urdf_export_helpers::values2str(i.izz).c_str()); inertial_xml->LinkEndChild(inertia_xml); xml->LinkEndChild(inertial_xml); - + return true; } -bool exportVisual(Visual &vis, TiXmlElement *xml) +bool exportVisual(Visual &vis, tinyxml2::XMLElement *xml) { // // @@ -623,7 +623,7 @@ bool exportVisual(Visual &vis, TiXmlElement *xml) // // // - TiXmlElement * visual_xml = new TiXmlElement("visual"); + auto *visual_xml = xml->GetDocument()->NewElement("visual"); exportPose(vis.origin, visual_xml); @@ -637,8 +637,8 @@ bool exportVisual(Visual &vis, TiXmlElement *xml) return true; } -bool exportCollision(Collision &col, TiXmlElement* xml) -{ +bool exportCollision(Collision &col, tinyxml2::XMLElement* xml) +{ // // // @@ -646,7 +646,7 @@ bool exportCollision(Collision &col, TiXmlElement* xml) // // // - TiXmlElement * collision_xml = new TiXmlElement("collision"); + auto *collision_xml = xml->GetDocument()->NewElement("collision"); exportPose(col.origin, collision_xml); @@ -657,10 +657,10 @@ bool exportCollision(Collision &col, TiXmlElement* xml) return true; } -bool exportLink(Link &link, TiXmlElement* xml) +bool exportLink(Link &link, tinyxml2::XMLElement* xml) { - TiXmlElement * link_xml = new TiXmlElement("link"); - link_xml->SetAttribute("name", link.name); + auto *link_xml = xml->GetDocument()->NewElement("link"); + link_xml->SetAttribute("name", link.name.c_str()); if (link.inertial) exportInertial(*link.inertial, link_xml); diff --git a/src/urdf/urdf_parser/model.cpp b/src/urdf/urdf_parser/model.cpp index 5337c3242..804c39435 100644 --- a/src/urdf/urdf_parser/model.cpp +++ b/src/urdf/urdf_parser/model.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -42,9 +42,9 @@ namespace urdf{ -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); -bool parseLink(Link &link, TiXmlElement *config); -bool parseJoint(Joint &joint, TiXmlElement *config); +bool parseMaterial(Material &material, tinyxml2::XMLElement *config, bool only_name_is_ok); +bool parseLink(Link &link, tinyxml2::XMLElement *config); +bool parseJoint(Joint &joint, tinyxml2::XMLElement *config); ModelInterfaceSharedPtr parseURDFFile(const std::string &path) { @@ -64,7 +64,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) ModelInterfaceSharedPtr model(new ModelInterface); model->clear(); - TiXmlDocument xml_doc; + tinyxml2::XMLDocument xml_doc; xml_doc.Parse(xml_string.c_str()); if (xml_doc.Error()) { @@ -73,7 +73,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) return model; } - TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); + tinyxml2::XMLElement *robot_xml = xml_doc.FirstChildElement("robot"); if (!robot_xml) { model.reset(); @@ -90,7 +90,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) model->name_ = std::string(name); // Get all Material elements - for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) + for (tinyxml2::XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { MaterialSharedPtr material; material.reset(new Material); @@ -116,7 +116,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } // Get all Link elements - for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) + for (tinyxml2::XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) { LinkSharedPtr link; link.reset(new Link); @@ -168,7 +168,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } // Get all Joint elements - for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + for (tinyxml2::XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) { JointSharedPtr joint; joint.reset(new Joint); @@ -199,7 +199,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) parent_link_tree.clear(); // building tree: name mapping - try + try { model->initTree(parent_link_tree); } @@ -219,19 +219,19 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) model.reset(); return model; } - + return model; } -bool exportMaterial(Material &material, TiXmlElement *config); -bool exportLink(Link &link, TiXmlElement *config); -bool exportJoint(Joint &joint, TiXmlElement *config); -TiXmlDocument* exportURDF(const ModelInterface &model) +bool exportMaterial(Material &material, tinyxml2::XMLElement *config); +bool exportLink(Link &link, tinyxml2::XMLElement *config); +bool exportJoint(Joint &joint, tinyxml2::XMLElement *config); +tinyxml2::XMLDocument* exportURDF(const ModelInterface &model) { - TiXmlDocument *doc = new TiXmlDocument(); + tinyxml2::XMLDocument *doc = new tinyxml2::XMLDocument(); + tinyxml2::XMLElement *robot = doc->NewElement("robot"); - TiXmlElement *robot = new TiXmlElement("robot"); - robot->SetAttribute("name", model.name_); + robot->SetAttribute("name", model.name_.c_str()); doc->LinkEndChild(robot); @@ -240,20 +240,20 @@ TiXmlDocument* exportURDF(const ModelInterface &model) exportMaterial(*(m->second), robot); } - for (std::map::const_iterator l=model.links_.begin(); l!=model.links_.end(); l++) + for (std::map::const_iterator l=model.links_.begin(); l!=model.links_.end(); l++) { exportLink(*(l->second), robot); } - - for (std::map::const_iterator j=model.joints_.begin(); j!=model.joints_.end(); j++) + + for (std::map::const_iterator j=model.joints_.begin(); j!=model.joints_.end(); j++) { exportJoint(*(j->second), robot); } return doc; } - -TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model) + +tinyxml2::XMLDocument* exportURDF(ModelInterfaceSharedPtr &model) { return exportURDF(*model); } diff --git a/src/urdf/urdf_parser/pose.cpp b/src/urdf/urdf_parser/pose.cpp index 44998dd32..32207e1be 100644 --- a/src/urdf/urdf_parser/pose.cpp +++ b/src/urdf/urdf_parser/pose.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -40,7 +40,7 @@ #include #include // #include -#include +#include #include namespace urdf_export_helpers { @@ -87,7 +87,7 @@ std::string values2str(double d) namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml) +bool parsePose(Pose &pose, tinyxml2::XMLElement* xml) { pose.clear(); if (xml) @@ -117,13 +117,13 @@ bool parsePose(Pose &pose, TiXmlElement* xml) return true; } -bool exportPose(Pose &pose, TiXmlElement* xml) +bool exportPose(Pose &pose, tinyxml2::XMLElement* xml) { - TiXmlElement *origin = new TiXmlElement("origin"); + tinyxml2::XMLElement *origin = xml->GetDocument()->NewElement("origin"); std::string pose_xyz_str = urdf_export_helpers::values2str(pose.position); std::string pose_rpy_str = urdf_export_helpers::values2str(pose.rotation); - origin->SetAttribute("xyz", pose_xyz_str); - origin->SetAttribute("rpy", pose_rpy_str); + origin->SetAttribute("xyz", pose_xyz_str.c_str()); + origin->SetAttribute("rpy", pose_rpy_str.c_str()); xml->LinkEndChild(origin); return true; } diff --git a/src/urdf/urdf_parser/twist.cpp b/src/urdf/urdf_parser/twist.cpp index af2271326..895d50c4b 100644 --- a/src/urdf/urdf_parser/twist.cpp +++ b/src/urdf/urdf_parser/twist.cpp @@ -39,12 +39,12 @@ #include #include #include -#include +#include //#include namespace urdf{ -bool parseTwist(Twist &twist, TiXmlElement* xml) +bool parseTwist(Twist &twist, tinyxml2::XMLElement* xml) { twist.clear(); if (xml) diff --git a/src/urdf/urdf_parser/urdf_model_state.cpp b/src/urdf/urdf_parser/urdf_model_state.cpp index ddb8e11ea..add84ebe2 100644 --- a/src/urdf/urdf_parser/urdf_model_state.cpp +++ b/src/urdf/urdf_parser/urdf_model_state.cpp @@ -42,12 +42,12 @@ #include #include #include -#include +#include // #include namespace urdf{ -bool parseModelState(ModelState &ms, TiXmlElement* config) +bool parseModelState(ModelState &ms, tinyxml2::XMLElement* config) { ms.clear(); @@ -73,7 +73,7 @@ bool parseModelState(ModelState &ms, TiXmlElement* config) } } - TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); + tinyxml2::XMLElement *joint_state_elem = config->FirstChildElement("joint_state"); if (joint_state_elem) { JointStateSharedPtr joint_state; diff --git a/src/urdf/urdf_parser/urdf_parser.h b/src/urdf/urdf_parser/urdf_parser.h index 939a91a54..60fd509f7 100644 --- a/src/urdf/urdf_parser/urdf_parser.h +++ b/src/urdf/urdf_parser/urdf_parser.h @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include #include @@ -60,9 +60,9 @@ namespace urdf{ URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); - URDFDOM_DLLAPI TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model); - URDFDOM_DLLAPI TiXmlDocument* exportURDF(const ModelInterface &model); - URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*); + URDFDOM_DLLAPI tinyxml2::XMLDocument* exportURDF(ModelInterfaceSharedPtr &model); + URDFDOM_DLLAPI tinyxml2::XMLDocument* exportURDF(const ModelInterface &model); + URDFDOM_DLLAPI bool parsePose(Pose&, tinyxml2::XMLElement*); } #endif diff --git a/src/urdf/urdf_parser/urdf_sensor.cpp b/src/urdf/urdf_parser/urdf_sensor.cpp index c87ec0a15..657ac4fdc 100644 --- a/src/urdf/urdf_parser/urdf_sensor.cpp +++ b/src/urdf/urdf_parser/urdf_sensor.cpp @@ -41,19 +41,19 @@ #include #include #include -#include +#include // #include namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml); +bool parsePose(Pose &pose, tinyxml2::XMLElement* xml); -bool parseCamera(Camera &camera, TiXmlElement* config) +bool parseCamera(Camera &camera, tinyxml2::XMLElement* config) { camera.clear(); camera.type = VisualSensor::CAMERA; - TiXmlElement *image = config->FirstChildElement("image"); + tinyxml2::XMLElement *image = config->FirstChildElement("image"); if (image) { const char* width_char = image->Attribute("width"); @@ -177,12 +177,12 @@ bool parseCamera(Camera &camera, TiXmlElement* config) return true; } -bool parseRay(Ray &ray, TiXmlElement* config) +bool parseRay(Ray &ray, tinyxml2::XMLElement* config) { ray.clear(); ray.type = VisualSensor::RAY; - TiXmlElement *horizontal = config->FirstChildElement("horizontal"); + tinyxml2::XMLElement *horizontal = config->FirstChildElement("horizontal"); if (horizontal) { const char* samples_char = horizontal->Attribute("samples"); @@ -254,7 +254,7 @@ bool parseRay(Ray &ray, TiXmlElement* config) } } - TiXmlElement *vertical = config->FirstChildElement("vertical"); + tinyxml2::XMLElement *vertical = config->FirstChildElement("vertical"); if (vertical) { const char* samples_char = vertical->Attribute("samples"); @@ -328,12 +328,12 @@ bool parseRay(Ray &ray, TiXmlElement* config) return false; } -VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g) +VisualSensorSharedPtr parseVisualSensor(tinyxml2::XMLElement *g) { VisualSensorSharedPtr visual_sensor; // get sensor type - TiXmlElement *sensor_xml; + tinyxml2::XMLElement *sensor_xml; if (g->FirstChildElement("camera")) { Camera *camera = new Camera(); @@ -357,7 +357,7 @@ VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g) } -bool parseSensor(Sensor &sensor, TiXmlElement* config) +bool parseSensor(Sensor &sensor, tinyxml2::XMLElement* config) { sensor.clear(); @@ -377,7 +377,7 @@ bool parseSensor(Sensor &sensor, TiXmlElement* config) sensor.parent_link_name = std::string(parent_link_name_char); // parse origin - TiXmlElement *o = config->FirstChildElement("origin"); + tinyxml2::XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(sensor.origin, o)) diff --git a/src/urdf/urdf_parser/world.cpp b/src/urdf/urdf_parser/world.cpp index 141074049..c1dcb7d37 100644 --- a/src/urdf/urdf_parser/world.cpp +++ b/src/urdf/urdf_parser/world.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -41,12 +41,12 @@ #include #include #include -#include +#include //#include namespace urdf{ -bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) +bool parseWorld(World &/*world*/, tinyxml2::XMLElement* /*config*/) { // to be implemented @@ -54,10 +54,10 @@ bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) return true; } -bool exportWorld(World &world, TiXmlElement* xml) +bool exportWorld(World &world, tinyxml2::XMLElement* xml) { - TiXmlElement * world_xml = new TiXmlElement("world"); - world_xml->SetAttribute("name", world.name); + tinyxml2::XMLElement * world_xml = xml->GetDocument()->NewElement("world"); + world_xml->SetAttribute("name", world.name.c_str()); // to be implemented // exportModels(*world.models, world_xml); diff --git a/src/urdf/urdf_world/world.h b/src/urdf/urdf_world/world.h index 7b0e8c101..566d08ba3 100644 --- a/src/urdf/urdf_world/world.h +++ b/src/urdf/urdf_world/world.h @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include "urdf_model/model.h" #include "urdf_model/pose.h" @@ -100,7 +100,7 @@ class World std::vector models; - void initXml(TiXmlElement* config); + void initXml(tinyxml2::XMLElement* config); void clear() { diff --git a/src/win/tinyxml/VERSION_2.6.2 b/src/win/tinyxml/VERSION_2.6.2 deleted file mode 100644 index ca466dd3d..000000000 --- a/src/win/tinyxml/VERSION_2.6.2 +++ /dev/null @@ -1,2 +0,0 @@ -with the patch: -https://raw.githubusercontent.com/robotology/yarp/master/extern/tinyxml/patches/entity-encoding.patch diff --git a/src/win/tinyxml/tinystr.cpp b/src/win/tinyxml/tinystr.cpp deleted file mode 100644 index 066576820..000000000 --- a/src/win/tinyxml/tinystr.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. -*/ - - -#ifndef TIXML_USE_STL - -#include "tinystr.h" - -// Error value for find primitive -const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1); - - -// Null rep. -TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } }; - - -void TiXmlString::reserve (size_type cap) -{ - if (cap > capacity()) - { - TiXmlString tmp; - tmp.init(length(), cap); - memcpy(tmp.start(), data(), length()); - swap(tmp); - } -} - - -TiXmlString& TiXmlString::assign(const char* str, size_type len) -{ - size_type cap = capacity(); - if (len > cap || cap > 3*(len + 8)) - { - TiXmlString tmp; - tmp.init(len); - memcpy(tmp.start(), str, len); - swap(tmp); - } - else - { - memmove(start(), str, len); - set_size(len); - } - return *this; -} - - -TiXmlString& TiXmlString::append(const char* str, size_type len) -{ - size_type newsize = length() + len; - if (newsize > capacity()) - { - reserve (newsize + capacity()); - } - memmove(finish(), str, len); - set_size(newsize); - return *this; -} - - -TiXmlString operator + (const TiXmlString & a, const TiXmlString & b) -{ - TiXmlString tmp; - tmp.reserve(a.length() + b.length()); - tmp += a; - tmp += b; - return tmp; -} - -TiXmlString operator + (const TiXmlString & a, const char* b) -{ - TiXmlString tmp; - TiXmlString::size_type b_len = static_cast( strlen(b) ); - tmp.reserve(a.length() + b_len); - tmp += a; - tmp.append(b, b_len); - return tmp; -} - -TiXmlString operator + (const char* a, const TiXmlString & b) -{ - TiXmlString tmp; - TiXmlString::size_type a_len = static_cast( strlen(a) ); - tmp.reserve(a_len + b.length()); - tmp.append(a, a_len); - tmp += b; - return tmp; -} - - -#endif // TIXML_USE_STL diff --git a/src/win/tinyxml/tinystr.h b/src/win/tinyxml/tinystr.h deleted file mode 100644 index 89cca3341..000000000 --- a/src/win/tinyxml/tinystr.h +++ /dev/null @@ -1,305 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. -*/ - - -#ifndef TIXML_USE_STL - -#ifndef TIXML_STRING_INCLUDED -#define TIXML_STRING_INCLUDED - -#include -#include - -/* The support for explicit isn't that universal, and it isn't really - required - it is used to check that the TiXmlString class isn't incorrectly - used. Be nice to old compilers and macro it here: -*/ -#if defined(_MSC_VER) && (_MSC_VER >= 1200 ) - // Microsoft visual studio, version 6 and higher. - #define TIXML_EXPLICIT explicit -#elif defined(__GNUC__) && (__GNUC__ >= 3 ) - // GCC version 3 and higher.s - #define TIXML_EXPLICIT explicit -#else - #define TIXML_EXPLICIT -#endif - - -/* - TiXmlString is an emulation of a subset of the std::string template. - Its purpose is to allow compiling TinyXML on compilers with no or poor STL support. - Only the member functions relevant to the TinyXML project have been implemented. - The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase - a string and there's no more room, we allocate a buffer twice as big as we need. -*/ -class TiXmlString -{ - public : - // The size type used - typedef size_t size_type; - - // Error value for find primitive - static const size_type npos; // = -1; - - - // TiXmlString empty constructor - TiXmlString () : rep_(&nullrep_) - { - } - - // TiXmlString copy constructor - TiXmlString ( const TiXmlString & copy) : rep_(0) - { - init(copy.length()); - memcpy(start(), copy.data(), length()); - } - - // TiXmlString constructor, based on a string - TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0) - { - init( static_cast( strlen(copy) )); - memcpy(start(), copy, length()); - } - - // TiXmlString constructor, based on a string - TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0) - { - init(len); - memcpy(start(), str, len); - } - - // TiXmlString destructor - ~TiXmlString () - { - quit(); - } - - TiXmlString& operator = (const char * copy) - { - return assign( copy, (size_type)strlen(copy)); - } - - TiXmlString& operator = (const TiXmlString & copy) - { - return assign(copy.start(), copy.length()); - } - - - // += operator. Maps to append - TiXmlString& operator += (const char * suffix) - { - return append(suffix, static_cast( strlen(suffix) )); - } - - // += operator. Maps to append - TiXmlString& operator += (char single) - { - return append(&single, 1); - } - - // += operator. Maps to append - TiXmlString& operator += (const TiXmlString & suffix) - { - return append(suffix.data(), suffix.length()); - } - - - // Convert a TiXmlString into a null-terminated char * - const char * c_str () const { return rep_->str; } - - // Convert a TiXmlString into a char * (need not be null terminated). - const char * data () const { return rep_->str; } - - // Return the length of a TiXmlString - size_type length () const { return rep_->size; } - - // Alias for length() - size_type size () const { return rep_->size; } - - // Checks if a TiXmlString is empty - bool empty () const { return rep_->size == 0; } - - // Return capacity of string - size_type capacity () const { return rep_->capacity; } - - - // single char extraction - const char& at (size_type index) const - { - assert( index < length() ); - return rep_->str[ index ]; - } - - // [] operator - char& operator [] (size_type index) const - { - assert( index < length() ); - return rep_->str[ index ]; - } - - // find a char in a string. Return TiXmlString::npos if not found - size_type find (char lookup) const - { - return find(lookup, 0); - } - - // find a char in a string from an offset. Return TiXmlString::npos if not found - size_type find (char tofind, size_type offset) const - { - if (offset >= length()) return npos; - - for (const char* p = c_str() + offset; *p != '\0'; ++p) - { - if (*p == tofind) return static_cast< size_type >( p - c_str() ); - } - return npos; - } - - void clear () - { - //Lee: - //The original was just too strange, though correct: - // TiXmlString().swap(*this); - //Instead use the quit & re-init: - quit(); - init(0,0); - } - - /* Function to reserve a big amount of data when we know we'll need it. Be aware that this - function DOES NOT clear the content of the TiXmlString if any exists. - */ - void reserve (size_type cap); - - TiXmlString& assign (const char* str, size_type len); - - TiXmlString& append (const char* str, size_type len); - - void swap (TiXmlString& other) - { - Rep* r = rep_; - rep_ = other.rep_; - other.rep_ = r; - } - - private: - - void init(size_type sz) { init(sz, sz); } - void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; } - char* start() const { return rep_->str; } - char* finish() const { return rep_->str + rep_->size; } - - struct Rep - { - size_type size, capacity; - char str[1]; - }; - - void init(size_type sz, size_type cap) - { - if (cap) - { - // Lee: the original form: - // rep_ = static_cast(operator new(sizeof(Rep) + cap)); - // doesn't work in some cases of new being overloaded. Switching - // to the normal allocation, although use an 'int' for systems - // that are overly picky about structure alignment. - const size_type bytesNeeded = sizeof(Rep) + cap; - const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int ); - rep_ = reinterpret_cast( new int[ intsNeeded ] ); - - rep_->str[ rep_->size = sz ] = '\0'; - rep_->capacity = cap; - } - else - { - rep_ = &nullrep_; - } - } - - void quit() - { - if (rep_ != &nullrep_) - { - // The rep_ is really an array of ints. (see the allocator, above). - // Cast it back before delete, so the compiler won't incorrectly call destructors. - delete [] ( reinterpret_cast( rep_ ) ); - } - } - - Rep * rep_; - static Rep nullrep_; - -} ; - - -inline bool operator == (const TiXmlString & a, const TiXmlString & b) -{ - return ( a.length() == b.length() ) // optimization on some platforms - && ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare -} -inline bool operator < (const TiXmlString & a, const TiXmlString & b) -{ - return strcmp(a.c_str(), b.c_str()) < 0; -} - -inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); } -inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; } -inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); } -inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); } - -inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; } -inline bool operator == (const char* a, const TiXmlString & b) { return b == a; } -inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); } -inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); } - -TiXmlString operator + (const TiXmlString & a, const TiXmlString & b); -TiXmlString operator + (const TiXmlString & a, const char* b); -TiXmlString operator + (const char* a, const TiXmlString & b); - - -/* - TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString. - Only the operators that we need for TinyXML have been developped. -*/ -class TiXmlOutStream : public TiXmlString -{ -public : - - // TiXmlOutStream << operator. - TiXmlOutStream & operator << (const TiXmlString & in) - { - *this += in; - return *this; - } - - // TiXmlOutStream << operator. - TiXmlOutStream & operator << (const char * in) - { - *this += in; - return *this; - } - -} ; - -#endif // TIXML_STRING_INCLUDED -#endif // TIXML_USE_STL diff --git a/src/win/tinyxml/tinyxml.cpp b/src/win/tinyxml/tinyxml.cpp deleted file mode 100644 index 654a51428..000000000 --- a/src/win/tinyxml/tinyxml.cpp +++ /dev/null @@ -1,1861 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml -Original code by Lee Thomason (www.grinninglizard.com) - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. -*/ - -#include - -#ifdef TIXML_USE_STL -#include -#include -#endif - -#include "tinyxml.h" - -FILE* TiXmlFOpen( const char* filename, const char* mode ); - -bool TiXmlBase::condenseWhiteSpace = true; - -// Microsoft compiler security -FILE* TiXmlFOpen( const char* filename, const char* mode ) -{ - #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) - FILE* fp = 0; - errno_t err = fopen_s( &fp, filename, mode ); - if ( !err && fp ) - return fp; - return 0; - #else - return fopen( filename, mode ); - #endif -} - -void TiXmlBase::EncodeString( const TIXML_STRING& str, TIXML_STRING* outString ) -{ - int i=0; - - while( i<(int)str.length() ) - { - unsigned char c = (unsigned char) str[i]; - - if ( c == '&' ) - { - outString->append( entity[0].str, entity[0].strLength ); - ++i; - } - else if ( c == '<' ) - { - outString->append( entity[1].str, entity[1].strLength ); - ++i; - } - else if ( c == '>' ) - { - outString->append( entity[2].str, entity[2].strLength ); - ++i; - } - else if ( c == '\"' ) - { - outString->append( entity[3].str, entity[3].strLength ); - ++i; - } - else if ( c == '\'' ) - { - outString->append( entity[4].str, entity[4].strLength ); - ++i; - } - else if ( c < 32 ) - { - // Easy pass at non-alpha/numeric/symbol - // Below 32 is symbolic. - char buf[ 32 ]; - - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF( buf, sizeof(buf), "&#x%02X;", (unsigned) ( c & 0xff ) ); - #else - sprintf( buf, "&#x%02X;", (unsigned) ( c & 0xff ) ); - #endif - - //*ME: warning C4267: convert 'size_t' to 'int' - //*ME: Int-Cast to make compiler happy ... - outString->append( buf, (int)strlen( buf ) ); - ++i; - } - else - { - //char realc = (char) c; - //outString->append( &realc, 1 ); - *outString += (char) c; // somewhat more efficient function call. - ++i; - } - } -} - - -TiXmlNode::TiXmlNode( NodeType _type ) : TiXmlBase() -{ - parent = 0; - type = _type; - firstChild = 0; - lastChild = 0; - prev = 0; - next = 0; -} - - -TiXmlNode::~TiXmlNode() -{ - TiXmlNode* node = firstChild; - TiXmlNode* temp = 0; - - while ( node ) - { - temp = node; - node = node->next; - delete temp; - } -} - - -void TiXmlNode::CopyTo( TiXmlNode* target ) const -{ - target->SetValue (value.c_str() ); - target->userData = userData; - target->location = location; -} - - -void TiXmlNode::Clear() -{ - TiXmlNode* node = firstChild; - TiXmlNode* temp = 0; - - while ( node ) - { - temp = node; - node = node->next; - delete temp; - } - - firstChild = 0; - lastChild = 0; -} - - -TiXmlNode* TiXmlNode::LinkEndChild( TiXmlNode* node ) -{ - assert( node->parent == 0 || node->parent == this ); - assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() ); - - if ( node->Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - delete node; - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - node->parent = this; - - node->prev = lastChild; - node->next = 0; - - if ( lastChild ) - lastChild->next = node; - else - firstChild = node; // it was an empty list. - - lastChild = node; - return node; -} - - -TiXmlNode* TiXmlNode::InsertEndChild( const TiXmlNode& addThis ) -{ - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - - return LinkEndChild( node ); -} - - -TiXmlNode* TiXmlNode::InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ) -{ - if ( !beforeThis || beforeThis->parent != this ) { - return 0; - } - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - node->parent = this; - - node->next = beforeThis; - node->prev = beforeThis->prev; - if ( beforeThis->prev ) - { - beforeThis->prev->next = node; - } - else - { - assert( firstChild == beforeThis ); - firstChild = node; - } - beforeThis->prev = node; - return node; -} - - -TiXmlNode* TiXmlNode::InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ) -{ - if ( !afterThis || afterThis->parent != this ) { - return 0; - } - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - node->parent = this; - - node->prev = afterThis; - node->next = afterThis->next; - if ( afterThis->next ) - { - afterThis->next->prev = node; - } - else - { - assert( lastChild == afterThis ); - lastChild = node; - } - afterThis->next = node; - return node; -} - - -TiXmlNode* TiXmlNode::ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ) -{ - if ( !replaceThis ) - return 0; - - if ( replaceThis->parent != this ) - return 0; - - if ( withThis.ToDocument() ) { - // A document can never be a child. Thanks to Noam. - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = withThis.Clone(); - if ( !node ) - return 0; - - node->next = replaceThis->next; - node->prev = replaceThis->prev; - - if ( replaceThis->next ) - replaceThis->next->prev = node; - else - lastChild = node; - - if ( replaceThis->prev ) - replaceThis->prev->next = node; - else - firstChild = node; - - delete replaceThis; - node->parent = this; - return node; -} - - -bool TiXmlNode::RemoveChild( TiXmlNode* removeThis ) -{ - if ( !removeThis ) { - return false; - } - - if ( removeThis->parent != this ) - { - assert( 0 ); - return false; - } - - if ( removeThis->next ) - removeThis->next->prev = removeThis->prev; - else - lastChild = removeThis->prev; - - if ( removeThis->prev ) - removeThis->prev->next = removeThis->next; - else - firstChild = removeThis->next; - - delete removeThis; - return true; -} - -const TiXmlNode* TiXmlNode::FirstChild( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = firstChild; node; node = node->next ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::LastChild( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = lastChild; node; node = node->prev ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::IterateChildren( const TiXmlNode* previous ) const -{ - if ( !previous ) - { - return FirstChild(); - } - else - { - assert( previous->parent == this ); - return previous->NextSibling(); - } -} - - -const TiXmlNode* TiXmlNode::IterateChildren( const char * val, const TiXmlNode* previous ) const -{ - if ( !previous ) - { - return FirstChild( val ); - } - else - { - assert( previous->parent == this ); - return previous->NextSibling( val ); - } -} - - -const TiXmlNode* TiXmlNode::NextSibling( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = next; node; node = node->next ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::PreviousSibling( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = prev; node; node = node->prev ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -void TiXmlElement::RemoveAttribute( const char * name ) -{ - #ifdef TIXML_USE_STL - TIXML_STRING str( name ); - TiXmlAttribute* node = attributeSet.Find( str ); - #else - TiXmlAttribute* node = attributeSet.Find( name ); - #endif - if ( node ) - { - attributeSet.Remove( node ); - delete node; - } -} - -const TiXmlElement* TiXmlNode::FirstChildElement() const -{ - const TiXmlNode* node; - - for ( node = FirstChild(); - node; - node = node->NextSibling() ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::FirstChildElement( const char * _value ) const -{ - const TiXmlNode* node; - - for ( node = FirstChild( _value ); - node; - node = node->NextSibling( _value ) ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::NextSiblingElement() const -{ - const TiXmlNode* node; - - for ( node = NextSibling(); - node; - node = node->NextSibling() ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::NextSiblingElement( const char * _value ) const -{ - const TiXmlNode* node; - - for ( node = NextSibling( _value ); - node; - node = node->NextSibling( _value ) ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlDocument* TiXmlNode::GetDocument() const -{ - const TiXmlNode* node; - - for( node = this; node; node = node->parent ) - { - if ( node->ToDocument() ) - return node->ToDocument(); - } - return 0; -} - - -TiXmlElement::TiXmlElement (const char * _value) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - value = _value; -} - - -#ifdef TIXML_USE_STL -TiXmlElement::TiXmlElement( const std::string& _value ) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - value = _value; -} -#endif - - -TiXmlElement::TiXmlElement( const TiXmlElement& copy) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - copy.CopyTo( this ); -} - - -TiXmlElement& TiXmlElement::operator=( const TiXmlElement& base ) -{ - ClearThis(); - base.CopyTo( this ); - return *this; -} - - -TiXmlElement::~TiXmlElement() -{ - ClearThis(); -} - - -void TiXmlElement::ClearThis() -{ - Clear(); - while( attributeSet.First() ) - { - TiXmlAttribute* node = attributeSet.First(); - attributeSet.Remove( node ); - delete node; - } -} - - -const char* TiXmlElement::Attribute( const char* name ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( node ) - return node->Value(); - return 0; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( attrib ) - return &attrib->ValueStr(); - return 0; -} -#endif - - -const char* TiXmlElement::Attribute( const char* name, int* i ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const char* result = 0; - - if ( attrib ) { - result = attrib->Value(); - if ( i ) { - attrib->QueryIntValue( i ); - } - } - return result; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name, int* i ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const std::string* result = 0; - - if ( attrib ) { - result = &attrib->ValueStr(); - if ( i ) { - attrib->QueryIntValue( i ); - } - } - return result; -} -#endif - - -const char* TiXmlElement::Attribute( const char* name, double* d ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const char* result = 0; - - if ( attrib ) { - result = attrib->Value(); - if ( d ) { - attrib->QueryDoubleValue( d ); - } - } - return result; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name, double* d ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const std::string* result = 0; - - if ( attrib ) { - result = &attrib->ValueStr(); - if ( d ) { - attrib->QueryDoubleValue( d ); - } - } - return result; -} -#endif - - -int TiXmlElement::QueryIntAttribute( const char* name, int* ival ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryIntValue( ival ); -} - -int TiXmlElement::QueryUnsignedAttribute( const char* name, unsigned* _value ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - - int ival = 0; - int result = node->QueryIntValue( &ival ); - *_value = (unsigned)ival; - return result; -} - -int TiXmlElement::QueryBoolAttribute( const char* name, bool* bval ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - - int result = TIXML_WRONG_TYPE; - if ( StringEqual( node->Value(), "true", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "yes", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "1", true, TIXML_ENCODING_UNKNOWN ) ) - { - *bval = true; - result = TIXML_SUCCESS; - } - else if ( StringEqual( node->Value(), "false", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "no", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "0", true, TIXML_ENCODING_UNKNOWN ) ) - { - *bval = false; - result = TIXML_SUCCESS; - } - return result; -} - - - -#ifdef TIXML_USE_STL -int TiXmlElement::QueryIntAttribute( const std::string& name, int* ival ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryIntValue( ival ); -} -#endif - - -int TiXmlElement::QueryDoubleAttribute( const char* name, double* dval ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryDoubleValue( dval ); -} - - -#ifdef TIXML_USE_STL -int TiXmlElement::QueryDoubleAttribute( const std::string& name, double* dval ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryDoubleValue( dval ); -} -#endif - - -void TiXmlElement::SetAttribute( const char * name, int val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetIntValue( val ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetAttribute( const std::string& name, int val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetIntValue( val ); - } -} -#endif - - -void TiXmlElement::SetDoubleAttribute( const char * name, double val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetDoubleValue( val ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetDoubleAttribute( const std::string& name, double val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetDoubleValue( val ); - } -} -#endif - - -void TiXmlElement::SetAttribute( const char * cname, const char * cvalue ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( cname ); - if ( attrib ) { - attrib->SetValue( cvalue ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetAttribute( const std::string& _name, const std::string& _value ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( _name ); - if ( attrib ) { - attrib->SetValue( _value ); - } -} -#endif - - -void TiXmlElement::Print( FILE* cfile, int depth ) const -{ - int i; - assert( cfile ); - for ( i=0; iNext() ) - { - fprintf( cfile, " " ); - attrib->Print( cfile, depth ); - } - - // There are 3 different formatting approaches: - // 1) An element without children is printed as a node - // 2) An element with only a text child is printed as text - // 3) An element with children is printed on multiple lines. - TiXmlNode* node; - if ( !firstChild ) - { - fprintf( cfile, " />" ); - } - else if ( firstChild == lastChild && firstChild->ToText() ) - { - fprintf( cfile, ">" ); - firstChild->Print( cfile, depth + 1 ); - fprintf( cfile, "", value.c_str() ); - } - else - { - fprintf( cfile, ">" ); - - for ( node = firstChild; node; node=node->NextSibling() ) - { - if ( !node->ToText() ) - { - fprintf( cfile, "\n" ); - } - node->Print( cfile, depth+1 ); - } - fprintf( cfile, "\n" ); - for( i=0; i", value.c_str() ); - } -} - - -void TiXmlElement::CopyTo( TiXmlElement* target ) const -{ - // superclass: - TiXmlNode::CopyTo( target ); - - // Element class: - // Clone the attributes, then clone the children. - const TiXmlAttribute* attribute = 0; - for( attribute = attributeSet.First(); - attribute; - attribute = attribute->Next() ) - { - target->SetAttribute( attribute->Name(), attribute->Value() ); - } - - TiXmlNode* node = 0; - for ( node = firstChild; node; node = node->NextSibling() ) - { - target->LinkEndChild( node->Clone() ); - } -} - -bool TiXmlElement::Accept( TiXmlVisitor* visitor ) const -{ - if ( visitor->VisitEnter( *this, attributeSet.First() ) ) - { - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - if ( !node->Accept( visitor ) ) - break; - } - } - return visitor->VisitExit( *this ); -} - - -TiXmlNode* TiXmlElement::Clone() const -{ - TiXmlElement* clone = new TiXmlElement( Value() ); - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -const char* TiXmlElement::GetText() const -{ - const TiXmlNode* child = this->FirstChild(); - if ( child ) { - const TiXmlText* childText = child->ToText(); - if ( childText ) { - return childText->Value(); - } - } - return 0; -} - - -TiXmlDocument::TiXmlDocument() : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - ClearError(); -} - -TiXmlDocument::TiXmlDocument( const char * documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - value = documentName; - ClearError(); -} - - -#ifdef TIXML_USE_STL -TiXmlDocument::TiXmlDocument( const std::string& documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - value = documentName; - ClearError(); -} -#endif - - -TiXmlDocument::TiXmlDocument( const TiXmlDocument& copy ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - copy.CopyTo( this ); -} - - -TiXmlDocument& TiXmlDocument::operator=( const TiXmlDocument& copy ) -{ - Clear(); - copy.CopyTo( this ); - return *this; -} - - -bool TiXmlDocument::LoadFile( TiXmlEncoding encoding ) -{ - return LoadFile( Value(), encoding ); -} - - -bool TiXmlDocument::SaveFile() const -{ - return SaveFile( Value() ); -} - -bool TiXmlDocument::LoadFile( const char* _filename, TiXmlEncoding encoding ) -{ - TIXML_STRING filename( _filename ); - value = filename; - - // reading in binary mode so that tinyxml can normalize the EOL - FILE* file = TiXmlFOpen( value.c_str (), "rb" ); - - if ( file ) - { - bool result = LoadFile( file, encoding ); - fclose( file ); - return result; - } - else - { - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } -} - -bool TiXmlDocument::LoadFile( FILE* file, TiXmlEncoding encoding ) -{ - if ( !file ) - { - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Delete the existing data: - Clear(); - location.Clear(); - - // Get the file size, so we can pre-allocate the string. HUGE speed impact. - long length = 0; - fseek( file, 0, SEEK_END ); - length = ftell( file ); - fseek( file, 0, SEEK_SET ); - - // Strange case, but good to handle up front. - if ( length <= 0 ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Subtle bug here. TinyXml did use fgets. But from the XML spec: - // 2.11 End-of-Line Handling - // - // - // ...the XML processor MUST behave as if it normalized all line breaks in external - // parsed entities (including the document entity) on input, before parsing, by translating - // both the two-character sequence #xD #xA and any #xD that is not followed by #xA to - // a single #xA character. - // - // - // It is not clear fgets does that, and certainly isn't clear it works cross platform. - // Generally, you expect fgets to translate from the convention of the OS to the c/unix - // convention, and not work generally. - - /* - while( fgets( buf, sizeof(buf), file ) ) - { - data += buf; - } - */ - - char* buf = new char[ length+1 ]; - buf[0] = 0; - - if ( fread( buf, length, 1, file ) != 1 ) { - delete [] buf; - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Process the buffer in place to normalize new lines. (See comment above.) - // Copies from the 'p' to 'q' pointer, where p can advance faster if - // a newline-carriage return is hit. - // - // Wikipedia: - // Systems based on ASCII or a compatible character set use either LF (Line feed, '\n', 0x0A, 10 in decimal) or - // CR (Carriage return, '\r', 0x0D, 13 in decimal) individually, or CR followed by LF (CR+LF, 0x0D 0x0A)... - // * LF: Multics, Unix and Unix-like systems (GNU/Linux, AIX, Xenix, Mac OS X, FreeBSD, etc.), BeOS, Amiga, RISC OS, and others - // * CR+LF: DEC RT-11 and most other early non-Unix, non-IBM OSes, CP/M, MP/M, DOS, OS/2, Microsoft Windows, Symbian OS - // * CR: Commodore 8-bit machines, Apple II family, Mac OS up to version 9 and OS-9 - - const char* p = buf; // the read head - char* q = buf; // the write head - const char CR = 0x0d; - const char LF = 0x0a; - - buf[length] = 0; - while( *p ) { - assert( p < (buf+length) ); - assert( q <= (buf+length) ); - assert( q <= p ); - - if ( *p == CR ) { - *q++ = LF; - p++; - if ( *p == LF ) { // check for CR+LF (and skip LF) - p++; - } - } - else { - *q++ = *p++; - } - } - assert( q <= (buf+length) ); - *q = 0; - - Parse( buf, 0, encoding ); - - delete [] buf; - return !Error(); -} - - -bool TiXmlDocument::SaveFile( const char * filename ) const -{ - // The old c stuff lives on... - FILE* fp = TiXmlFOpen( filename, "w" ); - if ( fp ) - { - bool result = SaveFile( fp ); - fclose( fp ); - return result; - } - return false; -} - - -bool TiXmlDocument::SaveFile( FILE* fp ) const -{ - if ( useMicrosoftBOM ) - { - const unsigned char TIXML_UTF_LEAD_0 = 0xefU; - const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; - const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; - - fputc( TIXML_UTF_LEAD_0, fp ); - fputc( TIXML_UTF_LEAD_1, fp ); - fputc( TIXML_UTF_LEAD_2, fp ); - } - Print( fp, 0 ); - return (ferror(fp) == 0); -} - - -void TiXmlDocument::CopyTo( TiXmlDocument* target ) const -{ - TiXmlNode::CopyTo( target ); - - target->error = error; - target->errorId = errorId; - target->errorDesc = errorDesc; - target->tabsize = tabsize; - target->errorLocation = errorLocation; - target->useMicrosoftBOM = useMicrosoftBOM; - - TiXmlNode* node = 0; - for ( node = firstChild; node; node = node->NextSibling() ) - { - target->LinkEndChild( node->Clone() ); - } -} - - -TiXmlNode* TiXmlDocument::Clone() const -{ - TiXmlDocument* clone = new TiXmlDocument(); - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlDocument::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - node->Print( cfile, depth ); - fprintf( cfile, "\n" ); - } -} - - -bool TiXmlDocument::Accept( TiXmlVisitor* visitor ) const -{ - if ( visitor->VisitEnter( *this ) ) - { - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - if ( !node->Accept( visitor ) ) - break; - } - } - return visitor->VisitExit( *this ); -} - - -const TiXmlAttribute* TiXmlAttribute::Next() const -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( next->value.empty() && next->name.empty() ) - return 0; - return next; -} - -/* -TiXmlAttribute* TiXmlAttribute::Next() -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( next->value.empty() && next->name.empty() ) - return 0; - return next; -} -*/ - -const TiXmlAttribute* TiXmlAttribute::Previous() const -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( prev->value.empty() && prev->name.empty() ) - return 0; - return prev; -} - -/* -TiXmlAttribute* TiXmlAttribute::Previous() -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( prev->value.empty() && prev->name.empty() ) - return 0; - return prev; -} -*/ - -void TiXmlAttribute::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const -{ - TIXML_STRING n, v; - - EncodeString( name, &n ); - EncodeString( value, &v ); - - if (value.find ('\"') == TIXML_STRING::npos) { - if ( cfile ) { - fprintf (cfile, "%s=\"%s\"", n.c_str(), v.c_str() ); - } - if ( str ) { - (*str) += n; (*str) += "=\""; (*str) += v; (*str) += "\""; - } - } - else { - if ( cfile ) { - fprintf (cfile, "%s='%s'", n.c_str(), v.c_str() ); - } - if ( str ) { - (*str) += n; (*str) += "='"; (*str) += v; (*str) += "'"; - } - } -} - - -int TiXmlAttribute::QueryIntValue( int* ival ) const -{ - if ( TIXML_SSCANF( value.c_str(), "%d", ival ) == 1 ) - return TIXML_SUCCESS; - return TIXML_WRONG_TYPE; -} - -int TiXmlAttribute::QueryDoubleValue( double* dval ) const -{ - if ( TIXML_SSCANF( value.c_str(), "%lf", dval ) == 1 ) - return TIXML_SUCCESS; - return TIXML_WRONG_TYPE; -} - -void TiXmlAttribute::SetIntValue( int _value ) -{ - char buf [64]; - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF(buf, sizeof(buf), "%d", _value); - #else - sprintf (buf, "%d", _value); - #endif - SetValue (buf); -} - -void TiXmlAttribute::SetDoubleValue( double _value ) -{ - char buf [256]; - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF( buf, sizeof(buf), "%g", _value); - #else - sprintf (buf, "%g", _value); - #endif - SetValue (buf); -} - -int TiXmlAttribute::IntValue() const -{ - return atoi (value.c_str ()); -} - -double TiXmlAttribute::DoubleValue() const -{ - return atof (value.c_str ()); -} - - -TiXmlComment::TiXmlComment( const TiXmlComment& copy ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) -{ - copy.CopyTo( this ); -} - - -TiXmlComment& TiXmlComment::operator=( const TiXmlComment& base ) -{ - Clear(); - base.CopyTo( this ); - return *this; -} - - -void TiXmlComment::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - for ( int i=0; i", value.c_str() ); -} - - -void TiXmlComment::CopyTo( TiXmlComment* target ) const -{ - TiXmlNode::CopyTo( target ); -} - - -bool TiXmlComment::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlComment::Clone() const -{ - TiXmlComment* clone = new TiXmlComment(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlText::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - if ( cdata ) - { - int i; - fprintf( cfile, "\n" ); - for ( i=0; i\n", value.c_str() ); // unformatted output - } - else - { - TIXML_STRING buffer; - EncodeString( value, &buffer ); - fprintf( cfile, "%s", buffer.c_str() ); - } -} - - -void TiXmlText::CopyTo( TiXmlText* target ) const -{ - TiXmlNode::CopyTo( target ); - target->cdata = cdata; -} - - -bool TiXmlText::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlText::Clone() const -{ - TiXmlText* clone = 0; - clone = new TiXmlText( "" ); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -TiXmlDeclaration::TiXmlDeclaration( const char * _version, - const char * _encoding, - const char * _standalone ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - version = _version; - encoding = _encoding; - standalone = _standalone; -} - - -#ifdef TIXML_USE_STL -TiXmlDeclaration::TiXmlDeclaration( const std::string& _version, - const std::string& _encoding, - const std::string& _standalone ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - version = _version; - encoding = _encoding; - standalone = _standalone; -} -#endif - - -TiXmlDeclaration::TiXmlDeclaration( const TiXmlDeclaration& copy ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - copy.CopyTo( this ); -} - - -TiXmlDeclaration& TiXmlDeclaration::operator=( const TiXmlDeclaration& copy ) -{ - Clear(); - copy.CopyTo( this ); - return *this; -} - - -void TiXmlDeclaration::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const -{ - if ( cfile ) fprintf( cfile, "" ); - if ( str ) (*str) += "?>"; -} - - -void TiXmlDeclaration::CopyTo( TiXmlDeclaration* target ) const -{ - TiXmlNode::CopyTo( target ); - - target->version = version; - target->encoding = encoding; - target->standalone = standalone; -} - - -bool TiXmlDeclaration::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlDeclaration::Clone() const -{ - TiXmlDeclaration* clone = new TiXmlDeclaration(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlUnknown::Print( FILE* cfile, int depth ) const -{ - for ( int i=0; i", value.c_str() ); -} - - -void TiXmlUnknown::CopyTo( TiXmlUnknown* target ) const -{ - TiXmlNode::CopyTo( target ); -} - - -bool TiXmlUnknown::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlUnknown::Clone() const -{ - TiXmlUnknown* clone = new TiXmlUnknown(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -TiXmlAttributeSet::TiXmlAttributeSet() -{ - sentinel.next = &sentinel; - sentinel.prev = &sentinel; -} - - -TiXmlAttributeSet::~TiXmlAttributeSet() -{ - assert( sentinel.next == &sentinel ); - assert( sentinel.prev == &sentinel ); -} - - -void TiXmlAttributeSet::Add( TiXmlAttribute* addMe ) -{ - #ifdef TIXML_USE_STL - assert( !Find( TIXML_STRING( addMe->Name() ) ) ); // Shouldn't be multiply adding to the set. - #else - assert( !Find( addMe->Name() ) ); // Shouldn't be multiply adding to the set. - #endif - - addMe->next = &sentinel; - addMe->prev = sentinel.prev; - - sentinel.prev->next = addMe; - sentinel.prev = addMe; -} - -void TiXmlAttributeSet::Remove( TiXmlAttribute* removeMe ) -{ - TiXmlAttribute* node; - - for( node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( node == removeMe ) - { - node->prev->next = node->next; - node->next->prev = node->prev; - node->next = 0; - node->prev = 0; - return; - } - } - assert( 0 ); // we tried to remove a non-linked attribute. -} - - -#ifdef TIXML_USE_STL -TiXmlAttribute* TiXmlAttributeSet::Find( const std::string& name ) const -{ - for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( node->name == name ) - return node; - } - return 0; -} - -TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const std::string& _name ) -{ - TiXmlAttribute* attrib = Find( _name ); - if ( !attrib ) { - attrib = new TiXmlAttribute(); - Add( attrib ); - attrib->SetName( _name ); - } - return attrib; -} -#endif - - -TiXmlAttribute* TiXmlAttributeSet::Find( const char* name ) const -{ - for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( strcmp( node->name.c_str(), name ) == 0 ) - return node; - } - return 0; -} - - -TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const char* _name ) -{ - TiXmlAttribute* attrib = Find( _name ); - if ( !attrib ) { - attrib = new TiXmlAttribute(); - Add( attrib ); - attrib->SetName( _name ); - } - return attrib; -} - - -#ifdef TIXML_USE_STL -std::istream& operator>> (std::istream & in, TiXmlNode & base) -{ - TIXML_STRING tag; - tag.reserve( 8 * 1000 ); - base.StreamIn( &in, &tag ); - - base.Parse( tag.c_str(), 0, TIXML_DEFAULT_ENCODING ); - return in; -} -#endif - - -#ifdef TIXML_USE_STL -std::ostream& operator<< (std::ostream & out, const TiXmlNode & base) -{ - TiXmlPrinter printer; - printer.SetStreamPrinting(); - base.Accept( &printer ); - out << printer.Str(); - - return out; -} - - -std::string& operator<< (std::string& out, const TiXmlNode& base ) -{ - TiXmlPrinter printer; - printer.SetStreamPrinting(); - base.Accept( &printer ); - out.append( printer.Str() ); - - return out; -} -#endif - - -TiXmlHandle TiXmlHandle::FirstChild() const -{ - if ( node ) - { - TiXmlNode* child = node->FirstChild(); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChild( const char * _value ) const -{ - if ( node ) - { - TiXmlNode* child = node->FirstChild( _value ); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChildElement() const -{ - if ( node ) - { - TiXmlElement* child = node->FirstChildElement(); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChildElement( const char * _value ) const -{ - if ( node ) - { - TiXmlElement* child = node->FirstChildElement( _value ); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::Child( int count ) const -{ - if ( node ) - { - int i; - TiXmlNode* child = node->FirstChild(); - for ( i=0; - child && iNextSibling(), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::Child( const char* _value, int count ) const -{ - if ( node ) - { - int i; - TiXmlNode* child = node->FirstChild( _value ); - for ( i=0; - child && iNextSibling( _value ), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::ChildElement( int count ) const -{ - if ( node ) - { - int i; - TiXmlElement* child = node->FirstChildElement(); - for ( i=0; - child && iNextSiblingElement(), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::ChildElement( const char* _value, int count ) const -{ - if ( node ) - { - int i; - TiXmlElement* child = node->FirstChildElement( _value ); - for ( i=0; - child && iNextSiblingElement( _value ), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -bool TiXmlPrinter::VisitEnter( const TiXmlDocument& ) -{ - return true; -} - -bool TiXmlPrinter::VisitExit( const TiXmlDocument& ) -{ - return true; -} - -bool TiXmlPrinter::VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ) -{ - DoIndent(); - buffer += "<"; - buffer += element.Value(); - - for( const TiXmlAttribute* attrib = firstAttribute; attrib; attrib = attrib->Next() ) - { - buffer += " "; - attrib->Print( 0, 0, &buffer ); - } - - if ( !element.FirstChild() ) - { - buffer += " />"; - DoLineBreak(); - } - else - { - buffer += ">"; - if ( element.FirstChild()->ToText() - && element.LastChild() == element.FirstChild() - && element.FirstChild()->ToText()->CDATA() == false ) - { - simpleTextPrint = true; - // no DoLineBreak()! - } - else - { - DoLineBreak(); - } - } - ++depth; - return true; -} - - -bool TiXmlPrinter::VisitExit( const TiXmlElement& element ) -{ - --depth; - if ( !element.FirstChild() ) - { - // nothing. - } - else - { - if ( simpleTextPrint ) - { - simpleTextPrint = false; - } - else - { - DoIndent(); - } - buffer += ""; - DoLineBreak(); - } - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlText& text ) -{ - if ( text.CDATA() ) - { - DoIndent(); - buffer += ""; - DoLineBreak(); - } - else if ( simpleTextPrint ) - { - TIXML_STRING str; - TiXmlBase::EncodeString( text.ValueTStr(), &str ); - buffer += str; - } - else - { - DoIndent(); - TIXML_STRING str; - TiXmlBase::EncodeString( text.ValueTStr(), &str ); - buffer += str; - DoLineBreak(); - } - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlDeclaration& declaration ) -{ - DoIndent(); - declaration.Print( 0, 0, &buffer ); - DoLineBreak(); - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlComment& comment ) -{ - DoIndent(); - buffer += ""; - DoLineBreak(); - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlUnknown& unknown ) -{ - DoIndent(); - buffer += "<"; - buffer += unknown.Value(); - buffer += ">"; - DoLineBreak(); - return true; -} - diff --git a/src/win/tinyxml/tinyxml.h b/src/win/tinyxml/tinyxml.h deleted file mode 100644 index a3589e5b2..000000000 --- a/src/win/tinyxml/tinyxml.h +++ /dev/null @@ -1,1805 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml -Original code by Lee Thomason (www.grinninglizard.com) - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any -damages arising from the use of this software. - -Permission is granted to anyone to use this software for any -purpose, including commercial applications, and to alter it and -redistribute it freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. - -2. Altered source versions must be plainly marked as such, and -must not be misrepresented as being the original software. - -3. This notice may not be removed or altered from any source -distribution. -*/ - - -#ifndef TINYXML_INCLUDED -#define TINYXML_INCLUDED - -#ifdef _MSC_VER -#pragma warning( push ) -#pragma warning( disable : 4530 ) -#pragma warning( disable : 4786 ) -#endif - -#include -#include -#include -#include -#include - -// Help out windows: -#if defined( _DEBUG ) && !defined( DEBUG ) -#define DEBUG -#endif - -#ifdef TIXML_USE_STL - #include - #include - #include - #define TIXML_STRING std::string -#else - #include "tinystr.h" - #define TIXML_STRING TiXmlString -#endif - -// Deprecated library function hell. Compilers want to use the -// new safe versions. This probably doesn't fully address the problem, -// but it gets closer. There are too many compilers for me to fully -// test. If you get compilation troubles, undefine TIXML_SAFE -#define TIXML_SAFE - -#ifdef TIXML_SAFE - #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) - // Microsoft visual studio, version 2005 and higher. - #define TIXML_SNPRINTF _snprintf_s - #define TIXML_SSCANF sscanf_s - #elif defined(_MSC_VER) && (_MSC_VER >= 1200 ) - // Microsoft visual studio, version 6 and higher. - //#pragma message( "Using _sn* functions." ) - #define TIXML_SNPRINTF _snprintf - #define TIXML_SSCANF sscanf - #elif defined(__GNUC__) && (__GNUC__ >= 3 ) - // GCC version 3 and higher.s - //#warning( "Using sn* functions." ) - #define TIXML_SNPRINTF snprintf - #define TIXML_SSCANF sscanf - #else - #define TIXML_SNPRINTF snprintf - #define TIXML_SSCANF sscanf - #endif -#endif - -class TiXmlDocument; -class TiXmlElement; -class TiXmlComment; -class TiXmlUnknown; -class TiXmlAttribute; -class TiXmlText; -class TiXmlDeclaration; -class TiXmlParsingData; - -const int TIXML_MAJOR_VERSION = 2; -const int TIXML_MINOR_VERSION = 6; -const int TIXML_PATCH_VERSION = 2; - -/* Internal structure for tracking location of items - in the XML file. -*/ -struct TiXmlCursor -{ - TiXmlCursor() { Clear(); } - void Clear() { row = col = -1; } - - int row; // 0 based. - int col; // 0 based. -}; - - -/** - Implements the interface to the "Visitor pattern" (see the Accept() method.) - If you call the Accept() method, it requires being passed a TiXmlVisitor - class to handle callbacks. For nodes that contain other nodes (Document, Element) - you will get called with a VisitEnter/VisitExit pair. Nodes that are always leaves - are simply called with Visit(). - - If you return 'true' from a Visit method, recursive parsing will continue. If you return - false, no children of this node or its sibilings will be Visited. - - All flavors of Visit methods have a default implementation that returns 'true' (continue - visiting). You need to only override methods that are interesting to you. - - Generally Accept() is called on the TiXmlDocument, although all nodes suppert Visiting. - - You should never change the document from a callback. - - @sa TiXmlNode::Accept() -*/ -class TiXmlVisitor -{ -public: - virtual ~TiXmlVisitor() {} - - /// Visit a document. - virtual bool VisitEnter( const TiXmlDocument& /*doc*/ ) { return true; } - /// Visit a document. - virtual bool VisitExit( const TiXmlDocument& /*doc*/ ) { return true; } - - /// Visit an element. - virtual bool VisitEnter( const TiXmlElement& /*element*/, const TiXmlAttribute* /*firstAttribute*/ ) { return true; } - /// Visit an element. - virtual bool VisitExit( const TiXmlElement& /*element*/ ) { return true; } - - /// Visit a declaration - virtual bool Visit( const TiXmlDeclaration& /*declaration*/ ) { return true; } - /// Visit a text node - virtual bool Visit( const TiXmlText& /*text*/ ) { return true; } - /// Visit a comment node - virtual bool Visit( const TiXmlComment& /*comment*/ ) { return true; } - /// Visit an unknown node - virtual bool Visit( const TiXmlUnknown& /*unknown*/ ) { return true; } -}; - -// Only used by Attribute::Query functions -enum -{ - TIXML_SUCCESS, - TIXML_NO_ATTRIBUTE, - TIXML_WRONG_TYPE -}; - - -// Used by the parsing routines. -enum TiXmlEncoding -{ - TIXML_ENCODING_UNKNOWN, - TIXML_ENCODING_UTF8, - TIXML_ENCODING_LEGACY -}; - -const TiXmlEncoding TIXML_DEFAULT_ENCODING = TIXML_ENCODING_UNKNOWN; - -/** TiXmlBase is a base class for every class in TinyXml. - It does little except to establish that TinyXml classes - can be printed and provide some utility functions. - - In XML, the document and elements can contain - other elements and other types of nodes. - - @verbatim - A Document can contain: Element (container or leaf) - Comment (leaf) - Unknown (leaf) - Declaration( leaf ) - - An Element can contain: Element (container or leaf) - Text (leaf) - Attributes (not on tree) - Comment (leaf) - Unknown (leaf) - - A Decleration contains: Attributes (not on tree) - @endverbatim -*/ -class TiXmlBase -{ - friend class TiXmlNode; - friend class TiXmlElement; - friend class TiXmlDocument; - -public: - TiXmlBase() : userData(0) {} - virtual ~TiXmlBase() {} - - /** All TinyXml classes can print themselves to a filestream - or the string class (TiXmlString in non-STL mode, std::string - in STL mode.) Either or both cfile and str can be null. - - This is a formatted print, and will insert - tabs and newlines. - - (For an unformatted stream, use the << operator.) - */ - virtual void Print( FILE* cfile, int depth ) const = 0; - - /** The world does not agree on whether white space should be kept or - not. In order to make everyone happy, these global, static functions - are provided to set whether or not TinyXml will condense all white space - into a single space or not. The default is to condense. Note changing this - value is not thread safe. - */ - static void SetCondenseWhiteSpace( bool condense ) { condenseWhiteSpace = condense; } - - /// Return the current white space setting. - static bool IsWhiteSpaceCondensed() { return condenseWhiteSpace; } - - /** Return the position, in the original source file, of this node or attribute. - The row and column are 1-based. (That is the first row and first column is - 1,1). If the returns values are 0 or less, then the parser does not have - a row and column value. - - Generally, the row and column value will be set when the TiXmlDocument::Load(), - TiXmlDocument::LoadFile(), or any TiXmlNode::Parse() is called. It will NOT be set - when the DOM was created from operator>>. - - The values reflect the initial load. Once the DOM is modified programmatically - (by adding or changing nodes and attributes) the new values will NOT update to - reflect changes in the document. - - There is a minor performance cost to computing the row and column. Computation - can be disabled if TiXmlDocument::SetTabSize() is called with 0 as the value. - - @sa TiXmlDocument::SetTabSize() - */ - int Row() const { return location.row + 1; } - int Column() const { return location.col + 1; } ///< See Row() - - void SetUserData( void* user ) { userData = user; } ///< Set a pointer to arbitrary user data. - void* GetUserData() { return userData; } ///< Get a pointer to arbitrary user data. - const void* GetUserData() const { return userData; } ///< Get a pointer to arbitrary user data. - - // Table that returs, for a given lead byte, the total number of bytes - // in the UTF-8 sequence. - static const int utf8ByteTable[256]; - - virtual const char* Parse( const char* p, - TiXmlParsingData* data, - TiXmlEncoding encoding /*= TIXML_ENCODING_UNKNOWN */ ) = 0; - - /** Expands entities in a string. Note this should not contian the tag's '<', '>', etc, - or they will be transformed into entities! - */ - static void EncodeString( const TIXML_STRING& str, TIXML_STRING* out ); - - enum - { - TIXML_NO_ERROR = 0, - TIXML_ERROR, - TIXML_ERROR_OPENING_FILE, - TIXML_ERROR_PARSING_ELEMENT, - TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME, - TIXML_ERROR_READING_ELEMENT_VALUE, - TIXML_ERROR_READING_ATTRIBUTES, - TIXML_ERROR_PARSING_EMPTY, - TIXML_ERROR_READING_END_TAG, - TIXML_ERROR_PARSING_UNKNOWN, - TIXML_ERROR_PARSING_COMMENT, - TIXML_ERROR_PARSING_DECLARATION, - TIXML_ERROR_DOCUMENT_EMPTY, - TIXML_ERROR_EMBEDDED_NULL, - TIXML_ERROR_PARSING_CDATA, - TIXML_ERROR_DOCUMENT_TOP_ONLY, - - TIXML_ERROR_STRING_COUNT - }; - -protected: - - static const char* SkipWhiteSpace( const char*, TiXmlEncoding encoding ); - - inline static bool IsWhiteSpace( char c ) - { - return ( isspace( (unsigned char) c ) || c == '\n' || c == '\r' ); - } - inline static bool IsWhiteSpace( int c ) - { - if ( c < 256 ) - return IsWhiteSpace( (char) c ); - return false; // Again, only truly correct for English/Latin...but usually works. - } - - #ifdef TIXML_USE_STL - static bool StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ); - static bool StreamTo( std::istream * in, int character, TIXML_STRING * tag ); - #endif - - /* Reads an XML name into the string provided. Returns - a pointer just past the last character of the name, - or 0 if the function has an error. - */ - static const char* ReadName( const char* p, TIXML_STRING* name, TiXmlEncoding encoding ); - - /* Reads text. Returns a pointer past the given end tag. - Wickedly complex options, but it keeps the (sensitive) code in one place. - */ - static const char* ReadText( const char* in, // where to start - TIXML_STRING* text, // the string read - bool ignoreWhiteSpace, // whether to keep the white space - const char* endTag, // what ends this text - bool ignoreCase, // whether to ignore case in the end tag - TiXmlEncoding encoding ); // the current encoding - - // If an entity has been found, transform it into a character. - static const char* GetEntity( const char* in, char* value, int* length, TiXmlEncoding encoding ); - - // Get a character, while interpreting entities. - // The length can be from 0 to 4 bytes. - inline static const char* GetChar( const char* p, char* _value, int* length, TiXmlEncoding encoding ) - { - assert( p ); - if ( encoding == TIXML_ENCODING_UTF8 ) - { - *length = utf8ByteTable[ *((const unsigned char*)p) ]; - assert( *length >= 0 && *length < 5 ); - } - else - { - *length = 1; - } - - if ( *length == 1 ) - { - if ( *p == '&' ) - return GetEntity( p, _value, length, encoding ); - *_value = *p; - return p+1; - } - else if ( *length ) - { - //strncpy( _value, p, *length ); // lots of compilers don't like this function (unsafe), - // and the null terminator isn't needed - for( int i=0; p[i] && i<*length; ++i ) { - _value[i] = p[i]; - } - return p + (*length); - } - else - { - // Not valid text. - return 0; - } - } - - // Return true if the next characters in the stream are any of the endTag sequences. - // Ignore case only works for english, and should only be relied on when comparing - // to English words: StringEqual( p, "version", true ) is fine. - static bool StringEqual( const char* p, - const char* endTag, - bool ignoreCase, - TiXmlEncoding encoding ); - - static const char* errorString[ TIXML_ERROR_STRING_COUNT ]; - - TiXmlCursor location; - - /// Field containing a generic user pointer - void* userData; - - // None of these methods are reliable for any language except English. - // Good for approximation, not great for accuracy. - static int IsAlpha( unsigned char anyByte, TiXmlEncoding encoding ); - static int IsAlphaNum( unsigned char anyByte, TiXmlEncoding encoding ); - inline static int ToLower( int v, TiXmlEncoding encoding ) - { - if ( encoding == TIXML_ENCODING_UTF8 ) - { - if ( v < 128 ) return tolower( v ); - return v; - } - else - { - return tolower( v ); - } - } - static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ); - -private: - TiXmlBase( const TiXmlBase& ); // not implemented. - void operator=( const TiXmlBase& base ); // not allowed. - - struct Entity - { - const char* str; - unsigned int strLength; - char chr; - }; - enum - { - NUM_ENTITY = 5, - MAX_ENTITY_LENGTH = 6 - - }; - static Entity entity[ NUM_ENTITY ]; - static bool condenseWhiteSpace; -}; - - -/** The parent class for everything in the Document Object Model. - (Except for attributes). - Nodes have siblings, a parent, and children. A node can be - in a document, or stand on its own. The type of a TiXmlNode - can be queried, and it can be cast to its more defined type. -*/ -class TiXmlNode : public TiXmlBase -{ - friend class TiXmlDocument; - friend class TiXmlElement; - -public: - #ifdef TIXML_USE_STL - - /** An input stream operator, for every class. Tolerant of newlines and - formatting, but doesn't expect them. - */ - friend std::istream& operator >> (std::istream& in, TiXmlNode& base); - - /** An output stream operator, for every class. Note that this outputs - without any newlines or formatting, as opposed to Print(), which - includes tabs and new lines. - - The operator<< and operator>> are not completely symmetric. Writing - a node to a stream is very well defined. You'll get a nice stream - of output, without any extra whitespace or newlines. - - But reading is not as well defined. (As it always is.) If you create - a TiXmlElement (for example) and read that from an input stream, - the text needs to define an element or junk will result. This is - true of all input streams, but it's worth keeping in mind. - - A TiXmlDocument will read nodes until it reads a root element, and - all the children of that root element. - */ - friend std::ostream& operator<< (std::ostream& out, const TiXmlNode& base); - - /// Appends the XML node or attribute to a std::string. - friend std::string& operator<< (std::string& out, const TiXmlNode& base ); - - #endif - - /** The types of XML nodes supported by TinyXml. (All the - unsupported types are picked up by UNKNOWN.) - */ - enum NodeType - { - TINYXML_DOCUMENT, - TINYXML_ELEMENT, - TINYXML_COMMENT, - TINYXML_UNKNOWN, - TINYXML_TEXT, - TINYXML_DECLARATION, - TINYXML_TYPECOUNT - }; - - virtual ~TiXmlNode(); - - /** The meaning of 'value' changes for the specific type of - TiXmlNode. - @verbatim - Document: filename of the xml file - Element: name of the element - Comment: the comment text - Unknown: the tag contents - Text: the text string - @endverbatim - - The subclasses will wrap this function. - */ - const char *Value() const { return value.c_str (); } - - #ifdef TIXML_USE_STL - /** Return Value() as a std::string. If you only use STL, - this is more efficient than calling Value(). - Only available in STL mode. - */ - const std::string& ValueStr() const { return value; } - #endif - - const TIXML_STRING& ValueTStr() const { return value; } - - /** Changes the value of the node. Defined as: - @verbatim - Document: filename of the xml file - Element: name of the element - Comment: the comment text - Unknown: the tag contents - Text: the text string - @endverbatim - */ - void SetValue(const char * _value) { value = _value;} - - #ifdef TIXML_USE_STL - /// STL std::string form. - void SetValue( const std::string& _value ) { value = _value; } - #endif - - /// Delete all the children of this node. Does not affect 'this'. - void Clear(); - - /// One step up the DOM. - TiXmlNode* Parent() { return parent; } - const TiXmlNode* Parent() const { return parent; } - - const TiXmlNode* FirstChild() const { return firstChild; } ///< The first child of this node. Will be null if there are no children. - TiXmlNode* FirstChild() { return firstChild; } - const TiXmlNode* FirstChild( const char * value ) const; ///< The first child of this node with the matching 'value'. Will be null if none found. - /// The first child of this node with the matching 'value'. Will be null if none found. - TiXmlNode* FirstChild( const char * _value ) { - // Call through to the const version - safe since nothing is changed. Exiting syntax: cast this to a const (always safe) - // call the method, cast the return back to non-const. - return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->FirstChild( _value )); - } - const TiXmlNode* LastChild() const { return lastChild; } /// The last child of this node. Will be null if there are no children. - TiXmlNode* LastChild() { return lastChild; } - - const TiXmlNode* LastChild( const char * value ) const; /// The last child of this node matching 'value'. Will be null if there are no children. - TiXmlNode* LastChild( const char * _value ) { - return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->LastChild( _value )); - } - - #ifdef TIXML_USE_STL - const TiXmlNode* FirstChild( const std::string& _value ) const { return FirstChild (_value.c_str ()); } ///< STL std::string form. - TiXmlNode* FirstChild( const std::string& _value ) { return FirstChild (_value.c_str ()); } ///< STL std::string form. - const TiXmlNode* LastChild( const std::string& _value ) const { return LastChild (_value.c_str ()); } ///< STL std::string form. - TiXmlNode* LastChild( const std::string& _value ) { return LastChild (_value.c_str ()); } ///< STL std::string form. - #endif - - /** An alternate way to walk the children of a node. - One way to iterate over nodes is: - @verbatim - for( child = parent->FirstChild(); child; child = child->NextSibling() ) - @endverbatim - - IterateChildren does the same thing with the syntax: - @verbatim - child = 0; - while( child = parent->IterateChildren( child ) ) - @endverbatim - - IterateChildren takes the previous child as input and finds - the next one. If the previous child is null, it returns the - first. IterateChildren will return null when done. - */ - const TiXmlNode* IterateChildren( const TiXmlNode* previous ) const; - TiXmlNode* IterateChildren( const TiXmlNode* previous ) { - return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( previous ) ); - } - - /// This flavor of IterateChildren searches for children with a particular 'value' - const TiXmlNode* IterateChildren( const char * value, const TiXmlNode* previous ) const; - TiXmlNode* IterateChildren( const char * _value, const TiXmlNode* previous ) { - return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( _value, previous ) ); - } - - #ifdef TIXML_USE_STL - const TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) const { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. - TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. - #endif - - /** Add a new node related to this. Adds a child past the LastChild. - Returns a pointer to the new object or NULL if an error occured. - */ - TiXmlNode* InsertEndChild( const TiXmlNode& addThis ); - - - /** Add a new node related to this. Adds a child past the LastChild. - - NOTE: the node to be added is passed by pointer, and will be - henceforth owned (and deleted) by tinyXml. This method is efficient - and avoids an extra copy, but should be used with care as it - uses a different memory model than the other insert functions. - - @sa InsertEndChild - */ - TiXmlNode* LinkEndChild( TiXmlNode* addThis ); - - /** Add a new node related to this. Adds a child before the specified child. - Returns a pointer to the new object or NULL if an error occured. - */ - TiXmlNode* InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ); - - /** Add a new node related to this. Adds a child after the specified child. - Returns a pointer to the new object or NULL if an error occured. - */ - TiXmlNode* InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ); - - /** Replace a child of this node. - Returns a pointer to the new object or NULL if an error occured. - */ - TiXmlNode* ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ); - - /// Delete a child of this node. - bool RemoveChild( TiXmlNode* removeThis ); - - /// Navigate to a sibling node. - const TiXmlNode* PreviousSibling() const { return prev; } - TiXmlNode* PreviousSibling() { return prev; } - - /// Navigate to a sibling node. - const TiXmlNode* PreviousSibling( const char * ) const; - TiXmlNode* PreviousSibling( const char *_prev ) { - return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->PreviousSibling( _prev ) ); - } - - #ifdef TIXML_USE_STL - const TiXmlNode* PreviousSibling( const std::string& _value ) const { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. - TiXmlNode* PreviousSibling( const std::string& _value ) { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. - const TiXmlNode* NextSibling( const std::string& _value) const { return NextSibling (_value.c_str ()); } ///< STL std::string form. - TiXmlNode* NextSibling( const std::string& _value) { return NextSibling (_value.c_str ()); } ///< STL std::string form. - #endif - - /// Navigate to a sibling node. - const TiXmlNode* NextSibling() const { return next; } - TiXmlNode* NextSibling() { return next; } - - /// Navigate to a sibling node with the given 'value'. - const TiXmlNode* NextSibling( const char * ) const; - TiXmlNode* NextSibling( const char* _next ) { - return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->NextSibling( _next ) ); - } - - /** Convenience function to get through elements. - Calls NextSibling and ToElement. Will skip all non-Element - nodes. Returns 0 if there is not another element. - */ - const TiXmlElement* NextSiblingElement() const; - TiXmlElement* NextSiblingElement() { - return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement() ); - } - - /** Convenience function to get through elements. - Calls NextSibling and ToElement. Will skip all non-Element - nodes. Returns 0 if there is not another element. - */ - const TiXmlElement* NextSiblingElement( const char * ) const; - TiXmlElement* NextSiblingElement( const char *_next ) { - return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement( _next ) ); - } - - #ifdef TIXML_USE_STL - const TiXmlElement* NextSiblingElement( const std::string& _value) const { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. - TiXmlElement* NextSiblingElement( const std::string& _value) { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. - #endif - - /// Convenience function to get through elements. - const TiXmlElement* FirstChildElement() const; - TiXmlElement* FirstChildElement() { - return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement() ); - } - - /// Convenience function to get through elements. - const TiXmlElement* FirstChildElement( const char * _value ) const; - TiXmlElement* FirstChildElement( const char * _value ) { - return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement( _value ) ); - } - - #ifdef TIXML_USE_STL - const TiXmlElement* FirstChildElement( const std::string& _value ) const { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. - TiXmlElement* FirstChildElement( const std::string& _value ) { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. - #endif - - /** Query the type (as an enumerated value, above) of this node. - The possible types are: TINYXML_DOCUMENT, TINYXML_ELEMENT, TINYXML_COMMENT, - TINYXML_UNKNOWN, TINYXML_TEXT, and TINYXML_DECLARATION. - */ - int Type() const { return type; } - - /** Return a pointer to the Document this node lives in. - Returns null if not in a document. - */ - const TiXmlDocument* GetDocument() const; - TiXmlDocument* GetDocument() { - return const_cast< TiXmlDocument* >( (const_cast< const TiXmlNode* >(this))->GetDocument() ); - } - - /// Returns true if this node has no children. - bool NoChildren() const { return !firstChild; } - - virtual const TiXmlDocument* ToDocument() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlElement* ToElement() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlComment* ToComment() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlUnknown* ToUnknown() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlText* ToText() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual const TiXmlDeclaration* ToDeclaration() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - - virtual TiXmlDocument* ToDocument() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlElement* ToElement() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlComment* ToComment() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlUnknown* ToUnknown() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlText* ToText() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - virtual TiXmlDeclaration* ToDeclaration() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. - - /** Create an exact duplicate of this node and return it. The memory must be deleted - by the caller. - */ - virtual TiXmlNode* Clone() const = 0; - - /** Accept a hierchical visit the nodes in the TinyXML DOM. Every node in the - XML tree will be conditionally visited and the host will be called back - via the TiXmlVisitor interface. - - This is essentially a SAX interface for TinyXML. (Note however it doesn't re-parse - the XML for the callbacks, so the performance of TinyXML is unchanged by using this - interface versus any other.) - - The interface has been based on ideas from: - - - http://www.saxproject.org/ - - http://c2.com/cgi/wiki?HierarchicalVisitorPattern - - Which are both good references for "visiting". - - An example of using Accept(): - @verbatim - TiXmlPrinter printer; - tinyxmlDoc.Accept( &printer ); - const char* xmlcstr = printer.CStr(); - @endverbatim - */ - virtual bool Accept( TiXmlVisitor* visitor ) const = 0; - -protected: - TiXmlNode( NodeType _type ); - - // Copy to the allocated object. Shared functionality between Clone, Copy constructor, - // and the assignment operator. - void CopyTo( TiXmlNode* target ) const; - - #ifdef TIXML_USE_STL - // The real work of the input operator. - virtual void StreamIn( std::istream* in, TIXML_STRING* tag ) = 0; - #endif - - // Figure out what is at *p, and parse it. Returns null if it is not an xml node. - TiXmlNode* Identify( const char* start, TiXmlEncoding encoding ); - - TiXmlNode* parent; - NodeType type; - - TiXmlNode* firstChild; - TiXmlNode* lastChild; - - TIXML_STRING value; - - TiXmlNode* prev; - TiXmlNode* next; - -private: - TiXmlNode( const TiXmlNode& ); // not implemented. - void operator=( const TiXmlNode& base ); // not allowed. -}; - - -/** An attribute is a name-value pair. Elements have an arbitrary - number of attributes, each with a unique name. - - @note The attributes are not TiXmlNodes, since they are not - part of the tinyXML document object model. There are other - suggested ways to look at this problem. -*/ -class TiXmlAttribute : public TiXmlBase -{ - friend class TiXmlAttributeSet; - -public: - /// Construct an empty attribute. - TiXmlAttribute() : TiXmlBase() - { - document = 0; - prev = next = 0; - } - - #ifdef TIXML_USE_STL - /// std::string constructor. - TiXmlAttribute( const std::string& _name, const std::string& _value ) - { - name = _name; - value = _value; - document = 0; - prev = next = 0; - } - #endif - - /// Construct an attribute with a name and value. - TiXmlAttribute( const char * _name, const char * _value ) - { - name = _name; - value = _value; - document = 0; - prev = next = 0; - } - - const char* Name() const { return name.c_str(); } ///< Return the name of this attribute. - const char* Value() const { return value.c_str(); } ///< Return the value of this attribute. - #ifdef TIXML_USE_STL - const std::string& ValueStr() const { return value; } ///< Return the value of this attribute. - #endif - int IntValue() const; ///< Return the value of this attribute, converted to an integer. - double DoubleValue() const; ///< Return the value of this attribute, converted to a double. - - // Get the tinyxml string representation - const TIXML_STRING& NameTStr() const { return name; } - - /** QueryIntValue examines the value string. It is an alternative to the - IntValue() method with richer error checking. - If the value is an integer, it is stored in 'value' and - the call returns TIXML_SUCCESS. If it is not - an integer, it returns TIXML_WRONG_TYPE. - - A specialized but useful call. Note that for success it returns 0, - which is the opposite of almost all other TinyXml calls. - */ - int QueryIntValue( int* _value ) const; - /// QueryDoubleValue examines the value string. See QueryIntValue(). - int QueryDoubleValue( double* _value ) const; - - void SetName( const char* _name ) { name = _name; } ///< Set the name of this attribute. - void SetValue( const char* _value ) { value = _value; } ///< Set the value. - - void SetIntValue( int _value ); ///< Set the value from an integer. - void SetDoubleValue( double _value ); ///< Set the value from a double. - - #ifdef TIXML_USE_STL - /// STL std::string form. - void SetName( const std::string& _name ) { name = _name; } - /// STL std::string form. - void SetValue( const std::string& _value ) { value = _value; } - #endif - - /// Get the next sibling attribute in the DOM. Returns null at end. - const TiXmlAttribute* Next() const; - TiXmlAttribute* Next() { - return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Next() ); - } - - /// Get the previous sibling attribute in the DOM. Returns null at beginning. - const TiXmlAttribute* Previous() const; - TiXmlAttribute* Previous() { - return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Previous() ); - } - - bool operator==( const TiXmlAttribute& rhs ) const { return rhs.name == name; } - bool operator<( const TiXmlAttribute& rhs ) const { return name < rhs.name; } - bool operator>( const TiXmlAttribute& rhs ) const { return name > rhs.name; } - - /* Attribute parsing starts: first letter of the name - returns: the next char after the value end quote - */ - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - // Prints this Attribute to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const { - Print( cfile, depth, 0 ); - } - void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; - - // [internal use] - // Set the document pointer so the attribute can report errors. - void SetDocument( TiXmlDocument* doc ) { document = doc; } - -private: - TiXmlAttribute( const TiXmlAttribute& ); // not implemented. - void operator=( const TiXmlAttribute& base ); // not allowed. - - TiXmlDocument* document; // A pointer back to a document, for error reporting. - TIXML_STRING name; - TIXML_STRING value; - TiXmlAttribute* prev; - TiXmlAttribute* next; -}; - - -/* A class used to manage a group of attributes. - It is only used internally, both by the ELEMENT and the DECLARATION. - - The set can be changed transparent to the Element and Declaration - classes that use it, but NOT transparent to the Attribute - which has to implement a next() and previous() method. Which makes - it a bit problematic and prevents the use of STL. - - This version is implemented with circular lists because: - - I like circular lists - - it demonstrates some independence from the (typical) doubly linked list. -*/ -class TiXmlAttributeSet -{ -public: - TiXmlAttributeSet(); - ~TiXmlAttributeSet(); - - void Add( TiXmlAttribute* attribute ); - void Remove( TiXmlAttribute* attribute ); - - const TiXmlAttribute* First() const { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } - TiXmlAttribute* First() { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } - const TiXmlAttribute* Last() const { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } - TiXmlAttribute* Last() { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } - - TiXmlAttribute* Find( const char* _name ) const; - TiXmlAttribute* FindOrCreate( const char* _name ); - -# ifdef TIXML_USE_STL - TiXmlAttribute* Find( const std::string& _name ) const; - TiXmlAttribute* FindOrCreate( const std::string& _name ); -# endif - - -private: - //*ME: Because of hidden/disabled copy-construktor in TiXmlAttribute (sentinel-element), - //*ME: this class must be also use a hidden/disabled copy-constructor !!! - TiXmlAttributeSet( const TiXmlAttributeSet& ); // not allowed - void operator=( const TiXmlAttributeSet& ); // not allowed (as TiXmlAttribute) - - TiXmlAttribute sentinel; -}; - - -/** The element is a container class. It has a value, the element name, - and can contain other elements, text, comments, and unknowns. - Elements also contain an arbitrary number of attributes. -*/ -class TiXmlElement : public TiXmlNode -{ -public: - /// Construct an element. - TiXmlElement (const char * in_value); - - #ifdef TIXML_USE_STL - /// std::string constructor. - TiXmlElement( const std::string& _value ); - #endif - - TiXmlElement( const TiXmlElement& ); - - TiXmlElement& operator=( const TiXmlElement& base ); - - virtual ~TiXmlElement(); - - /** Given an attribute name, Attribute() returns the value - for the attribute of that name, or null if none exists. - */ - const char* Attribute( const char* name ) const; - - /** Given an attribute name, Attribute() returns the value - for the attribute of that name, or null if none exists. - If the attribute exists and can be converted to an integer, - the integer value will be put in the return 'i', if 'i' - is non-null. - */ - const char* Attribute( const char* name, int* i ) const; - - /** Given an attribute name, Attribute() returns the value - for the attribute of that name, or null if none exists. - If the attribute exists and can be converted to an double, - the double value will be put in the return 'd', if 'd' - is non-null. - */ - const char* Attribute( const char* name, double* d ) const; - - /** QueryIntAttribute examines the attribute - it is an alternative to the - Attribute() method with richer error checking. - If the attribute is an integer, it is stored in 'value' and - the call returns TIXML_SUCCESS. If it is not - an integer, it returns TIXML_WRONG_TYPE. If the attribute - does not exist, then TIXML_NO_ATTRIBUTE is returned. - */ - int QueryIntAttribute( const char* name, int* _value ) const; - /// QueryUnsignedAttribute examines the attribute - see QueryIntAttribute(). - int QueryUnsignedAttribute( const char* name, unsigned* _value ) const; - /** QueryBoolAttribute examines the attribute - see QueryIntAttribute(). - Note that '1', 'true', or 'yes' are considered true, while '0', 'false' - and 'no' are considered false. - */ - int QueryBoolAttribute( const char* name, bool* _value ) const; - /// QueryDoubleAttribute examines the attribute - see QueryIntAttribute(). - int QueryDoubleAttribute( const char* name, double* _value ) const; - /// QueryFloatAttribute examines the attribute - see QueryIntAttribute(). - int QueryFloatAttribute( const char* name, float* _value ) const { - double d; - int result = QueryDoubleAttribute( name, &d ); - if ( result == TIXML_SUCCESS ) { - *_value = (float)d; - } - return result; - } - - #ifdef TIXML_USE_STL - /// QueryStringAttribute examines the attribute - see QueryIntAttribute(). - int QueryStringAttribute( const char* name, std::string* _value ) const { - const char* cstr = Attribute( name ); - if ( cstr ) { - *_value = std::string( cstr ); - return TIXML_SUCCESS; - } - return TIXML_NO_ATTRIBUTE; - } - - /** Template form of the attribute query which will try to read the - attribute into the specified type. Very easy, very powerful, but - be careful to make sure to call this with the correct type. - - NOTE: This method doesn't work correctly for 'string' types that contain spaces. - - @return TIXML_SUCCESS, TIXML_WRONG_TYPE, or TIXML_NO_ATTRIBUTE - */ - template< typename T > int QueryValueAttribute( const std::string& name, T* outValue ) const - { - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - - std::stringstream sstream( node->ValueStr() ); - sstream >> *outValue; - if ( !sstream.fail() ) - return TIXML_SUCCESS; - return TIXML_WRONG_TYPE; - } - - int QueryValueAttribute( const std::string& name, std::string* outValue ) const - { - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - *outValue = node->ValueStr(); - return TIXML_SUCCESS; - } - #endif - - /** Sets an attribute of name to a given value. The attribute - will be created if it does not exist, or changed if it does. - */ - void SetAttribute( const char* name, const char * _value ); - - #ifdef TIXML_USE_STL - const std::string* Attribute( const std::string& name ) const; - const std::string* Attribute( const std::string& name, int* i ) const; - const std::string* Attribute( const std::string& name, double* d ) const; - int QueryIntAttribute( const std::string& name, int* _value ) const; - int QueryDoubleAttribute( const std::string& name, double* _value ) const; - - /// STL std::string form. - void SetAttribute( const std::string& name, const std::string& _value ); - ///< STL std::string form. - void SetAttribute( const std::string& name, int _value ); - ///< STL std::string form. - void SetDoubleAttribute( const std::string& name, double value ); - #endif - - /** Sets an attribute of name to a given value. The attribute - will be created if it does not exist, or changed if it does. - */ - void SetAttribute( const char * name, int value ); - - /** Sets an attribute of name to a given value. The attribute - will be created if it does not exist, or changed if it does. - */ - void SetDoubleAttribute( const char * name, double value ); - - /** Deletes an attribute with the given name. - */ - void RemoveAttribute( const char * name ); - #ifdef TIXML_USE_STL - void RemoveAttribute( const std::string& name ) { RemoveAttribute (name.c_str ()); } ///< STL std::string form. - #endif - - const TiXmlAttribute* FirstAttribute() const { return attributeSet.First(); } ///< Access the first attribute in this element. - TiXmlAttribute* FirstAttribute() { return attributeSet.First(); } - const TiXmlAttribute* LastAttribute() const { return attributeSet.Last(); } ///< Access the last attribute in this element. - TiXmlAttribute* LastAttribute() { return attributeSet.Last(); } - - /** Convenience function for easy access to the text inside an element. Although easy - and concise, GetText() is limited compared to getting the TiXmlText child - and accessing it directly. - - If the first child of 'this' is a TiXmlText, the GetText() - returns the character string of the Text node, else null is returned. - - This is a convenient method for getting the text of simple contained text: - @verbatim - This is text - const char* str = fooElement->GetText(); - @endverbatim - - 'str' will be a pointer to "This is text". - - Note that this function can be misleading. If the element foo was created from - this XML: - @verbatim - This is text - @endverbatim - - then the value of str would be null. The first child node isn't a text node, it is - another element. From this XML: - @verbatim - This is text - @endverbatim - GetText() will return "This is ". - - WARNING: GetText() accesses a child node - don't become confused with the - similarly named TiXmlHandle::Text() and TiXmlNode::ToText() which are - safe type casts on the referenced node. - */ - const char* GetText() const; - - /// Creates a new Element and returns it - the returned element is a copy. - virtual TiXmlNode* Clone() const; - // Print the Element to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const; - - /* Attribtue parsing starts: next char past '<' - returns: next char past '>' - */ - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlElement* ToElement() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlElement* ToElement() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* visitor ) const; - -protected: - - void CopyTo( TiXmlElement* target ) const; - void ClearThis(); // like clear, but initializes 'this' object as well - - // Used to be public [internal use] - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - /* [internal use] - Reads the "value" of the element -- another element, or text. - This should terminate with the current end tag. - */ - const char* ReadValue( const char* in, TiXmlParsingData* prevData, TiXmlEncoding encoding ); - -private: - TiXmlAttributeSet attributeSet; -}; - - -/** An XML comment. -*/ -class TiXmlComment : public TiXmlNode -{ -public: - /// Constructs an empty comment. - TiXmlComment() : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) {} - /// Construct a comment from text. - TiXmlComment( const char* _value ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) { - SetValue( _value ); - } - TiXmlComment( const TiXmlComment& ); - TiXmlComment& operator=( const TiXmlComment& base ); - - virtual ~TiXmlComment() {} - - /// Returns a copy of this Comment. - virtual TiXmlNode* Clone() const; - // Write this Comment to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const; - - /* Attribtue parsing starts: at the ! of the !-- - returns: next char past '>' - */ - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlComment* ToComment() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlComment* ToComment() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* visitor ) const; - -protected: - void CopyTo( TiXmlComment* target ) const; - - // used to be public - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif -// virtual void StreamOut( TIXML_OSTREAM * out ) const; - -private: - -}; - - -/** XML text. A text node can have 2 ways to output the next. "normal" output - and CDATA. It will default to the mode it was parsed from the XML file and - you generally want to leave it alone, but you can change the output mode with - SetCDATA() and query it with CDATA(). -*/ -class TiXmlText : public TiXmlNode -{ - friend class TiXmlElement; -public: - /** Constructor for text element. By default, it is treated as - normal, encoded text. If you want it be output as a CDATA text - element, set the parameter _cdata to 'true' - */ - TiXmlText (const char * initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) - { - SetValue( initValue ); - cdata = false; - } - virtual ~TiXmlText() {} - - #ifdef TIXML_USE_STL - /// Constructor. - TiXmlText( const std::string& initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) - { - SetValue( initValue ); - cdata = false; - } - #endif - - TiXmlText( const TiXmlText& copy ) : TiXmlNode( TiXmlNode::TINYXML_TEXT ) { copy.CopyTo( this ); } - TiXmlText& operator=( const TiXmlText& base ) { base.CopyTo( this ); return *this; } - - // Write this text object to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const; - - /// Queries whether this represents text using a CDATA section. - bool CDATA() const { return cdata; } - /// Turns on or off a CDATA representation of text. - void SetCDATA( bool _cdata ) { cdata = _cdata; } - - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlText* ToText() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlText* ToText() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* content ) const; - -protected : - /// [internal use] Creates a new Element and returns it. - virtual TiXmlNode* Clone() const; - void CopyTo( TiXmlText* target ) const; - - bool Blank() const; // returns true if all white space and new lines - // [internal use] - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - -private: - bool cdata; // true if this should be input and output as a CDATA style text element -}; - - -/** In correct XML the declaration is the first entry in the file. - @verbatim - - @endverbatim - - TinyXml will happily read or write files without a declaration, - however. There are 3 possible attributes to the declaration: - version, encoding, and standalone. - - Note: In this version of the code, the attributes are - handled as special cases, not generic attributes, simply - because there can only be at most 3 and they are always the same. -*/ -class TiXmlDeclaration : public TiXmlNode -{ -public: - /// Construct an empty declaration. - TiXmlDeclaration() : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) {} - -#ifdef TIXML_USE_STL - /// Constructor. - TiXmlDeclaration( const std::string& _version, - const std::string& _encoding, - const std::string& _standalone ); -#endif - - /// Construct. - TiXmlDeclaration( const char* _version, - const char* _encoding, - const char* _standalone ); - - TiXmlDeclaration( const TiXmlDeclaration& copy ); - TiXmlDeclaration& operator=( const TiXmlDeclaration& copy ); - - virtual ~TiXmlDeclaration() {} - - /// Version. Will return an empty string if none was found. - const char *Version() const { return version.c_str (); } - /// Encoding. Will return an empty string if none was found. - const char *Encoding() const { return encoding.c_str (); } - /// Is this a standalone document? - const char *Standalone() const { return standalone.c_str (); } - - /// Creates a copy of this Declaration and returns it. - virtual TiXmlNode* Clone() const; - // Print this declaration to a FILE stream. - virtual void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; - virtual void Print( FILE* cfile, int depth ) const { - Print( cfile, depth, 0 ); - } - - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlDeclaration* ToDeclaration() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlDeclaration* ToDeclaration() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* visitor ) const; - -protected: - void CopyTo( TiXmlDeclaration* target ) const; - // used to be public - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - -private: - - TIXML_STRING version; - TIXML_STRING encoding; - TIXML_STRING standalone; -}; - - -/** Any tag that tinyXml doesn't recognize is saved as an - unknown. It is a tag of text, but should not be modified. - It will be written back to the XML, unchanged, when the file - is saved. - - DTD tags get thrown into TiXmlUnknowns. -*/ -class TiXmlUnknown : public TiXmlNode -{ -public: - TiXmlUnknown() : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) {} - virtual ~TiXmlUnknown() {} - - TiXmlUnknown( const TiXmlUnknown& copy ) : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) { copy.CopyTo( this ); } - TiXmlUnknown& operator=( const TiXmlUnknown& copy ) { copy.CopyTo( this ); return *this; } - - /// Creates a copy of this Unknown and returns it. - virtual TiXmlNode* Clone() const; - // Print this Unknown to a FILE stream. - virtual void Print( FILE* cfile, int depth ) const; - - virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); - - virtual const TiXmlUnknown* ToUnknown() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlUnknown* ToUnknown() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* content ) const; - -protected: - void CopyTo( TiXmlUnknown* target ) const; - - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - -private: - -}; - - -/** Always the top level node. A document binds together all the - XML pieces. It can be saved, loaded, and printed to the screen. - The 'value' of a document node is the xml file name. -*/ -class TiXmlDocument : public TiXmlNode -{ -public: - /// Create an empty document, that has no name. - TiXmlDocument(); - /// Create a document with a name. The name of the document is also the filename of the xml. - TiXmlDocument( const char * documentName ); - - #ifdef TIXML_USE_STL - /// Constructor. - TiXmlDocument( const std::string& documentName ); - #endif - - TiXmlDocument( const TiXmlDocument& copy ); - TiXmlDocument& operator=( const TiXmlDocument& copy ); - - virtual ~TiXmlDocument() {} - - /** Load a file using the current document value. - Returns true if successful. Will delete any existing - document data before loading. - */ - bool LoadFile( TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); - /// Save a file using the current document value. Returns true if successful. - bool SaveFile() const; - /// Load a file using the given filename. Returns true if successful. - bool LoadFile( const char * filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); - /// Save a file using the given filename. Returns true if successful. - bool SaveFile( const char * filename ) const; - /** Load a file using the given FILE*. Returns true if successful. Note that this method - doesn't stream - the entire object pointed at by the FILE* - will be interpreted as an XML file. TinyXML doesn't stream in XML from the current - file location. Streaming may be added in the future. - */ - bool LoadFile( FILE*, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); - /// Save a file using the given FILE*. Returns true if successful. - bool SaveFile( FILE* ) const; - - #ifdef TIXML_USE_STL - bool LoadFile( const std::string& filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ) ///< STL std::string version. - { - return LoadFile( filename.c_str(), encoding ); - } - bool SaveFile( const std::string& filename ) const ///< STL std::string version. - { - return SaveFile( filename.c_str() ); - } - #endif - - /** Parse the given null terminated block of xml data. Passing in an encoding to this - method (either TIXML_ENCODING_LEGACY or TIXML_ENCODING_UTF8 will force TinyXml - to use that encoding, regardless of what TinyXml might otherwise try to detect. - */ - virtual const char* Parse( const char* p, TiXmlParsingData* data = 0, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); - - /** Get the root element -- the only top level element -- of the document. - In well formed XML, there should only be one. TinyXml is tolerant of - multiple elements at the document level. - */ - const TiXmlElement* RootElement() const { return FirstChildElement(); } - TiXmlElement* RootElement() { return FirstChildElement(); } - - /** If an error occurs, Error will be set to true. Also, - - The ErrorId() will contain the integer identifier of the error (not generally useful) - - The ErrorDesc() method will return the name of the error. (very useful) - - The ErrorRow() and ErrorCol() will return the location of the error (if known) - */ - bool Error() const { return error; } - - /// Contains a textual (english) description of the error if one occurs. - const char * ErrorDesc() const { return errorDesc.c_str (); } - - /** Generally, you probably want the error string ( ErrorDesc() ). But if you - prefer the ErrorId, this function will fetch it. - */ - int ErrorId() const { return errorId; } - - /** Returns the location (if known) of the error. The first column is column 1, - and the first row is row 1. A value of 0 means the row and column wasn't applicable - (memory errors, for example, have no row/column) or the parser lost the error. (An - error in the error reporting, in that case.) - - @sa SetTabSize, Row, Column - */ - int ErrorRow() const { return errorLocation.row+1; } - int ErrorCol() const { return errorLocation.col+1; } ///< The column where the error occured. See ErrorRow() - - /** SetTabSize() allows the error reporting functions (ErrorRow() and ErrorCol()) - to report the correct values for row and column. It does not change the output - or input in any way. - - By calling this method, with a tab size - greater than 0, the row and column of each node and attribute is stored - when the file is loaded. Very useful for tracking the DOM back in to - the source file. - - The tab size is required for calculating the location of nodes. If not - set, the default of 4 is used. The tabsize is set per document. Setting - the tabsize to 0 disables row/column tracking. - - Note that row and column tracking is not supported when using operator>>. - - The tab size needs to be enabled before the parse or load. Correct usage: - @verbatim - TiXmlDocument doc; - doc.SetTabSize( 8 ); - doc.Load( "myfile.xml" ); - @endverbatim - - @sa Row, Column - */ - void SetTabSize( int _tabsize ) { tabsize = _tabsize; } - - int TabSize() const { return tabsize; } - - /** If you have handled the error, it can be reset with this call. The error - state is automatically cleared if you Parse a new XML block. - */ - void ClearError() { error = false; - errorId = 0; - errorDesc = ""; - errorLocation.row = errorLocation.col = 0; - //errorLocation.last = 0; - } - - /** Write the document to standard out using formatted printing ("pretty print"). */ - void Print() const { Print( stdout, 0 ); } - - /* Write the document to a string using formatted printing ("pretty print"). This - will allocate a character array (new char[]) and return it as a pointer. The - calling code pust call delete[] on the return char* to avoid a memory leak. - */ - //char* PrintToMemory() const; - - /// Print this Document to a FILE stream. - virtual void Print( FILE* cfile, int depth = 0 ) const; - // [internal use] - void SetError( int err, const char* errorLocation, TiXmlParsingData* prevData, TiXmlEncoding encoding ); - - virtual const TiXmlDocument* ToDocument() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - virtual TiXmlDocument* ToDocument() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. - - /** Walk the XML tree visiting this node and all of its children. - */ - virtual bool Accept( TiXmlVisitor* content ) const; - -protected : - // [internal use] - virtual TiXmlNode* Clone() const; - #ifdef TIXML_USE_STL - virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); - #endif - -private: - void CopyTo( TiXmlDocument* target ) const; - - bool error; - int errorId; - TIXML_STRING errorDesc; - int tabsize; - TiXmlCursor errorLocation; - bool useMicrosoftBOM; // the UTF-8 BOM were found when read. Note this, and try to write. -}; - - -/** - A TiXmlHandle is a class that wraps a node pointer with null checks; this is - an incredibly useful thing. Note that TiXmlHandle is not part of the TinyXml - DOM structure. It is a separate utility class. - - Take an example: - @verbatim - - - - - - - @endverbatim - - Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very - easy to write a *lot* of code that looks like: - - @verbatim - TiXmlElement* root = document.FirstChildElement( "Document" ); - if ( root ) - { - TiXmlElement* element = root->FirstChildElement( "Element" ); - if ( element ) - { - TiXmlElement* child = element->FirstChildElement( "Child" ); - if ( child ) - { - TiXmlElement* child2 = child->NextSiblingElement( "Child" ); - if ( child2 ) - { - // Finally do something useful. - @endverbatim - - And that doesn't even cover "else" cases. TiXmlHandle addresses the verbosity - of such code. A TiXmlHandle checks for null pointers so it is perfectly safe - and correct to use: - - @verbatim - TiXmlHandle docHandle( &document ); - TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement(); - if ( child2 ) - { - // do something useful - @endverbatim - - Which is MUCH more concise and useful. - - It is also safe to copy handles - internally they are nothing more than node pointers. - @verbatim - TiXmlHandle handleCopy = handle; - @endverbatim - - What they should not be used for is iteration: - - @verbatim - int i=0; - while ( true ) - { - TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", i ).ToElement(); - if ( !child ) - break; - // do something - ++i; - } - @endverbatim - - It seems reasonable, but it is in fact two embedded while loops. The Child method is - a linear walk to find the element, so this code would iterate much more than it needs - to. Instead, prefer: - - @verbatim - TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).FirstChild( "Child" ).ToElement(); - - for( child; child; child=child->NextSiblingElement() ) - { - // do something - } - @endverbatim -*/ -class TiXmlHandle -{ -public: - /// Create a handle from any node (at any depth of the tree.) This can be a null pointer. - TiXmlHandle( TiXmlNode* _node ) { this->node = _node; } - /// Copy constructor - TiXmlHandle( const TiXmlHandle& ref ) { this->node = ref.node; } - TiXmlHandle operator=( const TiXmlHandle& ref ) { if ( &ref != this ) this->node = ref.node; return *this; } - - /// Return a handle to the first child node. - TiXmlHandle FirstChild() const; - /// Return a handle to the first child node with the given name. - TiXmlHandle FirstChild( const char * value ) const; - /// Return a handle to the first child element. - TiXmlHandle FirstChildElement() const; - /// Return a handle to the first child element with the given name. - TiXmlHandle FirstChildElement( const char * value ) const; - - /** Return a handle to the "index" child with the given name. - The first child is 0, the second 1, etc. - */ - TiXmlHandle Child( const char* value, int index ) const; - /** Return a handle to the "index" child. - The first child is 0, the second 1, etc. - */ - TiXmlHandle Child( int index ) const; - /** Return a handle to the "index" child element with the given name. - The first child element is 0, the second 1, etc. Note that only TiXmlElements - are indexed: other types are not counted. - */ - TiXmlHandle ChildElement( const char* value, int index ) const; - /** Return a handle to the "index" child element. - The first child element is 0, the second 1, etc. Note that only TiXmlElements - are indexed: other types are not counted. - */ - TiXmlHandle ChildElement( int index ) const; - - #ifdef TIXML_USE_STL - TiXmlHandle FirstChild( const std::string& _value ) const { return FirstChild( _value.c_str() ); } - TiXmlHandle FirstChildElement( const std::string& _value ) const { return FirstChildElement( _value.c_str() ); } - - TiXmlHandle Child( const std::string& _value, int index ) const { return Child( _value.c_str(), index ); } - TiXmlHandle ChildElement( const std::string& _value, int index ) const { return ChildElement( _value.c_str(), index ); } - #endif - - /** Return the handle as a TiXmlNode. This may return null. - */ - TiXmlNode* ToNode() const { return node; } - /** Return the handle as a TiXmlElement. This may return null. - */ - TiXmlElement* ToElement() const { return ( ( node && node->ToElement() ) ? node->ToElement() : 0 ); } - /** Return the handle as a TiXmlText. This may return null. - */ - TiXmlText* ToText() const { return ( ( node && node->ToText() ) ? node->ToText() : 0 ); } - /** Return the handle as a TiXmlUnknown. This may return null. - */ - TiXmlUnknown* ToUnknown() const { return ( ( node && node->ToUnknown() ) ? node->ToUnknown() : 0 ); } - - /** @deprecated use ToNode. - Return the handle as a TiXmlNode. This may return null. - */ - TiXmlNode* Node() const { return ToNode(); } - /** @deprecated use ToElement. - Return the handle as a TiXmlElement. This may return null. - */ - TiXmlElement* Element() const { return ToElement(); } - /** @deprecated use ToText() - Return the handle as a TiXmlText. This may return null. - */ - TiXmlText* Text() const { return ToText(); } - /** @deprecated use ToUnknown() - Return the handle as a TiXmlUnknown. This may return null. - */ - TiXmlUnknown* Unknown() const { return ToUnknown(); } - -private: - TiXmlNode* node; -}; - - -/** Print to memory functionality. The TiXmlPrinter is useful when you need to: - - -# Print to memory (especially in non-STL mode) - -# Control formatting (line endings, etc.) - - When constructed, the TiXmlPrinter is in its default "pretty printing" mode. - Before calling Accept() you can call methods to control the printing - of the XML document. After TiXmlNode::Accept() is called, the printed document can - be accessed via the CStr(), Str(), and Size() methods. - - TiXmlPrinter uses the Visitor API. - @verbatim - TiXmlPrinter printer; - printer.SetIndent( "\t" ); - - doc.Accept( &printer ); - fprintf( stdout, "%s", printer.CStr() ); - @endverbatim -*/ -class TiXmlPrinter : public TiXmlVisitor -{ -public: - TiXmlPrinter() : depth( 0 ), simpleTextPrint( false ), - buffer(), indent( " " ), lineBreak( "\n" ) {} - - virtual bool VisitEnter( const TiXmlDocument& doc ); - virtual bool VisitExit( const TiXmlDocument& doc ); - - virtual bool VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ); - virtual bool VisitExit( const TiXmlElement& element ); - - virtual bool Visit( const TiXmlDeclaration& declaration ); - virtual bool Visit( const TiXmlText& text ); - virtual bool Visit( const TiXmlComment& comment ); - virtual bool Visit( const TiXmlUnknown& unknown ); - - /** Set the indent characters for printing. By default 4 spaces - but tab (\t) is also useful, or null/empty string for no indentation. - */ - void SetIndent( const char* _indent ) { indent = _indent ? _indent : "" ; } - /// Query the indention string. - const char* Indent() { return indent.c_str(); } - /** Set the line breaking string. By default set to newline (\n). - Some operating systems prefer other characters, or can be - set to the null/empty string for no indenation. - */ - void SetLineBreak( const char* _lineBreak ) { lineBreak = _lineBreak ? _lineBreak : ""; } - /// Query the current line breaking string. - const char* LineBreak() { return lineBreak.c_str(); } - - /** Switch over to "stream printing" which is the most dense formatting without - linebreaks. Common when the XML is needed for network transmission. - */ - void SetStreamPrinting() { indent = ""; - lineBreak = ""; - } - /// Return the result. - const char* CStr() { return buffer.c_str(); } - /// Return the length of the result string. - size_t Size() { return buffer.size(); } - - #ifdef TIXML_USE_STL - /// Return the result. - const std::string& Str() { return buffer; } - #endif - -private: - void DoIndent() { - for( int i=0; i -#include - -#include "tinyxml.h" - -//#define DEBUG_PARSER -#if defined( DEBUG_PARSER ) -# if defined( DEBUG ) && defined( _MSC_VER ) -# include -# define TIXML_LOG OutputDebugString -# else -# define TIXML_LOG printf -# endif -#endif - -// Note tha "PutString" hardcodes the same list. This -// is less flexible than it appears. Changing the entries -// or order will break putstring. -TiXmlBase::Entity TiXmlBase::entity[ TiXmlBase::NUM_ENTITY ] = -{ - { "&", 5, '&' }, - { "<", 4, '<' }, - { ">", 4, '>' }, - { """, 6, '\"' }, - { "'", 6, '\'' } -}; - -// Bunch of unicode info at: -// http://www.unicode.org/faq/utf_bom.html -// Including the basic of this table, which determines the #bytes in the -// sequence from the lead byte. 1 placed for invalid sequences -- -// although the result will be junk, pass it through as much as possible. -// Beware of the non-characters in UTF-8: -// ef bb bf (Microsoft "lead bytes") -// ef bf be -// ef bf bf - -const unsigned char TIXML_UTF_LEAD_0 = 0xefU; -const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; -const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; - -const int TiXmlBase::utf8ByteTable[256] = -{ - // 0 1 2 3 4 5 6 7 8 9 a b c d e f - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x00 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x10 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x20 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x30 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x40 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x50 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x60 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x70 End of ASCII range - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x80 0x80 to 0xc1 invalid - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x90 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xa0 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xb0 - 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xc0 0xc2 to 0xdf 2 byte - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xd0 - 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 0xe0 0xe0 to 0xef 3 byte - 4, 4, 4, 4, 4, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // 0xf0 0xf0 to 0xf4 4 byte, 0xf5 and higher invalid -}; - - -void TiXmlBase::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ) -{ - const unsigned long BYTE_MASK = 0xBF; - const unsigned long BYTE_MARK = 0x80; - const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC }; - - if (input < 0x80) - *length = 1; - else if ( input < 0x800 ) - *length = 2; - else if ( input < 0x10000 ) - *length = 3; - else if ( input < 0x200000 ) - *length = 4; - else - { *length = 0; return; } // This code won't covert this correctly anyway. - - output += *length; - - // Scary scary fall throughs. - #ifndef _WIN32 - #pragma GCC diagnostic ignored "-Wswitch-default" - #endif - switch (*length) - { - case 4: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 3: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 2: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 1: - --output; - *output = (char)(input | FIRST_BYTE_MARK[*length]); - } - #ifndef _WIN32 - #pragma GCC diagnostic pop - #endif -} - - -/*static*/ int TiXmlBase::IsAlpha( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) -{ - // This will only work for low-ascii, everything else is assumed to be a valid - // letter. I'm not sure this is the best approach, but it is quite tricky trying - // to figure out alhabetical vs. not across encoding. So take a very - // conservative approach. - -// if ( encoding == TIXML_ENCODING_UTF8 ) -// { - if ( anyByte < 127 ) - return isalpha( anyByte ); - else - return 1; // What else to do? The unicode set is huge...get the english ones right. -// } -// else -// { -// return isalpha( anyByte ); -// } -} - - -/*static*/ int TiXmlBase::IsAlphaNum( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) -{ - // This will only work for low-ascii, everything else is assumed to be a valid - // letter. I'm not sure this is the best approach, but it is quite tricky trying - // to figure out alhabetical vs. not across encoding. So take a very - // conservative approach. - -// if ( encoding == TIXML_ENCODING_UTF8 ) -// { - if ( anyByte < 127 ) - return isalnum( anyByte ); - else - return 1; // What else to do? The unicode set is huge...get the english ones right. -// } -// else -// { -// return isalnum( anyByte ); -// } -} - - -class TiXmlParsingData -{ - friend class TiXmlDocument; - public: - void Stamp( const char* now, TiXmlEncoding encoding ); - - const TiXmlCursor& Cursor() const { return cursor; } - - private: - // Only used by the document! - TiXmlParsingData( const char* start, int _tabsize, int row, int col ) - { - assert( start ); - stamp = start; - tabsize = _tabsize; - cursor.row = row; - cursor.col = col; - } - - TiXmlCursor cursor; - const char* stamp; - int tabsize; -}; - - -void TiXmlParsingData::Stamp( const char* now, TiXmlEncoding encoding ) -{ - assert( now ); - - // Do nothing if the tabsize is 0. - if ( tabsize < 1 ) - { - return; - } - - // Get the current row, column. - int row = cursor.row; - int col = cursor.col; - const char* p = stamp; - assert( p ); - - while ( p < now ) - { - // Treat p as unsigned, so we have a happy compiler. - const unsigned char* pU = (const unsigned char*)p; - - // Code contributed by Fletcher Dunn: (modified by lee) - switch (*pU) { - case 0: - // We *should* never get here, but in case we do, don't - // advance past the terminating null character, ever - return; - - case '\r': - // bump down to the next line - ++row; - col = 0; - // Eat the character - ++p; - - // Check for \r\n sequence, and treat this as a single character - if (*p == '\n') { - ++p; - } - break; - - case '\n': - // bump down to the next line - ++row; - col = 0; - - // Eat the character - ++p; - - // Check for \n\r sequence, and treat this as a single - // character. (Yes, this bizarre thing does occur still - // on some arcane platforms...) - if (*p == '\r') { - ++p; - } - break; - - case '\t': - // Eat the character - ++p; - - // Skip to next tab stop - col = (col / tabsize + 1) * tabsize; - break; - - case TIXML_UTF_LEAD_0: - if ( encoding == TIXML_ENCODING_UTF8 ) - { - if ( *(p+1) && *(p+2) ) - { - // In these cases, don't advance the column. These are - // 0-width spaces. - if ( *(pU+1)==TIXML_UTF_LEAD_1 && *(pU+2)==TIXML_UTF_LEAD_2 ) - p += 3; - else if ( *(pU+1)==0xbfU && *(pU+2)==0xbeU ) - p += 3; - else if ( *(pU+1)==0xbfU && *(pU+2)==0xbfU ) - p += 3; - else - { p +=3; ++col; } // A normal character. - } - } - else - { - ++p; - ++col; - } - break; - - default: - if ( encoding == TIXML_ENCODING_UTF8 ) - { - // Eat the 1 to 4 byte utf8 character. - int step = TiXmlBase::utf8ByteTable[*((const unsigned char*)p)]; - if ( step == 0 ) - step = 1; // Error case from bad encoding, but handle gracefully. - p += step; - - // Just advance one column, of course. - ++col; - } - else - { - ++p; - ++col; - } - break; - } - } - cursor.row = row; - cursor.col = col; - assert( cursor.row >= -1 ); - assert( cursor.col >= -1 ); - stamp = p; - assert( stamp ); -} - - -const char* TiXmlBase::SkipWhiteSpace( const char* p, TiXmlEncoding encoding ) -{ - if ( !p || !*p ) - { - return 0; - } - if ( encoding == TIXML_ENCODING_UTF8 ) - { - while ( *p ) - { - const unsigned char* pU = (const unsigned char*)p; - - // Skip the stupid Microsoft UTF-8 Byte order marks - if ( *(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==TIXML_UTF_LEAD_1 - && *(pU+2)==TIXML_UTF_LEAD_2 ) - { - p += 3; - continue; - } - else if(*(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==0xbfU - && *(pU+2)==0xbeU ) - { - p += 3; - continue; - } - else if(*(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==0xbfU - && *(pU+2)==0xbfU ) - { - p += 3; - continue; - } - - if ( IsWhiteSpace( *p ) ) // Still using old rules for white space. - ++p; - else - break; - } - } - else - { - while ( *p && IsWhiteSpace( *p ) ) - ++p; - } - - return p; -} - -#ifdef TIXML_USE_STL -/*static*/ bool TiXmlBase::StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ) -{ - for( ;; ) - { - if ( !in->good() ) return false; - - int c = in->peek(); - // At this scope, we can't get to a document. So fail silently. - if ( !IsWhiteSpace( c ) || c <= 0 ) - return true; - - *tag += (char) in->get(); - } -} - -/*static*/ bool TiXmlBase::StreamTo( std::istream * in, int character, TIXML_STRING * tag ) -{ - //assert( character > 0 && character < 128 ); // else it won't work in utf-8 - while ( in->good() ) - { - int c = in->peek(); - if ( c == character ) - return true; - if ( c <= 0 ) // Silent failure: can't get document at this scope - return false; - - in->get(); - *tag += (char) c; - } - return false; -} -#endif - -// One of TinyXML's more performance demanding functions. Try to keep the memory overhead down. The -// "assign" optimization removes over 10% of the execution time. -// -const char* TiXmlBase::ReadName( const char* p, TIXML_STRING * name, TiXmlEncoding encoding ) -{ - // Oddly, not supported on some comilers, - //name->clear(); - // So use this: - *name = ""; - assert( p ); - - // Names start with letters or underscores. - // Of course, in unicode, tinyxml has no idea what a letter *is*. The - // algorithm is generous. - // - // After that, they can be letters, underscores, numbers, - // hyphens, or colons. (Colons are valid ony for namespaces, - // but tinyxml can't tell namespaces from names.) - if ( p && *p - && ( IsAlpha( (unsigned char) *p, encoding ) || *p == '_' ) ) - { - const char* start = p; - while( p && *p - && ( IsAlphaNum( (unsigned char ) *p, encoding ) - || *p == '_' - || *p == '-' - || *p == '.' - || *p == ':' ) ) - { - //(*name) += *p; // expensive - ++p; - } - if ( p-start > 0 ) { - name->assign( start, p-start ); - } - return p; - } - return 0; -} - -const char* TiXmlBase::GetEntity( const char* p, char* value, int* length, TiXmlEncoding encoding ) -{ - // Presume an entity, and pull it out. - TIXML_STRING ent; - int i; - *length = 0; - - if ( *(p+1) && *(p+1) == '#' && *(p+2) ) - { - unsigned long ucs = 0; - ptrdiff_t delta = 0; - unsigned mult = 1; - - if ( *(p+2) == 'x' ) - { - // Hexadecimal. - if ( !*(p+3) ) return 0; - - const char* q = p+3; - q = strchr( q, ';' ); - - if ( !q || !*q ) return 0; - - delta = q-p; - --q; - - while ( *q != 'x' ) - { - if ( *q >= '0' && *q <= '9' ) - ucs += mult * (*q - '0'); - else if ( *q >= 'a' && *q <= 'f' ) - ucs += mult * (*q - 'a' + 10); - else if ( *q >= 'A' && *q <= 'F' ) - ucs += mult * (*q - 'A' + 10 ); - else - return 0; - mult *= 16; - --q; - } - } - else - { - // Decimal. - if ( !*(p+2) ) return 0; - - const char* q = p+2; - q = strchr( q, ';' ); - - if ( !q || !*q ) return 0; - - delta = q-p; - --q; - - while ( *q != '#' ) - { - if ( *q >= '0' && *q <= '9' ) - ucs += mult * (*q - '0'); - else - return 0; - mult *= 10; - --q; - } - } - if ( encoding == TIXML_ENCODING_UTF8 ) - { - // convert the UCS to UTF-8 - ConvertUTF32ToUTF8( ucs, value, length ); - } - else - { - *value = (char)ucs; - *length = 1; - } - return p + delta + 1; - } - - // Now try to match it. - for( i=0; iappend( cArr, len ); - } - } - else - { - bool whitespace = false; - - // Remove leading white space: - p = SkipWhiteSpace( p, encoding ); - while ( p && *p - && !StringEqual( p, endTag, caseInsensitive, encoding ) ) - { - if ( *p == '\r' || *p == '\n' ) - { - whitespace = true; - ++p; - } - else if ( IsWhiteSpace( *p ) ) - { - whitespace = true; - ++p; - } - else - { - // If we've found whitespace, add it before the - // new character. Any whitespace just becomes a space. - if ( whitespace ) - { - (*text) += ' '; - whitespace = false; - } - int len; - char cArr[4] = { 0, 0, 0, 0 }; - p = GetChar( p, cArr, &len, encoding ); - if ( len == 1 ) - (*text) += cArr[0]; // more efficient - else - text->append( cArr, len ); - } - } - } - if ( p && *p ) - p += strlen( endTag ); - return ( p && *p ) ? p : 0; -} - -#ifdef TIXML_USE_STL - -void TiXmlDocument::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - // The basic issue with a document is that we don't know what we're - // streaming. Read something presumed to be a tag (and hope), then - // identify it, and call the appropriate stream method on the tag. - // - // This "pre-streaming" will never read the closing ">" so the - // sub-tag can orient itself. - - if ( !StreamTo( in, '<', tag ) ) - { - SetError( TIXML_ERROR_PARSING_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - - while ( in->good() ) - { - int tagIndex = (int) tag->length(); - while ( in->good() && in->peek() != '>' ) - { - int c = in->get(); - if ( c <= 0 ) - { - SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - break; - } - (*tag) += (char) c; - } - - if ( in->good() ) - { - // We now have something we presume to be a node of - // some sort. Identify it, and call the node to - // continue streaming. - TiXmlNode* node = Identify( tag->c_str() + tagIndex, TIXML_DEFAULT_ENCODING ); - - if ( node ) - { - node->StreamIn( in, tag ); - bool isElement = node->ToElement() != 0; - delete node; - node = 0; - - // If this is the root element, we're done. Parsing will be - // done by the >> operator. - if ( isElement ) - { - return; - } - } - else - { - SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - } - } - // We should have returned sooner. - SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); -} - -#endif - -const char* TiXmlDocument::Parse( const char* p, TiXmlParsingData* prevData, TiXmlEncoding encoding ) -{ - ClearError(); - - // Parse away, at the document level. Since a document - // contains nothing but other tags, most of what happens - // here is skipping white space. - if ( !p || !*p ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - // Note that, for a document, this needs to come - // before the while space skip, so that parsing - // starts from the pointer we are given. - location.Clear(); - if ( prevData ) - { - location.row = prevData->cursor.row; - location.col = prevData->cursor.col; - } - else - { - location.row = 0; - location.col = 0; - } - TiXmlParsingData data( p, TabSize(), location.row, location.col ); - location = data.Cursor(); - - if ( encoding == TIXML_ENCODING_UNKNOWN ) - { - // Check for the Microsoft UTF-8 lead bytes. - const unsigned char* pU = (const unsigned char*)p; - if ( *(pU+0) && *(pU+0) == TIXML_UTF_LEAD_0 - && *(pU+1) && *(pU+1) == TIXML_UTF_LEAD_1 - && *(pU+2) && *(pU+2) == TIXML_UTF_LEAD_2 ) - { - encoding = TIXML_ENCODING_UTF8; - useMicrosoftBOM = true; - } - } - - p = SkipWhiteSpace( p, encoding ); - if ( !p ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - while ( p && *p ) - { - TiXmlNode* node = Identify( p, encoding ); - if ( node ) - { - p = node->Parse( p, &data, encoding ); - LinkEndChild( node ); - } - else - { - break; - } - - // Did we get encoding info? - if ( encoding == TIXML_ENCODING_UNKNOWN - && node->ToDeclaration() ) - { - TiXmlDeclaration* dec = node->ToDeclaration(); - const char* enc = dec->Encoding(); - assert( enc ); - - if ( *enc == 0 ) - encoding = TIXML_ENCODING_UTF8; - else if ( StringEqual( enc, "UTF-8", true, TIXML_ENCODING_UNKNOWN ) ) - encoding = TIXML_ENCODING_UTF8; - else if ( StringEqual( enc, "UTF8", true, TIXML_ENCODING_UNKNOWN ) ) - encoding = TIXML_ENCODING_UTF8; // incorrect, but be nice - else - encoding = TIXML_ENCODING_LEGACY; - } - - p = SkipWhiteSpace( p, encoding ); - } - - // Was this empty? - if ( !firstChild ) { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, encoding ); - return 0; - } - - // All is well. - return p; -} - -void TiXmlDocument::SetError( int err, const char* pError, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - // The first error in a chain is more accurate - don't set again! - if ( error ) - return; - - assert( err > 0 && err < TIXML_ERROR_STRING_COUNT ); - error = true; - errorId = err; - errorDesc = errorString[ errorId ]; - - errorLocation.Clear(); - if ( pError && data ) - { - data->Stamp( pError, encoding ); - errorLocation = data->Cursor(); - } -} - - -TiXmlNode* TiXmlNode::Identify( const char* p, TiXmlEncoding encoding ) -{ - TiXmlNode* returnNode = 0; - - p = SkipWhiteSpace( p, encoding ); - if( !p || !*p || *p != '<' ) - { - return 0; - } - - p = SkipWhiteSpace( p, encoding ); - - if ( !p || !*p ) - { - return 0; - } - - // What is this thing? - // - Elements start with a letter or underscore, but xml is reserved. - // - Comments: "; - - if ( !StringEqual( p, startTag, false, encoding ) ) - { - if ( document ) - document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding ); - return 0; - } - p += strlen( startTag ); - - // [ 1475201 ] TinyXML parses entities in comments - // Oops - ReadText doesn't work, because we don't want to parse the entities. - // p = ReadText( p, &value, false, endTag, false, encoding ); - // - // from the XML spec: - /* - [Definition: Comments may appear anywhere in a document outside other markup; in addition, - they may appear within the document type declaration at places allowed by the grammar. - They are not part of the document's character data; an XML processor MAY, but need not, - make it possible for an application to retrieve the text of comments. For compatibility, - the string "--" (double-hyphen) MUST NOT occur within comments.] Parameter entity - references MUST NOT be recognized within comments. - - An example of a comment: - - - */ - - value = ""; - // Keep all the white space. - while ( p && *p && !StringEqual( p, endTag, false, encoding ) ) - { - value.append( p, 1 ); - ++p; - } - if ( p && *p ) - p += strlen( endTag ); - - return p; -} - - -const char* TiXmlAttribute::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p ) return 0; - - if ( data ) - { - data->Stamp( p, encoding ); - location = data->Cursor(); - } - // Read the name, the '=' and the value. - const char* pErr = p; - p = ReadName( p, &name, encoding ); - if ( !p || !*p ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding ); - return 0; - } - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p || *p != '=' ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - - ++p; // skip '=' - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - - const char* end; - const char SINGLE_QUOTE = '\''; - const char DOUBLE_QUOTE = '\"'; - - if ( *p == SINGLE_QUOTE ) - { - ++p; - end = "\'"; // single quote in string - p = ReadText( p, &value, false, end, false, encoding ); - } - else if ( *p == DOUBLE_QUOTE ) - { - ++p; - end = "\""; // double quote in string - p = ReadText( p, &value, false, end, false, encoding ); - } - else - { - // All attribute values should be in single or double quotes. - // But this is such a common error that the parser will try - // its best, even without them. - value = ""; - while ( p && *p // existence - && !IsWhiteSpace( *p ) // whitespace - && *p != '/' && *p != '>' ) // tag end - { - if ( *p == SINGLE_QUOTE || *p == DOUBLE_QUOTE ) { - // [ 1451649 ] Attribute values with trailing quotes not handled correctly - // We did not have an opening quote but seem to have a - // closing one. Give up and throw an error. - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - value += *p; - ++p; - } - } - return p; -} - -#ifdef TIXML_USE_STL -void TiXmlText::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - while ( in->good() ) - { - int c = in->peek(); - if ( !cdata && (c == '<' ) ) - { - return; - } - if ( c <= 0 ) - { - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - - (*tag) += (char) c; - in->get(); // "commits" the peek made above - - if ( cdata && c == '>' && tag->size() >= 3 ) { - size_t len = tag->size(); - if ( (*tag)[len-2] == ']' && (*tag)[len-3] == ']' ) { - // terminator of cdata. - return; - } - } - } -} -#endif - -const char* TiXmlText::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - value = ""; - TiXmlDocument* document = GetDocument(); - - if ( data ) - { - data->Stamp( p, encoding ); - location = data->Cursor(); - } - - const char* const startTag = ""; - - if ( cdata || StringEqual( p, startTag, false, encoding ) ) - { - cdata = true; - - if ( !StringEqual( p, startTag, false, encoding ) ) - { - if ( document ) - document->SetError( TIXML_ERROR_PARSING_CDATA, p, data, encoding ); - return 0; - } - p += strlen( startTag ); - - // Keep all the white space, ignore the encoding, etc. - while ( p && *p - && !StringEqual( p, endTag, false, encoding ) - ) - { - value += *p; - ++p; - } - - TIXML_STRING dummy; - p = ReadText( p, &dummy, false, endTag, false, encoding ); - return p; - } - else - { - bool ignoreWhite = true; - - const char* end = "<"; - p = ReadText( p, &value, ignoreWhite, end, false, encoding ); - if ( p && *p ) - return p-1; // don't truncate the '<' - return 0; - } -} - -#ifdef TIXML_USE_STL -void TiXmlDeclaration::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - while ( in->good() ) - { - int c = in->get(); - if ( c <= 0 ) - { - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - (*tag) += (char) c; - - if ( c == '>' ) - { - // All is well. - return; - } - } -} -#endif - -const char* TiXmlDeclaration::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding _encoding ) -{ - p = SkipWhiteSpace( p, _encoding ); - // Find the beginning, find the end, and look for - // the stuff in-between. - TiXmlDocument* document = GetDocument(); - if ( !p || !*p || !StringEqual( p, "SetError( TIXML_ERROR_PARSING_DECLARATION, 0, 0, _encoding ); - return 0; - } - if ( data ) - { - data->Stamp( p, _encoding ); - location = data->Cursor(); - } - p += 5; - - version = ""; - encoding = ""; - standalone = ""; - - while ( p && *p ) - { - if ( *p == '>' ) - { - ++p; - return p; - } - - p = SkipWhiteSpace( p, _encoding ); - if ( StringEqual( p, "version", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - version = attrib.Value(); - } - else if ( StringEqual( p, "encoding", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - encoding = attrib.Value(); - } - else if ( StringEqual( p, "standalone", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - standalone = attrib.Value(); - } - else - { - // Read over whatever it is. - while( p && *p && *p != '>' && !IsWhiteSpace( *p ) ) - ++p; - } - } - return 0; -} - -bool TiXmlText::Blank() const -{ - for ( unsigned i=0; iScriptLoop()); EXPECT_DOUBLE_EQ(1.0, actor2->ScriptDelayStart()); EXPECT_TRUE(actor2->ScriptAutoStart()); + + EXPECT_EQ(2u, actor2->LinkCount()); + EXPECT_EQ(1u, actor2->JointCount()); } +////////////////////////////////////////////////// +TEST(DOMActor, CopySdfLoadedProperties) +{ + // Verify that copying an actor also copies the underlying ElementPtr + // Joints and Links + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_complete.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + + ASSERT_NE(nullptr, root.Element()); + + const sdf::World *world = root.WorldByIndex(0); + const sdf::Actor *actor2 = world->ActorByIndex(1); + sdf::Actor actor1(*actor2); + + EXPECT_EQ(actor1.Element().get(), actor2->Element().get()); + EXPECT_EQ(actor1.LinkCount(), actor2->LinkCount()); + EXPECT_EQ(actor1.JointCount(), actor2->JointCount()); +} diff --git a/test/integration/collision_dom.cc b/test/integration/collision_dom.cc index 06ea0b23e..1d5894b9e 100644 --- a/test/integration/collision_dom.cc +++ b/test/integration/collision_dom.cc @@ -71,7 +71,7 @@ TEST(DOMCollision, DoublePendulum) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_TRUE(model != nullptr); const sdf::Link *baseLink = model->LinkByIndex(0); @@ -105,7 +105,7 @@ TEST(DOMCollision, LoadModelFramesRelativeToJoint) using Pose = ignition::math::Pose3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_frame_relative_to_joint", model->Name()); EXPECT_EQ(2u, model->LinkCount()); @@ -284,4 +284,3 @@ TEST(DOMCollision, LoadModelFramesRelativeToJoint) linkC->CollisionByName("F4")->SemanticPose().Resolve(pose).empty()); EXPECT_EQ(Pose(-18, 3, 4, 0, -IGN_PI/2, 0), pose); } - diff --git a/test/integration/element_memory_leak.cc b/test/integration/element_memory_leak.cc index 5b7eda3e6..4b4d5b625 100644 --- a/test/integration/element_memory_leak.cc +++ b/test/integration/element_memory_leak.cc @@ -70,7 +70,7 @@ const std::string sdfString( const std::string getMemInfoPath = sdf::filesystem::append(PROJECT_SOURCE_PATH, "tools", "get_mem_info.py"); -const std::string pythonMeminfo("python " + getMemInfoPath); +const std::string pythonMeminfo("python3 " + getMemInfoPath); int getMemoryUsage() { diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 6c9c89b12..cb0ffd5b5 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -423,7 +423,7 @@ TEST(DOMFrame, LoadModelFramesAttachedTo) EXPECT_TRUE(root.Load(testFile).empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_frame_attached_to", model->Name()); EXPECT_EQ(1u, model->LinkCount()); @@ -489,7 +489,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) EXPECT_NE(std::string::npos, errors[0].Message().find( "attached_to name[A] specified by frame with name[F3] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_CYCLE); EXPECT_NE(std::string::npos, errors[1].Message().find( @@ -502,7 +502,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) EXPECT_NE(std::string::npos, errors[5].Message().find( "attached_to name[A] specified by frame with name[F3] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE); EXPECT_NE(std::string::npos, errors[6].Message().find( @@ -525,7 +525,7 @@ TEST(DOMFrame, LoadModelFramesAttachedToJoint) EXPECT_TRUE(root.Load(testFile).empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_frame_attached_to_joint", model->Name()); EXPECT_EQ(2u, model->LinkCount()); @@ -578,6 +578,56 @@ TEST(DOMFrame, LoadModelFramesAttachedToJoint) EXPECT_EQ("C", body); } +///////////////////////////////////////////////// +TEST(DOMFrame, LoadModelFramesAttachedToNestedModel) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_frame_attached_to_nested_model.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("model_frame_attached_to_nested_model", model->Name()); + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_EQ(nullptr, model->LinkByIndex(1)); + + EXPECT_TRUE(model->LinkNameExists("link")); + + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + EXPECT_EQ(0u, model->JointCount()); + EXPECT_EQ(nullptr, model->JointByIndex(0)); + + EXPECT_EQ(1u, model->ModelCount()); + EXPECT_NE(nullptr, model->ModelByIndex(0)); + EXPECT_EQ(nullptr, model->ModelByIndex(1)); + + EXPECT_TRUE(model->ModelNameExists("nested_model")); + + EXPECT_EQ(2u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_NE(nullptr, model->FrameByIndex(1)); + EXPECT_EQ(nullptr, model->FrameByIndex(2)); + ASSERT_TRUE(model->FrameNameExists("F1")); + ASSERT_TRUE(model->FrameNameExists("F2")); + + EXPECT_EQ("nested_model", model->FrameByName("F1")->AttachedTo()); + EXPECT_EQ("F1", model->FrameByName("F2")->AttachedTo()); + + std::string body; + EXPECT_TRUE(model->FrameByName("F1")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("nested_model::nested_link", body); + EXPECT_TRUE(model->FrameByName("F2")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("nested_model::nested_link", body); +} + ///////////////////////////////////////////////// TEST(DOMFrame, LoadWorldFramesAttachedTo) { @@ -642,7 +692,7 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo) EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); EXPECT_EQ("world", body); EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("M1", body); + EXPECT_EQ("M1::L", body); } ///////////////////////////////////////////////// @@ -700,7 +750,7 @@ TEST(DOMFrame, LoadModelFramesRelativeTo) using Pose = ignition::math::Pose3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_frame_relative_to", model->Name()); EXPECT_EQ(1u, model->LinkCount()); @@ -837,7 +887,7 @@ TEST(DOMFrame, LoadModelFramesInvalidRelativeTo) EXPECT_NE(std::string::npos, errors[0].Message().find( "relative_to name[A] specified by frame with name[F] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE); EXPECT_NE(std::string::npos, errors[1].Message().find( @@ -861,7 +911,7 @@ TEST(DOMFrame, LoadModelFramesRelativeToJoint) using Pose = ignition::math::Pose3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_frame_relative_to_joint", model->Name()); EXPECT_EQ(2u, model->LinkCount()); @@ -1060,7 +1110,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidRelativeTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(11u, errors.size()); + EXPECT_EQ(15u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -1126,7 +1176,7 @@ TEST(DOMFrame, WorldIncludeModel) ASSERT_NE(nullptr, model); ignition::math::Pose3d modelPose; sdf::Errors resolveErrors = model->SemanticPose().Resolve(modelPose); - EXPECT_TRUE(resolveErrors.empty()); + EXPECT_TRUE(resolveErrors.empty()) << resolveErrors; EXPECT_EQ(expectedPoses[i], modelPose); } } diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 3f1e44f21..765c99a4e 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -19,11 +19,14 @@ #include #include "sdf/Box.hh" -#include "sdf/Cylinder.hh" +#include "sdf/Capsule.hh" #include "sdf/Collision.hh" +#include "sdf/Cylinder.hh" #include "sdf/Element.hh" +#include "sdf/Ellipsoid.hh" #include "sdf/Filesystem.hh" #include "sdf/Geometry.hh" +#include "sdf/Heightmap.hh" #include "sdf/Link.hh" #include "sdf/Mesh.hh" #include "sdf/Model.hh" @@ -31,8 +34,8 @@ #include "sdf/Root.hh" #include "sdf/Sphere.hh" #include "sdf/Types.hh" -#include "sdf/World.hh" #include "sdf/Visual.hh" +#include "sdf/World.hh" #include "test_config.h" ////////////////////////////////////////////////// @@ -47,7 +50,7 @@ TEST(DOMGeometry, Shapes) EXPECT_TRUE(root.Load(testFile).empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); const sdf::Link *link = model->LinkByIndex(0); @@ -71,6 +74,26 @@ TEST(DOMGeometry, Shapes) ASSERT_NE(nullptr, boxVisGeom); EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), boxVisGeom->Size()); + // Test capsule collision + const sdf::Collision *capsuleCol = link->CollisionByName("capsule_col"); + ASSERT_NE(nullptr, capsuleCol); + ASSERT_NE(nullptr, capsuleCol->Geom()); + EXPECT_EQ(sdf::GeometryType::CAPSULE, capsuleCol->Geom()->Type()); + const sdf::Capsule *capsuleColGeom = capsuleCol->Geom()->CapsuleShape(); + ASSERT_NE(nullptr, capsuleColGeom); + EXPECT_DOUBLE_EQ(0.2, capsuleColGeom->Radius()); + EXPECT_DOUBLE_EQ(0.1, capsuleColGeom->Length()); + + // Test capsule visual + const sdf::Visual *capsuleVis = link->VisualByName("capsule_vis"); + ASSERT_NE(nullptr, capsuleVis); + ASSERT_NE(nullptr, capsuleVis->Geom()); + EXPECT_EQ(sdf::GeometryType::CAPSULE, capsuleVis->Geom()->Type()); + const sdf::Capsule *capsuleVisGeom = capsuleVis->Geom()->CapsuleShape(); + ASSERT_NE(nullptr, capsuleVisGeom); + EXPECT_DOUBLE_EQ(2.1, capsuleVisGeom->Radius()); + EXPECT_DOUBLE_EQ(10.2, capsuleVisGeom->Length()); + // Test cylinder collision const sdf::Collision *cylinderCol = link->CollisionByName("cylinder_col"); ASSERT_NE(nullptr, cylinderCol); @@ -91,6 +114,26 @@ TEST(DOMGeometry, Shapes) EXPECT_DOUBLE_EQ(2.1, cylinderVisGeom->Radius()); EXPECT_DOUBLE_EQ(10.2, cylinderVisGeom->Length()); + // Test ellipsoid collision + const sdf::Collision *ellipsoidCol = link->CollisionByName("ellipsoid_col"); + ASSERT_NE(nullptr, ellipsoidCol); + ASSERT_NE(nullptr, ellipsoidCol->Geom()); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, ellipsoidCol->Geom()->Type()); + const sdf::Ellipsoid *ellipsoidColGeom = + ellipsoidCol->Geom()->EllipsoidShape(); + ASSERT_NE(nullptr, ellipsoidColGeom); + EXPECT_EQ(ignition::math::Vector3d(1.0, 2.0, 3.0), ellipsoidColGeom->Radii()); + + // Test ellipsoid visual + const sdf::Visual *ellipsoidVis = link->VisualByName("ellipsoid_vis"); + ASSERT_NE(nullptr, ellipsoidVis); + ASSERT_NE(nullptr, ellipsoidVis->Geom()); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, ellipsoidVis->Geom()->Type()); + const sdf::Ellipsoid *ellipsoidVisGeom = + ellipsoidVis->Geom()->EllipsoidShape(); + ASSERT_NE(nullptr, ellipsoidVisGeom); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), ellipsoidVisGeom->Radii()); + // Test plane collision const sdf::Collision *planeCol = link->CollisionByName("plane_col"); ASSERT_NE(nullptr, planeCol); @@ -136,8 +179,8 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::MESH, meshCol->Geom()->Type()); const sdf::Mesh *meshColGeom = meshCol->Geom()->MeshShape(); ASSERT_NE(nullptr, meshColGeom); - EXPECT_EQ("https://ignitionfuel.org/an_org/models/a_model/mesh/mesh.dae", - meshColGeom->Uri()); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/mesh/" + "mesh.dae", meshColGeom->Uri()); EXPECT_TRUE(ignition::math::Vector3d(0.1, 0.2, 0.3) == meshColGeom->Scale()); EXPECT_EQ("my_submesh", meshColGeom->Submesh()); @@ -150,10 +193,67 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::MESH, meshVis->Geom()->Type()); const sdf::Mesh *meshVisGeom = meshVis->Geom()->MeshShape(); ASSERT_NE(nullptr, meshVisGeom); - EXPECT_EQ("https://ignitionfuel.org/an_org/models/a_model/mesh/mesh.dae", - meshVisGeom->Uri()); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/mesh" + "/mesh.dae", meshVisGeom->Uri()); EXPECT_TRUE(ignition::math::Vector3d(1.2, 2.3, 3.4) == meshVisGeom->Scale()); EXPECT_EQ("another_submesh", meshVisGeom->Submesh()); EXPECT_FALSE(meshVisGeom->CenterSubmesh()); + + // Test heightmap collision + auto heightmapCol = link->CollisionByName("heightmap_col"); + ASSERT_NE(nullptr, heightmapCol); + ASSERT_NE(nullptr, heightmapCol->Geom()); + EXPECT_EQ(sdf::GeometryType::HEIGHTMAP, heightmapCol->Geom()->Type()); + auto heightmapColGeom = heightmapCol->Geom()->HeightmapShape(); + ASSERT_NE(nullptr, heightmapColGeom); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" + "materials/textures/heightmap.png", heightmapColGeom->Uri()); + EXPECT_EQ(ignition::math::Vector3d(500, 500, 100), heightmapColGeom->Size()); + EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), heightmapColGeom->Position()); + EXPECT_EQ(0u, heightmapColGeom->TextureCount()); + EXPECT_EQ(0u, heightmapColGeom->BlendCount()); + + // Test heightmap visual + auto heightmapVis = link->VisualByName("heightmap_vis"); + ASSERT_NE(nullptr, heightmapVis); + ASSERT_NE(nullptr, heightmapVis->Geom()); + EXPECT_EQ(sdf::GeometryType::HEIGHTMAP, heightmapVis->Geom()->Type()); + auto heightmapVisGeom = heightmapVis->Geom()->HeightmapShape(); + ASSERT_NE(nullptr, heightmapVisGeom); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" + "materials/textures/heightmap.png", heightmapVisGeom->Uri()); + EXPECT_EQ(ignition::math::Vector3d(500, 500, 100), heightmapVisGeom->Size()); + EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), heightmapVisGeom->Position()); + EXPECT_EQ(3u, heightmapVisGeom->TextureCount()); + EXPECT_EQ(2u, heightmapVisGeom->BlendCount()); + + auto texture0 = heightmapVisGeom->TextureByIndex(0u); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" + "materials/textures/diffuse0.png", texture0->Diffuse()); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" + "materials/textures/normal0.png", texture0->Normal()); + EXPECT_DOUBLE_EQ(5.0, texture0->Size()); + + auto texture1 = heightmapVisGeom->TextureByIndex(1u); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" + "materials/textures/diffuse1.png", texture1->Diffuse()); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" + "materials/textures/normal1.png", texture1->Normal()); + EXPECT_DOUBLE_EQ(10.0, texture1->Size()); + + auto texture2 = heightmapVisGeom->TextureByIndex(2u); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" + "materials/textures/diffuse2.png", texture2->Diffuse()); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" + "materials/textures/normal2.png", texture2->Normal()); + EXPECT_DOUBLE_EQ(20.0, texture2->Size()); + + auto blend0 = heightmapVisGeom->BlendByIndex(0u); + EXPECT_DOUBLE_EQ(15.0, blend0->MinHeight()); + EXPECT_DOUBLE_EQ(5.0, blend0->FadeDistance()); + + auto blend1 = heightmapVisGeom->BlendByIndex(1u); + EXPECT_DOUBLE_EQ(30.0, blend1->MinHeight()); + EXPECT_DOUBLE_EQ(10.0, blend1->FadeDistance()); } diff --git a/test/integration/includes.cc b/test/integration/includes.cc index bed8b9a8b..6c7b8a6a0 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -60,11 +60,11 @@ TEST(IncludesTest, Includes) ASSERT_NE(nullptr, root.Element()); EXPECT_EQ(worldFile, root.Element()->FilePath()); - EXPECT_EQ("1.6", root.Element()->OriginalVersion()); + EXPECT_EQ("1.8", root.Element()->OriginalVersion()); const sdf::World *world = root.WorldByIndex(0); ASSERT_NE(nullptr, world); - EXPECT_EQ("1.6", world->Element()->OriginalVersion()); + EXPECT_EQ("1.8", world->Element()->OriginalVersion()); // Actors EXPECT_EQ(2u, world->ActorCount()); @@ -149,7 +149,7 @@ TEST(IncludesTest, Includes) EXPECT_EQ("", pointLight1->PoseRelativeTo()); // Models - EXPECT_EQ(2u, world->ModelCount()); + EXPECT_EQ(3u, world->ModelCount()); EXPECT_FALSE(world->ModelNameExists("")); // Model without overrides @@ -209,6 +209,19 @@ TEST(IncludesTest, Includes) EXPECT_EQ("", model1->PoseRelativeTo()); ASSERT_NE(nullptr, model1->Element()); EXPECT_TRUE(model1->Element()->HasElement("plugin")); + + const sdf::Model *model2 = world->ModelByIndex(2); + ASSERT_NE(nullptr, model2); + EXPECT_EQ("test_model_with_file", model2->Name()); + EXPECT_FALSE(model2->Static()); + EXPECT_EQ(1u, model2->LinkCount()); + ASSERT_NE(nullptr, model2->LinkByIndex(0)); + ASSERT_NE(nullptr, model2->LinkByName("link")); + EXPECT_EQ(model2->LinkByName("link")->Name(), model2->LinkByIndex(0)->Name()); + EXPECT_EQ(nullptr, model2->LinkByIndex(1)); + EXPECT_TRUE(model2->LinkNameExists("link")); + EXPECT_FALSE(model2->LinkNameExists("coconut")); + EXPECT_EQ("1.6", model2->Element()->OriginalVersion()); } ////////////////////////////////////////////////// @@ -318,3 +331,40 @@ TEST(IncludesTest, Includes_15_convert) EXPECT_EQ("1.6", modelElem->OriginalVersion()); EXPECT_EQ("1.6", linkElem->OriginalVersion()); } + +////////////////////////////////////////////////// +TEST(IncludesTest, IncludeModelMissingConfig) +{ + sdf::setFindCallback(findFileCb); + + std::ostringstream stream; + stream + << "" + << "" + << " box_missing_config" + << "" + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + sdf::Errors errors; + ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed, errors)); + + ASSERT_GE(1u, errors.size()); + EXPECT_EQ(1u, errors.size()); + std::cout << errors[0] << std::endl; + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::URI_LOOKUP); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Unable to resolve uri[box_missing_config] to model path")) << errors[0]; + EXPECT_NE(std::string::npos, errors[0].Message().find( + "box_missing_config] since it does not contain a model.config file")) + << errors[0]; + + sdf::Root root; + errors = root.Load(sdfParsed); + for (auto e : errors) + std::cout << e.Message() << std::endl; + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(nullptr, root.Model()); +} diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc index 311cff0ec..a5fa27172 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -15,6 +15,7 @@ * */ +#include #include #include #include @@ -42,7 +43,7 @@ TEST(DOMJointAxis, Complete) EXPECT_TRUE(errors.empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); // The model should have nine joints. @@ -132,7 +133,7 @@ TEST(DOMJointAxis, XyzExpressedIn) using Vector3 = ignition::math::Vector3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_joint_axis_expressed_in", model->Name()); EXPECT_EQ(4u, model->LinkCount()); @@ -224,3 +225,122 @@ TEST(DOMJointAxis, XyzExpressedIn) EXPECT_EQ(0u, model->FrameCount()); EXPECT_EQ(nullptr, model->FrameByIndex(0)); } + +////////////////////////////////////////////////// +TEST(DOMJointAxis, InfiniteLimits) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_axis_infinite_limits.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + + EXPECT_TRUE(errors.empty()); + for (auto e : errors) + std::cout << e << std::endl; + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("joint_axis_infinite_limits", model->Name()); + + const double kInf = std::numeric_limits::infinity(); + { + auto joint = model->JointByName("default_joint_limits"); + ASSERT_NE(nullptr, joint); + auto axis = joint->Axis(0); + ASSERT_NE(nullptr, axis); + EXPECT_DOUBLE_EQ(-1e16, axis->Lower()); + EXPECT_DOUBLE_EQ(1e16, axis->Upper()); + EXPECT_DOUBLE_EQ(kInf, axis->Effort()); + EXPECT_DOUBLE_EQ(kInf, axis->MaxVelocity()); + } + + { + auto joint = model->JointByName("finite_joint_limits"); + ASSERT_NE(nullptr, joint); + auto axis = joint->Axis(0); + ASSERT_NE(nullptr, axis); + EXPECT_DOUBLE_EQ(-1.5, axis->Lower()); + EXPECT_DOUBLE_EQ(1.5, axis->Upper()); + EXPECT_DOUBLE_EQ(2.5, axis->MaxVelocity()); + EXPECT_DOUBLE_EQ(5.5, axis->Effort()); + } + + { + auto joint = model->JointByName("infinite_joint_limits_inf"); + ASSERT_NE(nullptr, joint); + auto axis = joint->Axis(0); + ASSERT_NE(nullptr, axis); + EXPECT_DOUBLE_EQ(-kInf, axis->Lower()); + EXPECT_DOUBLE_EQ(kInf, axis->Upper()); + EXPECT_DOUBLE_EQ(kInf, axis->Effort()); + EXPECT_DOUBLE_EQ(kInf, axis->MaxVelocity()); + } + + { + auto joint = model->JointByName("infinite_joint_limits_neg"); + ASSERT_NE(nullptr, joint); + auto axis = joint->Axis(0); + ASSERT_NE(nullptr, axis); + EXPECT_DOUBLE_EQ(-kInf, axis->Lower()); + EXPECT_DOUBLE_EQ(kInf, axis->Upper()); + EXPECT_DOUBLE_EQ(kInf, axis->Effort()); + EXPECT_DOUBLE_EQ(kInf, axis->MaxVelocity()); + } +} + +////////////////////////////////////////////////// +TEST(DOMJointAxis, XyzNormalization) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_axis_xyz_normalization.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + + ASSERT_EQ(1u, errors.size()); + EXPECT_TRUE( + errors[0].Message().find("The norm of the xyz vector cannot be zero") != + std::string::npos); + + using ignition::math::Vector3d; + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + + { + auto joint1 = model->JointByName("joint1"); + ASSERT_FALSE(nullptr == joint1); + ASSERT_FALSE(nullptr == joint1->Axis(0)); + EXPECT_EQ(Vector3d::UnitZ, joint1->Axis(0)->Xyz()); + } + + { + auto joint2 = model->JointByName("joint2"); + ASSERT_FALSE(nullptr == joint2); + ASSERT_FALSE(nullptr == joint2->Axis(0)); + EXPECT_EQ(Vector3d::UnitX, joint2->Axis(0)->Xyz()); + } + + { + auto joint3 = model->JointByName("joint3"); + ASSERT_FALSE(nullptr == joint3); + ASSERT_FALSE(nullptr == joint3->Axis(0)); + EXPECT_EQ(-Vector3d::UnitX, joint3->Axis(0)->Xyz()); + ASSERT_FALSE(nullptr == joint3->Axis(1)); + EXPECT_EQ(Vector3d::UnitY, joint3->Axis(1)->Xyz()); + } + + { + auto joint4 = model->JointByName("joint4"); + ASSERT_FALSE(nullptr == joint4); + ASSERT_FALSE(nullptr == joint4->Axis(0)); + EXPECT_EQ(Vector3d::UnitZ, joint4->Axis(0)->Xyz()); + } +} diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index a7d7b0cd3..777f48f3d 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -20,6 +20,7 @@ #include "sdf/Element.hh" #include "sdf/Filesystem.hh" +#include "sdf/Frame.hh" #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" #include "sdf/Link.hh" @@ -71,7 +72,7 @@ TEST(DOMJoint, DoublePendulum) EXPECT_TRUE(root.Load(testFile).empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); // The double pendulum should have two joints. @@ -126,7 +127,7 @@ TEST(DOMJoint, Complete) EXPECT_TRUE(errors.empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); std::vector jointPoses = @@ -170,7 +171,7 @@ TEST(DOMJoint, LoadJointParentWorld) using Pose = ignition::math::Pose3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("joint_parent_world", model->Name()); EXPECT_EQ(1u, model->LinkCount()); @@ -192,9 +193,16 @@ TEST(DOMJoint, LoadJointParentWorld) ASSERT_TRUE(model->JointNameExists("joint")); EXPECT_EQ("link", model->JointByName("joint")->ChildLinkName()); EXPECT_EQ("world", model->JointByName("joint")->ParentLinkName()); - EXPECT_TRUE(model->JointByName("joint")->PoseRelativeTo().empty()); + std::string resolvedLinkName; + EXPECT_TRUE( + model->JointByName("joint")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("link", resolvedLinkName); + EXPECT_TRUE( + model->JointByName("joint")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("world", resolvedLinkName); EXPECT_EQ(Pose(0, 0, 3, 0, 0, 0), model->JointByName("joint")->RawPose()); + EXPECT_TRUE(model->JointByName("joint")->PoseRelativeTo().empty()); EXPECT_EQ(0u, model->FrameCount()); EXPECT_EQ(nullptr, model->FrameByIndex(0)); @@ -212,8 +220,7 @@ TEST(DOMJoint, LoadInvalidJointChildWorld) auto errors = root.Load(testFile); for (auto e : errors) std::cout << e << std::endl; - EXPECT_FALSE(errors.empty()); - EXPECT_EQ(7u, errors.size()); + ASSERT_EQ(7u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -221,10 +228,196 @@ TEST(DOMJoint, LoadInvalidJointChildWorld) EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[1].Message().find( - "Child link with name[world] specified by joint with name[joint] " + "Child frame with name[world] specified by joint with name[joint] " "not found in model with name[joint_child_world]")); } +///////////////////////////////////////////////// +TEST(DOMJoint, LoadJointParentFrame) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_parent_frame.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("joint_parent_frame", model->Name()); + EXPECT_EQ(2u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_NE(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(nullptr, model->LinkByIndex(2)); + EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ("", model->PoseRelativeTo()); + + ASSERT_TRUE(model->LinkNameExists("parent_link")); + ASSERT_TRUE(model->LinkNameExists("child_link")); + EXPECT_TRUE(model->LinkByName("parent_link")->PoseRelativeTo().empty()); + EXPECT_TRUE(model->LinkByName("child_link")->PoseRelativeTo().empty()); + + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), + model->LinkByName("parent_link")->RawPose()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), + model->LinkByName("child_link")->RawPose()); + + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + EXPECT_EQ(1u, model->JointCount()); + EXPECT_NE(nullptr, model->JointByIndex(0)); + EXPECT_EQ(nullptr, model->JointByIndex(1)); + ASSERT_TRUE(model->JointNameExists("joint")); + EXPECT_EQ("child_link", model->JointByName("joint")->ChildLinkName()); + EXPECT_EQ("parent_frame", model->JointByName("joint")->ParentLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE( + model->JointByName("joint")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("child_link", resolvedLinkName); + EXPECT_TRUE( + model->JointByName("joint")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("parent_link", resolvedLinkName); + + EXPECT_TRUE(model->JointByName("joint")->PoseRelativeTo().empty()); + EXPECT_EQ(Pose(0, 1, 0, 0, 0, 0), model->JointByName("joint")->RawPose()); + + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + + ASSERT_TRUE(model->FrameNameExists("parent_frame")); + + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), + model->FrameByName("parent_frame")->RawPose()); + + // Test ResolveFrame to get each link, joint and frame pose in model frame. + Pose pose; + EXPECT_TRUE( + model->LinkByName("parent_link")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), pose); + EXPECT_TRUE( + model->LinkByName("child_link")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), pose); + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 1, 10, 0, 0, 0), pose); + EXPECT_TRUE( + model->FrameByName("parent_frame")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 1, 0, 0, 0), pose); + + // joint frame relative to parent and child links + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "child_link").empty()); + EXPECT_EQ(Pose(0, 1, 0, 0, 0, 0), pose); + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "parent_link").empty()); + EXPECT_EQ(Pose(0, 1, 9, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(DOMJoint, LoadJointChildFrame) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_child_frame.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("joint_child_frame", model->Name()); + EXPECT_EQ(2u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_NE(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(nullptr, model->LinkByIndex(2)); + EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ("", model->PoseRelativeTo()); + + ASSERT_TRUE(model->LinkNameExists("parent_link")); + ASSERT_TRUE(model->LinkNameExists("child_link")); + EXPECT_TRUE(model->LinkByName("parent_link")->PoseRelativeTo().empty()); + EXPECT_TRUE(model->LinkByName("child_link")->PoseRelativeTo().empty()); + + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), + model->LinkByName("parent_link")->RawPose()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), + model->LinkByName("child_link")->RawPose()); + + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + EXPECT_EQ(1u, model->JointCount()); + EXPECT_NE(nullptr, model->JointByIndex(0)); + EXPECT_EQ(nullptr, model->JointByIndex(1)); + ASSERT_TRUE(model->JointNameExists("joint")); + EXPECT_EQ("child_frame", model->JointByName("joint")->ChildLinkName()); + EXPECT_EQ("parent_link", model->JointByName("joint")->ParentLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE( + model->JointByName("joint")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("child_link", resolvedLinkName); + EXPECT_TRUE( + model->JointByName("joint")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("parent_link", resolvedLinkName); + + EXPECT_TRUE(model->JointByName("joint")->PoseRelativeTo().empty()); + EXPECT_EQ(Pose(0, 1, 0, 0, 0, 0), model->JointByName("joint")->RawPose()); + + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + + ASSERT_TRUE(model->FrameNameExists("child_frame")); + + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), + model->FrameByName("child_frame")->RawPose()); + + // Test ResolveFrame to get each link, joint and frame pose in model frame. + Pose pose; + EXPECT_TRUE( + model->LinkByName("parent_link")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), pose); + EXPECT_TRUE( + model->LinkByName("child_link")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), pose); + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 1, 10, 0, 0, 0), pose); + EXPECT_TRUE( + model->FrameByName("child_frame")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 10, 0, 0, 0), pose); + + // joint frame relative to parent and child links + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "child_link").empty()); + EXPECT_EQ(Pose(1, 1, 0, 0, 0, 0), pose); + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "parent_link").empty()); + EXPECT_EQ(Pose(1, 1, 9, 0, 0, 0), pose); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadJointPoseRelativeTo) { @@ -239,7 +432,7 @@ TEST(DOMJoint, LoadJointPoseRelativeTo) using Pose = ignition::math::Pose3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_joint_relative_to", model->Name()); EXPECT_EQ(4u, model->LinkCount()); @@ -343,7 +536,7 @@ TEST(DOMJoint, LoadInvalidJointPoseRelativeTo) EXPECT_NE(std::string::npos, errors[1].Message().find( "relative_to name[A] specified by joint with name[J] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); // errors[2] // errors[3] // errors[4] @@ -366,13 +559,13 @@ TEST(DOMJoint, LoadInvalidChild) EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Child link with name[invalid] specified by joint with name[joint] not " + "Child frame with name[invalid] specified by joint with name[joint] not " "found")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR); EXPECT_NE(std::string::npos, - errors[1].Message().find( - "FrameAttachedToGraph error, Non-LINK vertex with name [joint] is " - "disconnected")); + errors[1].Message().find("FrameAttachedToGraph error, Non-LINK vertex " + "with name [joint_invalid_child::joint] is " + "disconnected")); // errors[2] // errors[3] // errors[4] @@ -397,15 +590,15 @@ TEST(DOMJoint, LoadLinkJointSameName17Invalid) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(2u, errors.size()); + EXPECT_EQ(7u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, errors[0].Message().find( "Joint with non-unique name [attachment] detected in model with name " "[link_joint_same_name].")); - EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, - errors[1].Message().find( + errors[3].Message().find( "Joint with non-unique name [attachment] detected in model with name " "[link_joint_same_name].")); } @@ -427,7 +620,7 @@ TEST(DOMJoint, LoadLinkJointSameName16Valid) using Pose = ignition::math::Pose3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("link_joint_same_name", model->Name()); EXPECT_EQ(2u, model->LinkCount()); @@ -507,7 +700,7 @@ TEST(DOMJoint, LoadURDFJointPoseRelativeTo) using Vector3 = ignition::math::Vector3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("provide_feedback_test", model->Name()); EXPECT_EQ(3u, model->LinkCount()); @@ -572,3 +765,103 @@ TEST(DOMJoint, LoadURDFJointPoseRelativeTo) model->JointByName("joint12")->Axis()->ResolveXyz(vec3, "joint12").empty()); EXPECT_EQ(Vector3(0, 1.0, 0), vec3); } + +///////////////////////////////////////////////// +TEST(DOMJoint, LoadJointNestedParentChild) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_nested_parent_child.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + + { + const sdf::Joint *j1 = model->JointByName("J1"); + ASSERT_NE(nullptr, j1); + EXPECT_EQ("M1::L1", j1->ParentLinkName()); + EXPECT_EQ("L1", j1->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j1->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + EXPECT_TRUE(j1->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j1->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 9, 0, IGN_PI_2, 0), pose); + } + { + const sdf::Joint *j2 = model->JointByName("J2"); + ASSERT_NE(nullptr, j2); + EXPECT_EQ("F1", j2->ParentLinkName()); + EXPECT_EQ("L1", j2->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j2->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + EXPECT_TRUE(j2->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j2->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 1, 10, 0, IGN_PI_2, 0), pose); + } + { + const sdf::Joint *j3 = model->JointByName("J3"); + ASSERT_NE(nullptr, j3); + EXPECT_EQ("L1", j3->ParentLinkName()); + EXPECT_EQ("M1::L2", j3->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j3->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j3->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L2", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j3->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 1, 1, 0, 0, 0), pose); + } + { + const sdf::Joint *j4 = model->JointByName("J4"); + ASSERT_NE(nullptr, j4); + EXPECT_EQ("L1", j4->ParentLinkName()); + EXPECT_EQ("M1::F1", j4->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j4->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j4->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j4->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 1, 0, 0, 0), pose); + } + { + const sdf::Joint *j5 = model->JointByName("J5"); + ASSERT_NE(nullptr, j5); + EXPECT_EQ("L1", j5->ParentLinkName()); + EXPECT_EQ("M1::M2", j5->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j5->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j5->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::M2::L1", resolvedLinkName); + + Pose pose; + EXPECT_TRUE(j5->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, -1, 1, IGN_PI_2, 0, 0), pose); + } +} diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 0633d8f22..0a967fb97 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -122,7 +123,7 @@ TEST(DOMLink, InertialDoublePendulum) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); const sdf::Link *baseLink = model->LinkByIndex(0); @@ -177,7 +178,7 @@ TEST(DOMLink, InertialComplete) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); const sdf::Link *link = model->LinkByIndex(0); @@ -215,7 +216,7 @@ TEST(DOMLink, InertialInvalid) EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::LINK_INERTIA_INVALID); EXPECT_EQ(errors[0].Message(), "A link named link has invalid inertia."); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); ASSERT_EQ(1u, model->LinkCount()); @@ -239,7 +240,7 @@ TEST(DOMLink, Sensors) EXPECT_TRUE(errors.empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model", model->Name()); @@ -591,7 +592,7 @@ TEST(DOMLink, LoadLinkPoseRelativeTo) using Pose = ignition::math::Pose3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_link_relative_to", model->Name()); EXPECT_EQ(3u, model->LinkCount()); @@ -672,7 +673,7 @@ TEST(DOMLink, LoadInvalidLinkPoseRelativeTo) EXPECT_NE(std::string::npos, errors[0].Message().find( "relative_to name[A] specified by link with name[L] does not match a " - "link, joint, or frame name in model")); + "nested model, link, joint, or frame name in model")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE); EXPECT_NE(std::string::npos, errors[1].Message().find( @@ -682,3 +683,65 @@ TEST(DOMLink, LoadInvalidLinkPoseRelativeTo) // errors[3] // errors[4] } + +///////////////////////////////////////////////// +TEST(DOMLink, ValidInertialPoseRelTo) +{ + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " " + << " 0.1 1 0.2 0 0 -0.52" + << " " + << " " + << " " + << ""; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()); + + const sdf::Model *model = root.Model(); + ASSERT_NE(model, nullptr); + + const sdf::Link *link = model->LinkByName("B"); + ASSERT_NE(link, nullptr); + + EXPECT_EQ(link->Inertial().Pose(), + ignition::math::Pose3d(0.1, 1, 0.2, 0, 0, -0.52)); +} + +///////////////////////////////////////////////// +TEST(DOMLink, InvalidInertialPoseRelTo) +{ + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " 0 0 1 0 0 0" + << " " + << " " + << " " + << " 0.1 1 0.2 0 0 -0.52" + << " " + << " " + << " " + << ""; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + + // TODO(anyone) add test for warnings once it's implemented + + const sdf::Model *model = root.Model(); + ASSERT_NE(model, nullptr); + + const sdf::Link *link = model->LinkByName("B"); + ASSERT_NE(link, nullptr); + + EXPECT_EQ(link->Inertial().Pose(), + ignition::math::Pose3d(0.1, 1, 0.2, 0, 0, -0.52)); +} diff --git a/test/integration/material_pbr.cc b/test/integration/material_pbr.cc index 4eb8f6b3d..3db748fd4 100644 --- a/test/integration/material_pbr.cc +++ b/test/integration/material_pbr.cc @@ -34,7 +34,7 @@ TEST(Material, PbrDOM) EXPECT_TRUE(root.Load(testFile).empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); // Get the first link @@ -97,6 +97,10 @@ TEST(Material, PbrDOM) // emissive map EXPECT_EQ("emissive_map.png", workflow->EmissiveMap()); + + // light map + EXPECT_EQ("light_map.png", workflow->LightMap()); + EXPECT_EQ(1u, workflow->LightMapTexCoordSet()); } // visual specular workflow @@ -148,6 +152,10 @@ TEST(Material, PbrDOM) // emissive map EXPECT_EQ("emissive_map.png", workflow->EmissiveMap()); + + // light map + EXPECT_EQ("light_map.png", workflow->LightMap()); + EXPECT_EQ(2u, workflow->LightMapTexCoordSet()); } // visual all @@ -203,6 +211,10 @@ TEST(Material, PbrDOM) // emissive map EXPECT_EQ("emissive_map.png", workflow->EmissiveMap()); + + // light map + EXPECT_EQ("light_map.png", workflow->LightMap()); + EXPECT_EQ(3u, workflow->LightMapTexCoordSet()); } // metal { @@ -237,6 +249,10 @@ TEST(Material, PbrDOM) // emissive map EXPECT_EQ("emissive_map.png", workflow->EmissiveMap()); + + // light map + EXPECT_EQ("light_map.png", workflow->LightMap()); + EXPECT_EQ(0u, workflow->LightMapTexCoordSet()); } } } @@ -343,6 +359,13 @@ TEST(Material, MaterialPBR) EXPECT_TRUE(metalElem->HasElement("emissive_map")); sdf::ElementPtr emissiveMapElem = metalElem->GetElement("emissive_map"); EXPECT_EQ("emissive_map.png", emissiveMapElem->Get()); + + + // light map + EXPECT_TRUE(metalElem->HasElement("light_map")); + sdf::ElementPtr lightMapElem = metalElem->GetElement("light_map"); + EXPECT_EQ("light_map.png", lightMapElem->Get()); + EXPECT_EQ(1u, lightMapElem->Get("uv_set")); } // visual specular workflow @@ -412,6 +435,12 @@ TEST(Material, MaterialPBR) EXPECT_TRUE(specularElem->HasElement("emissive_map")); sdf::ElementPtr emissiveMapElem = specularElem->GetElement("emissive_map"); EXPECT_EQ("emissive_map.png", emissiveMapElem->Get()); + + // light map + EXPECT_TRUE(specularElem->HasElement("light_map")); + sdf::ElementPtr lightMapElem = specularElem->GetElement("light_map"); + EXPECT_EQ("light_map.png", lightMapElem->Get()); + EXPECT_EQ(2u, lightMapElem->Get("uv_set")); } // visual all @@ -484,6 +513,12 @@ TEST(Material, MaterialPBR) sdf::ElementPtr emissiveMapElem = specularElem->GetElement("emissive_map"); EXPECT_EQ("emissive_map.png", emissiveMapElem->Get()); + + // light map + EXPECT_TRUE(specularElem->HasElement("light_map")); + sdf::ElementPtr lightMapElem = specularElem->GetElement("light_map"); + EXPECT_EQ("light_map.png", lightMapElem->Get()); + EXPECT_EQ(3u, lightMapElem->Get("uv_set")); } { @@ -538,8 +573,12 @@ TEST(Material, MaterialPBR) EXPECT_TRUE(metalElem->HasElement("emissive_map")); sdf::ElementPtr emissiveMapElem = metalElem->GetElement("emissive_map"); EXPECT_EQ("emissive_map.png", emissiveMapElem->Get()); + + // light map + EXPECT_TRUE(metalElem->HasElement("light_map")); + sdf::ElementPtr lightMapElem = metalElem->GetElement("light_map"); + EXPECT_EQ("light_map.png", lightMapElem->Get()); + EXPECT_EQ(0u, lightMapElem->Get("uv_set")); } } } - - diff --git a/test/integration/model/box_missing_config/model.sdf b/test/integration/model/box_missing_config/model.sdf new file mode 100644 index 000000000..32d16f743 --- /dev/null +++ b/test/integration/model/box_missing_config/model.sdf @@ -0,0 +1,22 @@ + + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + diff --git a/test/integration/model/model_actor_light/model.config b/test/integration/model/model_actor_light/model.config new file mode 100644 index 000000000..1a288dcb0 --- /dev/null +++ b/test/integration/model/model_actor_light/model.config @@ -0,0 +1,6 @@ + + + + multiple_models + model.sdf + diff --git a/test/integration/model/model_actor_light/model.sdf b/test/integration/model/model_actor_light/model.sdf new file mode 100644 index 000000000..b4b7e2fe5 --- /dev/null +++ b/test/integration/model/model_actor_light/model.sdf @@ -0,0 +1,18 @@ + + + + + + 0 0 0 0 0 0 + + + + + 0 0 0 0 0 0 + + + + 2 0 0 3.14 0 0 + + diff --git a/test/integration/model/model_with_nested_placement_frame_attribute/model.config b/test/integration/model/model_with_nested_placement_frame_attribute/model.config new file mode 100644 index 000000000..4514d14ba --- /dev/null +++ b/test/integration/model/model_with_nested_placement_frame_attribute/model.config @@ -0,0 +1,7 @@ + + + + model_with_nested_placement_frame_attribute + model.sdf + + diff --git a/test/integration/model/model_with_nested_placement_frame_attribute/model.sdf b/test/integration/model/model_with_nested_placement_frame_attribute/model.sdf new file mode 100644 index 000000000..51ce83b22 --- /dev/null +++ b/test/integration/model/model_with_nested_placement_frame_attribute/model.sdf @@ -0,0 +1,17 @@ + + + + 0 0 10 0 0 0 + + 1 0 0 0 0 0 + + + + 0 2 0 0 0 0 + + + 0 0 3 0 0 0 + + + + diff --git a/test/integration/model/multiple_actors/model.config b/test/integration/model/multiple_actors/model.config new file mode 100644 index 000000000..647e80fb5 --- /dev/null +++ b/test/integration/model/multiple_actors/model.config @@ -0,0 +1,6 @@ + + + + multiple_actors + model.sdf + diff --git a/test/integration/model/multiple_actors/model.sdf b/test/integration/model/multiple_actors/model.sdf new file mode 100644 index 000000000..2b4428ec9 --- /dev/null +++ b/test/integration/model/multiple_actors/model.sdf @@ -0,0 +1,20 @@ + + + + + + 0 0 0 0 0 0 + + + + + 1 0 0 1.57 0 0 + + + + + 2 0 0 3.14 0 0 + + + diff --git a/test/integration/model/multiple_lights/model.config b/test/integration/model/multiple_lights/model.config new file mode 100644 index 000000000..1b13cc32f --- /dev/null +++ b/test/integration/model/multiple_lights/model.config @@ -0,0 +1,6 @@ + + + + multiple_lights + model.sdf + diff --git a/test/integration/model/multiple_lights/model.sdf b/test/integration/model/multiple_lights/model.sdf new file mode 100644 index 000000000..e51dd8f78 --- /dev/null +++ b/test/integration/model/multiple_lights/model.sdf @@ -0,0 +1,21 @@ + + + + + + 0 0 0 0 0 0 + 1 0 0 + + + + 1 0 0 1.57 0 0 + 0 1 0 + + + + 2 0 0 3.14 0 0 + 0 0 1 + + + diff --git a/test/integration/model/multiple_models/model.config b/test/integration/model/multiple_models/model.config new file mode 100644 index 000000000..1a288dcb0 --- /dev/null +++ b/test/integration/model/multiple_models/model.config @@ -0,0 +1,6 @@ + + + + multiple_models + model.sdf + diff --git a/test/integration/model/multiple_models/model.sdf b/test/integration/model/multiple_models/model.sdf new file mode 100644 index 000000000..0e5d3faa9 --- /dev/null +++ b/test/integration/model/multiple_models/model.sdf @@ -0,0 +1,22 @@ + + + + + + 0 0 0 0 0 0 + + + + + 1 0 0 1.570796326794895 0 0 + + + + + + 2 0 0 3.141592653589793 0 0 + + + + diff --git a/test/integration/model/nested_multiple_actors_error/model.config b/test/integration/model/nested_multiple_actors_error/model.config new file mode 100644 index 000000000..b6eedd072 --- /dev/null +++ b/test/integration/model/nested_multiple_actors_error/model.config @@ -0,0 +1,6 @@ + + + + nested_multiple_actors_error + model.sdf + diff --git a/test/integration/model/nested_multiple_actors_error/model.sdf b/test/integration/model/nested_multiple_actors_error/model.sdf new file mode 100644 index 000000000..38343b3b4 --- /dev/null +++ b/test/integration/model/nested_multiple_actors_error/model.sdf @@ -0,0 +1,14 @@ + + + + + + + multiple_actors + 1 1 1 1.570796326794895 0 0 + nested_actor + + + diff --git a/test/integration/model/nested_multiple_elements_error/model.config b/test/integration/model/nested_multiple_elements_error/model.config new file mode 100644 index 000000000..3094c298a --- /dev/null +++ b/test/integration/model/nested_multiple_elements_error/model.config @@ -0,0 +1,6 @@ + + + + nested_multiple_elements_error + model.sdf + diff --git a/test/integration/model/nested_multiple_elements_error/model.sdf b/test/integration/model/nested_multiple_elements_error/model.sdf new file mode 100644 index 000000000..9802678f1 --- /dev/null +++ b/test/integration/model/nested_multiple_elements_error/model.sdf @@ -0,0 +1,13 @@ + + + + + + + model_actor_light + 1 1 1 1.570796326794895 0 0 + nested_model + + diff --git a/test/integration/model/nested_multiple_lights_error/model.config b/test/integration/model/nested_multiple_lights_error/model.config new file mode 100644 index 000000000..848c78b33 --- /dev/null +++ b/test/integration/model/nested_multiple_lights_error/model.config @@ -0,0 +1,6 @@ + + + + nested_multiple_lights_error + model.sdf + diff --git a/test/integration/model/nested_multiple_lights_error/model.sdf b/test/integration/model/nested_multiple_lights_error/model.sdf new file mode 100644 index 000000000..34a1e0291 --- /dev/null +++ b/test/integration/model/nested_multiple_lights_error/model.sdf @@ -0,0 +1,13 @@ + + + + + + + multiple_lights + 1 1 1 1.570796326794895 0 0 + nested_light + + diff --git a/test/integration/model/nested_multiple_models_error/model.config b/test/integration/model/nested_multiple_models_error/model.config new file mode 100644 index 000000000..f6aeea162 --- /dev/null +++ b/test/integration/model/nested_multiple_models_error/model.config @@ -0,0 +1,6 @@ + + + + nested_multiple_models_error + model.sdf + diff --git a/test/integration/model/nested_multiple_models_error/model.sdf b/test/integration/model/nested_multiple_models_error/model.sdf new file mode 100644 index 000000000..c3cfddca7 --- /dev/null +++ b/test/integration/model/nested_multiple_models_error/model.sdf @@ -0,0 +1,13 @@ + + + + + + + multiple_models + 1 1 1 1.570796326794895 0 0 + nested_model + + diff --git a/test/integration/model/test_model_with_frames/model.sdf b/test/integration/model/test_model_with_frames/model.sdf index 430070b08..3bd0c31cc 100644 --- a/test/integration/model/test_model_with_frames/model.sdf +++ b/test/integration/model/test_model_with_frames/model.sdf @@ -59,5 +59,11 @@ L3 L4 + + 1 0 0 0 0 0 + + 1 0 0 0 0 0 + + diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 171ba1edb..258fcdfd9 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -22,6 +22,7 @@ #include "sdf/Element.hh" #include "sdf/Error.hh" #include "sdf/Filesystem.hh" +#include "sdf/Frame.hh" #include "sdf/Link.hh" #include "sdf/Model.hh" #include "sdf/Root.hh" @@ -69,7 +70,7 @@ TEST(DOMModel, NoLinks) sdf::Root root; auto errors = root.Load(testFile); EXPECT_FALSE(errors.empty()); - ASSERT_EQ(4u, errors.size()); + ASSERT_EQ(3u, errors.size()); EXPECT_EQ(sdf::ErrorCode::MODEL_WITHOUT_LINK, errors[0].Code()); EXPECT_TRUE(errors[0].Message().find("model must have at least one link") != std::string::npos); @@ -77,7 +78,6 @@ TEST(DOMModel, NoLinks) EXPECT_TRUE(errors[1].Message().find("model must have at least one link") != std::string::npos); // errors[2] - // errors[3] } ///////////////////////////////////////////////// @@ -121,7 +121,7 @@ TEST(DOMRoot, LoadDoublePendulum) EXPECT_TRUE(root.Load(testFile).empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("double_pendulum_with_base", model->Name()); EXPECT_EQ(3u, model->LinkCount()); @@ -155,15 +155,10 @@ TEST(DOMRoot, NestedModel) // Load the SDF file sdf::Root root; auto errors = root.Load(testFile); - - // it should complain because nested models aren't yet supported - EXPECT_FALSE(errors.empty()); - EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::NESTED_MODELS_UNSUPPORTED); - - EXPECT_EQ(1u, root.ModelCount()); + EXPECT_TRUE(errors.empty()) << errors; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("top_level_model", model->Name()); EXPECT_EQ(2u, model->LinkCount()); @@ -181,6 +176,211 @@ TEST(DOMRoot, NestedModel) EXPECT_EQ(nullptr, model->JointByIndex(1)); EXPECT_TRUE(model->JointNameExists("top_level_joint")); + + ASSERT_EQ(1u, model->ModelCount()); + const sdf::Model *nestedModel = model->ModelByIndex(0); + ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ(nullptr, model->ModelByIndex(1)); + + EXPECT_TRUE(model->ModelNameExists("nested_model")); + EXPECT_EQ(nestedModel, model->ModelByName("nested_model")); + EXPECT_EQ("nested_model", nestedModel->Name()); + + EXPECT_EQ(1u, nestedModel->LinkCount()); + EXPECT_NE(nullptr, nestedModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, nestedModel->LinkByIndex(1)); + + EXPECT_TRUE(nestedModel->LinkNameExists("nested_link01")); + + EXPECT_EQ(0u, nestedModel->JointCount()); + EXPECT_EQ(0u, nestedModel->FrameCount()); + + // get nested link from parent model + const std::string linkNestedName = "nested_model::nested_link01"; + EXPECT_TRUE(model->LinkNameExists(linkNestedName)); + const sdf::Link *nestedLink01 = model->LinkByName(linkNestedName); + EXPECT_NE(nullptr, nestedLink01); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, MultiNestedModel) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_multi_nested_model.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + + // Get the outer model + const sdf::Model *outerModel = root.Model(); + ASSERT_NE(nullptr, outerModel); + EXPECT_EQ("outer_model", outerModel->Name()); + EXPECT_EQ(1u, outerModel->LinkCount()); + EXPECT_NE(nullptr, outerModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, outerModel->LinkByIndex(1)); + + EXPECT_TRUE(outerModel->LinkNameExists("outer_link")); + + EXPECT_EQ(1u, outerModel->JointCount()); + EXPECT_TRUE(outerModel->JointNameExists("outer_joint")); + + EXPECT_EQ(1u, outerModel->FrameCount()); + EXPECT_TRUE(outerModel->FrameNameExists("outer_frame")); + + EXPECT_EQ(1u, outerModel->ModelCount()); + EXPECT_NE(nullptr, outerModel->ModelByIndex(0)); + EXPECT_EQ(nullptr, outerModel->ModelByIndex(1)); + + EXPECT_TRUE(outerModel->ModelNameExists("mid_model")); + + // Get the middle model + const sdf::Model *midModel = outerModel->ModelByIndex(0); + ASSERT_NE(nullptr, midModel); + EXPECT_EQ("mid_model", midModel->Name()); + EXPECT_EQ(1u, midModel->LinkCount()); + EXPECT_NE(nullptr, midModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, midModel->LinkByIndex(1)); + + EXPECT_TRUE(midModel->LinkNameExists("mid_link")); + + EXPECT_EQ(1u, midModel->JointCount()); + EXPECT_TRUE(midModel->JointNameExists("mid_joint")); + + EXPECT_EQ(1u, midModel->FrameCount()); + EXPECT_TRUE(midModel->FrameNameExists("mid_frame")); + + EXPECT_EQ(1u, midModel->ModelCount()); + EXPECT_NE(nullptr, midModel->ModelByIndex(0)); + EXPECT_EQ(nullptr, midModel->ModelByIndex(1)); + + EXPECT_TRUE(midModel->ModelNameExists("inner_model")); + + // Get the inner model + const sdf::Model *innerModel = midModel->ModelByIndex(0); + ASSERT_NE(nullptr, innerModel); + EXPECT_EQ("inner_model", innerModel->Name()); + EXPECT_EQ(1u, innerModel->LinkCount()); + EXPECT_NE(nullptr, innerModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, innerModel->LinkByIndex(1)); + + EXPECT_TRUE(innerModel->LinkNameExists("inner_link")); + + EXPECT_EQ(1u, innerModel->JointCount()); + EXPECT_TRUE(innerModel->JointNameExists("inner_joint")); + + EXPECT_EQ(1u, innerModel->FrameCount()); + EXPECT_TRUE(innerModel->FrameNameExists("inner_frame")); + + EXPECT_EQ(0u, innerModel->ModelCount()); + + // test nested names from outer model + const std::string innerModelNestedName = "mid_model::inner_model"; + EXPECT_TRUE(outerModel->ModelNameExists(innerModelNestedName)); + EXPECT_EQ(innerModel, outerModel->ModelByName(innerModelNestedName)); + + const std::string innerLinkNestedName = innerModelNestedName + "::inner_link"; + EXPECT_TRUE(outerModel->LinkNameExists(innerLinkNestedName)); + EXPECT_EQ(innerModel->LinkByIndex(0), + outerModel->LinkByName(innerLinkNestedName)); + EXPECT_NE(nullptr, outerModel->LinkByName(innerLinkNestedName)); + + std::string innerJointNestedName = innerModelNestedName + "::inner_joint"; + EXPECT_TRUE(outerModel->JointNameExists(innerJointNestedName)); + EXPECT_EQ(innerModel->JointByIndex(0), + outerModel->JointByName(innerJointNestedName)); + EXPECT_NE(nullptr, outerModel->JointByName(innerJointNestedName)); + + std::string innerFrameNestedName = innerModelNestedName + "::inner_frame"; + EXPECT_TRUE(outerModel->FrameNameExists(innerFrameNestedName)); + EXPECT_EQ(innerModel->FrameByIndex(0), + outerModel->FrameByName(innerFrameNestedName)); + EXPECT_NE(nullptr, outerModel->FrameByName(innerFrameNestedName)); +} + +///////////////////////////////////////////////// +TEST(DOMLink, NestedModelPoseRelativeTo) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_nested_model_relative_to.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("model_nested_model_relative_to", model->Name()); + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_EQ(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ("", model->PoseRelativeTo()); + + ASSERT_TRUE(model->LinkNameExists("L")); + EXPECT_TRUE(model->LinkByName("L")->PoseRelativeTo().empty()); + EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->LinkByName("L")->RawPose()); + + ASSERT_TRUE(model->ModelNameExists("M1")); + ASSERT_TRUE(model->ModelNameExists("M2")); + ASSERT_TRUE(model->ModelNameExists("M3")); + EXPECT_TRUE(model->ModelByName("M1")->PoseRelativeTo().empty()); + EXPECT_TRUE(model->ModelByName("M2")->PoseRelativeTo().empty()); + EXPECT_EQ("M1", model->ModelByName("M3")->PoseRelativeTo()); + + EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), model->ModelByName("M1")->RawPose()); + EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), model->ModelByName("M2")->RawPose()); + EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), model->ModelByName("M3")->RawPose()); + + EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI / 2, 0), + model->ModelByName("M1")->SemanticPose().RawPose()); + EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), + model->ModelByName("M2")->SemanticPose().RawPose()); + EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), + model->ModelByName("M3")->SemanticPose().RawPose()); + + // Test SemanticPose().Resolve to get each nested model pose in the + // __model__ frame + Pose pose; + EXPECT_TRUE( + model->ModelByName("M1")->SemanticPose().Resolve(pose, + "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), pose); + EXPECT_TRUE( + model->ModelByName("M2")->SemanticPose().Resolve(pose, + "__model__").empty()); + EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE( + model->ModelByName("M3")->SemanticPose().Resolve(pose, + "__model__").empty()); + EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose); + // test other API too + EXPECT_TRUE(model->ModelByName("M1")->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), pose); + EXPECT_TRUE(model->ModelByName("M2")->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE(model->ModelByName("M3")->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose); + + // resolve pose of M1 relative to M3 + // should be inverse of M3's Pose() + EXPECT_TRUE( + model->ModelByName("M1")->SemanticPose().Resolve(pose, "M3").empty()); + EXPECT_EQ(Pose(-3, 0, 0, 0, 0, 0), pose); + + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + EXPECT_EQ(0u, model->JointCount()); + EXPECT_EQ(nullptr, model->JointByIndex(0)); + + EXPECT_EQ(0u, model->FrameCount()); + EXPECT_EQ(nullptr, model->FrameByIndex(0)); } ///////////////////////////////////////////////// @@ -195,7 +395,7 @@ TEST(DOMRoot, LoadCanonicalLink) EXPECT_TRUE(root.Load(testFile).empty()); // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_canonical_link", model->Name()); EXPECT_EQ(2u, model->LinkCount()); @@ -215,5 +415,161 @@ TEST(DOMRoot, LoadCanonicalLink) EXPECT_EQ(0u, model->JointCount()); EXPECT_EQ(nullptr, model->JointByIndex(0)); + + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + + std::string body; + EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("link2", body); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, LoadNestedCanonicalLink) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "nested_canonical_link.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("top", model->Name()); + EXPECT_EQ(0u, model->LinkCount()); + EXPECT_EQ(nullptr, model->LinkByIndex(0)); + + EXPECT_EQ(0u, model->JointCount()); + EXPECT_EQ(nullptr, model->JointByIndex(0)); + + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + + EXPECT_EQ(2u, model->ModelCount()); + EXPECT_TRUE(model->ModelNameExists("nested")); + EXPECT_TRUE(model->ModelNameExists("shallow")); + EXPECT_EQ(model->ModelByName("nested"), model->ModelByIndex(0)); + EXPECT_EQ(model->ModelByName("shallow"), model->ModelByIndex(1)); + EXPECT_EQ(nullptr, model->ModelByIndex(2)); + + // expect implicit canonical link + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + // frame F is attached to __model__ and resolves to canonical link, + // which is "nested::link2" + std::string body; + EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("nested::link2", body); + + EXPECT_EQ(model->ModelByName("nested")->LinkByName("link2"), + model->CanonicalLink()); + // this reports the local name, not the nested name "nested::link2" + EXPECT_EQ("link2", model->CanonicalLink()->Name()); + + const sdf::Model *shallowModel = model->ModelByName("shallow"); + EXPECT_EQ(1u, shallowModel->FrameCount()); + EXPECT_TRUE( + shallowModel->FrameByName("F")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("deep::deeper::deepest::deepest_link", body); } +///////////////////////////////////////////////// +TEST(DOMRoot, LoadNestedExplicitCanonicalLink) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "nested_explicit_canonical_link.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("top", model->Name()); + EXPECT_EQ(0u, model->LinkCount()); + EXPECT_EQ(nullptr, model->LinkByIndex(0)); + + EXPECT_EQ(0u, model->JointCount()); + EXPECT_EQ(nullptr, model->JointByIndex(0)); + + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + + EXPECT_EQ(1u, model->ModelCount()); + EXPECT_TRUE(model->ModelNameExists("nested")); + EXPECT_NE(nullptr, model->ModelByIndex(0)); + EXPECT_EQ(model->ModelByName("nested"), model->ModelByIndex(0)); + EXPECT_EQ(nullptr, model->ModelByIndex(1)); + + // expect implicit canonical link + EXPECT_EQ("nested::link2", model->CanonicalLinkName()); + + // frame F is attached to __model__ and resolves to canonical link, + // which is "nested::link2" + std::string body; + EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("nested::link2", body); + + EXPECT_NE(nullptr, model->CanonicalLink()); + EXPECT_EQ(model->LinkByName("nested::link2"), model->CanonicalLink()); + EXPECT_EQ(model->ModelByName("nested")->LinkByName("link2"), + model->CanonicalLink()); + // this reports the local name, not the nested name "nested::link2" + EXPECT_EQ("link2", model->CanonicalLink()->Name()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, ModelPlacementFrameAttribute) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_with_placement_frame_attribute.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto *model = root.Model(); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, LoadInvalidNestedModelWithoutLinks) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "nested_without_links_invalid.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + EXPECT_FALSE(errors.empty()); + ASSERT_EQ(7u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::MODEL_WITHOUT_LINK); + EXPECT_NE(std::string::npos, + errors[0].Message().find("A model must have at least one link")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::MODEL_WITHOUT_LINK); + EXPECT_NE(std::string::npos, + errors[1].Message().find("A model must have at least one link")); + // errors[2] + // errors[3] + // errors[4] + // errors[5] + // errors[6] +} diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index b52d96417..50cf7526c 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -324,8 +324,10 @@ TEST(NestedModel, NestedInclude) // each model has 3 links, and the link names of the nested models have // been transformed EXPECT_EQ(3u, model1->LinkCount()); - EXPECT_EQ(3u, model2->LinkCount()); - EXPECT_EQ(3u, model3->LinkCount()); + // TODO (addisu): Update the following two expectations to account for the + // fact that included models are no longer flattened. + // EXPECT_EQ(3u, model2->LinkCount()); + // EXPECT_EQ(3u, model3->LinkCount()); const sdf::Link *baseLink1 = model1->LinkByName("base"); const sdf::Link *baseLink2 = model2->LinkByName(name + "::base"); const sdf::Link *baseLink3 = model3->LinkByName(name + "_14::base"); @@ -353,24 +355,14 @@ TEST(NestedModel, NestedInclude) EXPECT_EQ(lowerLink3->RawPose(), lowerLink1->RawPose()); EXPECT_EQ(upperLink2->RawPose(), upperLink1->RawPose()); EXPECT_EQ(upperLink3->RawPose(), upperLink1->RawPose()); - // expect //pose/@relative_to to contain the name of the nested model frame - // injected during parsing. model1 is not nested, so it's links should have - // empty //pose/@relative_to - EXPECT_TRUE(baseLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", baseLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", baseLink3->PoseRelativeTo()); - EXPECT_TRUE(lowerLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", lowerLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", lowerLink3->PoseRelativeTo()); - EXPECT_TRUE(upperLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", upperLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", upperLink3->PoseRelativeTo()); // each model has 2 joints, and the joint names of the nested models have // been transformed EXPECT_EQ(2u, model1->JointCount()); - EXPECT_EQ(2u, model2->JointCount()); - EXPECT_EQ(2u, model3->JointCount()); + // TODO (addisu): Update the following two expectations to account for the + // fact that included models are no longer flattened. + // EXPECT_EQ(2u, model2->JointCount()); + // EXPECT_EQ(2u, model3->JointCount()); auto *lowerJoint1 = model1->JointByName("lower_joint"); auto *lowerJoint2 = model2->JointByName(name + "::lower_joint"); auto *lowerJoint3 = model3->JointByName(name + "_14::lower_joint"); @@ -407,8 +399,6 @@ TEST(NestedModel, NestedInclude) // //axis/xyz/@expressed_in == "__model__" inside the nested model EXPECT_EQ(ignition::math::Vector3d::UnitX, lowerAxis3->Xyz()); EXPECT_EQ(ignition::math::Vector3d::UnitX, upperAxis3->Xyz()); - EXPECT_EQ(name + "_14::__model__", lowerAxis3->XyzExpressedIn()); - EXPECT_EQ(name + "_14::__model__", upperAxis3->XyzExpressedIn()); } ////////////////////////////////////////////////// @@ -452,6 +442,9 @@ TEST(NestedModel, NestedModelWithFrames) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + const sdf::Model *childModel = parentModel->ModelByIndex(0); + ASSERT_NE(nullptr, childModel); + using ignition::math::Pose3d; using ignition::math::Vector3d; // Expected poses for frames, links and joints after nesting. @@ -463,55 +456,52 @@ TEST(NestedModel, NestedModelWithFrames) Vector3d joint1AxisExpVector = frame2ExpPose.Rot() * Vector3d::UnitZ; Vector3d joint1Axis2ExpVector = frame2ExpPose.Rot() * Vector3d::UnitX; - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + auto parentSemPose = parentModel->SemanticPose(); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose).empty()); - EXPECT_EQ(link1ExpPose, link1Pose); + EXPECT_TRUE(parentSemPose.Resolve(link1Pose, "ParentModel::M1::L1").empty()); + EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); const auto *visual1 = link1->VisualByName("V1"); ASSERT_NE(nullptr, visual1); - EXPECT_EQ("M1::F2", visual1->PoseRelativeTo()); + EXPECT_EQ("F2", visual1->PoseRelativeTo()); const auto *collision1 = link1->CollisionByName("C1"); ASSERT_NE(nullptr, collision1); - EXPECT_EQ("M1::__model__", collision1->PoseRelativeTo()); + EXPECT_EQ("__model__", collision1->PoseRelativeTo()); const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose).empty()); - EXPECT_EQ(link2ExpPose, link2Pose); + EXPECT_TRUE(parentSemPose.Resolve(link2Pose, "ParentModel::M1::L2").empty()); + EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, "__model__").empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose); + EXPECT_TRUE(parentSemPose.Resolve(joint1Pose, "ParentModel::M1::J1").empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); const auto joint1Axis = joint1->Axis(0); ASSERT_NE(nullptr, joint1Axis); Vector3d joint1AxisVector; EXPECT_TRUE(joint1Axis->ResolveXyz(joint1AxisVector, "__model__").empty()); - EXPECT_EQ(joint1AxisExpVector, joint1AxisVector); + EXPECT_EQ(joint1AxisExpVector, model1Pose.Rot() * joint1AxisVector); const auto joint1Axis2 = joint1->Axis(1); ASSERT_NE(nullptr, joint1Axis2); Vector3d joint1Axis2Vector; EXPECT_TRUE(joint1Axis2->ResolveXyz(joint1Axis2Vector, "__model__").empty()); - EXPECT_EQ(joint1Axis2ExpVector, joint1Axis2Vector); + EXPECT_EQ(joint1Axis2ExpVector, model1Pose.Rot() * joint1Axis2Vector); } // Function to remove any element in //joint/axis that is not @@ -526,12 +516,39 @@ static void removeNoneXyz(const sdf::ElementPtr &_elem) } } - for (auto el : toRemove) + for (const auto &el : toRemove) { _elem->RemoveChild(el); } } +void prepareModelForDirectComparison(sdf::ElementPtr _modelElem) +{ + if (_modelElem->HasElement("joint")) + { + for (auto joint = _modelElem->GetElement("joint"); joint; + joint = joint->GetNextElement("joint")) + { + if (joint->HasElement("axis")) + { + removeNoneXyz(joint->GetElement("axis")); + } + if (joint->HasElement("axis2")) + { + removeNoneXyz(joint->GetElement("axis2")); + } + } + } + if (_modelElem->HasElement("model")) + { + for (auto elem = _modelElem->GetElement("model"); elem; + elem = elem->GetNextElement("model")) + { + prepareModelForDirectComparison(elem); + } + } +} + // Remove elements in world that are not model for comparison with // expectation. Also remove any element in //joint/axis that is not void prepareForDirectComparison(sdf::ElementPtr _worldElem) @@ -546,21 +563,7 @@ void prepareForDirectComparison(sdf::ElementPtr _worldElem) } else { - if (elem->HasElement("joint")) - { - for (auto joint = elem->GetElement("joint"); joint; - joint = joint->GetNextElement("joint")) - { - if (joint->HasElement("axis")) - { - removeNoneXyz(joint->GetElement("axis")); - } - if (joint->HasElement("axis2")) - { - removeNoneXyz(joint->GetElement("axis2")); - } - } - } + prepareModelForDirectComparison(elem); } } @@ -617,6 +620,82 @@ TEST(NestedModel, NestedModelWithFramesDirectComparison) EXPECT_EQ(expected.str(), sdfParsed->ToString()); } +////////////////////////////////////////////////// +// Test DOM APIs with partially flattened model +TEST(NestedModel, PartiallyFlattened) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "partially_flattened.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + // Get the outer model + const sdf::Model *outerModel = world->ModelByIndex(0); + ASSERT_NE(nullptr, outerModel); + EXPECT_EQ("ParentModel", outerModel->Name()); + EXPECT_EQ(4u, outerModel->LinkCount()); + EXPECT_NE(nullptr, outerModel->LinkByIndex(0)); + EXPECT_NE(nullptr, outerModel->LinkByIndex(1)); + EXPECT_NE(nullptr, outerModel->LinkByIndex(2)); + EXPECT_NE(nullptr, outerModel->LinkByIndex(3)); + EXPECT_EQ(nullptr, outerModel->LinkByIndex(4)); + + EXPECT_TRUE(outerModel->LinkNameExists("M1::L1")); + EXPECT_TRUE(outerModel->LinkNameExists("M1::L2")); + EXPECT_TRUE(outerModel->LinkNameExists("M1::L3")); + EXPECT_TRUE(outerModel->LinkNameExists("M1::L4")); + + EXPECT_EQ(3u, outerModel->JointCount()); + EXPECT_TRUE(outerModel->JointNameExists("M1::J1")); + EXPECT_TRUE(outerModel->JointNameExists("M1::J2")); + EXPECT_TRUE(outerModel->JointNameExists("M1::J3")); + + EXPECT_EQ(3u, outerModel->FrameCount()); + EXPECT_TRUE(outerModel->FrameNameExists("M1::__model__")); + EXPECT_TRUE(outerModel->FrameNameExists("M1::F1")); + EXPECT_TRUE(outerModel->FrameNameExists("M1::F2")); + + EXPECT_EQ(1u, outerModel->ModelCount()); + EXPECT_NE(nullptr, outerModel->ModelByIndex(0)); + EXPECT_EQ(nullptr, outerModel->ModelByIndex(1)); + + // Get the middle model + const sdf::Model *midModel = outerModel->ModelByIndex(0); + ASSERT_NE(nullptr, midModel); + EXPECT_EQ("M1::M2", midModel->Name()); + // TODO: the following expectations should be true, but ModelNameExists + // and ModelByName do not support names containing "::". + // EXPECT_TRUE(outerModel->ModelNameExists("M1::M2")); + // EXPECT_EQ(midModel, outerModel->ModelByName("M1::M2")); + + EXPECT_EQ(1u, midModel->LinkCount()); + EXPECT_NE(nullptr, midModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, midModel->LinkByIndex(1)); + + EXPECT_TRUE(midModel->LinkNameExists("L5")); + + EXPECT_EQ(0u, midModel->JointCount()); + + EXPECT_EQ(0u, midModel->FrameCount()); + + EXPECT_EQ(0u, midModel->ModelCount()); + + // test nested names from outer model + // TODO: the following expectations also fail due to the limitations of + // ModelNameExists and ModelByName not supporting names containing "::". + // const std::string midLinkNestedName = "M1::M2::L5"; + // EXPECT_TRUE(outerModel->LinkNameExists(midLinkNestedName)); + // EXPECT_NE(nullptr, outerModel->LinkByName(midLinkNestedName)); +} + ////////////////////////////////////////////////// // Test parsing models that have two levels of nesting with child models // containg frames nested via . @@ -691,6 +770,7 @@ TEST(NestedModel, NestedModelWithSiblingFrames) << "" << "" << " " + << " " << " " << " " << testFramePose << "" << " " @@ -718,12 +798,13 @@ TEST(NestedModel, NestedModelWithSiblingFrames) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + const sdf::Frame *parentModelFrame = parentModel->FrameByIndex(0); + ASSERT_NE(nullptr, parentModelFrame); - const sdf::Frame *nestedModel1Frame = - parentModel->FrameByName("M1::__model__"); - ASSERT_NE(nullptr, nestedModel1Frame); + const sdf::Model *nestedModel1 = parentModel->ModelByName("M1"); + ASSERT_NE(nullptr, nestedModel1); - EXPECT_EQ("testFrame", nestedModel1Frame->PoseRelativeTo()); + EXPECT_EQ("testFrame", nestedModel1->PoseRelativeTo()); using ignition::math::Pose3d; using ignition::math::Vector3d; @@ -738,38 +819,40 @@ TEST(NestedModel, NestedModelWithSiblingFrames) Pose3d nestedModel1FramePose; EXPECT_TRUE( - nestedModel1Frame->SemanticPose().Resolve(nestedModel1FramePose).empty()); + nestedModel1->SemanticPose().Resolve(nestedModel1FramePose).empty()); EXPECT_EQ(nestedModel1FrameExpPose, nestedModel1FramePose); + auto parentSemPose = parentModel->SemanticPose(); + const auto *frame1 = parentModel->FrameByName("M1::F1"); ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); const auto *frame2 = parentModel->FrameByName("M1::F2"); ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose).empty()); - EXPECT_EQ(link1ExpPose, link1Pose); + EXPECT_TRUE(parentSemPose.Resolve(link1Pose, "ParentModel::M1::L1").empty()); + EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose).empty()); - EXPECT_EQ(link2ExpPose, link2Pose); + EXPECT_TRUE(parentSemPose.Resolve(link2Pose, "ParentModel::M1::L2").empty()); + EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, "__model__").empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose); + EXPECT_TRUE(parentSemPose.Resolve(joint1Pose, "ParentModel::M1::J1").empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); } ////////////////////////////////////////////////// @@ -784,11 +867,12 @@ TEST(NestedModel, NestedFrameOnlyModel) const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); std::ostringstream stream; - std::string version = "1.7"; + std::string version = "1.8"; stream << "" << "" << " " + << " true" << " " << " " + MODEL_PATH + "" << " M1" @@ -804,9 +888,7 @@ TEST(NestedModel, NestedFrameOnlyModel) sdf::Root root; sdf::Errors errors = root.Load(sdfParsed); - for (auto e : errors) - std::cout << e.Message() << std::endl; - EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; const sdf::World *world = root.WorldByIndex(0); ASSERT_NE(nullptr, world); @@ -814,21 +896,478 @@ TEST(NestedModel, NestedFrameOnlyModel) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + auto parentSemPose = parentModel->SemanticPose(); + using ignition::math::Pose3d; using ignition::math::Vector3d; // Expected poses for frames after nesting. Pose3d frame1ExpPose = model1Pose * Pose3d(0, 0, 0, IGN_PI, 0, 0); Pose3d frame2ExpPose = frame1ExpPose * Pose3d(0, 0, 0, 0, IGN_PI, 0); - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); +} + +class PlacementFrame: public ::testing::Test +{ + protected: using Pose3d = ignition::math::Pose3d; + + protected: void SetUp() override + { + const std::string modelRootPath = sdf::filesystem::append( + PROJECT_SOURCE_PATH, "test", "integration", "model"); + + const std::string testModelPath = sdf::filesystem::append( + PROJECT_SOURCE_PATH, "test", "sdf", "placement_frame.sdf"); + + sdf::setFindCallback( + [&](const std::string &_file) + { + return sdf::filesystem::append(modelRootPath, _file); + }); + sdf::Errors errors = this->root.Load(testModelPath); + for (const auto &e : errors) + { + std::cout << e.Message() << std::endl; + } + if (!errors.empty()) + { + std::cout << this->root.Element()->ToString("") << std::endl; + } + EXPECT_TRUE(errors.empty()); + + this->world = this->root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + } + + public: template + const FrameType *GetFrameByName( + const sdf::Model *_model, const std::string &_testFrameName) + { + // cppcheck-suppress syntaxError + if constexpr (std::is_same_v) + { + return _model->FrameByName(_testFrameName); + } + else if constexpr (std::is_same_v) + { + return _model->LinkByName(_testFrameName); + } + else if constexpr (std::is_same_v) + { + return _model->JointByName(_testFrameName); + } + else + { + return nullptr; + } + } + + public: sdf::SemanticPose GetChildEntitySemanticPose( + const sdf::World *_world, const std::string _entityName) + { + return _world->ModelByName(_entityName)->SemanticPose(); + } + + public: template + void TestExpectedWorldPose(const std::string &_testModelName, + const std::string &_testFrameName) + { + const Pose3d placementPose(0, 10, 0, IGN_PI_2, 0, 0); + const sdf::Model *testModel = this->world->ModelByName(_testModelName); + ASSERT_NE(nullptr, testModel); + + // Pose of model in world frame + Pose3d modelPoseWorld; + { + sdf::Errors errors = + testModel->SemanticPose().Resolve(modelPoseWorld, "world"); + EXPECT_TRUE(errors.empty()) << errors[0].Message(); + } + + const auto *testFrame = + this->GetFrameByName(testModel, _testFrameName); + ASSERT_NE(nullptr, testFrame); + + // Pose of frame in its parent model frame. + Pose3d frameRelPose; + { + sdf::Errors errors = + testFrame->SemanticPose().Resolve(frameRelPose, "__model__"); + EXPECT_TRUE(errors.empty()) << errors[0].Message(); + } + + Pose3d framePoseWorld = modelPoseWorld * frameRelPose; + EXPECT_EQ(placementPose, framePoseWorld); + } + + public: template + void TestExpectedModelPose(const std::string &_parentModelName, + const std::string &_testFrameName) + { + const Pose3d placementPose(0, 10, 0, IGN_PI_2, 0, 0); + const sdf::Model *parentModel = this->world->ModelByName(_parentModelName); + ASSERT_NE(nullptr, parentModel); + + const auto *testFrame = this->GetFrameByName( + parentModel, _testFrameName); + ASSERT_NE(nullptr, testFrame); + + Pose3d testFramePose; + { + sdf::Errors errors = parentModel->SemanticPose().Resolve( + testFramePose, parentModel->Name() + "::" + _testFrameName); + EXPECT_TRUE(errors.empty()) << errors; + } + + // The expected pose of the test frame is precisely the placement pose + EXPECT_EQ(placementPose, testFramePose.Inverse()); + } + + protected: sdf::Root root; + protected: const sdf::World *world{nullptr}; +}; + +////////////////////////////////////////////////// +TEST_F(PlacementFrame, WorldInclude) +{ + // Test that link names can be used for + this->TestExpectedWorldPose("placement_frame_using_link", "L4"); + + // Test that frame names can be used for + this->TestExpectedWorldPose("placement_frame_using_frame", "F2"); + + // Test that joint names can be used for + this->TestExpectedWorldPose("placement_frame_using_joint", "J2"); + + // Test that the pose of an included model with placement_frame can use the + // relative_to attribute + this->TestExpectedWorldPose( + "include_with_placement_frame_and_pose_relative_to", "L4"); +} + +////////////////////////////////////////////////// +TEST_F(PlacementFrame, ModelInclude) +{ + // Test that link names can be used for + this->TestExpectedModelPose( + "parent_model_include", "placement_frame_using_link::L4"); + + // Test that frame names can be used for + this->TestExpectedModelPose( + "parent_model_include", "placement_frame_using_frame::F2"); + + // Test that joint names can be used for + this->TestExpectedModelPose( + "parent_model_include", "placement_frame_using_joint::J2"); + + // Test that the pose of an included model with placement_frame can use the + // relative_to attribute + this->TestExpectedModelPose("parent_model_include", + "nested_include_with_placement_frame_and_pose_relative_to::L4"); +} + +////////////////////////////////////////////////// +TEST_F(PlacementFrame, ModelPlacementFrameAttribute) +{ + // Test that link names can be used for + this->TestExpectedWorldPose( + "model_with_link_placement_frame", "L4"); + + // Test that frame names can be used for + this->TestExpectedWorldPose( + "model_with_frame_placement_frame", "F2"); + + // Test that joint names can be used for + this->TestExpectedWorldPose( + "model_with_joint_placement_frame", "J2"); + + // Test that the pose of a model with a placement_frame attribute can use the + // relative_to attribute + this->TestExpectedWorldPose( + "model_with_placement_frame_and_pose_relative_to", "L4"); +} + +////////////////////////////////////////////////// +TEST(NestedReference, PoseRelativeTo) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_relative_to_nested_reference.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = ignition::math::Pose3d; + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + + auto parentSemPose = model->SemanticPose(); + // The child models starting from M2 all reference an entity inside M1. Check + // that resolving poses works when referencing such entities. + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M2"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M2").empty()); + EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M3"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M3").empty()); + EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M4"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M4").empty()); + EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M5"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M5").empty()); + EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M6"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M6").empty()); + EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose.Inverse()); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M7"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M7").empty()); + EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose.Inverse()); + } +} + +////////////////////////////////////////////////// +TEST(NestedReference, PoseRelativeToInWorld) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_relative_to_nested_reference.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + using Pose = ignition::math::Pose3d; + + // The //world/frame elements reference an entity inside M1. Check + // that resolving poses works when referencing such entities. + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F2"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F3"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F4"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F5"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F6"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F7"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose); + } +} + +///////////////////////////////////////////////// +TEST(NestedReference, PlacementFrameAttribute) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "model", "model_with_nested_placement_frame_attribute", "model.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto *model = root.Model(); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(NestedReference, PlacementFrameElement) +{ + const std::string modelRootPath = sdf::filesystem::append( + PROJECT_SOURCE_PATH, "test", "integration", "model"); + + sdf::setFindCallback([&](const std::string &_file) + { + return sdf::filesystem::append(modelRootPath, _file); + }); + + // Test with //world/include without overriding the placement_frame + { + std::ostringstream stream; + stream << R"( + + + + model_with_nested_placement_frame_attribute + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); + } + + // Test with //world/include overriding the placement_frame + { + const ignition::math::Pose3d placementPose(0, 10, 0, IGN_PI_2, 0, 0); + std::ostringstream stream; + stream << R"( + + + + + model_with_nested_placement_frame_attribute + child_model::link3 + )" << placementPose << R"(" + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + auto *world_frame = world->FrameByName("world_frame"); + ASSERT_NE(nullptr, world_frame); + ignition::math::Pose3d pose; + errors = world_frame->SemanticPose().Resolve( + pose, "model_placement_frame::child_model::link3"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(placementPose, pose.Inverse()); + } + + + // Test with //model/include without overriding the placement_frame + { + std::ostringstream stream; + stream << R"( + + + + model_with_nested_placement_frame_attribute + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *parentModel = root.Model(); + ASSERT_NE(nullptr, parentModel); + auto *model = parentModel->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); + } + + // Test with //model/include overriding the placement_frame + { + const ignition::math::Pose3d placementPose(0, 10, 0, IGN_PI_2, 0, 0); + std::ostringstream stream; + stream << R"( + + + + + model_with_nested_placement_frame_attribute + child_model::link3 + )" << placementPose << R"(" + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *parentModel = root.Model(); + ASSERT_NE(nullptr, parentModel); + auto *model = parentModel ->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + auto *parentModelFrame = parentModel->FrameByName("parent_model_frame"); + auto *link = model->LinkByName("child_model::link3"); + ASSERT_NE(nullptr, link); + ignition::math::Pose3d pose; + errors = parentModelFrame->SemanticPose().Resolve( + pose, "model_placement_frame::child_model::link3"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(placementPose, pose.Inverse()); + } } diff --git a/test/integration/nested_model_with_frames_expected.sdf b/test/integration/nested_model_with_frames_expected.sdf index 1f5b66d23..8358e2519 100644 --- a/test/integration/nested_model_with_frames_expected.sdf +++ b/test/integration/nested_model_with_frames_expected.sdf @@ -2,67 +2,73 @@ - - 10 0 0 0 -0 1.5708 - - - 0 0 0 1.5708 -0 0 - - - 0 0 0 0 0.785398 0 - - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - - - 1.5707963267948966 - - - - - 0 0 0 0 -0 0 - - - 0.78539816339744828 - - - - - - 1 0 0 0 -0 0 - - - 0 1 0 0 -0 0 - - - 0 0 1 0 -0 0 - - - 0 0 0 0 -0 0 - M1::L1 - M1::L2 - - 0 0 1 - - - 1 0 0 - - - - 0 0 1 0 -0 0 - M1::L2 - M1::L3 - - 0 0 1 - - - - 1 0 1 0 -0 0 - M1::L3 - M1::L4 - + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1.5707963267948966 + + + + + 0 0 0 0 -0 0 + + + 0.78539816339744828 + + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + L1 + L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + L2 + L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + L3 + L4 + + + 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 + + + 10 0 0 0 -0 1.5708 + diff --git a/test/integration/nested_multiple_elements_error.cc b/test/integration/nested_multiple_elements_error.cc new file mode 100644 index 000000000..9f7aba3c2 --- /dev/null +++ b/test/integration/nested_multiple_elements_error.cc @@ -0,0 +1,197 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include + +#include "sdf/Actor.hh" +#include "sdf/Filesystem.hh" +#include "sdf/Geometry.hh" +#include "sdf/Light.hh" +#include "sdf/Model.hh" +#include "sdf/Root.hh" +#include "sdf/World.hh" +#include "test_config.h" + +const auto g_testPath = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test"); +const auto g_modelsPath = + sdf::filesystem::append(g_testPath, "integration", "model"); +const auto g_sdfPath = sdf::filesystem::append(g_testPath, "sdf"); + +///////////////////////////////////////////////// +std::string findFileCb(const std::string &_input) +{ + return sdf::filesystem::append(g_testPath, "integration", "model", _input); +} + +////////////////////////////////////////////////// +// Currently this only issues a parsing warning and should take the first model +// element +TEST(IncludesTest, NestedMultipleModelsError) +{ + sdf::setFindCallback(findFileCb); + + const auto sdfFile = + sdf::filesystem::append(g_modelsPath, "nested_multiple_models_error"); + + sdf::Root root; + sdf::Errors errors = root.Load(sdfFile); + if (!errors.empty()) + { + for (const auto &error : errors) + { + std::cout << error << std::endl; + } + ASSERT_TRUE(errors.empty()); + } + + const auto * model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("nested_model", model->Name()); + + ASSERT_EQ(1u, model->LinkCount()); + EXPECT_EQ("link1", model->LinkByIndex(0)->Name()); + + EXPECT_EQ(nullptr, root.Actor()); + EXPECT_EQ(nullptr, root.Light()); +} + +////////////////////////////////////////////////// +// Currently this only issues a parsing warning and should take the first actor +// element +TEST(IncludesTest, NestedMultipleActorsError) +{ + sdf::setFindCallback(findFileCb); + + const auto sdfFile = + sdf::filesystem::append(g_modelsPath, "nested_multiple_actors_error"); + + sdf::Root root; + sdf::Errors errors = root.Load(sdfFile); + if (!errors.empty()) + { + for (const auto &error : errors) + { + std::cout << error << std::endl; + } + ASSERT_TRUE(errors.empty()); + } + + EXPECT_EQ(nullptr, root.Model()); + EXPECT_EQ(nullptr, root.Light()); + + const auto * actor = root.Actor(); + ASSERT_NE(nullptr, actor); + EXPECT_EQ("nested_actor", actor->Name()); + ASSERT_EQ(1u, actor->LinkCount()); + EXPECT_EQ("link1", actor->LinkByIndex(0)->Name()); +} + +////////////////////////////////////////////////// +// Currently this only issues a parsing warning and should take the first light +// element +TEST(IncludesTest, NestedMultipleLightsError) +{ + sdf::setFindCallback(findFileCb); + + const auto sdfFile = + sdf::filesystem::append(g_modelsPath, "nested_multiple_lights_error"); + + sdf::Root root; + sdf::Errors errors = root.Load(sdfFile); + if (!errors.empty()) + { + for (const auto &error : errors) + { + std::cout << error << std::endl; + } + ASSERT_TRUE(errors.empty()); + } + + EXPECT_EQ(nullptr, root.Model()); + EXPECT_EQ(nullptr, root.Actor()); + + const auto * light = root.Light(); + ASSERT_NE(nullptr, light); + EXPECT_EQ("nested_light", light->Name()); + EXPECT_EQ(ignition::math::Vector3d(1, 0, 0), light->Direction()); +} + +////////////////////////////////////////////////// +// Currently this only issues a parsing warning and should take the first +// model, actor, light in that order +TEST(IncludesTest, NestedMultipleElementsError) +{ + sdf::setFindCallback(findFileCb); + + const auto sdfFile = + sdf::filesystem::append(g_modelsPath, "nested_multiple_elements_error"); + + sdf::Root root; + sdf::Errors errors = root.Load(sdfFile); + if (!errors.empty()) + { + for (const auto &error : errors) + { + std::cout << error << std::endl; + } + ASSERT_TRUE(errors.empty()); + } + + EXPECT_EQ(nullptr, root.Light()); + EXPECT_EQ(nullptr, root.Actor()); + + const auto * model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ("nested_model", model->Name()); +} + +////////////////////////////////////////////////// +TEST(IncludesTest, NestedMultipleElementsErrorWorld) +{ + sdf::setFindCallback(findFileCb); + + const auto sdfFile = + sdf::filesystem::append( + g_sdfPath, "nested_multiple_elements_error_world.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(sdfFile); + if (!errors.empty()) + { + for (const auto &error : errors) + { + std::cout << error << std::endl; + } + ASSERT_TRUE(errors.empty()); + } + + EXPECT_EQ(nullptr, root.Light()); + EXPECT_EQ(nullptr, root.Actor()); + EXPECT_EQ(nullptr, root.Model()); + + ASSERT_EQ(1u, root.WorldCount()); + const auto * world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + ASSERT_EQ(1u, world->ModelCount()); + + const auto model = world->ModelByIndex(0); + EXPECT_EQ("nested_model", model->Name()); + ASSERT_EQ(1u, model->LinkCount()); + EXPECT_EQ("link", model->LinkByIndex(0)->Name()); +} diff --git a/test/integration/partially_flattened.sdf b/test/integration/partially_flattened.sdf new file mode 100644 index 000000000..2385f000f --- /dev/null +++ b/test/integration/partially_flattened.sdf @@ -0,0 +1,74 @@ + + + + + + 10 0 0 0 -0 1.5708 + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1 + + + + + 0 0 0 0 -0 0 + + + 1 + + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + M1::L1 + M1::L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + M1::L2 + M1::L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + M1::L3 + M1::L4 + + + 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 + + + + + diff --git a/test/integration/root_dom.cc b/test/integration/root_dom.cc index e23775591..4fefe7e31 100644 --- a/test/integration/root_dom.cc +++ b/test/integration/root_dom.cc @@ -61,7 +61,8 @@ TEST(DOMRoot, Load) sdf::Root root; EXPECT_EQ(0u, root.WorldCount()); - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_EQ(SDF_PROTOCOL_VERSION, root.Version()); EXPECT_EQ(1u, root.WorldCount()); EXPECT_TRUE(root.WorldByIndex(0) != nullptr); @@ -83,7 +84,15 @@ TEST(DOMRoot, LoadMultipleModels) "root_multiple_models.sdf"); sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + + // Currently just warnings are issued in this case, eventually they may become + // errors. For now, only the first model is loaded. + EXPECT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + EXPECT_EQ("robot1", root.Model()->Name()); + + SDF_SUPPRESS_DEPRECATED_BEGIN EXPECT_EQ(3u, root.ModelCount()); EXPECT_EQ("robot1", root.ModelByIndex(0)->Name()); @@ -94,6 +103,7 @@ TEST(DOMRoot, LoadMultipleModels) EXPECT_TRUE(root.ModelNameExists("robot1")); EXPECT_TRUE(root.ModelNameExists("robot2")); EXPECT_TRUE(root.ModelNameExists("last_robot")); + SDF_SUPPRESS_DEPRECATED_END } ///////////////////////////////////////////////// @@ -106,6 +116,11 @@ TEST(DOMRoot, LoadDuplicateModels) sdf::Root root; sdf::Errors errors = root.Load(testFile); EXPECT_FALSE(errors.empty()); + EXPECT_NE(nullptr, root.Model()); + EXPECT_EQ("robot1", root.Model()->Name()); + + SDF_SUPPRESS_DEPRECATED_BEGIN EXPECT_EQ(1u, root.ModelCount()); EXPECT_EQ("robot1", root.ModelByIndex(0)->Name()); + SDF_SUPPRESS_DEPRECATED_END } diff --git a/test/integration/scene_dom.cc b/test/integration/scene_dom.cc new file mode 100644 index 000000000..84ae92805 --- /dev/null +++ b/test/integration/scene_dom.cc @@ -0,0 +1,73 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include + +#include + +#include "sdf/Filesystem.hh" +#include "sdf/parser.hh" +#include "sdf/Root.hh" +#include "sdf/Scene.hh" +#include "sdf/SDFImpl.hh" +#include "sdf/Sky.hh" +#include "sdf/World.hh" + +#include "test_config.h" + +////////////////////////////////////////////////// +TEST(DOMScene, LoadScene) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "scene_with_sky.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + for (auto e : errors) + std::cout << e.Message() << std::endl; + EXPECT_TRUE(errors.empty()); + + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(testFile, root.Element()->FilePath()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + const sdf::Scene *scene = world->Scene(); + ASSERT_NE(nullptr, scene); + + EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f), scene->Ambient()); + EXPECT_EQ(ignition::math::Color(0.6f, 0.7f, 0.8f), scene->Background()); + EXPECT_TRUE(scene->Grid()); + EXPECT_TRUE(scene->Shadows()); + EXPECT_TRUE(scene->OriginVisual()); + + const sdf::Sky *sky = scene->Sky(); + ASSERT_NE(nullptr, sky); + + EXPECT_DOUBLE_EQ(3.0, sky->Time()); + EXPECT_DOUBLE_EQ(4.0, sky->Sunrise()); + EXPECT_DOUBLE_EQ(21.0, sky->Sunset()); + EXPECT_DOUBLE_EQ(1.2, sky->CloudSpeed()); + EXPECT_EQ(ignition::math::Angle(1.5), sky->CloudDirection()); + EXPECT_DOUBLE_EQ(0.2, sky->CloudMeanSize()); + EXPECT_DOUBLE_EQ(0.9, sky->CloudHumidity()); + EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f), sky->CloudAmbient()); +} diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index 8363704f8..a09129b73 100644 --- a/test/integration/surface_dom.cc +++ b/test/integration/surface_dom.cc @@ -39,7 +39,7 @@ TEST(DOMSurface, Shapes) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - const auto model = root.ModelByIndex(0); + const auto model = root.Model(); ASSERT_NE(nullptr, model); const auto link = model->LinkByIndex(0); diff --git a/test/integration/two_level_nested_model_with_frames_expected.sdf b/test/integration/two_level_nested_model_with_frames_expected.sdf index c31494f5f..a73e206d2 100644 --- a/test/integration/two_level_nested_model_with_frames_expected.sdf +++ b/test/integration/two_level_nested_model_with_frames_expected.sdf @@ -2,70 +2,76 @@ - - 10 0 0 0 -0 1.5708 - - - 0 10 0 1.5708 -0 0 - - - 0 0 0 1.5708 -0 0 - - - 0 0 0 0 0.785398 0 - - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - - - 1.5707963267948966 - - - - - 0 0 0 0 -0 0 - - - 0.78539816339744828 - - - - - - 1 0 0 0 -0 0 - - - 0 1 0 0 -0 0 - - - 0 0 1 0 -0 0 - - - 0 0 0 0 -0 0 - M1::test_model_with_frames::L1 - M1::test_model_with_frames::L2 - - 0 0 1 - - - 1 0 0 - - - - 0 0 1 0 -0 0 - M1::test_model_with_frames::L2 - M1::test_model_with_frames::L3 - - 0 0 1 - - - - 1 0 1 0 -0 0 - M1::test_model_with_frames::L3 - M1::test_model_with_frames::L4 - + + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1.5707963267948966 + + + + + 0 0 0 0 -0 0 + + + 0.78539816339744828 + + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + L1 + L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + L2 + L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + L3 + L4 + + + 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 + + + 0 10 0 1.5708 -0 0 + + 10 0 0 0 -0 1.5708 + diff --git a/test/integration/visual_dom.cc b/test/integration/visual_dom.cc index b58e2aa4f..bb7ea8140 100644 --- a/test/integration/visual_dom.cc +++ b/test/integration/visual_dom.cc @@ -71,7 +71,7 @@ TEST(DOMVisual, DoublePendulum) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_TRUE(model != nullptr); const sdf::Link *baseLink = model->LinkByIndex(0); @@ -104,7 +104,7 @@ TEST(DOMVisual, Material) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); const sdf::Link *link = model->LinkByIndex(0); @@ -120,6 +120,9 @@ TEST(DOMVisual, Material) EXPECT_EQ(ignition::math::Color(0.2f, 0.5f, 0.1f, 1.0f), mat->Diffuse()); EXPECT_EQ(ignition::math::Color(0.7f, 0.3f, 0.5f, 0.9f), mat->Specular()); EXPECT_EQ(ignition::math::Color(1.0f, 0.0f, 0.2f, 1.0f), mat->Emissive()); + EXPECT_FALSE(mat->Lighting()); + EXPECT_TRUE(mat->DoubleSided()); + EXPECT_FLOAT_EQ(5.1f, mat->RenderOrder()); EXPECT_EQ(sdf::ShaderType::VERTEX, mat->Shader()); EXPECT_EQ("myuri", mat->ScriptUri()); EXPECT_EQ("myname", mat->ScriptName()); @@ -189,7 +192,7 @@ TEST(DOMVisual, Transparency) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); const sdf::Link *link = model->LinkByIndex(0); @@ -215,7 +218,7 @@ TEST(DOMVisual, LoadModelFramesRelativeToJoint) using Pose = ignition::math::Pose3d; // Get the first model - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); EXPECT_EQ("model_frame_relative_to_joint", model->Name()); EXPECT_EQ(2u, model->LinkCount()); @@ -406,7 +409,7 @@ TEST(DOMVisual, VisibilityFlags) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - const sdf::Model *model = root.ModelByIndex(0); + const sdf::Model *model = root.Model(); ASSERT_NE(nullptr, model); const sdf::Link *link = model->LinkByIndex(0); diff --git a/test/integration/whitespace.cc b/test/integration/whitespace.cc new file mode 100644 index 000000000..8389e7296 --- /dev/null +++ b/test/integration/whitespace.cc @@ -0,0 +1,64 @@ +/* + * Copyright 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include + +#include "sdf/Actor.hh" +#include "sdf/Collision.hh" +#include "sdf/Filesystem.hh" +#include "sdf/Geometry.hh" +#include "sdf/Light.hh" +#include "sdf/Link.hh" +#include "sdf/Mesh.hh" +#include "sdf/Model.hh" +#include "sdf/parser.hh" +#include "sdf/Root.hh" +#include "sdf/SDFImpl.hh" +#include "sdf/Visual.hh" +#include "sdf/World.hh" +#include "test_config.h" + +const auto g_testPath = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test"); +const auto g_modelsPath = + sdf::filesystem::append(g_testPath, "integration", "model"); + +///////////////////////////////////////////////// +std::string findFileCb(const std::string &_input) +{ + return sdf::filesystem::append(g_testPath, "integration", "model", _input); +} + +////////////////////////////////////////////////// +TEST(WhitespaceTest, Whitespace) +{ + sdf::setFindCallback(findFileCb); + + const auto worldFile = + sdf::filesystem::append(g_testPath, "sdf", "whitespace.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(worldFile); + for (auto e : errors) + std::cout << e.Message() << std::endl; + EXPECT_TRUE(errors.empty()); + + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(worldFile, root.Element()->FilePath()); + EXPECT_EQ("1.6", root.Element()->OriginalVersion()); +} diff --git a/test/sdf/custom_and_unknown_elements.sdf b/test/sdf/custom_and_unknown_elements.sdf new file mode 100644 index 000000000..a0e8bf8fc --- /dev/null +++ b/test/sdf/custom_and_unknown_elements.sdf @@ -0,0 +1,10 @@ + + + + + 1 0 0 0 0 0 + + + + + diff --git a/test/sdf/includes.sdf b/test/sdf/includes.sdf index 693f1d308..fd75a8deb 100644 --- a/test/sdf/includes.sdf +++ b/test/sdf/includes.sdf @@ -1,5 +1,5 @@ - + @@ -14,6 +14,11 @@ + + test_model/model.sdf + test_model_with_file + + test_light diff --git a/test/sdf/joint_axis_infinite_limits.sdf b/test/sdf/joint_axis_infinite_limits.sdf new file mode 100644 index 000000000..fb1ab831c --- /dev/null +++ b/test/sdf/joint_axis_infinite_limits.sdf @@ -0,0 +1,58 @@ + + + + + + + + + + + link1 + link2 + + 1 0 0 + + + + link3 + link4 + + 1 0 0 + + -1.5 + 1.5 + 2.5 + 5.5 + + + + + link4 + link5 + + 1 0 0 + + -inf + inf + inf + inf + + + + + link5 + link6 + + 1 0 0 + + -inf + inf + -1 + -1 + + + + + + diff --git a/test/sdf/joint_axis_xyz_normalization.sdf b/test/sdf/joint_axis_xyz_normalization.sdf new file mode 100644 index 000000000..86b4ab34b --- /dev/null +++ b/test/sdf/joint_axis_xyz_normalization.sdf @@ -0,0 +1,43 @@ + + + + + + + + + + + link1 + link2 + + 0 0 1 + + + + link3 + link4 + + 10 0 0 + + + + link4 + link5 + + -10 0 0 + + + 0 10 0 + + + + link5 + link6 + + 0 0 0 + + + + + diff --git a/test/sdf/joint_child_frame.sdf b/test/sdf/joint_child_frame.sdf new file mode 100644 index 000000000..2b2c900a2 --- /dev/null +++ b/test/sdf/joint_child_frame.sdf @@ -0,0 +1,25 @@ + + + + + + 0 0 1 0 0 0 + + + 0 0 10 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 0 0 0 + parent_link + child_frame + + + diff --git a/test/sdf/joint_invalid_resolved_parent_same_as_child.sdf b/test/sdf/joint_invalid_resolved_parent_same_as_child.sdf new file mode 100644 index 000000000..d449f983f --- /dev/null +++ b/test/sdf/joint_invalid_resolved_parent_same_as_child.sdf @@ -0,0 +1,24 @@ + + + + + + + L1 + J2 + + 1 0 0 + + + + + J1 + L2 + + 0 1 0 + + + + + + diff --git a/test/sdf/joint_invalid_self_child.sdf b/test/sdf/joint_invalid_self_child.sdf new file mode 100644 index 000000000..7f4763340 --- /dev/null +++ b/test/sdf/joint_invalid_self_child.sdf @@ -0,0 +1,13 @@ + + + + + 0 0 1 0 0 0 + + + 0 0 3 0 0 0 + link + self + + + diff --git a/test/sdf/joint_invalid_self_parent.sdf b/test/sdf/joint_invalid_self_parent.sdf new file mode 100644 index 000000000..7dde303e0 --- /dev/null +++ b/test/sdf/joint_invalid_self_parent.sdf @@ -0,0 +1,13 @@ + + + + + 0 0 1 0 0 0 + + + 0 0 3 0 0 0 + self + link + + + diff --git a/test/sdf/joint_nested_parent_child.sdf b/test/sdf/joint_nested_parent_child.sdf new file mode 100644 index 000000000..4a495a788 --- /dev/null +++ b/test/sdf/joint_nested_parent_child.sdf @@ -0,0 +1,61 @@ + + + + + + 0 0 1 0 0 0 + + + 1 1 0 0 0 0 + + + 1 0 0 0 0 0 + + + 0 0 1 1.570796326790 0 0 + + + + + + 0 0 10 0 1.57079632679 0 + + + + 1 0 0 0 0 0 + + + + + 1 0 0 0 0 0 + M1::L1 + L1 + + + + 0 1 0 0 0 0 + F1 + L1 + + + + 0 0 1 0 0 0 + L1 + M1::L2 + + + + + 0 0 1 0 0 0 + L1 + M1::F1 + + + + + 0 0 1 0 0 0 + L1 + M1::M2 + + + diff --git a/test/sdf/joint_parent_frame.sdf b/test/sdf/joint_parent_frame.sdf new file mode 100644 index 000000000..2ad7afc8e --- /dev/null +++ b/test/sdf/joint_parent_frame.sdf @@ -0,0 +1,25 @@ + + + + + + 0 0 1 0 0 0 + + + 0 0 10 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 0 0 0 + parent_frame + child_link + + + diff --git a/test/sdf/material.sdf b/test/sdf/material.sdf index 5869f6de4..106e5afdc 100644 --- a/test/sdf/material.sdf +++ b/test/sdf/material.sdf @@ -1,5 +1,5 @@ - + @@ -8,6 +8,9 @@ 0.2 0.5 0.1 1.0 0.7 0.3 0.5 0.9 1.0 0.0 0.2 1.0 + false + 5.1 + true + + + + + + 0 0 0 1 0 0 + link1 + link2 + diff --git a/test/sdf/world_frame_invalid_attached_to_scope.sdf b/test/sdf/world_frame_invalid_attached_to_scope.sdf new file mode 100644 index 000000000..9496ad782 --- /dev/null +++ b/test/sdf/world_frame_invalid_attached_to_scope.sdf @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/test/sdf/world_invalid_root_reference.sdf b/test/sdf/world_invalid_root_reference.sdf new file mode 100644 index 000000000..8b4e6eba5 --- /dev/null +++ b/test/sdf/world_invalid_root_reference.sdf @@ -0,0 +1,41 @@ + + + + + 1 0 0 0 0 0 + + + + + __root__ + link2 + + + + link1 + __root__ + + + + + + + + + + + + + + + + 1 0 0 0 0 0 + + + + test_model_with_frames + __root__ + 1 0 0 0 0 0 + + + diff --git a/test/sdf/world_nested_frame_attached_to.sdf b/test/sdf/world_nested_frame_attached_to.sdf new file mode 100644 index 000000000..1dc64fa72 --- /dev/null +++ b/test/sdf/world_nested_frame_attached_to.sdf @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + L + F0 + + + + + + + + + + + + diff --git a/test/sdf/world_relative_to_nested_reference.sdf b/test/sdf/world_relative_to_nested_reference.sdf new file mode 100644 index 000000000..e19f37233 --- /dev/null +++ b/test/sdf/world_relative_to_nested_reference.sdf @@ -0,0 +1,45 @@ + + + + + 1 0 0 0 1.5707963267948966 0 + + 0 1 0 0 0 0 + + + L1 + L2 + + + 0 0 1 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 1.5707963267948966 0 0 + + 1 0 0 0 -1.5707963267948966 0 + + + + + 2 0 0 0 0 0 + + + 3 0 0 0 0 0 + + + 4 0 0 0 0 0 + + + 5 0 0 0 0 0 + + + 6 0 0 0 0 0 + + + 7 0 0 0 0 0 + + + diff --git a/test/sdf/world_valid_root_reference.sdf b/test/sdf/world_valid_root_reference.sdf new file mode 100644 index 000000000..07e5634dd --- /dev/null +++ b/test/sdf/world_valid_root_reference.sdf @@ -0,0 +1,15 @@ + + + + + 1 0 0 0 0 0 + + + __root__ + + + + + + +