From 2554591e10f2259993ba06161407803b63ce70af Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 27 Apr 2023 23:01:23 +0000 Subject: [PATCH 1/4] Add Projector DOM Signed-off-by: Ian Chen --- include/sdf/Link.hh | 51 +++++ include/sdf/Projector.hh | 197 ++++++++++++++++++ include/sdf/SemanticPose.hh | 1 + sdf/1.10/projector.sdf | 4 + sdf/1.7/projector.sdf | 4 + sdf/1.8/projector.sdf | 4 + sdf/1.9/projector.sdf | 4 + src/Link.cc | 92 +++++++++ src/Link_TEST.cc | 49 +++++ src/Projector.cc | 333 ++++++++++++++++++++++++++++++ src/Projector_TEST.cc | 114 ++++++++++ test/integration/CMakeLists.txt | 1 + test/integration/projector_dom.cc | 82 ++++++++ test/sdf/world_complete.sdf | 10 + 14 files changed, 946 insertions(+) create mode 100644 include/sdf/Projector.hh create mode 100644 src/Projector.cc create mode 100644 src/Projector_TEST.cc create mode 100644 test/integration/projector_dom.cc diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index a5b3c4de7..1ff9e3179 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -39,6 +39,7 @@ namespace sdf class Light; class ParserConfig; class ParticleEmitter; + class Projector; class Sensor; class Visual; struct PoseRelativeToGraph; @@ -253,6 +254,47 @@ namespace sdf /// \sa bool ParticleEmitterNameExists(const std::string &_name) const public: ParticleEmitter *ParticleEmitterByName(const std::string &_name); + /// \brief Get the number of particle projectors. + /// \return Number of particle projectors contained in this Link object. + public: uint64_t ProjectorCount() const; + + /// \brief Get a projector based on an index. + /// \param[in] _index Index of the projector. + /// The index should be in the range [0..ProjectorCount()). + /// \return Pointer to the projector. Nullptr if the index does + /// not exist. + /// \sa uint64_t ProjectorCount() const + public: const Projector *ProjectorByIndex( + const uint64_t _index) const; + + /// \brief Get a mutable projector based on an index. + /// \param[in] _index Index of the projector. + /// The index should be in the range [0..ProjectorCount()). + /// \return Pointer to the projector. Nullptr if the index does + /// not exist. + /// \sa uint64_t ProjectorCount() const + public: Projector *ProjectorByIndex(uint64_t _index); + + /// \brief Get whether a projector name exists. + /// \param[in] _name Name of the projector to check. + /// \return True if there exists a projector with the given name. + public: bool ProjectorNameExists(const std::string &_name) const; + + /// \brief Get a projector based on a name. + /// \param[in] _name Name of the projector. + /// \return Pointer to the projector. Nullptr if a projector + /// with the given name does not exist. + /// \sa bool ProjectorNameExists(const std::string &_name) const + public: const Projector *ProjectorByName( + const std::string &_name) const; + + /// \brief Get a mutable projector based on a name. + /// \param[in] _name Name of the projector. + /// \return Pointer to the projector. Nullptr if a projector + /// with the given name does not exist. + /// \sa bool ProjectorNameExists(const std::string &_name) const + public: Projector *ProjectorByName(const std::string &_name); + /// \brief Get the inertial value for this link. The inertial object /// consists of the link's mass, a 3x3 rotational inertia matrix, and /// a pose for the inertial reference frame. The units for mass is @@ -368,6 +410,12 @@ namespace sdf /// already exists. public: bool AddParticleEmitter(const ParticleEmitter &_sensor); + /// \brief Add a projector to the link. + /// \param[in] _emitter Projector to add. + /// \return True if successful, false if a projector with the name + /// already exists. + public: bool AddProjector(const Projector &_sensor); + /// \brief Remove all collisions public: void ClearCollisions(); @@ -383,6 +431,9 @@ namespace sdf /// \brief Remove all particle emitters public: void ClearParticleEmitters(); + /// \brief Remove all projectors + public: void ClearProjectors(); + /// \brief Create and return an SDF element filled with data from this /// link. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Projector.hh b/include/sdf/Projector.hh new file mode 100644 index 000000000..c879e8197 --- /dev/null +++ b/include/sdf/Projector.hh @@ -0,0 +1,197 @@ +/* + * Copyright 2023 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_PROJECTOR_HH_ +#define SDF_PROJECTOR_HH_ + +#include +#include + +#include +#include +#include + +#include "sdf/Plugin.hh" +#include "sdf/SemanticPose.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. + struct PoseRelativeToGraph; + + /// \brief A description of a projector, which can be attached + /// to a link. A projector can be used to project texture onto other + /// visuals + class SDFORMAT_VISIBLE Projector + { + /// \brief Default constructor + public: Projector(); + + /// \brief Load the projector based on an element pointer. This is + /// *not* the usual entry point. Typical usage of the SDF DOM is through + /// the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the name of the projector. + /// The name of the projector should be unique within the scope of + /// a Link. + /// \return Name of the projector. + public: std::string Name() const; + + /// \brief Set the name of the projector. + /// The name of the projector should be unique within the scope of + /// a Link. + /// \param[in] _name Name of the projector. + public: void SetName(const std::string &_name); + + /// \brief Get the near clip distance. + /// \return The near clip distance. + public: double NearClip() const; + + /// \brief Set the near clip distance. + /// \param[in] _near The near clip distance. + public: void SetNearClip(double _near); + + /// \brief Get the far clip distance. + /// \return The far clip distance. + public: double FarClip() const; + + /// \brief Set the far clip distance. + /// \param[in] _far The far clip distance. + public: void SetFarClip(double _far); + + /// \brief Get the horizontal field of view in radians. + /// \return The horizontal field of view in radians. + public: gz::math::Angle HorizontalFov() const; + + /// \brief Set the horizontal field of view in radians. + /// \param[in] _hfov The horizontal field of view in radians. + public: void SetHorizontalFov(const gz::math::Angle &_hfov); + + /// \brief Get the visibility flags of a visual + /// \return visibility flags + public: uint32_t VisibilityFlags() const; + + /// \brief Set the visibility flags of a visual + /// \param[in] _flags visibility flags + public: void SetVisibilityFlags(uint32_t _flags); + + /// \brief Get the texture filename. This will be an empty string if + /// a texture has not been set. + /// \return Filename of the texture, or empty string if a texture + /// has not been specified. + public: std::string Texture() const; + + /// \brief Set the texture filename. + /// \param[in] _map Filename of the texture + public: void SetTexture(const std::string &_map); + + /// \brief Get the plugins attached to this projector. + /// \return A vector of Plugin, which will be empty if there are no + /// plugins. + public: const sdf::Plugins &Plugins() const; + + /// \brief Get a mutable vector of plugins attached to this projector. + /// \return A vector of Plugin, which will be empty if there are no + /// plugins. + public: sdf::Plugins &Plugins(); + + /// \brief Remove all plugins + public: void ClearPlugins(); + + /// \brief Add a plugin to this projector. + /// \param[in] _plugin Plugin to add. + public: void AddPlugin(const Plugin &_plugin); + + /// \brief Get the pose of the projector. This is the pose of the + /// emitter as specified in SDF + /// ( ... ). + /// \return The pose of the projector. + public: const gz::math::Pose3d &RawPose() const; + + /// \brief Set the pose of the projector object. + /// \sa const gz::math::Pose3d &RawPose() const + /// \param[in] _pose The pose of the projector. + public: void SetRawPose(const gz::math::Pose3d &_pose); + + /// \brief Get the name of the coordinate frame relative to which this + /// emitter's pose is expressed. An empty value indicates that the frame is + /// relative to the parent link. + /// \return The name of the pose relative-to frame. + public: const std::string &PoseRelativeTo() const; + + /// \brief Set the name of the coordinate frame relative to which this + /// emitter's pose is expressed. An empty value indicates that the frame is + /// relative to the parent link. + /// \param[in] _frame The name of the pose relative-to frame. + public: void SetPoseRelativeTo(const std::string &_frame); + + /// \brief Get SemanticPose object of this object to aid in resolving poses. + /// \return SemanticPose object for this emitter. + public: sdf::SemanticPose SemanticPose() 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 + /// not been called. + public: sdf::ElementPtr Element() 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 Create and return an SDF element filled with data from this + /// projector. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \return SDF element pointer with updated projector values. + public: sdf::ElementPtr ToElement() const; + + /// \brief Set 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. + /// \param[in] _xmlParentName Name of xml parent object. + private: void SetXmlParentName(const std::string &_xmlParentName); + + /// \brief Set a weak pointer to the PoseRelativeToGraph to be used + /// for resolving poses. This is private and is intended to be called by + /// Link::SetPoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. + private: void SetPoseRelativeToGraph( + sdf::ScopedGraph _graph); + + /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName + /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is + /// a private function, so we need to befriend the entire class. + friend class Link; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } +} +#endif diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 3ef4fe248..3280d1e47 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -110,6 +110,7 @@ namespace sdf friend class Light; friend class Link; friend class ParticleEmitter; + friend class Projector; friend class Model; friend class Sensor; friend class Visual; diff --git a/sdf/1.10/projector.sdf b/sdf/1.10/projector.sdf index a502cdfe2..28de4e3e1 100644 --- a/sdf/1.10/projector.sdf +++ b/sdf/1.10/projector.sdf @@ -22,6 +22,10 @@ far clip distance + + + + diff --git a/sdf/1.7/projector.sdf b/sdf/1.7/projector.sdf index a502cdfe2..28de4e3e1 100644 --- a/sdf/1.7/projector.sdf +++ b/sdf/1.7/projector.sdf @@ -22,6 +22,10 @@ far clip distance + + + + diff --git a/sdf/1.8/projector.sdf b/sdf/1.8/projector.sdf index a502cdfe2..28de4e3e1 100644 --- a/sdf/1.8/projector.sdf +++ b/sdf/1.8/projector.sdf @@ -22,6 +22,10 @@ far clip distance + + + + diff --git a/sdf/1.9/projector.sdf b/sdf/1.9/projector.sdf index a502cdfe2..28de4e3e1 100644 --- a/sdf/1.9/projector.sdf +++ b/sdf/1.9/projector.sdf @@ -22,6 +22,10 @@ far clip distance + + + + diff --git a/src/Link.cc b/src/Link.cc index 859848c4e..c74783a11 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -27,6 +27,7 @@ #include "sdf/Link.hh" #include "sdf/parser.hh" #include "sdf/ParticleEmitter.hh" +#include "sdf/Projector.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" @@ -63,6 +64,9 @@ class sdf::Link::Implementation /// \brief The particle emitters specified in this link. public: std::vector emitters; + /// \brief The projectors specified in this link. + public: std::vector projectors; + /// \brief The inertial information for this link. public: gz::math::Inertiald inertial {{1.0, gz::math::Vector3d::One, gz::math::Vector3d::Zero}, @@ -151,6 +155,12 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) errors.insert(errors.end(), emitterLoadErrors.begin(), emitterLoadErrors.end()); + // Load all the projectors + Errors projectorLoadErrors = loadUniqueRepeated(_sdf, + "projector", this->dataPtr->projectors); + errors.insert(errors.end(), projectorLoadErrors.begin(), + projectorLoadErrors.end()); + gz::math::Vector3d xxyyzz = gz::math::Vector3d::One; gz::math::Vector3d xyxzyz = gz::math::Vector3d::Zero; gz::math::Pose3d inertiaPose; @@ -470,6 +480,61 @@ ParticleEmitter *Link::ParticleEmitterByName(const std::string &_name) static_cast(this)->ParticleEmitterByName(_name)); } +///////////////////////////////////////////////// +uint64_t Link::ProjectorCount() const +{ + return this->dataPtr->projectors.size(); +} + +///////////////////////////////////////////////// +const Projector *Link::ProjectorByIndex(const uint64_t _index) const +{ + if (_index < this->dataPtr->projectors.size()) + return &this->dataPtr->projectors[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +Projector *Link::ProjectorByIndex(uint64_t _index) +{ + return const_cast( + static_cast(this)->ProjectorByIndex(_index)); +} + +///////////////////////////////////////////////// +bool Link::ProjectorNameExists(const std::string &_name) const +{ + for (auto const &e : this->dataPtr->projectors) + { + if (e.Name() == _name) + { + return true; + } + } + return false; +} + +///////////////////////////////////////////////// +const Projector *Link::ProjectorByName( + const std::string &_name) const +{ + for (auto const &e : this->dataPtr->projectors) + { + if (e.Name() == _name) + { + return &e; + } + } + return nullptr; +} + +///////////////////////////////////////////////// +Projector *Link::ProjectorByName(const std::string &_name) +{ + return const_cast( + static_cast(this)->ProjectorByName(_name)); +} + ///////////////////////////////////////////////// const gz::math::Inertiald &Link::Inertial() const { @@ -554,6 +619,12 @@ void Link::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) emitter.SetXmlParentName(this->dataPtr->name); emitter.SetPoseRelativeToGraph(_graph); } + + for (auto &projector : this->dataPtr->projectors) + { + projector.SetXmlParentName(this->dataPtr->name); + projector.SetPoseRelativeToGraph(_graph); + } } ///////////////////////////////////////////////// @@ -691,6 +762,15 @@ bool Link::AddParticleEmitter(const ParticleEmitter &_emitter) return true; } +////////////////////////////////////////////////// +bool Link::AddProjector(const Projector &_projector) +{ + if (this->ProjectorNameExists(_projector.Name())) + return false; + this->dataPtr->projectors.push_back(_projector); + return true; +} + ////////////////////////////////////////////////// void Link::ClearCollisions() { @@ -721,6 +801,12 @@ void Link::ClearParticleEmitters() this->dataPtr->emitters.clear(); } +////////////////////////////////////////////////// +void Link::ClearProjectors() +{ + this->dataPtr->projectors.clear(); +} + ///////////////////////////////////////////////// sdf::ElementPtr Link::ToElement() const { @@ -801,6 +887,12 @@ sdf::ElementPtr Link::ToElement() const elem->InsertElement(emitter.ToElement(), true); } + // Projectors + for (const sdf::Projector &projector : this->dataPtr->projectors) + { + elem->InsertElement(projector.ToElement(), true); + } + // Sensors for (const sdf::Sensor &sensor : this->dataPtr->sensors) { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 3ed716b45..bf53c118a 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -23,6 +23,7 @@ #include "sdf/Light.hh" #include "sdf/Link.hh" #include "sdf/ParticleEmitter.hh" +#include "sdf/Projector.hh" #include "sdf/Sensor.hh" #include "sdf/Visual.hh" @@ -56,6 +57,13 @@ TEST(DOMLink, Construction) EXPECT_FALSE(link.ParticleEmitterNameExists("default")); EXPECT_EQ(nullptr, link.ParticleEmitterByName("no_such_emitter")); + EXPECT_EQ(0u, link.ProjectorCount()); + EXPECT_EQ(nullptr, link.ProjectorByIndex(0)); + EXPECT_EQ(nullptr, link.ProjectorByIndex(1)); + EXPECT_FALSE(link.ProjectorNameExists("")); + EXPECT_FALSE(link.ProjectorNameExists("default")); + EXPECT_EQ(nullptr, link.ProjectorByName("no_such_projector")); + EXPECT_FALSE(link.EnableWind()); link.SetEnableWind(true); EXPECT_TRUE(link.EnableWind()); @@ -436,6 +444,20 @@ TEST(DOMLink, ToElement) link.ClearParticleEmitters(); } + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 3; i++) + { + sdf::Projector projector; + projector.SetName("projector" + std::to_string(i)); + projector.SetTexture("projector.png"); + EXPECT_TRUE(link.AddProjector(projector)); + EXPECT_FALSE(link.AddProjector(projector)); + } + if (j == 0) + link.ClearProjectors(); + } + sdf::ElementPtr elem = link.ToElement(); ASSERT_NE(nullptr, elem); @@ -465,6 +487,10 @@ TEST(DOMLink, ToElement) EXPECT_EQ(link.ParticleEmitterCount(), link2.ParticleEmitterCount()); for (uint64_t i = 0; i < link2.ParticleEmitterCount(); ++i) EXPECT_NE(nullptr, link2.ParticleEmitterByIndex(i)); + + EXPECT_EQ(link.ProjectorCount(), link2.ProjectorCount()); + for (uint64_t i = 0; i < link2.ProjectorCount(); ++i) + EXPECT_NE(nullptr, link2.ProjectorByIndex(i)); } ///////////////////////////////////////////////// @@ -493,6 +519,10 @@ TEST(DOMLink, MutableByIndex) pe.SetName("pe1"); EXPECT_TRUE(link.AddParticleEmitter(pe)); + sdf::Projector projector; + projector.SetName("projector1"); + EXPECT_TRUE(link.AddProjector(projector)); + // Modify the visual sdf::Visual *v = link.VisualByIndex(0); ASSERT_NE(nullptr, v); @@ -527,6 +557,13 @@ TEST(DOMLink, MutableByIndex) EXPECT_EQ("pe1", p->Name()); p->SetName("pe2"); EXPECT_EQ("pe2", link.ParticleEmitterByIndex(0)->Name()); + + // Modify the projector + sdf::Projector *pr = link.ProjectorByIndex(0); + ASSERT_NE(nullptr, pr); + EXPECT_EQ("projector1", pr->Name()); + pr->SetName("projector2"); + EXPECT_EQ("projector2", link.ProjectorByIndex(0)->Name()); } ///////////////////////////////////////////////// @@ -555,6 +592,10 @@ TEST(DOMLink, MutableByName) pe.SetName("pe1"); EXPECT_TRUE(link.AddParticleEmitter(pe)); + sdf::Projector projector; + projector.SetName("projector1"); + EXPECT_TRUE(link.AddProjector(projector)); + // Modify the visual sdf::Visual *v = link.VisualByName("visual1"); ASSERT_NE(nullptr, v); @@ -594,4 +635,12 @@ TEST(DOMLink, MutableByName) p->SetName("pe2"); EXPECT_FALSE(link.ParticleEmitterNameExists("pe1")); EXPECT_TRUE(link.ParticleEmitterNameExists("pe2")); + + // Modify the projector + sdf::Projector *pr = link.ProjectorByName("projector1"); + ASSERT_NE(nullptr, pr); + EXPECT_EQ("projector1", pr->Name()); + pr->SetName("projector2"); + EXPECT_FALSE(link.ProjectorNameExists("projector1")); + EXPECT_TRUE(link.ProjectorNameExists("projector2")); } diff --git a/src/Projector.cc b/src/Projector.cc new file mode 100644 index 000000000..e1a662200 --- /dev/null +++ b/src/Projector.cc @@ -0,0 +1,333 @@ +/* + * Copyright 2023 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/Error.hh" +#include "sdf/parser.hh" +#include "sdf/Projector.hh" +#include "sdf/Types.hh" + +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" +#include "Utils.hh" + +using namespace sdf; + +class sdf::Projector::Implementation +{ + /// \brief Name of the projector + public: std::string name; + + /// \brief Near clip plane + public: double nearClip{0.1}; + + /// \brief far clip plane + public: double farClip{10.0}; + + /// \brief Visibility flags + public: uint32_t visibilityFlags{UINT32_MAX}; + + /// \brief Horizontal field of view + public: gz::math::Angle hfov{0.785}; + + /// \brief Texture used by the projector + public: std::string texture; + + /// \brief Pose of the emitter + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; + + /// \brief Frame of the pose. + public: std::string poseRelativeTo; + + /// \brief Weak pointer to model's Pose Relative-To Graph. + public: sdf::ScopedGraph poseRelativeToGraph; + + /// \brief Name of xml parent object. + public: std::string xmlParentName = ""; + + /// \brief The path to the file where this emitter was defined. + public: std::string filePath = ""; + + /// \brief Sensor plugins. + public: std::vector plugins; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Projector::Projector() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Projector::Load(ElementPtr _sdf) +{ + Errors errors; + + 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() != "projector") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a projector, but the provided SDF element " + "is not a ."}); + return errors; + } + + // Read the projector's name + if (!loadName(_sdf, this->dataPtr->name)) + { + errors.push_back({ErrorCode::ATTRIBUTE_MISSING, + "A projector name is required, but the name is not set."}); + } + + // Check that the projector's name is valid + if (isReservedName(this->dataPtr->name)) + { + errors.push_back({ErrorCode::RESERVED_NAME, + "The supplied projector name [" + this->dataPtr->name + + "] is reserved."}); + } + + // Load the pose. Ignore the return value since the pose is optional. + loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); + + this->dataPtr->nearClip = _sdf->Get("near_clip", + this->dataPtr->nearClip).first; + + this->dataPtr->farClip = _sdf->Get("far_clip", + this->dataPtr->farClip).first; + + double fov = _sdf->Get("fov", + this->dataPtr->hfov.Radian()).first; + this->dataPtr->hfov = gz::math::Angle(fov); + + this->dataPtr->visibilityFlags = _sdf->Get("visibility_flags", + this->dataPtr->visibilityFlags).first; + + this->dataPtr->texture = _sdf->Get("texture", + this->dataPtr->texture).first; + if (this->dataPtr->texture == "__default__") + this->dataPtr->texture = ""; + + // Load the sensor plugins + Errors pluginErrors = loadRepeated(_sdf, "plugin", + this->dataPtr->plugins); + errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); + + return errors; +} + +///////////////////////////////////////////////// +std::string Projector::Name() const +{ + return this->dataPtr->name; +} + +///////////////////////////////////////////////// +void Projector::SetName(const std::string &_name) +{ + this->dataPtr->name = _name; +} + +///////////////////////////////////////////////// +const gz::math::Pose3d &Projector::RawPose() const +{ + return this->dataPtr->pose; +} + +///////////////////////////////////////////////// +void Projector::SetRawPose(const gz::math::Pose3d &_pose) +{ + this->dataPtr->pose = _pose; +} + +///////////////////////////////////////////////// +const std::string &Projector::PoseRelativeTo() const +{ + return this->dataPtr->poseRelativeTo; +} + +///////////////////////////////////////////////// +void Projector::SetPoseRelativeTo(const std::string &_frame) +{ + this->dataPtr->poseRelativeTo = _frame; +} + +///////////////////////////////////////////////// +sdf::SemanticPose Projector::SemanticPose() const +{ + return sdf::SemanticPose( + this->dataPtr->pose, + this->dataPtr->poseRelativeTo, + this->dataPtr->xmlParentName, + this->dataPtr->poseRelativeToGraph); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Projector::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +const std::string &Projector::FilePath() const +{ + return this->dataPtr->filePath; +} + +////////////////////////////////////////////////// +void Projector::SetFilePath(const std::string &_filePath) +{ + this->dataPtr->filePath = _filePath; +} + +///////////////////////////////////////////////// +void Projector::SetXmlParentName(const std::string &_xmlParentName) +{ + this->dataPtr->xmlParentName = _xmlParentName; +} + +///////////////////////////////////////////////// +void Projector::SetPoseRelativeToGraph( + sdf::ScopedGraph _graph) +{ + this->dataPtr->poseRelativeToGraph = _graph; +} + + +////////////////////////////////////////////////// +double Projector::NearClip() const +{ + return this->dataPtr->nearClip; +} + +////////////////////////////////////////////////// +void Projector::SetNearClip(double _near) +{ + this->dataPtr->nearClip = _near; +} + +////////////////////////////////////////////////// +double Projector::FarClip() const +{ + return this->dataPtr->farClip; +} + +////////////////////////////////////////////////// +void Projector::SetFarClip(double _far) +{ + this->dataPtr->farClip = _far; +} + +///////////////////////////////////////////////// +gz::math::Angle Projector::HorizontalFov() const +{ + return this->dataPtr->hfov; +} + +///////////////////////////////////////////////// +void Projector::SetHorizontalFov(const gz::math::Angle &_hfov) +{ + this->dataPtr->hfov = _hfov; +} + +///////////////////////////////////////////////// +uint32_t Projector::VisibilityFlags() const +{ + return this->dataPtr->visibilityFlags; +} + +///////////////////////////////////////////////// +void Projector::SetVisibilityFlags(uint32_t _flags) +{ + this->dataPtr->visibilityFlags = _flags; +} + +////////////////////////////////////////////////// +std::string Projector::Texture() const +{ + return this->dataPtr->texture; +} + +////////////////////////////////////////////////// +void Projector::SetTexture(const std::string &_texture) +{ + this->dataPtr->texture = _texture; +} + +///////////////////////////////////////////////// +const sdf::Plugins &Projector::Plugins() const +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +sdf::Plugins &Projector::Plugins() +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +void Projector::ClearPlugins() +{ + this->dataPtr->plugins.clear(); +} + +///////////////////////////////////////////////// +void Projector::AddPlugin(const Plugin &_plugin) +{ + this->dataPtr->plugins.push_back(_plugin); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Projector::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("projector.sdf", elem); + + // Set pose + sdf::ElementPtr poseElem = elem->GetElement("pose"); + if (!this->dataPtr->poseRelativeTo.empty()) + { + poseElem->GetAttribute("relative_to")->Set( + this->dataPtr->poseRelativeTo); + } + poseElem->Set(this->RawPose()); + + elem->GetAttribute("name")->Set(this->Name()); + elem->GetElement("near_clip")->Set(this->NearClip()); + elem->GetElement("far_clip")->Set(this->FarClip()); + elem->GetElement("fov")->Set(this->HorizontalFov()); + elem->GetElement("texture")->Set(this->Texture()); + elem->GetElement("visibility_flags")->Set(this->VisibilityFlags()); + + // Add in the plugins + for (const Plugin &plugin : this->dataPtr->plugins) + elem->InsertElement(plugin.ToElement(), true); + + return elem; +} diff --git a/src/Projector_TEST.cc b/src/Projector_TEST.cc new file mode 100644 index 000000000..a1454fc05 --- /dev/null +++ b/src/Projector_TEST.cc @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2023 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/Projector.hh" + +///////////////////////////////////////////////// +TEST(DOMProjector, Construction) +{ + sdf::Projector projector; + + EXPECT_EQ(nullptr, projector.Element()); + EXPECT_TRUE(projector.Name().empty()); + + projector.SetName("test_projector"); + EXPECT_EQ(projector.Name(), "test_projector"); + + EXPECT_DOUBLE_EQ(0.1, projector.NearClip()); + projector.SetNearClip(2.0); + EXPECT_DOUBLE_EQ(2.0, projector.NearClip()); + + EXPECT_DOUBLE_EQ(10.0, projector.FarClip()); + projector.SetFarClip(20.0); + EXPECT_DOUBLE_EQ(20.0, projector.FarClip()); + + EXPECT_EQ(gz::math::Angle(0.785), projector.HorizontalFov()); + projector.SetHorizontalFov(gz::math::Angle(GZ_PI * 0.5)); + EXPECT_EQ(gz::math::Angle(GZ_PI * 0.5), projector.HorizontalFov()); + + EXPECT_EQ(UINT32_MAX, projector.VisibilityFlags()); + projector.SetVisibilityFlags(0x03); + EXPECT_EQ(0x03, projector.VisibilityFlags()); + + EXPECT_TRUE(projector.Texture().empty()); + projector.SetTexture("texture.png"); + EXPECT_EQ("texture.png", projector.Texture()); + + EXPECT_EQ(gz::math::Pose3d::Zero, projector.RawPose()); + projector.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 1.5707)); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 1.5707), projector.RawPose()); + + EXPECT_TRUE(projector.PoseRelativeTo().empty()); + projector.SetPoseRelativeTo("/test/relative"); + EXPECT_EQ("/test/relative", projector.PoseRelativeTo()); + + EXPECT_TRUE(projector.Plugins().empty()); + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + + projector.AddPlugin(plugin); + ASSERT_EQ(1u, projector.Plugins().size()); + + plugin.SetName("name2"); + projector.AddPlugin(plugin); + ASSERT_EQ(2u, projector.Plugins().size()); + + EXPECT_EQ("name1", projector.Plugins()[0].Name()); + EXPECT_EQ("name2", projector.Plugins()[1].Name()); + + projector.ClearPlugins(); + EXPECT_TRUE(projector.Plugins().empty()); +} + +///////////////////////////////////////////////// +TEST(DOMProjector, ToElement) +{ + sdf::Projector projector; + + projector.SetName("my-projector"); + projector.SetNearClip(0.3); + projector.SetFarClip(12.3); + projector.SetHorizontalFov(gz::math::Angle(0.5)); + projector.SetVisibilityFlags(0x01); + projector.SetTexture("test_texture.png"); + projector.SetRawPose(gz::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + projector.AddPlugin(plugin); + + sdf::ElementPtr elem = projector.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Projector projector2; + projector2.Load(elem); + + EXPECT_EQ(projector.Name(), projector2.Name()); + EXPECT_DOUBLE_EQ(projector.NearClip(), projector2.NearClip()); + EXPECT_DOUBLE_EQ(projector.FarClip(), projector2.FarClip()); + EXPECT_EQ(projector.VisibilityFlags(), projector2.VisibilityFlags()); + EXPECT_EQ(projector.HorizontalFov(), projector2.HorizontalFov()); + EXPECT_EQ(projector.Texture(), projector2.Texture()); + EXPECT_EQ(projector.RawPose(), projector2.RawPose()); + + ASSERT_EQ(1u, projector2.Plugins().size()); + EXPECT_EQ("name1", projector2.Plugins()[0].Name()); + EXPECT_EQ("filename1", projector2.Plugins()[0].Filename()); +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 02865d8b9..9bad38ea4 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -44,6 +44,7 @@ set(tests pose_1_9_sdf.cc precision.cc print_config.cc + projector_dom.cc provide_feedback.cc resolve_uris.cc root_dom.cc diff --git a/test/integration/projector_dom.cc b/test/integration/projector_dom.cc new file mode 100644 index 000000000..6c4b40739 --- /dev/null +++ b/test/integration/projector_dom.cc @@ -0,0 +1,82 @@ +/* + * Copyright 2023 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/SDFImpl.hh" +#include "sdf/parser.hh" +#include "sdf/Root.hh" +#include "sdf/World.hh" +#include "sdf/Light.hh" +#include "sdf/Link.hh" +#include "sdf/Material.hh" +#include "sdf/Model.hh" +#include "sdf/Projector.hh" +#include "sdf/Pbr.hh" +#include "sdf/Filesystem.hh" +#include "test_config.hh" + +////////////////////////////////////////////////// +TEST(DOMWorld, LoadProjector) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_complete.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + for (auto e : errors) + std::cout << e.Message() << std::endl; + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + gz::math::Pose3d pose; + + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ("frame1", model->PoseRelativeTo()); + EXPECT_TRUE(model->SemanticPose().Resolve(pose, "frame1").empty()); + errors = model->SemanticPose().Resolve(pose, "frame1"); + for (auto e : errors) + std::cout << e.Message() << std::endl; + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE(model->SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), pose); + EXPECT_TRUE(model->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), pose); + + const sdf::Link *link = model->LinkByIndex(0); + ASSERT_NE(nullptr, link); + const sdf::Projector *linkProjector = link->ProjectorByIndex(0); + ASSERT_NE(nullptr, linkProjector); + EXPECT_EQ("projector", linkProjector->Name()); + EXPECT_TRUE(linkProjector->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), pose); + + EXPECT_DOUBLE_EQ(0.03, linkProjector->NearClip()); + EXPECT_DOUBLE_EQ(3.0, linkProjector->FarClip()); + EXPECT_EQ(gz::math::Angle(0.8), linkProjector->HorizontalFov()); + EXPECT_EQ(0x01, linkProjector->VisibilityFlags()); + EXPECT_EQ("materials/textures/projector.png", linkProjector->Texture()); + + ASSERT_EQ(1u, linkProjector->Plugins().size()); + EXPECT_EQ("projector_plugin", linkProjector->Plugins()[0].Name()); + EXPECT_EQ("test/file/projector", linkProjector->Plugins()[0].Filename()); +} diff --git a/test/sdf/world_complete.sdf b/test/sdf/world_complete.sdf index 4260d9076..28e89eb27 100644 --- a/test/sdf/world_complete.sdf +++ b/test/sdf/world_complete.sdf @@ -104,6 +104,16 @@ 0.2 + + 1 2 3 0 0 0 + 0.03 + 3 + 0.8 + 0x01 + materials/textures/projector.png + + + 7 8 9 0 0 0 true From 7f76df9af91352d72d99ad015f88bb4326ffe044 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 1 May 2023 21:16:43 +0000 Subject: [PATCH 2/4] fix doxygen comments Signed-off-by: Ian Chen --- include/sdf/Link.hh | 10 +++++----- include/sdf/Projector.hh | 10 +++++----- sdf/1.10/projector.sdf | 2 +- sdf/1.7/projector.sdf | 2 +- sdf/1.8/projector.sdf | 2 +- sdf/1.9/projector.sdf | 2 +- src/Projector.cc | 4 ++-- 7 files changed, 16 insertions(+), 16 deletions(-) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 1ff9e3179..a2a5c93d0 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -254,8 +254,8 @@ namespace sdf /// \sa bool ParticleEmitterNameExists(const std::string &_name) const public: ParticleEmitter *ParticleEmitterByName(const std::string &_name); - /// \brief Get the number of particle projectors. - /// \return Number of particle projectors contained in this Link object. + /// \brief Get the number of projectors. + /// \return Number of projectors contained in this Link object. public: uint64_t ProjectorCount() const; /// \brief Get a projector based on an index. @@ -408,13 +408,13 @@ namespace sdf /// \param[in] _emitter Particle emitter to add. /// \return True if successful, false if a particle emitter with the name /// already exists. - public: bool AddParticleEmitter(const ParticleEmitter &_sensor); + public: bool AddParticleEmitter(const ParticleEmitter &_emitter); /// \brief Add a projector to the link. - /// \param[in] _emitter Projector to add. + /// \param[in] _projector Projector to add. /// \return True if successful, false if a projector with the name /// already exists. - public: bool AddProjector(const Projector &_sensor); + public: bool AddProjector(const Projector &_projector); /// \brief Remove all collisions public: void ClearCollisions(); diff --git a/include/sdf/Projector.hh b/include/sdf/Projector.hh index c879e8197..9fe1f4c44 100644 --- a/include/sdf/Projector.hh +++ b/include/sdf/Projector.hh @@ -125,8 +125,8 @@ namespace sdf public: void AddPlugin(const Plugin &_plugin); /// \brief Get the pose of the projector. This is the pose of the - /// emitter as specified in SDF - /// ( ... ). + /// projector as specified in SDF + /// ( ... ). /// \return The pose of the projector. public: const gz::math::Pose3d &RawPose() const; @@ -136,19 +136,19 @@ namespace sdf public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this - /// emitter's pose is expressed. An empty value indicates that the frame is + /// projector's pose is expressed. An empty value indicates that the frame is /// relative to the parent link. /// \return The name of the pose relative-to frame. public: const std::string &PoseRelativeTo() const; /// \brief Set the name of the coordinate frame relative to which this - /// emitter's pose is expressed. An empty value indicates that the frame is + /// projector's pose is expressed. An empty value indicates that the frame is /// relative to the parent link. /// \param[in] _frame The name of the pose relative-to frame. public: void SetPoseRelativeTo(const std::string &_frame); /// \brief Get SemanticPose object of this object to aid in resolving poses. - /// \return SemanticPose object for this emitter. + /// \return SemanticPose object for this projector. public: sdf::SemanticPose SemanticPose() const; /// \brief Get a pointer to the SDF element that was used during load. diff --git a/sdf/1.10/projector.sdf b/sdf/1.10/projector.sdf index 28de4e3e1..ca8cc8fd3 100644 --- a/sdf/1.10/projector.sdf +++ b/sdf/1.10/projector.sdf @@ -23,7 +23,7 @@ - + diff --git a/sdf/1.7/projector.sdf b/sdf/1.7/projector.sdf index 28de4e3e1..ca8cc8fd3 100644 --- a/sdf/1.7/projector.sdf +++ b/sdf/1.7/projector.sdf @@ -23,7 +23,7 @@ - + diff --git a/sdf/1.8/projector.sdf b/sdf/1.8/projector.sdf index 28de4e3e1..ca8cc8fd3 100644 --- a/sdf/1.8/projector.sdf +++ b/sdf/1.8/projector.sdf @@ -23,7 +23,7 @@ - + diff --git a/sdf/1.9/projector.sdf b/sdf/1.9/projector.sdf index 28de4e3e1..ca8cc8fd3 100644 --- a/sdf/1.9/projector.sdf +++ b/sdf/1.9/projector.sdf @@ -23,7 +23,7 @@ - + diff --git a/src/Projector.cc b/src/Projector.cc index e1a662200..3c9cd97fa 100644 --- a/src/Projector.cc +++ b/src/Projector.cc @@ -50,7 +50,7 @@ class sdf::Projector::Implementation /// \brief Texture used by the projector public: std::string texture; - /// \brief Pose of the emitter + /// \brief Pose of the projector public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. @@ -62,7 +62,7 @@ class sdf::Projector::Implementation /// \brief Name of xml parent object. public: std::string xmlParentName = ""; - /// \brief The path to the file where this emitter was defined. + /// \brief The path to the file where this projector was defined. public: std::string filePath = ""; /// \brief Sensor plugins. From 77405a068c4cf56eefac3e9ffd70c1dbadd0b068 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 1 May 2023 21:27:11 +0000 Subject: [PATCH 3/4] style Signed-off-by: Ian Chen --- include/sdf/Projector.hh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/sdf/Projector.hh b/include/sdf/Projector.hh index 9fe1f4c44..2e64d8d2e 100644 --- a/include/sdf/Projector.hh +++ b/include/sdf/Projector.hh @@ -136,14 +136,14 @@ namespace sdf public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this - /// projector's pose is expressed. An empty value indicates that the frame is - /// relative to the parent link. + /// projector's pose is expressed. An empty value indicates that the frame + /// is relative to the parent link. /// \return The name of the pose relative-to frame. public: const std::string &PoseRelativeTo() const; /// \brief Set the name of the coordinate frame relative to which this - /// projector's pose is expressed. An empty value indicates that the frame is - /// relative to the parent link. + /// projector's pose is expressed. An empty value indicates that the frame + /// is relative to the parent link. /// \param[in] _frame The name of the pose relative-to frame. public: void SetPoseRelativeTo(const std::string &_frame); From 927a0c97b779f41766e97c85ed7eaf9c8cf9ffe1 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 4 May 2023 17:06:59 +0000 Subject: [PATCH 4/4] fix doxy, improve coverage Signed-off-by: Ian Chen --- include/sdf/Projector.hh | 6 +++--- src/Projector.cc | 2 +- src/Projector_TEST.cc | 6 ++++++ test/integration/projector_dom.cc | 1 + 4 files changed, 11 insertions(+), 4 deletions(-) diff --git a/include/sdf/Projector.hh b/include/sdf/Projector.hh index 2e64d8d2e..b13fd8797 100644 --- a/include/sdf/Projector.hh +++ b/include/sdf/Projector.hh @@ -89,11 +89,11 @@ namespace sdf /// \param[in] _hfov The horizontal field of view in radians. public: void SetHorizontalFov(const gz::math::Angle &_hfov); - /// \brief Get the visibility flags of a visual + /// \brief Get the visibility flags of a projector /// \return visibility flags public: uint32_t VisibilityFlags() const; - /// \brief Set the visibility flags of a visual + /// \brief Set the visibility flags of a projector /// \param[in] _flags visibility flags public: void SetVisibilityFlags(uint32_t _flags); @@ -126,7 +126,7 @@ namespace sdf /// \brief Get the pose of the projector. This is the pose of the /// projector as specified in SDF - /// ( ... ). + /// ( ... ). /// \return The pose of the projector. public: const gz::math::Pose3d &RawPose() const; diff --git a/src/Projector.cc b/src/Projector.cc index 3c9cd97fa..b8c1f7b98 100644 --- a/src/Projector.cc +++ b/src/Projector.cc @@ -133,7 +133,7 @@ Errors Projector::Load(ElementPtr _sdf) if (this->dataPtr->texture == "__default__") this->dataPtr->texture = ""; - // Load the sensor plugins + // Load the projector plugins Errors pluginErrors = loadRepeated(_sdf, "plugin", this->dataPtr->plugins); errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); diff --git a/src/Projector_TEST.cc b/src/Projector_TEST.cc index a1454fc05..89d792da9 100644 --- a/src/Projector_TEST.cc +++ b/src/Projector_TEST.cc @@ -57,6 +57,10 @@ TEST(DOMProjector, Construction) projector.SetPoseRelativeTo("/test/relative"); EXPECT_EQ("/test/relative", projector.PoseRelativeTo()); + EXPECT_TRUE(projector.FilePath().empty()); + projector.SetFilePath("/test/path"); + EXPECT_EQ("/test/path", projector.FilePath()); + EXPECT_TRUE(projector.Plugins().empty()); sdf::Plugin plugin; plugin.SetName("name1"); @@ -88,6 +92,7 @@ TEST(DOMProjector, ToElement) projector.SetVisibilityFlags(0x01); projector.SetTexture("test_texture.png"); projector.SetRawPose(gz::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + projector.SetPoseRelativeTo("link"); sdf::Plugin plugin; plugin.SetName("name1"); @@ -107,6 +112,7 @@ TEST(DOMProjector, ToElement) EXPECT_EQ(projector.HorizontalFov(), projector2.HorizontalFov()); EXPECT_EQ(projector.Texture(), projector2.Texture()); EXPECT_EQ(projector.RawPose(), projector2.RawPose()); + EXPECT_EQ("link", projector2.PoseRelativeTo()); ASSERT_EQ(1u, projector2.Plugins().size()); EXPECT_EQ("name1", projector2.Plugins()[0].Name()); diff --git a/test/integration/projector_dom.cc b/test/integration/projector_dom.cc index 6c4b40739..ce79088fa 100644 --- a/test/integration/projector_dom.cc +++ b/test/integration/projector_dom.cc @@ -79,4 +79,5 @@ TEST(DOMWorld, LoadProjector) ASSERT_EQ(1u, linkProjector->Plugins().size()); EXPECT_EQ("projector_plugin", linkProjector->Plugins()[0].Name()); EXPECT_EQ("test/file/projector", linkProjector->Plugins()[0].Filename()); + EXPECT_EQ(testFile, linkProjector->FilePath()); }