diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml new file mode 100644 index 000000000..cb0c3a4a7 --- /dev/null +++ b/.github/workflows/macos.yml @@ -0,0 +1,45 @@ +name: macOS latest + +on: [push, pull_request] + +jobs: + build: + + env: + PACKAGE: sdformat10 + 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 index 99e9730bc..7d7b4e179 100644 --- a/.github/workflows/pr-collection-labeler.yml +++ b/.github/workflows/pr-collection-labeler.yml @@ -1,6 +1,6 @@ name: PR Collection Labeler -on: pull_request +on: pull_request_target jobs: pr_collection_labeler: diff --git a/CMakeLists.txt b/CMakeLists.txt index 1151ec21f..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) diff --git a/Changelog.md b/Changelog.md index 7dface310..b5fa5a091 100644 --- a/Changelog.md +++ b/Changelog.md @@ -23,14 +23,67 @@ ### libsdformat 10.X.X (202X-XX-XX) -### libsdformat 10.0.0 (202X-XX-XX) +### libsdformat 10.1.0 (2020-12-15) -1. Normalize joint axis xyz vector when parsing from SDFormat +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) @@ -38,12 +91,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) @@ -53,6 +109,9 @@ ### SDFormat 9.3.0 (2020-XX-XX) +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) @@ -63,12 +122,19 @@ 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. +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) @@ -272,9 +338,21 @@ ### libsdformat 8.X.X (202X-XX-XX) +### SDFormat 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 079e733a6..8eba2afa0 100644 --- a/Migration.md +++ b/Migration.md @@ -86,6 +86,10 @@ but with improved human-readability.. + 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** @@ -331,6 +335,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 diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake index b25c54600..46f504b1b 100644 --- a/cmake/SearchForStuff.cmake +++ b/cmake/SearchForStuff.cmake @@ -31,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() @@ -94,6 +100,13 @@ 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 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/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index bfa65f3cf..b2e6d91a2 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -21,6 +21,7 @@ set (headers Frame.hh Geometry.hh Gui.hh + Heightmap.hh Imu.hh Joint.hh JointAxis.hh @@ -42,6 +43,7 @@ set (headers SDFImpl.hh SemanticPose.hh Sensor.hh + Sky.hh Sphere.hh Surface.hh Types.hh 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/Geometry.hh b/include/sdf/Geometry.hh index 1ce3c730e..6892ef76a 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -33,6 +33,7 @@ namespace sdf class Capsule; class Cylinder; class Ellipsoid; + class Heightmap; class Mesh; class Plane; class Sphere; @@ -59,6 +60,9 @@ namespace sdf /// \brief A mesh geometry. MESH = 5, + /// \brief A heightmap geometry. + HEIGHTMAP = 6, + /// \brief A capsule geometry. CAPSULE = 7, @@ -189,6 +193,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 diff --git a/include/sdf/Heightmap.hh b/include/sdf/Heightmap.hh new file mode 100644 index 000000000..9a24ec1e5 --- /dev/null +++ b/include/sdf/Heightmap.hh @@ -0,0 +1,291 @@ +/* + * 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 + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + + // Forward declare private data class. + class HeightmapPrivate; + class HeightmapTexturePrivate; + class HeightmapBlendPrivate; + + /// \brief Texture to be used on heightmaps. + class SDFORMAT_VISIBLE HeightmapTexture + { + /// \brief Constructor + public: HeightmapTexture(); + + /// \brief Copy constructor + /// \param[in] _texture HeightmapTexture to copy. + public: HeightmapTexture(const HeightmapTexture &_texture); + + /// \brief Move constructor + /// \param[in] _texture HeightmapTexture to move. + public: HeightmapTexture(HeightmapTexture &&_texture) noexcept; + + /// \brief Destructor + public: virtual ~HeightmapTexture(); + + /// \brief Move assignment operator. + /// \param[in] _texture Heightmap texture to move. + /// \return Reference to this. + public: HeightmapTexture &operator=(HeightmapTexture &&_texture); + + /// \brief Copy Assignment operator. + /// \param[in] _texture The heightmap texture to set values from. + /// \return *this + public: HeightmapTexture &operator=(const HeightmapTexture &_texture); + + /// \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. + private: HeightmapTexturePrivate *dataPtr; + }; + + /// \brief Blend information to be used between textures on heightmaps. + class SDFORMAT_VISIBLE HeightmapBlend + { + /// \brief Constructor + public: HeightmapBlend(); + + /// \brief Copy constructor + /// \param[in] _blend HeightmapBlend to copy. + public: HeightmapBlend(const HeightmapBlend &_blend); + + /// \brief Move constructor + /// \param[in] _blend HeightmapBlend to move. + public: HeightmapBlend(HeightmapBlend &&_blend) noexcept; + + /// \brief Destructor + public: virtual ~HeightmapBlend(); + + /// \brief Move assignment operator. + /// \param[in] _blend Heightmap blend to move. + /// \return Reference to this. + public: HeightmapBlend &operator=(HeightmapBlend &&_blend); + + /// \brief Copy Assignment operator. + /// \param[in] _blend The heightmap blend to set values from. + /// \return *this + public: HeightmapBlend &operator=(const HeightmapBlend &_blend); + + /// \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. + private: HeightmapBlendPrivate *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 Copy constructor + /// \param[in] _heightmap Heightmap to copy. + public: Heightmap(const Heightmap &_heightmap); + + /// \brief Move constructor + /// \param[in] _heightmap Heightmap to move. + public: Heightmap(Heightmap &&_heightmap) noexcept; + + /// \brief Destructor + public: virtual ~Heightmap(); + + /// \brief Move assignment operator. + /// \param[in] _heightmap Heightmap to move. + /// \return Reference to this. + public: Heightmap &operator=(Heightmap &&_heightmap); + + /// \brief Copy Assignment operator. + /// \param[in] _heightmap The heightmap to set values from. + /// \return *this + public: Heightmap &operator=(const Heightmap &_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 2. + /// \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. + private: HeightmapPrivate *dataPtr; + }; + } +} +#endif diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index 71ae8278c..1c536d6a2 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -85,8 +85,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. @@ -100,22 +101,6 @@ namespace sdf public: [[nodiscard]] sdf::Errors 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); - /// \brief Get the physical velocity dependent viscous damping coefficient /// of the joint axis. The default value is zero (0.0). /// \return The physical velocity dependent viscous damping coefficient @@ -191,27 +176,31 @@ namespace sdf /// \sa double Upper() const public: void SetUpper(const double _upper) const; - /// \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 + /// \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) const; /// \brief Get the joint stop stiffness. The default value is 1e8. diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index 9b3245554..f95f006eb 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -124,6 +124,14 @@ namespace sdf /// \param[in] _color Emissive color. public: void SetEmissive(const ignition::math::Color &_color) const; + /// \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. /// \return False if dynamic lighting should be disabled. @@ -133,6 +141,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 @@ -186,6 +203,14 @@ namespace sdf /// \return Pointer to the PBR material. Null if it does not exist. public: 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; }; diff --git a/include/sdf/Pbr.hh b/include/sdf/Pbr.hh index 564315d85..f1b5a0498 100644 --- a/include/sdf/Pbr.hh +++ b/include/sdf/Pbr.hh @@ -179,6 +179,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; diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index b4fea303b..27dc1a326 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -20,6 +20,7 @@ #include #include "sdf/Element.hh" +#include "sdf/Sky.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" @@ -107,6 +108,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 diff --git a/include/sdf/Sky.hh b/include/sdf/Sky.hh new file mode 100644 index 000000000..84f327a24 --- /dev/null +++ b/include/sdf/Sky.hh @@ -0,0 +1,146 @@ +/* + * 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 "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 { + // + + // Forward declarations. + class SkyPrivate; + + class SDFORMAT_VISIBLE Sky + { + /// \brief Default constructor + public: Sky(); + + /// \brief Copy constructor + /// \param[in] _sky Sky element to copy. + public: Sky(const Sky &_sky); + + /// \brief Move constructor + /// \param[in] _sky Sky to move. + public: Sky(Sky &&_sky) noexcept; + + /// \brief Destructor + public: ~Sky(); + + /// \brief Assignment operator. + /// \param[in] _sky The sky to set values from. + /// \return *this + public: Sky &operator=(const Sky &_sky); + + /// \brief Move assignment operator. + /// \param[in] _workflow The sky to move from. + /// \return *this + public: Sky &operator=(Sky &&_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. + private: SkyPrivate *dataPtr = nullptr; + }; + } +} +#endif 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.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/material.sdf b/sdf/1.7/material.sdf index 7cd011598..285b476ee 100644 --- a/sdf/1.7/material.sdf +++ b/sdf/1.7/material.sdf @@ -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/src/CMakeLists.txt b/src/CMakeLists.txt index c9769c651..7ebef4d02 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -31,6 +31,7 @@ set (sources ForceTorque.cc Geometry.cc Gui.cc + Heightmap.cc ign.cc Imu.cc Joint.cc @@ -55,6 +56,7 @@ set (sources SDFExtension.cc SemanticPose.cc Sensor.cc + Sky.cc Sphere.cc Surface.cc Types.cc @@ -101,6 +103,7 @@ if (BUILD_SDF_TEST) ForceTorque_TEST.cc Geometry_TEST.cc Gui_TEST.cc + Heightmap_TEST.cc Imu_TEST.cc Joint_TEST.cc JointAxis_TEST.cc @@ -122,6 +125,7 @@ if (BUILD_SDF_TEST) SemanticPose_TEST.cc SDF_TEST.cc Sensor_TEST.cc + Sky_TEST.cc Sphere_TEST.cc Surface_TEST.cc Types_TEST.cc @@ -138,6 +142,14 @@ if (BUILD_SDF_TEST) sdf_build_tests(${gtest_sources}) + 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 Utils.cc) sdf_build_tests(Utils_TEST.cc) 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/Filesystem.cc b/src/Filesystem.cc index 2e1f206af..5096a6427 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 { 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/Geometry.cc b/src/Geometry.cc index 624f93920..b7245bcd9 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -19,6 +19,7 @@ #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" @@ -52,6 +53,9 @@ class sdf::GeometryPrivate /// \brief Pointer to a mesh. public: std::unique_ptr mesh; + /// \brief Pointer to a heightmap. + public: std::unique_ptr heightmap; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; }; @@ -115,6 +119,12 @@ Geometry::Geometry(const Geometry &_geometry) this->dataPtr->mesh = std::make_unique(*_geometry.dataPtr->mesh); } + if (_geometry.dataPtr->heightmap) + { + this->dataPtr->heightmap = + std::make_unique(*_geometry.dataPtr->heightmap); + } + this->dataPtr->sdf = _geometry.dataPtr->sdf; } @@ -212,6 +222,13 @@ Errors Geometry::Load(ElementPtr _sdf) 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.reset(new Heightmap()); + Errors err = this->dataPtr->heightmap->Load(_sdf->GetElement("heightmap")); + errors.insert(errors.end(), err.begin(), err.end()); + } return errors; } @@ -312,6 +329,18 @@ void Geometry::SetMeshShape(const Mesh &_mesh) this->dataPtr->mesh = std::make_unique(_mesh); } +///////////////////////////////////////////////// +const Heightmap *Geometry::HeightmapShape() const +{ + return this->dataPtr->heightmap.get(); +} + +///////////////////////////////////////////////// +void Geometry::SetHeightmapShape(const Heightmap &_heightmap) +{ + this->dataPtr->heightmap = std::make_unique(_heightmap); +} + ///////////////////////////////////////////////// sdf::ElementPtr Geometry::Element() const { diff --git a/src/Heightmap.cc b/src/Heightmap.cc new file mode 100644 index 000000000..70b161fb7 --- /dev/null +++ b/src/Heightmap.cc @@ -0,0 +1,559 @@ +/* + * 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::HeightmapTexturePrivate +{ + /// \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::HeightmapBlendPrivate +{ + /// \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::HeightmapPrivate +{ + /// \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{2u}; + + /// \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(new HeightmapTexturePrivate) +{ +} + +///////////////////////////////////////////////// +HeightmapTexture::~HeightmapTexture() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +HeightmapTexture::HeightmapTexture(const HeightmapTexture &_heightmap) + : dataPtr(new HeightmapTexturePrivate(*_heightmap.dataPtr)) +{ +} + +////////////////////////////////////////////////// +HeightmapTexture::HeightmapTexture(HeightmapTexture &&_heightmap) noexcept + : dataPtr(std::exchange(_heightmap.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +HeightmapTexture &HeightmapTexture::operator=( + const HeightmapTexture &_heightmap) +{ + return *this = HeightmapTexture(_heightmap); +} + +///////////////////////////////////////////////// +HeightmapTexture &HeightmapTexture::operator=(HeightmapTexture &&_heightmap) +{ + std::swap(this->dataPtr, _heightmap.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +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(new HeightmapBlendPrivate) +{ +} + +///////////////////////////////////////////////// +HeightmapBlend::~HeightmapBlend() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +HeightmapBlend::HeightmapBlend(const HeightmapBlend &_heightmap) + : dataPtr(new HeightmapBlendPrivate(*_heightmap.dataPtr)) +{ +} + +////////////////////////////////////////////////// +HeightmapBlend::HeightmapBlend(HeightmapBlend &&_heightmap) noexcept + : dataPtr(std::exchange(_heightmap.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +HeightmapBlend &HeightmapBlend::operator=( + const HeightmapBlend &_heightmap) +{ + return *this = HeightmapBlend(_heightmap); +} + +///////////////////////////////////////////////// +HeightmapBlend &HeightmapBlend::operator=(HeightmapBlend &&_heightmap) +{ + std::swap(this->dataPtr, _heightmap.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +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(new HeightmapPrivate) +{ +} + +///////////////////////////////////////////////// +Heightmap::~Heightmap() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +Heightmap::Heightmap(const Heightmap &_heightmap) + : dataPtr(new HeightmapPrivate(*_heightmap.dataPtr)) +{ +} + +////////////////////////////////////////////////// +Heightmap::Heightmap(Heightmap &&_heightmap) noexcept + : dataPtr(std::exchange(_heightmap.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +Heightmap &Heightmap::operator=(const Heightmap &_heightmap) +{ + return *this = Heightmap(_heightmap); +} + +///////////////////////////////////////////////// +Heightmap &Heightmap::operator=(Heightmap &&_heightmap) +{ + std::swap(this->dataPtr, _heightmap.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +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..41d0774c3 --- /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(2u, 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(2u, 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/JointAxis.cc b/src/JointAxis.cc index e7b410974..5a43b56ea 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -25,6 +25,7 @@ #include "sdf/JointAxis.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" +#include "Utils.hh" using namespace sdf; @@ -228,17 +229,6 @@ sdf::Errors JointAxis::SetXyz(const ignition::math::Vector3d &_xyz) return sdf::Errors(); } -///////////////////////////////////////////////// -bool JointAxis::UseParentModelFrame() const -{ - return this->dataPtr->useParentModelFrame; -} -///////////////////////////////////////////////// -void JointAxis::SetUseParentModelFrame(const bool _parentModelFrame) -{ - this->dataPtr->useParentModelFrame = _parentModelFrame; -} - ///////////////////////////////////////////////// double JointAxis::Damping() const { @@ -313,7 +303,7 @@ void JointAxis::SetUpper(const double _upper) const ///////////////////////////////////////////////// double JointAxis::Effort() const { - return this->dataPtr->effort; + return infiniteIfNegative(this->dataPtr->effort); } ///////////////////////////////////////////////// @@ -325,7 +315,7 @@ void JointAxis::SetEffort(double _effort) ///////////////////////////////////////////////// double JointAxis::MaxVelocity() const { - return this->dataPtr->maxVelocity; + return infiniteIfNegative(this->dataPtr->maxVelocity); } ///////////////////////////////////////////////// diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index dc370773c..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()); diff --git a/src/Material.cc b/src/Material.cc index 4e0870931..41bf0a0f2 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -42,6 +42,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,11 +57,17 @@ 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; /// \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 = ""; }; ///////////////////////////////////////////////// @@ -83,11 +92,14 @@ Material::Material(const Material &_material) this->dataPtr->shader = _material.dataPtr->shader; this->dataPtr->normalMap = _material.dataPtr->normalMap; this->dataPtr->lighting = _material.dataPtr->lighting; + this->dataPtr->renderOrder = _material.dataPtr->renderOrder; + this->dataPtr->doubleSided = _material.dataPtr->doubleSided; 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; + this->dataPtr->filePath = _material.dataPtr->filePath; if (_material.dataPtr->pbr) this->dataPtr->pbr = std::make_unique(*_material.dataPtr->pbr); } @@ -118,6 +130,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") @@ -197,6 +211,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; @@ -209,6 +226,12 @@ 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")) { @@ -268,6 +291,18 @@ void Material::SetEmissive(const ignition::math::Color &_color) const 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 { @@ -280,6 +315,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 { @@ -345,3 +392,15 @@ Pbr *Material::PbrMaterial() const { return this->dataPtr->pbr.get(); } + +////////////////////////////////////////////////// +const std::string &Material::FilePath() const +{ + return this->dataPtr->filePath; +} + +////////////////////////////////////////////////// +void Material::SetFilePath(const std::string &_filePath) +{ + 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/Pbr.cc b/src/Pbr.cc index dbfd06041..63fd5800c 100644 --- a/src/Pbr.cc +++ b/src/Pbr.cc @@ -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; @@ -140,6 +146,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 +219,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; } @@ -366,6 +381,25 @@ void PbrWorkflow::SetEmissiveMap(const std::string &_map) this->dataPtr->emissiveMap = _map; } +////////////////////////////////////////////////// +std::string PbrWorkflow::LightMap() const +{ + return this->dataPtr->lightMapFilename; +} + +////////////////////////////////////////////////// +void PbrWorkflow::SetLightMap(const std::string &_map, unsigned int _uvSet) +{ + this->dataPtr->lightMapFilename = _map; + this->dataPtr->lightMapUvSet = _uvSet; +} + +////////////////////////////////////////////////// +unsigned int PbrWorkflow::LightMapTexCoordSet() const +{ + return this->dataPtr->lightMapUvSet; +} + ////////////////////////////////////////////////// sdf::ElementPtr PbrWorkflow::Element() const { 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/Scene.cc b/src/Scene.cc index 9458d0dbb..a042a1b07 100644 --- a/src/Scene.cc +++ b/src/Scene.cc @@ -22,6 +22,16 @@ using namespace sdf; /// \brief Scene private data. class sdf::ScenePrivate { + /// \brief Default constructor + public: ScenePrivate() = default; + + /// \brief Copy constructor + /// \param[in] _scenePrivate private data to copy + public: explicit ScenePrivate(const ScenePrivate &_scenePrivate); + + // Delete copy assignment so it is not accidentally used + public: ScenePrivate &operator=(const ScenePrivate &) = delete; + /// \brief True if grid should be enabled public: bool grid = true; @@ -39,10 +49,29 @@ class sdf::ScenePrivate public: ignition::math::Color background = ignition::math::Color(0.7f, 0.7f, .7f); + /// \brief Pointer to the sky properties. + public: std::unique_ptr sky; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; }; +///////////////////////////////////////////////// +ScenePrivate::ScenePrivate(const ScenePrivate &_scenePrivate) + : grid(_scenePrivate.grid), + shadows(_scenePrivate.shadows), + originVisual(_scenePrivate.originVisual), + ambient(_scenePrivate.ambient), + background(_scenePrivate.background), + sdf(_scenePrivate.sdf) +{ + if (_scenePrivate.sky) + { + this->sky = + std::make_unique(*(_scenePrivate.sky)); + } +} + ///////////////////////////////////////////////// Scene::Scene() : dataPtr(new ScenePrivate) @@ -118,6 +147,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 = std::make_unique(); + Errors err = this->dataPtr->sky->Load(_sdf->GetElement("sky")); + errors.insert(errors.end(), err.begin(), err.end()); + } + return errors; } @@ -180,6 +217,18 @@ void Scene::SetOriginVisual(const bool _enabled) this->dataPtr->originVisual = _enabled; } +///////////////////////////////////////////////// +void Scene::SetSky(const sdf::Sky &_sky) +{ + this->dataPtr->sky = std::make_unique(_sky); +} + +///////////////////////////////////////////////// +const sdf::Sky *Scene::Sky() const +{ + return this->dataPtr->sky.get(); +} + ///////////////////////////////////////////////// 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/Sky.cc b/src/Sky.cc new file mode 100644 index 000000000..2ce2ffb0a --- /dev/null +++ b/src/Sky.cc @@ -0,0 +1,235 @@ +/* + * 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::SkyPrivate +{ + /// \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(new SkyPrivate) +{ +} + +///////////////////////////////////////////////// +Sky::~Sky() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +///////////////////////////////////////////////// +Sky::Sky(const Sky &_sky) + : dataPtr(new SkyPrivate(*_sky.dataPtr)) +{ +} + +///////////////////////////////////////////////// +Sky::Sky(Sky &&_sky) noexcept + : dataPtr(std::exchange(_sky.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +Sky &Sky::operator=(const Sky &_sky) +{ + return *this = Sky(_sky); +} + +///////////////////////////////////////////////// +Sky &Sky::operator=(Sky &&_sky) +{ + std::swap(this->dataPtr, _sky.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +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/Utils.cc b/src/Utils.cc index b28ac26ee..8e6613a2d 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -75,6 +75,15 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, 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) { diff --git a/src/Utils.hh b/src/Utils.hh index f46be539e..c49489b26 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -61,6 +61,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. diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 1dd77bf29..1c38e76c7 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"; @@ -833,7 +835,7 @@ TEST(check_shapes_sdf, SDF) } ///////////////////////////////////////////////// -TEST(check_model_sdf, SDF) +TEST(check_model_sdf, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { std::string pathBase = PROJECT_SOURCE_PATH; pathBase += "/test/integration/model/box"; @@ -859,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 = @@ -871,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"; diff --git a/src/parser.cc b/src/parser.cc index 757bc524e..a8a6b0e1c 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -81,11 +81,27 @@ 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) { - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); if (tinyxml2::XML_SUCCESS != xmlDoc.LoadFile(_filename.c_str())) { sdferr << "Unable to load file[" @@ -100,7 +116,7 @@ static inline bool _initFile(const std::string &_filename, TPtr _sdf) bool init(SDFPtr _sdf) { std::string xmldata = SDF::EmbeddedSpec("root.sdf", false); - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); return initDoc(&xmlDoc, _sdf); } @@ -111,7 +127,7 @@ bool initFile(const std::string &_filename, SDFPtr _sdf) std::string xmldata = SDF::EmbeddedSpec(_filename, true); if (!xmldata.empty()) { - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); return initDoc(&xmlDoc, _sdf); } @@ -124,7 +140,7 @@ bool initFile(const std::string &_filename, ElementPtr _sdf) std::string xmldata = SDF::EmbeddedSpec(_filename, true); if (!xmldata.empty()) { - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(xmldata.c_str()); return initDoc(&xmlDoc, _sdf); } @@ -134,7 +150,7 @@ bool initFile(const std::string &_filename, ElementPtr _sdf) ////////////////////////////////////////////////// bool initString(const std::string &_xmlString, SDFPtr _sdf) { - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); if (xmlDoc.Parse(_xmlString.c_str())) { sdferr << "Failed to parse string as XML: " << xmlDoc.ErrorStr() << '\n'; @@ -393,7 +409,7 @@ bool readFileWithoutConversion( bool readFileInternal(const std::string &_filename, SDFPtr _sdf, const bool _convert, Errors &_errors) { - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); std::string filename = sdf::findFile(_filename, true, true); if (filename.empty()) @@ -429,7 +445,7 @@ bool readFileInternal(const std::string &_filename, SDFPtr _sdf, else if (URDF2SDF::IsURDF(filename)) { URDF2SDF u2g; - tinyxml2::XMLDocument doc; + auto doc = makeSdfDoc(); u2g.InitModelFile(filename, &doc); if (sdf::readDoc(&doc, _sdf, "urdf file", _convert, _errors)) { @@ -476,7 +492,7 @@ bool readStringWithoutConversion( bool readStringInternal(const std::string &_xmlString, SDFPtr _sdf, const bool _convert, Errors &_errors) { - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(_xmlString.c_str()); if (xmlDoc.Error()) { @@ -490,7 +506,7 @@ bool readStringInternal(const std::string &_xmlString, SDFPtr _sdf, else { URDF2SDF u2g; - tinyxml2::XMLDocument doc; + auto doc = makeSdfDoc(); u2g.InitModelString(_xmlString, &doc); if (sdf::readDoc(&doc, _sdf, "urdf string", _convert, _errors)) @@ -524,7 +540,7 @@ bool readString(const std::string &_xmlString, ElementPtr _sdf) ////////////////////////////////////////////////// bool readString(const std::string &_xmlString, ElementPtr _sdf, Errors &_errors) { - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); xmlDoc.Parse(_xmlString.c_str()); if (xmlDoc.Error()) { @@ -761,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 @@ -773,7 +790,7 @@ std::string getModelFilePath(const std::string &_modelDirPath) } } - tinyxml2::XMLDocument configFileDoc; + auto configFileDoc = makeSdfDoc(); if (tinyxml2::XML_SUCCESS != configFileDoc.LoadFile(configFilePath.c_str())) { sdferr << "Error parsing XML in file [" @@ -863,7 +880,7 @@ bool readXml(tinyxml2::XMLElement *_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( @@ -971,6 +988,15 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) // Get the config.xml 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 { @@ -1161,7 +1187,8 @@ bool readXml(tinyxml2::XMLElement *_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() @@ -1282,7 +1309,7 @@ bool convertFile(const std::string &_filename, const std::string &_version, return false; } - tinyxml2::XMLDocument xmlDoc; + auto xmlDoc = makeSdfDoc(); if (!xmlDoc.LoadFile(filename.c_str())) { // read initial sdf version diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index f9649d9fc..3c735a696 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -15,9 +15,13 @@ * */ +#include +#include +#include #include #include "sdf/parser.hh" #include "sdf/Element.hh" +#include "sdf/Console.hh" #include "sdf/Filesystem.hh" #include "test_config.h" @@ -60,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) { @@ -425,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_urdf_TEST.cc b/src/parser_urdf_TEST.cc index c9d5a6ada..cf65e1845 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -852,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/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2d822170d..9babec636 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -34,6 +34,7 @@ set(tests plugin_include.cc provide_feedback.cc root_dom.cc + scene_dom.cc sdf_basic.cc sdf_custom.cc surface_dom.cc @@ -41,6 +42,7 @@ set(tests urdf_gazebo_extensions.cc urdf_joint_parameters.cc visual_dom.cc + whitespace.cc world_dom.cc ) diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 5ca0d6897..765c99a4e 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -20,12 +20,13 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" -#include "sdf/Cylinder.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" @@ -33,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" ////////////////////////////////////////////////// @@ -178,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()); @@ -192,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..a5430cd10 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -318,3 +318,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 f331984d8..a5fa27172 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -15,6 +15,7 @@ * */ +#include #include #include #include @@ -225,6 +226,72 @@ TEST(DOMJointAxis, XyzExpressedIn) 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) { diff --git a/test/integration/material_pbr.cc b/test/integration/material_pbr.cc index 2585e456d..3db748fd4 100644 --- a/test/integration/material_pbr.cc +++ b/test/integration/material_pbr.cc @@ -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,6 +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/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/visual_dom.cc b/test/integration/visual_dom.cc index f06118d70..bb7ea8140 100644 --- a/test/integration/visual_dom.cc +++ b/test/integration/visual_dom.cc @@ -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()); 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/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/material.sdf b/test/sdf/material.sdf index 5b1c2e68f..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