From 0494ea18e98e4696a2413dbe53cc021accb319ed Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 30 Dec 2020 16:40:06 +0100 Subject: [PATCH 01/11] add load Signed-off-by: Guillaume Doisy --- src/Collision.cc | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/Collision.cc b/src/Collision.cc index 47f4c33e5..3516e52fc 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -51,6 +51,9 @@ class sdf::CollisionPrivate /// \brief Weak pointer to model's Pose Relative-To Graph. public: std::weak_ptr poseRelativeToGraph; + + /// \brief Lidar reflective intensity + public: double laserRetro; }; ///////////////////////////////////////////////// @@ -136,6 +139,13 @@ Errors Collision::Load(ElementPtr _sdf) this->dataPtr->surface.Load(_sdf->GetElement("surface")); } + // Load the lidar reflective intensity if it is given + if (_sdf->HasElement("laser_retro")) + { + this->dataPtr->laserRetro = _sdf->Get("laser_retro"); + std::cerr << "laser_retro: " << this->dataPtr->laserRetro << std::endl; + } + return errors; } From 311f3bdc8947413d181b7db2c5aaeeecf9138e79 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 30 Dec 2020 17:17:02 +0100 Subject: [PATCH 02/11] add Has and Set functions Signed-off-by: Guillaume Doisy --- include/sdf/Collision.hh | 18 ++++++++++++++++++ src/Collision.cc | 32 ++++++++++++++++++++++++++++++-- 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index a61d723a9..523e55c60 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -186,6 +186,24 @@ namespace sdf /// \brief Private data pointer. private: CollisionPrivate *dataPtr = nullptr; + + /// \brief Set whether the lidar reflective intensity + /// has been specified. + /// \param[in] _laserRetro True if the lidar reflective intensity + /// has been set in the sdf. + public: void SetHasLaserRetro(bool _laserRetro); + + /// \brief Get whether the lidar reflective intensity was set was set. + /// \return True if the lidar reflective intensity was set was set. + public: bool HasLaserRetro() const; + + /// \brief Get the flidar reflective intensity. + /// \return The lidar reflective intensity. + public: double LaserRetro() const; + + /// \brief Set the lidar reflective intensity. + /// \param[in] _laserRetro The lidar reflective intensity. + public: void SetLaserRetro(double _laserRetro); }; } } diff --git a/src/Collision.cc b/src/Collision.cc index 3516e52fc..1fdb2a4d2 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -52,6 +52,9 @@ class sdf::CollisionPrivate /// \brief Weak pointer to model's Pose Relative-To Graph. public: std::weak_ptr poseRelativeToGraph; + /// \brief True indicates the lidar reflective intensity was set. + public: bool hasLaserRetro{false}; + /// \brief Lidar reflective intensity public: double laserRetro; }; @@ -142,8 +145,9 @@ Errors Collision::Load(ElementPtr _sdf) // Load the lidar reflective intensity if it is given if (_sdf->HasElement("laser_retro")) { - this->dataPtr->laserRetro = _sdf->Get("laser_retro"); - std::cerr << "laser_retro: " << this->dataPtr->laserRetro << std::endl; + this->SetHasLaserRetro(true); + this->SetLaserRetro(_sdf->Get("laser_retro")); + std::cerr << "laser_retro: " << this->LaserRetro() << std::endl; } return errors; @@ -261,3 +265,27 @@ sdf::ElementPtr Collision::Element() const { return this->dataPtr->sdf; } + +////////////////////////////////////////////////// +void Collision::SetHasLaserRetro(bool _laserRetro) +{ + this->dataPtr->hasLaserRetro = _laserRetro; +} + +////////////////////////////////////////////////// +bool Collision::HasLaserRetro() const +{ + return this->dataPtr->hasLaserRetro; +} + +////////////////////////////////////////////////// +double Collision::LaserRetro() const +{ + return this->dataPtr->laserRetro; +} + +////////////////////////////////////////////////// +void Collision::SetLaserRetro(double _laserRetro) +{ + this->dataPtr->laserRetro = _laserRetro; +} From 3573cb1b99b4498b41a4ea36e47b2ef02e0fe376 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 4 Jan 2021 10:39:59 +0100 Subject: [PATCH 03/11] add visual laser retro + tests Signed-off-by: Guillaume Doisy --- include/sdf/Visual.hh | 18 ++++++++++++++++++ src/Collision.cc | 2 +- src/Collision_TEST.cc | 13 +++++++++++++ src/Visual.cc | 38 ++++++++++++++++++++++++++++++++++++++ src/Visual_TEST.cc | 13 +++++++++++++ 5 files changed, 83 insertions(+), 1 deletion(-) diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index 065083971..418fa6637 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -213,6 +213,24 @@ namespace sdf /// \brief Private data pointer. private: VisualPrivate *dataPtr = nullptr; + + /// \brief Set whether the lidar reflective intensity + /// has been specified. + /// \param[in] _laserRetro True if the lidar reflective intensity + /// has been set in the sdf. + public: void SetHasLaserRetro(bool _laserRetro); + + /// \brief Get whether the lidar reflective intensity was set was set. + /// \return True if the lidar reflective intensity was set was set. + public: bool HasLaserRetro() const; + + /// \brief Get the flidar reflective intensity. + /// \return The lidar reflective intensity. + public: double LaserRetro() const; + + /// \brief Set the lidar reflective intensity. + /// \param[in] _laserRetro The lidar reflective intensity. + public: void SetLaserRetro(double _laserRetro); }; } } diff --git a/src/Collision.cc b/src/Collision.cc index 1fdb2a4d2..01afba709 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -56,7 +56,7 @@ class sdf::CollisionPrivate public: bool hasLaserRetro{false}; /// \brief Lidar reflective intensity - public: double laserRetro; + public: double laserRetro = 0; }; ///////////////////////////////////////////////// diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index c830f15aa..d14565175 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -166,3 +166,16 @@ TEST(DOMcollision, SetSurface) ASSERT_NE(nullptr, collision.Surface()->Contact()); EXPECT_EQ(collision.Surface()->Contact()->CollideBitmask(), 0x2); } + +///////////////////////////////////////////////// +TEST(DOMcollision, SetLaserRetro) +{ + sdf::Collision collision; + EXPECT_EQ(nullptr, collision.Element()); + EXPECT_TRUE(collision.Name().empty()); + + collision.SetLaserRetro(150); + + EXPECT_TRUE(collision.HasLaserRetro()); + EXPECT_DOUBLE_EQ(150, collision.LaserRetro()); +} diff --git a/src/Visual.cc b/src/Visual.cc index 066b01e65..37471eddc 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -69,6 +69,12 @@ class sdf::VisualPrivate /// \brief Visibility flags of a visual. Defaults to 0xFFFFFFFF public: uint32_t visibilityFlags = 4294967295u; + + /// \brief True indicates the lidar reflective intensity was set. + public: bool hasLaserRetro{false}; + + /// \brief Lidar reflective intensity + public: double laserRetro = 0; }; ///////////////////////////////////////////////// @@ -193,6 +199,14 @@ Errors Visual::Load(ElementPtr _sdf) Errors geomErr = this->dataPtr->geom.Load(_sdf->GetElement("geometry")); errors.insert(errors.end(), geomErr.begin(), geomErr.end()); + // Load the lidar reflective intensity if it is given + if (_sdf->HasElement("laser_retro")) + { + this->SetHasLaserRetro(true); + this->SetLaserRetro(_sdf->Get("laser_retro")); + std::cerr << "Visual laser_retro: " << this->LaserRetro() << std::endl; + } + return errors; } @@ -344,3 +358,27 @@ void Visual::SetVisibilityFlags(uint32_t _flags) { this->dataPtr->visibilityFlags = _flags; } + +////////////////////////////////////////////////// +void Visual::SetHasLaserRetro(bool _laserRetro) +{ + this->dataPtr->hasLaserRetro = _laserRetro; +} + +////////////////////////////////////////////////// +bool Visual::HasLaserRetro() const +{ + return this->dataPtr->hasLaserRetro; +} + +////////////////////////////////////////////////// +double Visual::LaserRetro() const +{ + return this->dataPtr->laserRetro; +} + +////////////////////////////////////////////////// +void Visual::SetLaserRetro(double _laserRetro) +{ + this->dataPtr->laserRetro = _laserRetro; +} diff --git a/src/Visual_TEST.cc b/src/Visual_TEST.cc index e824c8d21..836abf3a8 100644 --- a/src/Visual_TEST.cc +++ b/src/Visual_TEST.cc @@ -272,3 +272,16 @@ TEST(DOMVisual, SetMaterial) EXPECT_EQ(ignition::math::Color(0.f, 0.1f, 0.9f), visual.Material()->Specular()); } + +///////////////////////////////////////////////// +TEST(DOMVisual, SetLaserRetro) +{ + sdf::Visual visual; + EXPECT_EQ(nullptr, visual.Element()); + EXPECT_TRUE(visual.Name().empty()); + + visual.SetLaserRetro(150); + + EXPECT_TRUE(visual.HasLaserRetro()); + EXPECT_DOUBLE_EQ(150, visual.LaserRetro()); +} From 1d099b569e695d4153358693a7a7f6d481c61f25 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 5 Jan 2021 18:36:52 +0100 Subject: [PATCH 04/11] laserRetro variable copy Signed-off-by: Guillaume Doisy --- src/Visual.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/Visual.cc b/src/Visual.cc index 37471eddc..7265632e7 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -86,7 +86,9 @@ VisualPrivate::VisualPrivate(const VisualPrivate &_visualPrivate) poseRelativeTo(_visualPrivate.poseRelativeTo), geom(_visualPrivate.geom), sdf(_visualPrivate.sdf), - visibilityFlags(_visualPrivate.visibilityFlags) + visibilityFlags(_visualPrivate.visibilityFlags), + hasLaserRetro(_visualPrivate.hasLaserRetro), + laserRetro(_visualPrivate.laserRetro) { if (_visualPrivate.material) { @@ -204,7 +206,6 @@ Errors Visual::Load(ElementPtr _sdf) { this->SetHasLaserRetro(true); this->SetLaserRetro(_sdf->Get("laser_retro")); - std::cerr << "Visual laser_retro: " << this->LaserRetro() << std::endl; } return errors; @@ -380,5 +381,6 @@ double Visual::LaserRetro() const ////////////////////////////////////////////////// void Visual::SetLaserRetro(double _laserRetro) { + this->dataPtr->hasLaserRetro = true; this->dataPtr->laserRetro = _laserRetro; } From 2ae6fc758890ce2708aaaf54157f28d0b080cd49 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 4 Jan 2021 16:28:14 -0800 Subject: [PATCH 05/11] Add Heightmap class (#388) Signed-off-by: Louise Poubel Co-authored-by: Steve Peters Signed-off-by: Guillaume Doisy --- include/sdf/CMakeLists.txt | 1 + include/sdf/Geometry.hh | 15 + include/sdf/Heightmap.hh | 291 ++++++++++++++++ src/CMakeLists.txt | 12 +- src/Geometry.cc | 29 ++ src/Heightmap.cc | 559 +++++++++++++++++++++++++++++++ src/Heightmap_TEST.cc | 408 ++++++++++++++++++++++ test/integration/geometry_dom.cc | 70 +++- test/sdf/shapes.sdf | 47 ++- 9 files changed, 1419 insertions(+), 13 deletions(-) create mode 100644 include/sdf/Heightmap.hh create mode 100644 src/Heightmap.cc create mode 100644 src/Heightmap_TEST.cc diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index 8aa166955..92498d93b 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -18,6 +18,7 @@ set (headers Frame.hh Geometry.hh Gui.hh + Heightmap.hh Imu.hh Joint.hh JointAxis.hh diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 148943ad3..7d3b4eecb 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -31,6 +31,7 @@ namespace sdf class GeometryPrivate; class Box; class Cylinder; + class Heightmap; class Mesh; class Plane; class Sphere; @@ -56,6 +57,9 @@ namespace sdf /// \brief A mesh geometry. MESH = 5, + + /// \brief A heightmap geometry. + HEIGHTMAP = 6, }; /// \brief Geometry provides access to a shape, such as a Box. Use the @@ -159,6 +163,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/src/CMakeLists.txt b/src/CMakeLists.txt index 62638f5cc..01f348a54 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -31,6 +31,7 @@ set (sources Filesystem.cc Geometry.cc Gui.cc + Heightmap.cc ign.cc Imu.cc Joint.cc @@ -110,6 +111,7 @@ if (BUILD_SDF_TEST) Filesystem_TEST.cc Geometry_TEST.cc Gui_TEST.cc + Heightmap_TEST.cc Imu_TEST.cc Joint_TEST.cc JointAxis_TEST.cc @@ -139,26 +141,26 @@ if (BUILD_SDF_TEST) Visual_TEST.cc World_TEST.cc ) - + # Build this test file only if Ignition Tools is installed. if (IGNITION-TOOLS_BINARY_DIRS) set (gtest_sources ${gtest_sources} ign_TEST.cc ) endif() - + sdf_build_tests(${gtest_sources}) - + if (NOT WIN32) set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS Utils.cc) sdf_build_tests(Utils_TEST.cc) endif() - + if (NOT WIN32) set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS FrameSemantics.cc) sdf_build_tests(FrameSemantics_TEST.cc) endif() - + if (NOT WIN32) set(SDF_BUILD_TESTS_EXTRA_EXE_SRCS Converter.cc) sdf_build_tests(Converter_TEST.cc) diff --git a/src/Geometry.cc b/src/Geometry.cc index 96eba0f0a..099c9bd50 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -17,6 +17,7 @@ #include "sdf/Geometry.hh" #include "sdf/Box.hh" #include "sdf/Cylinder.hh" +#include "sdf/Heightmap.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" @@ -44,6 +45,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; }; @@ -95,6 +99,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; } @@ -178,6 +188,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; } @@ -254,6 +271,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/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 3f1e44f21..9955c3ab1 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -19,11 +19,12 @@ #include #include "sdf/Box.hh" -#include "sdf/Cylinder.hh" #include "sdf/Collision.hh" +#include "sdf/Cylinder.hh" #include "sdf/Element.hh" #include "sdf/Filesystem.hh" #include "sdf/Geometry.hh" +#include "sdf/Heightmap.hh" #include "sdf/Link.hh" #include "sdf/Mesh.hh" #include "sdf/Model.hh" @@ -31,8 +32,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" ////////////////////////////////////////////////// @@ -136,8 +137,8 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::MESH, meshCol->Geom()->Type()); const sdf::Mesh *meshColGeom = meshCol->Geom()->MeshShape(); ASSERT_NE(nullptr, meshColGeom); - EXPECT_EQ("https://ignitionfuel.org/an_org/models/a_model/mesh/mesh.dae", - meshColGeom->Uri()); + EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/mesh/" + "mesh.dae", meshColGeom->Uri()); EXPECT_TRUE(ignition::math::Vector3d(0.1, 0.2, 0.3) == meshColGeom->Scale()); EXPECT_EQ("my_submesh", meshColGeom->Submesh()); @@ -150,10 +151,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/sdf/shapes.sdf b/test/sdf/shapes.sdf index 3ba9278d2..b5e2ce799 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -78,7 +78,7 @@ - https://ignitionfuel.org/an_org/models/a_model/mesh/mesh.dae + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/mesh/mesh.dae my_submesh
true
@@ -91,7 +91,7 @@ - https://ignitionfuel.org/an_org/models/a_model/mesh/mesh.dae + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/mesh/mesh.dae another_submesh
false
@@ -101,6 +101,49 @@ + + + + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/materials/textures/heightmap.png + 500 500 100 + 1 2 3 + + + + + + + + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/materials/textures/heightmap.png + 500 500 100 + 1 2 3 + + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/materials/textures/diffuse0.png + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/materials/textures/normal0.png + 5 + + + 15 + 5 + + + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/materials/textures/diffuse1.png + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/materials/textures/normal1.png + 10 + + + 30 + 10 + + + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/materials/textures/diffuse2.png + https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/materials/textures/normal2.png + 20 + + + + + 0.22 1 From 28dd4c7bc5b9d88b9bceffdff1d7ae94367dfdab Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 5 Jan 2021 18:40:13 +0100 Subject: [PATCH 06/11] restore collision Signed-off-by: Guillaume Doisy --- src/Collision.cc | 38 -------------------------------------- src/Collision_TEST.cc | 13 ------------- 2 files changed, 51 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index 01afba709..47f4c33e5 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -51,12 +51,6 @@ class sdf::CollisionPrivate /// \brief Weak pointer to model's Pose Relative-To Graph. public: std::weak_ptr poseRelativeToGraph; - - /// \brief True indicates the lidar reflective intensity was set. - public: bool hasLaserRetro{false}; - - /// \brief Lidar reflective intensity - public: double laserRetro = 0; }; ///////////////////////////////////////////////// @@ -142,14 +136,6 @@ Errors Collision::Load(ElementPtr _sdf) this->dataPtr->surface.Load(_sdf->GetElement("surface")); } - // Load the lidar reflective intensity if it is given - if (_sdf->HasElement("laser_retro")) - { - this->SetHasLaserRetro(true); - this->SetLaserRetro(_sdf->Get("laser_retro")); - std::cerr << "laser_retro: " << this->LaserRetro() << std::endl; - } - return errors; } @@ -265,27 +251,3 @@ sdf::ElementPtr Collision::Element() const { return this->dataPtr->sdf; } - -////////////////////////////////////////////////// -void Collision::SetHasLaserRetro(bool _laserRetro) -{ - this->dataPtr->hasLaserRetro = _laserRetro; -} - -////////////////////////////////////////////////// -bool Collision::HasLaserRetro() const -{ - return this->dataPtr->hasLaserRetro; -} - -////////////////////////////////////////////////// -double Collision::LaserRetro() const -{ - return this->dataPtr->laserRetro; -} - -////////////////////////////////////////////////// -void Collision::SetLaserRetro(double _laserRetro) -{ - this->dataPtr->laserRetro = _laserRetro; -} diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index d14565175..c830f15aa 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -166,16 +166,3 @@ TEST(DOMcollision, SetSurface) ASSERT_NE(nullptr, collision.Surface()->Contact()); EXPECT_EQ(collision.Surface()->Contact()->CollideBitmask(), 0x2); } - -///////////////////////////////////////////////// -TEST(DOMcollision, SetLaserRetro) -{ - sdf::Collision collision; - EXPECT_EQ(nullptr, collision.Element()); - EXPECT_TRUE(collision.Name().empty()); - - collision.SetLaserRetro(150); - - EXPECT_TRUE(collision.HasLaserRetro()); - EXPECT_DOUBLE_EQ(150, collision.LaserRetro()); -} From f3f81a21129d38c1db0097fb72bbbe964b404a03 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 6 Jan 2021 19:07:29 +0100 Subject: [PATCH 07/11] add integration test Signed-off-by: Guillaume Doisy --- test/integration/visual_dom.cc | 24 ++++++++++++++++++++++++ test/sdf/shapes.sdf | 9 +++++++++ 2 files changed, 33 insertions(+) diff --git a/test/integration/visual_dom.cc b/test/integration/visual_dom.cc index dd7d27fcc..df124f51b 100644 --- a/test/integration/visual_dom.cc +++ b/test/integration/visual_dom.cc @@ -203,6 +203,30 @@ TEST(DOMVisual, Transparency) EXPECT_FLOAT_EQ(0.22f, vis1->Transparency()); } +////////////////////////////////////////////////// +TEST(DOMVisual, LaserRetro) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "shapes.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + const sdf::Link *link = model->LinkByIndex(0); + ASSERT_NE(nullptr, link); + + const sdf::Visual *vis1 = link->VisualByName("sphere_vis_laser_retro"); + ASSERT_NE(nullptr, vis1); + + ASSERT_EQ(true, vis1->HasLaserRetro()); + EXPECT_DOUBLE_EQ(1150, vis1->LaserRetro()); +} + ///////////////////////////////////////////////// TEST(DOMVisual, LoadModelFramesRelativeToJoint) { diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index b5e2ce799..3401fbb15 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -153,6 +153,15 @@ + + + 1150 + + + 0.5 + + + From e40cf0e785daaf491df15d0ac6a00bc9b7dd7a00 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 6 Jan 2021 21:12:46 +0100 Subject: [PATCH 08/11] remove collision laserRetro Signed-off-by: Guillaume Doisy --- include/sdf/Collision.hh | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 523e55c60..a61d723a9 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -186,24 +186,6 @@ namespace sdf /// \brief Private data pointer. private: CollisionPrivate *dataPtr = nullptr; - - /// \brief Set whether the lidar reflective intensity - /// has been specified. - /// \param[in] _laserRetro True if the lidar reflective intensity - /// has been set in the sdf. - public: void SetHasLaserRetro(bool _laserRetro); - - /// \brief Get whether the lidar reflective intensity was set was set. - /// \return True if the lidar reflective intensity was set was set. - public: bool HasLaserRetro() const; - - /// \brief Get the flidar reflective intensity. - /// \return The lidar reflective intensity. - public: double LaserRetro() const; - - /// \brief Set the lidar reflective intensity. - /// \param[in] _laserRetro The lidar reflective intensity. - public: void SetLaserRetro(double _laserRetro); }; } } From 5efdbed06495a560d5e17c732320cdc0af7b4bbe Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 6 Jan 2021 21:14:39 +0100 Subject: [PATCH 09/11] remove unnecessary this->SetHasLaserRetro(true); Signed-off-by: Guillaume Doisy --- src/Visual.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Visual.cc b/src/Visual.cc index 7265632e7..c409638e0 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -204,7 +204,6 @@ Errors Visual::Load(ElementPtr _sdf) // Load the lidar reflective intensity if it is given if (_sdf->HasElement("laser_retro")) { - this->SetHasLaserRetro(true); this->SetLaserRetro(_sdf->Get("laser_retro")); } From 19a8587c84e5d510057e80530498bd858a184eb1 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 25 Jan 2021 11:37:55 +0100 Subject: [PATCH 10/11] move the public functions above the private ones Signed-off-by: Guillaume Doisy --- include/sdf/Visual.hh | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index 418fa6637..cd93aceea 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -193,6 +193,24 @@ namespace sdf /// \param[in] _flags visibility flags public: void SetVisibilityFlags(uint32_t _flags); + /// \brief Set whether the lidar reflective intensity + /// has been specified. + /// \param[in] _laserRetro True if the lidar reflective intensity + /// has been set in the sdf. + public: void SetHasLaserRetro(bool _laserRetro); + + /// \brief Get whether the lidar reflective intensity was set was set. + /// \return True if the lidar reflective intensity was set was set. + public: bool HasLaserRetro() const; + + /// \brief Get the flidar reflective intensity. + /// \return The lidar reflective intensity. + public: double LaserRetro() const; + + /// \brief Set the lidar reflective intensity. + /// \param[in] _laserRetro The lidar reflective intensity. + public: void SetLaserRetro(double _laserRetro); + /// \brief Give the name of the xml parent of this object, to be used /// for resolving poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. @@ -213,24 +231,6 @@ namespace sdf /// \brief Private data pointer. private: VisualPrivate *dataPtr = nullptr; - - /// \brief Set whether the lidar reflective intensity - /// has been specified. - /// \param[in] _laserRetro True if the lidar reflective intensity - /// has been set in the sdf. - public: void SetHasLaserRetro(bool _laserRetro); - - /// \brief Get whether the lidar reflective intensity was set was set. - /// \return True if the lidar reflective intensity was set was set. - public: bool HasLaserRetro() const; - - /// \brief Get the flidar reflective intensity. - /// \return The lidar reflective intensity. - public: double LaserRetro() const; - - /// \brief Set the lidar reflective intensity. - /// \param[in] _laserRetro The lidar reflective intensity. - public: void SetLaserRetro(double _laserRetro); }; } } From 2b0d37ff37b1b3378b7382a583ea0aaa08a27b3b Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 25 Jan 2021 11:42:08 +0100 Subject: [PATCH 11/11] move the public functions above the private ones in *.cc Signed-off-by: Guillaume Doisy --- src/Visual.cc | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/Visual.cc b/src/Visual.cc index c409638e0..d5e848068 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -306,19 +306,6 @@ void Visual::SetGeom(const Geometry &_geom) this->dataPtr->geom = _geom; } -///////////////////////////////////////////////// -void Visual::SetXmlParentName(const std::string &_xmlParentName) -{ - this->dataPtr->xmlParentName = _xmlParentName; -} - -///////////////////////////////////////////////// -void Visual::SetPoseRelativeToGraph( - std::weak_ptr _graph) -{ - this->dataPtr->poseRelativeToGraph = _graph; -} - ///////////////////////////////////////////////// sdf::SemanticPose Visual::SemanticPose() const { @@ -383,3 +370,16 @@ void Visual::SetLaserRetro(double _laserRetro) this->dataPtr->hasLaserRetro = true; this->dataPtr->laserRetro = _laserRetro; } + +///////////////////////////////////////////////// +void Visual::SetXmlParentName(const std::string &_xmlParentName) +{ + this->dataPtr->xmlParentName = _xmlParentName; +} + +///////////////////////////////////////////////// +void Visual::SetPoseRelativeToGraph( + std::weak_ptr _graph) +{ + this->dataPtr->poseRelativeToGraph = _graph; +}