From dc3d10076558c9da0ac023d07b2dd2881edbf7f7 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 6 Apr 2021 07:57:41 -0700 Subject: [PATCH 1/4] Particle emitter (#528) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Adding particle emitter to SDF Signed-off-by: Nate Koenig * missing files Signed-off-by: Nate Koenig * Update documentation Signed-off-by: Nate Koenig * Update documentation and clamps Signed-off-by: Nate Koenig * Minor tweaks. Signed-off-by: Carlos Agüero * Fix windows warnings and scale rate default Signed-off-by: Nate Koenig * Use empty string for topic and image path Signed-off-by: Nate Koenig * Fix more windows warnings Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Carlos Agüero --- include/sdf/CMakeLists.txt | 1 + include/sdf/Link.hh | 27 ++ include/sdf/ParticleEmitter.hh | 352 ++++++++++++++ include/sdf/SemanticPose.hh | 1 + sdf/1.6/link.sdf | 1 + sdf/1.6/particle_emitter.sdf | 109 +++++ sdf/1.7/link.sdf | 1 + sdf/1.7/particle_emitter.sdf | 109 +++++ src/CMakeLists.txt | 2 + src/Link.cc | 58 +++ src/Link_TEST.cc | 8 + src/ParticleEmitter.cc | 565 +++++++++++++++++++++++ src/ParticleEmitter_TEST.cc | 114 +++++ src/Visual.cc | 2 +- test/integration/CMakeLists.txt | 1 + test/integration/particle_emitter_dom.cc | 94 ++++ test/sdf/world_complete.sdf | 22 + 17 files changed, 1466 insertions(+), 1 deletion(-) create mode 100644 include/sdf/ParticleEmitter.hh create mode 100644 sdf/1.6/particle_emitter.sdf create mode 100644 sdf/1.7/particle_emitter.sdf create mode 100644 src/ParticleEmitter.cc create mode 100644 src/ParticleEmitter_TEST.cc create mode 100644 test/integration/particle_emitter_dom.cc diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index 49245e802..9d6f526c8 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -32,6 +32,7 @@ set (headers Noise.hh Param.hh parser.hh + ParticleEmitter.hh Pbr.hh Physics.hh Plane.hh diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 7b8532ac5..52d4e920f 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -36,6 +36,7 @@ namespace sdf class Collision; class Light; class LinkPrivate; + class ParticleEmitter; class Sensor; class Visual; class LinkPrivate; @@ -171,6 +172,32 @@ namespace sdf /// \sa bool SensorNameExists(const std::string &_name) const public: const Sensor *SensorByName(const std::string &_name) const; + /// \brief Get the number of particle emitters. + /// \return Number of particle emitters contained in this Link object. + public: uint64_t ParticleEmitterCount() const; + + /// \brief Get a particle emitter based on an index. + /// \param[in] _index Index of the particle emitter. + /// The index should be in the range [0..ParticleEmitterCount()). + /// \return Pointer to the particle emitter. Nullptr if the index does + /// not exist. + /// \sa uint64_t ParticleEmitterCount() const + public: const ParticleEmitter *ParticleEmitterByIndex( + const uint64_t _index) const; + + /// \brief Get whether a particle emitter name exists. + /// \param[in] _name Name of the particle emitter to check. + /// \return True if there exists a particle emitter with the given name. + public: bool ParticleEmitterNameExists(const std::string &_name) const; + + /// \brief Get a particle emitter based on a name. + /// \param[in] _name Name of the particle emitter. + /// \return Pointer to the particle emitter. Nullptr if a particle emitter + /// with the given name does not exist. + /// \sa bool ParticleEmitterNameExists(const std::string &_name) const + public: const ParticleEmitter *ParticleEmitterByName( + const std::string &_name) const; + /// \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 diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh new file mode 100644 index 000000000..251b4ff60 --- /dev/null +++ b/include/sdf/ParticleEmitter.hh @@ -0,0 +1,352 @@ +/* + * Copyright 2021 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_PARTICLE_EMITTER_HH_ +#define SDF_PARTICLE_EMITTER_HH_ + +#include + +#include +#include +#include "sdf/Material.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. + class ParticleEmitterPrivate; + struct PoseRelativeToGraph; + + /// \enum ParticleEmitterType + /// \brief The set of particle emitter types. + // Developer note: Make sure to update emitterTypeStrs in the source + // file when changing this enum. + enum class ParticleEmitterType + { + /// \brief A point emitter. + POINT = 0, + + /// \brief A box emitter. + BOX = 1, + + /// \brief A cylinder emitter. + CYLINDER = 2, + + /// \brief An ellipsoid emitter. + ELLIPSOID = 3, + }; + + /// \brief A description of a particle emitter, which can be attached + /// to a link. A particle emitter can be used to describe fog, smoke, and + /// dust. + class SDFORMAT_VISIBLE ParticleEmitter + { + /// \brief Default constructor + public: ParticleEmitter(); + + /// \brief Copy constructor + /// \param[in] _emitter Particle emitter to copy. + public: ParticleEmitter(const ParticleEmitter &_emitter); + + /// \brief Move constructor + /// \param[in] _model Emitter to move. + public: ParticleEmitter(ParticleEmitter &&_emitter) noexcept; + + /// \brief Move assignment operator. + /// \param[in] _emitter Particle emitter to move. + /// \return Reference to this. + public: ParticleEmitter &operator=(ParticleEmitter &&_emitter); + + /// \brief Copy assignment operator. + /// \param[in] _emitter Particle emitter to copy. + /// \return Reference to this. + public: ParticleEmitter &operator=(const ParticleEmitter &_emitter); + + /// \brief Destructor + public: ~ParticleEmitter(); + + /// \brief Load the particle emitter 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 particle emitter. + /// The name of the particle emitter should be unique within the scope of + /// a Link. + /// \return Name of the particle emitter. + public: std::string Name() const; + + /// \brief Set the name of the particle emitter. + /// The name of the particle emitter should be unique within the scope of + /// a Link. + /// \param[in] _name Name of the particle emitter. + public: void SetName(const std::string &_name); + + /// \brief Get the type of the particle emitter. + /// The type of the particle emitter should be unique within the scope of + /// a Link. + /// \return Type of the particle emitter. + public: ParticleEmitterType Type() const; + + /// \brief Set the type of the particle emitter. + /// \param[in] _type Type of the particle emitter. + public: void SetType(const ParticleEmitterType _type); + + /// \brief Set the type of the particle emitter. + /// The type of the particle emitter should be unique within the scope of + /// a Link. + /// \param[in] _typeStr Type of the particle emitter. + /// \return True if the _typeStr parameter matched a known emitter type. + /// False if the emitter type could not be set. + public: bool SetType(const std::string &_typeStr); + + /// \brief Get the particle emitter type as a string. + /// \return The particle emitter type as a string. + public: std::string TypeStr() const; + + /// \brief Get whether the particle emitter should run (emit + /// particles). + /// \return True if particles should be emitted. + public: bool Emitting() const; + + /// \brief Set whether the particle emitter is running, emitting + /// particles. + /// \param[in] _emitting True if the emitter should produce particles. + public: void SetEmitting(bool _emitting); + + /// \brief Get the number of seconds the emitter is active. + /// A value less than or equal to zero indicates infinite duration. + /// \return The number of seconds the emitter is active. + public: double Duration() const; + + /// \brief Set the number of seconds the emitter is active. + /// \param[in] _duration The number of seconds the emitter is active. + /// A value less than or equal to zero means infinite duration. + public: void SetDuration(double _duration); + + /// \brief Get the number of seconds each particle will 'live' for + /// before being destroyed. + /// \return The lifetime of a particle in seconds. + public: double Lifetime() const; + + /// \brief Set the number of seconds each particle will 'live' for. + /// \param[in] _duration The number of seconds a particle will 'life' + /// for. If _duration is <= 0, then the + /// value std::numeric_limits::min() will be used. + public: void SetLifetime(double _duration); + + /// \brief Get the number of particles per second that should be emitted. + /// \return The number of particles to emit per second. + public: double Rate() const; + + /// \brief Set the number of particles per second that should be emitted. + /// \param[in] _rate The number of particle to emit per second. + /// A value of zero will be used if _rate is negative. + public: void SetRate(double _rate); + + /// \brief Get the amount by which to scale the particles in both x + /// and y direction per second. + /// \return The scaling amount in the x and y directions. + public: double ScaleRate() const; + + /// \brief Set the amount by which to scale the particles in both x + /// and y direction per second. + /// \param[in] _scaleRate The caling amount in the x and y directions. + /// A value of zero will be used if _scaleRate is negative. + public: void SetScaleRate(double _scaleRate); + + /// \brief Get the minimum velocity for each particle. + /// \return The minimum velocity for each particle in m/s. + public: double MinVelocity() const; + + /// \brief Set the minimum velocity for each particle. + /// \param[in] _vel The minimum velocity for each particle in m/s. + /// A value of zero will be used if _vel is negative. + public: void SetMinVelocity(double _vel); + + /// \brief Get the maximum velocity for each particle. + /// \return The maximum velocity for each particle in m/s. + public: double MaxVelocity() const; + + /// \brief Set the maximum velocity for each particle. + /// \param[in] _vel The maximum velocity for each particle in m/s. + /// A value of zero will be used if _vel is negative. + public: void SetMaxVelocity(double _vel); + + /// \brief Get the size of the emitter where the particles are sampled. + // Default value is (1, 1, 1). + // Note that the interpretation of the emitter area varies + // depending on the emmiter type: + // - point: The area is ignored. + // - box: The area is interpreted as width X height X depth. + // - cylinder: The area is interpreted as the bounding box of the + // cylinder. The cylinder is oriented along the Z-axis. + // - ellipsoid: The area is interpreted as the bounding box of an + // ellipsoid shaped area, i.e. a sphere or + // squashed-sphere area. The parameters are again + // identical to EM_BOX, except that the dimensions + // describe the widest points along each of the axes. + /// \return Size of the emitter region in meters. + public: ignition::math::Vector3d Size() const; + + /// \brief Set the size of the emitter where the particles are sampled. + /// \param[in] _size Size of the emitter in meters. + /// Each component of _size must be greater than or equal to zero. Any + /// negative value will be replaced with zero. + /// \sa ignition::math::Vector3d Size() + public: void SetSize(const ignition::math::Vector3d &_size); + + /// \brief Get the size of a particle in meters. + /// \return Size of a particle in meters. + public: ignition::math::Vector3d ParticleSize() const; + + /// \brief Set the size of a particle in meters. + /// \param[in] _size Size of a particle in meters. + /// Each component of _size must be greater than or equal to zero. Any + /// negative value will be replaced with zero. + public: void SetParticleSize(const ignition::math::Vector3d &_size); + + /// \brief Gets the starting color for all particles emitted. + /// The actual color will be interpolated between this color + /// and the one set using ColorEnd(). + /// Color::White is the default color for the particles + /// unless a specific function is used. + /// \return The starting color. + public: ignition::math::Color ColorStart() const; + + /// \brief Set the starting color for all particles emitted. + /// \param[in] _colorStart The starting color for all particles emitted. + /// \sa ignition::math::Color ColorStart() + public: void SetColorStart(const ignition::math::Color &_colorStart); + + /// \brief Get the end color for all particles emitted. + /// The actual color will be interpolated between this color + /// and the one set under ColorStart(). + /// Color::White is the default color for the particles + /// unless a specific function is used. + /// \return The end color. + public: ignition::math::Color ColorEnd() const; + + /// \brief Set the end color for all particles emitted. + /// \param[in] _colorEnd The end color for all particles emitted. + /// \sa ignition::math::Color ColorEnd() + public: void SetColorEnd(const ignition::math::Color &_colorEnd); + + /// \brief Get the path to the color image used as an affector. + /// This affector modifies the color of particles in flight. + /// The colors are taken from a specified image file. The range of + /// color values begins from the left side of the image and moves to the + /// right over the lifetime of the particle, therefore only the horizontal + /// dimension of the image is used. + /// The ColorRangeImage has higher priority than ColorEnd and + /// ColorStart. If all three are set, ColorRangeImage should be used. + public: std::string ColorRangeImage() const; + + /// \brief Set the path to the color image used as an affector. + /// \param[in] _image The path to the color image. + /// \sa std::String ColorRangeImage() + public: void SetColorRangeImage(const std::string &_image); + + /// \brief Get the topic used to update the particle emitter properties. + /// \return The topic used to update the particle emitter. + public: std::string Topic() const; + + /// \brief Set the topic used to update the particle emitter properties. + /// \param[in] _topic The topic used to update the particle emitter. + public: void SetTopic(const std::string &_topic); + + /// \brief Get the pose of the particle emitter. This is the pose of the + /// emitter as specified in SDF + /// ( ... ). + /// \return The pose of the particle emitter. + public: const ignition::math::Pose3d &RawPose() const; + + /// \brief Set the pose of the particle emitter object. + /// \sa const ignition::math::Pose3d &RawPose() const + /// \param[in] _pose The pose of the particle emitter. + public: void SetRawPose(const ignition::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 Get a pointer to the emitter's material properties. This can + /// be a nullptr if material properties have not been set. + /// \return Pointer to the emitter's material properties. Nullptr + /// indicates that material properties have not been set. + public: sdf::Material *Material() const; + + /// \brief Set the emitter's material + /// \param[in] _material The material of the particle emitter. + public: void SetMaterial(const sdf::Material &_material); + + /// \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 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 Weak pointer to PoseRelativeToGraph. + private: void SetPoseRelativeToGraph( + std::weak_ptr _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. + private: ParticleEmitterPrivate *dataPtr = nullptr; + }; + } +} +#endif diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 6baa76035..55671b32f 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -111,6 +111,7 @@ namespace sdf friend class Joint; friend class Light; friend class Link; + friend class ParticleEmitter; friend class Model; friend class Sensor; friend class Visual; diff --git a/sdf/1.6/link.sdf b/sdf/1.6/link.sdf index 238d44506..b1a9a426d 100644 --- a/sdf/1.6/link.sdf +++ b/sdf/1.6/link.sdf @@ -47,5 +47,6 @@ + diff --git a/sdf/1.6/particle_emitter.sdf b/sdf/1.6/particle_emitter.sdf new file mode 100644 index 000000000..428ed251e --- /dev/null +++ b/sdf/1.6/particle_emitter.sdf @@ -0,0 +1,109 @@ + + + A particle emitter that can be used to describe fog, smoke, and dust. + + + A unique name for the particle emitter. + + + + The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". + + + + True indicates that the particle emitter should generate particles when loaded + + + + The number of seconds the emitter is active. A value less than or equal to zero means infinite duration. + + + + + The size of the emitter where the particles are sampled. + Default value is (1, 1, 1). + Note that the interpretation of the emitter area varies + depending on the emmiter type: + - point: The area is ignored. + - box: The area is interpreted as width X height X depth. + - cylinder: The area is interpreted as the bounding box of the + cylinder. The cylinder is oriented along the Z-axis. + - ellipsoid: The area is interpreted as the bounding box of an + ellipsoid shaped area, i.e. a sphere or + squashed-sphere area. The parameters are again + identical to EM_BOX, except that the dimensions + describe the widest points along each of the axes. + + + + + The particle dimensions (width, height, depth). + + + + The number of seconds each particle will ’live’ for before being destroyed. This value must be greater than zero. + + + + The number of particles per second that should be emitted. + + + + Sets a minimum velocity for each particle (m/s). + + + + Sets a maximum velocity for each particle (m/s). + + + + Sets the amount by which to scale the particles in both x and y direction per second. + + + + + Sets the starting color for all particles emitted. + The actual color will be interpolated between this color + and the one set under color_end. + Color::White is the default color for the particles + unless a specific function is used. + To specify a color, RGB values should be passed in. + For example, to specify red, a user should enter: + 1 0 0 + Note that this function overrides the particle colors set + with color_range_image. + + + + + + Sets the end color for all particles emitted. + The actual color will be interpolated between this color + and the one set under color_start. + Color::White is the default color for the particles + unless a specific function is used (see color_start for + more information about defining custom colors with RGB + values). + Note that this function overrides the particle colors set + with color_range_image. + + + + + + Sets the path to the color image used as an affector. This affector modifies the color of particles in flight. The colors are taken from a specified image file. The range of color values begins from the left side of the image and moves to the right over the lifetime of the particle, therefore only the horizontal dimension of the image is used. Note that this function overrides the particle colors set with color_start and color_end. + + + + + + Topic used to update particle emitter properties at runtime. + The default topic is + /model/{model_name}/particle_emitter/{emitter_name} + Note that the emitter id and name may not be changed. + + + + + + diff --git a/sdf/1.7/link.sdf b/sdf/1.7/link.sdf index 44664dcb3..b518ff3c5 100644 --- a/sdf/1.7/link.sdf +++ b/sdf/1.7/link.sdf @@ -46,5 +46,6 @@ + diff --git a/sdf/1.7/particle_emitter.sdf b/sdf/1.7/particle_emitter.sdf new file mode 100644 index 000000000..428ed251e --- /dev/null +++ b/sdf/1.7/particle_emitter.sdf @@ -0,0 +1,109 @@ + + + A particle emitter that can be used to describe fog, smoke, and dust. + + + A unique name for the particle emitter. + + + + The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". + + + + True indicates that the particle emitter should generate particles when loaded + + + + The number of seconds the emitter is active. A value less than or equal to zero means infinite duration. + + + + + The size of the emitter where the particles are sampled. + Default value is (1, 1, 1). + Note that the interpretation of the emitter area varies + depending on the emmiter type: + - point: The area is ignored. + - box: The area is interpreted as width X height X depth. + - cylinder: The area is interpreted as the bounding box of the + cylinder. The cylinder is oriented along the Z-axis. + - ellipsoid: The area is interpreted as the bounding box of an + ellipsoid shaped area, i.e. a sphere or + squashed-sphere area. The parameters are again + identical to EM_BOX, except that the dimensions + describe the widest points along each of the axes. + + + + + The particle dimensions (width, height, depth). + + + + The number of seconds each particle will ’live’ for before being destroyed. This value must be greater than zero. + + + + The number of particles per second that should be emitted. + + + + Sets a minimum velocity for each particle (m/s). + + + + Sets a maximum velocity for each particle (m/s). + + + + Sets the amount by which to scale the particles in both x and y direction per second. + + + + + Sets the starting color for all particles emitted. + The actual color will be interpolated between this color + and the one set under color_end. + Color::White is the default color for the particles + unless a specific function is used. + To specify a color, RGB values should be passed in. + For example, to specify red, a user should enter: + 1 0 0 + Note that this function overrides the particle colors set + with color_range_image. + + + + + + Sets the end color for all particles emitted. + The actual color will be interpolated between this color + and the one set under color_start. + Color::White is the default color for the particles + unless a specific function is used (see color_start for + more information about defining custom colors with RGB + values). + Note that this function overrides the particle colors set + with color_range_image. + + + + + + Sets the path to the color image used as an affector. This affector modifies the color of particles in flight. The colors are taken from a specified image file. The range of color values begins from the left side of the image and moves to the right over the lifetime of the particle, therefore only the horizontal dimension of the image is used. Note that this function overrides the particle colors set with color_start and color_end. + + + + + + Topic used to update particle emitter properties at runtime. + The default topic is + /model/{model_name}/particle_emitter/{emitter_name} + Note that the emitter id and name may not be changed. + + + + + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4aaaa400a..9b91292e6 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -44,6 +44,7 @@ set (sources parser.cc parser_urdf.cc Param.cc + ParticleEmitter.cc Pbr.cc Physics.cc Plane.cc @@ -111,6 +112,7 @@ if (BUILD_SDF_TEST) Noise_TEST.cc Param_TEST.cc parser_TEST.cc + ParticleEmitter_TEST.cc Pbr_TEST.cc Physics_TEST.cc Plane_TEST.cc diff --git a/src/Link.cc b/src/Link.cc index 9ec72fc60..3933d1ab7 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -25,6 +25,7 @@ #include "sdf/Error.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" +#include "sdf/ParticleEmitter.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" @@ -55,6 +56,9 @@ class sdf::LinkPrivate /// \brief The sensors specified in this link. public: std::vector sensors; + /// \brief The particle emitters specified in this link. + public: std::vector emitters; + /// \brief The inertial information for this link. public: ignition::math::Inertiald inertial {{1.0, ignition::math::Vector3d::One, ignition::math::Vector3d::Zero}, @@ -163,6 +167,12 @@ Errors Link::Load(ElementPtr _sdf) this->dataPtr->sensors); errors.insert(errors.end(), sensorLoadErrors.begin(), sensorLoadErrors.end()); + // Load all the particle emitters. + Errors emitterLoadErrors = loadUniqueRepeated(_sdf, + "particle_emitter", this->dataPtr->emitters); + errors.insert(errors.end(), emitterLoadErrors.begin(), + emitterLoadErrors.end()); + ignition::math::Vector3d xxyyzz = ignition::math::Vector3d::One; ignition::math::Vector3d xyxzyz = ignition::math::Vector3d::Zero; ignition::math::Pose3d inertiaPose; @@ -336,6 +346,48 @@ const Sensor *Link::SensorByName(const std::string &_name) const return nullptr; } +///////////////////////////////////////////////// +uint64_t Link::ParticleEmitterCount() const +{ + return this->dataPtr->emitters.size(); +} + +///////////////////////////////////////////////// +const ParticleEmitter *Link::ParticleEmitterByIndex(const uint64_t _index) const +{ + if (_index < this->dataPtr->emitters.size()) + return &this->dataPtr->emitters[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +bool Link::ParticleEmitterNameExists(const std::string &_name) const +{ + for (auto const &e : this->dataPtr->emitters) + { + if (e.Name() == _name) + { + return true; + } + } + return false; +} + +///////////////////////////////////////////////// +const ParticleEmitter *Link::ParticleEmitterByName( + const std::string &_name) const +{ + for (auto const &e : this->dataPtr->emitters) + { + if (e.Name() == _name) + { + return &e; + } + } + return nullptr; +} + + ///////////////////////////////////////////////// const ignition::math::Inertiald &Link::Inertial() const { @@ -400,6 +452,12 @@ void Link::SetPoseRelativeToGraph( visual.SetXmlParentName(this->dataPtr->name); visual.SetPoseRelativeToGraph(_graph); } + + for (auto &emitter : this->dataPtr->emitters) + { + emitter.SetXmlParentName(this->dataPtr->name); + emitter.SetPoseRelativeToGraph(_graph); + } } ///////////////////////////////////////////////// diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index fd460e6df..8b11381ff 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -22,6 +22,7 @@ #include "sdf/Collision.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" +#include "sdf/ParticleEmitter.hh" #include "sdf/Sensor.hh" #include "sdf/Visual.hh" @@ -48,6 +49,13 @@ TEST(DOMLink, Construction) EXPECT_FALSE(link.LightNameExists("default")); EXPECT_EQ(nullptr, link.LightByName("no_such_light")); + EXPECT_EQ(0u, link.ParticleEmitterCount()); + EXPECT_EQ(nullptr, link.ParticleEmitterByIndex(0)); + EXPECT_EQ(nullptr, link.ParticleEmitterByIndex(1)); + EXPECT_FALSE(link.ParticleEmitterNameExists("")); + EXPECT_FALSE(link.ParticleEmitterNameExists("default")); + EXPECT_EQ(nullptr, link.ParticleEmitterByName("no_such_emitter")); + EXPECT_FALSE(link.EnableWind()); link.SetEnableWind(true); EXPECT_TRUE(link.EnableWind()); diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc new file mode 100644 index 000000000..e343077e3 --- /dev/null +++ b/src/ParticleEmitter.cc @@ -0,0 +1,565 @@ +/* + * Copyright 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include +#include + +#include "sdf/ParticleEmitter.hh" +#include "sdf/Error.hh" +#include "sdf/Types.hh" +#include "Utils.hh" + +using namespace sdf; + +/// Particle emitter type strings. These should match the data in +/// `enum class ParticleEmitterType` located in ParticleEmitter.hh. +const std::vector emitterTypeStrs = +{ + "point", + "box", + "cylinder", + "ellipsoid", +}; + +class sdf::ParticleEmitterPrivate +{ + /// \brief Default constructor + public: ParticleEmitterPrivate() = default; + + /// \brief Copy constructor + /// \param[in] _private ParticleEmitterPrivate to move. + public: explicit ParticleEmitterPrivate( + const ParticleEmitterPrivate &_private); + + // Delete copy assignment so it is not accidentally used + public: ParticleEmitterPrivate &operator=( + const ParticleEmitterPrivate &) = delete; + + /// \brief Name of the particle emitter. + public: std::string name = ""; + + /// \brief Type of the particle emitter. + public: ParticleEmitterType type = ParticleEmitterType::POINT; + + /// \brief True if the emitting should produce particles. + public: bool emitting = true; + + /// \brief The number of seconds the emitter is active. + /// A value of 0 means infinite duration. + public: double duration = 0; + + /// \brief The number of seconds each particle will 'live' for. + public: double lifetime = 5; + + /// \brief The number of particles per second that should be emitted. + public: double rate = 10; + + /// \brief The amount by which to scale the particles in both x and y + /// direction per second. + public: double scaleRate = 0; + + /// \brief The minimum velocity for each particle (m/s). + public: double minVelocity = 1; + + /// \brief The maximum velocity for each particle (m/s). + public: double maxVelocity = 1; + + /// \brief The size of the emitter where the particles are sampled. + public: ignition::math::Vector3d size = ignition::math::Vector3d::One; + + /// \brief The size of a particle. + public: ignition::math::Vector3d particleSize = ignition::math::Vector3d::One; + + /// \brief The starting color for all particle emitted. + public: ignition::math::Color colorStart = ignition::math::Color::White; + + /// \brief The ending color for all particle emitted. + public: ignition::math::Color colorEnd = ignition::math::Color::White; + + /// \brief The color range image + public: std::string colorRangeImage = ""; + + /// \brief Topic used to update particle emitter properties at runtime. + public: std::string topic = ""; + + /// \brief Pose of the emitter + public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + + /// \brief Frame of the pose. + public: std::string poseRelativeTo = ""; + + /// \brief Weak pointer to model's Pose Relative-To Graph. + public: std::weak_ptr poseRelativeToGraph; + + /// \brief Name of xml parent object. + public: std::string xmlParentName = ""; + + /// \brief Pointer to the emitter's material properties. + public: std::unique_ptr material; + + /// \brief The path to the file where this emitter was defined. + public: std::string filePath = ""; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +ParticleEmitterPrivate::ParticleEmitterPrivate( + const ParticleEmitterPrivate &_private) + : name(_private.name), + type(_private.type), + emitting(_private.emitting), + duration(_private.duration), + lifetime(_private.lifetime), + rate(_private.rate), + scaleRate(_private.scaleRate), + minVelocity(_private.minVelocity), + maxVelocity(_private.maxVelocity), + size(_private.size), + particleSize(_private.particleSize), + colorStart(_private.colorStart), + colorEnd(_private.colorEnd), + colorRangeImage(_private.colorRangeImage), + topic(_private.topic), + pose(_private.pose), + poseRelativeTo(_private.poseRelativeTo), + xmlParentName(_private.xmlParentName), + filePath(_private.filePath), + sdf(_private.sdf) +{ + if (_private.material) + { + this->material = std::make_unique(*(_private.material)); + } +} + +///////////////////////////////////////////////// +ParticleEmitter::ParticleEmitter() + : dataPtr(new ParticleEmitterPrivate) +{ +} + +///////////////////////////////////////////////// +ParticleEmitter::~ParticleEmitter() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +///////////////////////////////////////////////// +ParticleEmitter::ParticleEmitter(const ParticleEmitter &_particleEmitter) + : dataPtr(new ParticleEmitterPrivate(*_particleEmitter.dataPtr)) +{ +} + +///////////////////////////////////////////////// +ParticleEmitter::ParticleEmitter(ParticleEmitter &&_particleEmitter) noexcept + : dataPtr(std::exchange(_particleEmitter.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +ParticleEmitter &ParticleEmitter::operator=( + const ParticleEmitter &_particleEmitter) +{ + return *this = ParticleEmitter(_particleEmitter); +} + +///////////////////////////////////////////////// +ParticleEmitter &ParticleEmitter::operator=(ParticleEmitter &&_particleEmitter) +{ + std::swap(this->dataPtr, _particleEmitter.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +Errors ParticleEmitter::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() != "particle_emitter") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a particle emitter, but the provided SDF element " + "is not a ."}); + return errors; + } + + // Read the emitter's name + if (!loadName(_sdf, this->dataPtr->name)) + { + errors.push_back({ErrorCode::ATTRIBUTE_MISSING, + "A link name is required, but the name is not set."}); + } + + // Check that the emitter's name is valid + if (isReservedName(this->dataPtr->name)) + { + errors.push_back({ErrorCode::RESERVED_NAME, + "The supplied particle emitter 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); + + if (!this->SetType(_sdf->Get("type", + this->TypeStr()).first)) + { + errors.push_back({ErrorCode::ATTRIBUTE_INVALID, + "Attempting to load a particle emitter, but the provided " + "particle emitter type is missing or invalid."}); + return errors; + } + + this->dataPtr->emitting = _sdf->Get("emitting", + this->dataPtr->emitting).first; + + this->dataPtr->duration = _sdf->Get("duration", + this->dataPtr->duration).first; + + this->dataPtr->lifetime = _sdf->Get("lifetime", + this->dataPtr->lifetime).first; + + this->dataPtr->rate = _sdf->Get("rate", + this->dataPtr->rate).first; + + this->dataPtr->scaleRate = _sdf->Get("scale_rate", + this->dataPtr->scaleRate).first; + + this->dataPtr->minVelocity = _sdf->Get("min_velocity", + this->dataPtr->minVelocity).first; + + this->dataPtr->maxVelocity = _sdf->Get("max_velocity", + this->dataPtr->maxVelocity).first; + + this->dataPtr->size = _sdf->Get("size", + this->dataPtr->size).first; + + this->dataPtr->particleSize = _sdf->Get( + "particle_size", this->dataPtr->particleSize).first; + + this->dataPtr->colorStart = _sdf->Get( + "color_start", this->dataPtr->colorStart).first; + + this->dataPtr->colorEnd = _sdf->Get( + "color_end", this->dataPtr->colorEnd).first; + + this->dataPtr->colorRangeImage = _sdf->Get( + "color_range_image", this->dataPtr->colorRangeImage).first; + + this->dataPtr->topic = _sdf->Get( + "topic", this->dataPtr->topic).first; + + if (_sdf->HasElement("material")) + { + this->dataPtr->material.reset(new sdf::Material()); + Errors err = this->dataPtr->material->Load(_sdf->GetElement("material")); + errors.insert(errors.end(), err.begin(), err.end()); + } + + return errors; +} + +///////////////////////////////////////////////// +std::string ParticleEmitter::Name() const +{ + return this->dataPtr->name; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetName(const std::string &_name) +{ + this->dataPtr->name = _name; +} + +///////////////////////////////////////////////// +ParticleEmitterType ParticleEmitter::Type() const +{ + return this->dataPtr->type; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetType(const ParticleEmitterType _type) +{ + this->dataPtr->type = _type; +} + +///////////////////////////////////////////////// +bool ParticleEmitter::SetType(const std::string &_typeStr) +{ + for (size_t i = 0; i < emitterTypeStrs.size(); ++i) + { + if (_typeStr == emitterTypeStrs[i]) + { + this->dataPtr->type = static_cast(i); + return true; + } + } + return false; +} + +///////////////////////////////////////////////// +std::string ParticleEmitter::TypeStr() const +{ + size_t index = static_cast(this->dataPtr->type); + if (index < emitterTypeStrs.size()) + return emitterTypeStrs[index]; + return "point"; +} + +///////////////////////////////////////////////// +bool ParticleEmitter::Emitting() const +{ + return this->dataPtr->emitting; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetEmitting(bool _emitting) +{ + this->dataPtr->emitting = _emitting; +} + +///////////////////////////////////////////////// +double ParticleEmitter::Duration() const +{ + return this->dataPtr->duration; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetDuration(double _duration) +{ + this->dataPtr->duration = _duration; +} + +///////////////////////////////////////////////// +double ParticleEmitter::Lifetime() const +{ + return this->dataPtr->lifetime; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetLifetime(double _lifetime) +{ + this->dataPtr->lifetime = std::max(_lifetime, ignition::math::MIN_D); +} + +///////////////////////////////////////////////// +double ParticleEmitter::Rate() const +{ + return this->dataPtr->rate; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetRate(double _rate) +{ + this->dataPtr->rate = std::max(_rate, 0.0); +} + +///////////////////////////////////////////////// +double ParticleEmitter::ScaleRate() const +{ + return this->dataPtr->scaleRate; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetScaleRate(double _scaleRate) +{ + this->dataPtr->scaleRate = std::max(_scaleRate, 0.0); +} + +///////////////////////////////////////////////// +double ParticleEmitter::MinVelocity() const +{ + return this->dataPtr->minVelocity; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetMinVelocity(double _vel) +{ + this->dataPtr->minVelocity = std::max(_vel, 0.0); +} + +///////////////////////////////////////////////// +double ParticleEmitter::MaxVelocity() const +{ + return this->dataPtr->maxVelocity; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetMaxVelocity(double _vel) +{ + this->dataPtr->maxVelocity = std::max(_vel, 0.0); +} + +///////////////////////////////////////////////// +ignition::math::Vector3d ParticleEmitter::Size() const +{ + return this->dataPtr->size; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetSize(const ignition::math::Vector3d &_size) +{ + this->dataPtr->size = _size; + this->dataPtr->size.Max(ignition::math::Vector3d::Zero); +} + +///////////////////////////////////////////////// +ignition::math::Vector3d ParticleEmitter::ParticleSize() const +{ + return this->dataPtr->particleSize; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetParticleSize(const ignition::math::Vector3d &_size) +{ + this->dataPtr->particleSize = _size; + this->dataPtr->particleSize.Max(ignition::math::Vector3d::Zero); +} + +///////////////////////////////////////////////// +ignition::math::Color ParticleEmitter::ColorStart() const +{ + return this->dataPtr->colorStart; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetColorStart(const ignition::math::Color &_colorStart) +{ + this->dataPtr->colorStart = _colorStart; +} + +///////////////////////////////////////////////// +ignition::math::Color ParticleEmitter::ColorEnd() const +{ + return this->dataPtr->colorEnd; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetColorEnd(const ignition::math::Color &_colorEnd) +{ + this->dataPtr->colorEnd = _colorEnd; +} + +///////////////////////////////////////////////// +std::string ParticleEmitter::ColorRangeImage() const +{ + return this->dataPtr->colorRangeImage; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetColorRangeImage(const std::string &_image) +{ + this->dataPtr->colorRangeImage = _image; +} + +///////////////////////////////////////////////// +std::string ParticleEmitter::Topic() const +{ + return this->dataPtr->topic; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetTopic(const std::string &_topic) +{ + this->dataPtr->topic = _topic; +} + +///////////////////////////////////////////////// +const ignition::math::Pose3d &ParticleEmitter::RawPose() const +{ + return this->dataPtr->pose; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetRawPose(const ignition::math::Pose3d &_pose) +{ + this->dataPtr->pose = _pose; +} + +///////////////////////////////////////////////// +const std::string &ParticleEmitter::PoseRelativeTo() const +{ + return this->dataPtr->poseRelativeTo; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetPoseRelativeTo(const std::string &_frame) +{ + this->dataPtr->poseRelativeTo = _frame; +} + +///////////////////////////////////////////////// +sdf::SemanticPose ParticleEmitter::SemanticPose() const +{ + return sdf::SemanticPose( + this->dataPtr->pose, + this->dataPtr->poseRelativeTo, + this->dataPtr->xmlParentName, + this->dataPtr->poseRelativeToGraph); +} + +///////////////////////////////////////////////// +sdf::ElementPtr ParticleEmitter::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +sdf::Material *ParticleEmitter::Material() const +{ + return this->dataPtr->material.get(); +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetMaterial(const sdf::Material &_material) +{ + this->dataPtr->material.reset(new sdf::Material(_material)); +} + +////////////////////////////////////////////////// +const std::string &ParticleEmitter::FilePath() const +{ + return this->dataPtr->filePath; +} + +////////////////////////////////////////////////// +void ParticleEmitter::SetFilePath(const std::string &_filePath) +{ + this->dataPtr->filePath = _filePath; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetXmlParentName(const std::string &_xmlParentName) +{ + this->dataPtr->xmlParentName = _xmlParentName; +} + +///////////////////////////////////////////////// +void ParticleEmitter::SetPoseRelativeToGraph( + std::weak_ptr _graph) +{ + this->dataPtr->poseRelativeToGraph = _graph; +} diff --git a/src/ParticleEmitter_TEST.cc b/src/ParticleEmitter_TEST.cc new file mode 100644 index 000000000..8dc350de3 --- /dev/null +++ b/src/ParticleEmitter_TEST.cc @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2021 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/ParticleEmitter.hh" + +///////////////////////////////////////////////// +TEST(DOMParticleEmitter, Construction) +{ + sdf::ParticleEmitter emitter; + + EXPECT_EQ(nullptr, emitter.Element()); + EXPECT_TRUE(emitter.Name().empty()); + + emitter.SetName("test_emitter"); + EXPECT_EQ(emitter.Name(), "test_emitter"); + + EXPECT_EQ("point", emitter.TypeStr()); + EXPECT_EQ(sdf::ParticleEmitterType::POINT, emitter.Type()); + EXPECT_TRUE(emitter.SetType("box")); + EXPECT_EQ("box", emitter.TypeStr()); + EXPECT_EQ(sdf::ParticleEmitterType::BOX, emitter.Type()); + emitter.SetType(sdf::ParticleEmitterType::CYLINDER); + EXPECT_EQ("cylinder", emitter.TypeStr()); + + EXPECT_TRUE(emitter.Emitting()); + emitter.SetEmitting(false); + EXPECT_FALSE(emitter.Emitting()); + + EXPECT_DOUBLE_EQ(0.0, emitter.Duration()); + emitter.SetDuration(10.0); + EXPECT_DOUBLE_EQ(10.0, emitter.Duration()); + + EXPECT_DOUBLE_EQ(5.0, emitter.Lifetime()); + emitter.SetLifetime(22.0); + EXPECT_DOUBLE_EQ(22.0, emitter.Lifetime()); + emitter.SetLifetime(-1.0); + EXPECT_DOUBLE_EQ(ignition::math::MIN_D, emitter.Lifetime()); + + EXPECT_DOUBLE_EQ(10.0, emitter.Rate()); + emitter.SetRate(123.0); + EXPECT_DOUBLE_EQ(123.0, emitter.Rate()); + emitter.SetRate(-123.0); + EXPECT_DOUBLE_EQ(0.0, emitter.Rate()); + + EXPECT_DOUBLE_EQ(0.0, emitter.ScaleRate()); + emitter.SetScaleRate(1.2); + EXPECT_DOUBLE_EQ(1.2, emitter.ScaleRate()); + emitter.SetScaleRate(-1.2); + EXPECT_DOUBLE_EQ(0.0, emitter.ScaleRate()); + + EXPECT_DOUBLE_EQ(1.0, emitter.MinVelocity()); + emitter.SetMinVelocity(12.4); + EXPECT_DOUBLE_EQ(12.4, emitter.MinVelocity()); + emitter.SetMinVelocity(-12.4); + EXPECT_DOUBLE_EQ(0.0, emitter.MinVelocity()); + + EXPECT_DOUBLE_EQ(1.0, emitter.MaxVelocity()); + emitter.SetMaxVelocity(20.6); + EXPECT_DOUBLE_EQ(20.6, emitter.MaxVelocity()); + emitter.SetMaxVelocity(-12.4); + EXPECT_DOUBLE_EQ(0.0, emitter.MaxVelocity()); + + EXPECT_EQ(ignition::math::Vector3d::One, emitter.Size()); + emitter.SetSize(ignition::math::Vector3d(3, 2, 1)); + EXPECT_EQ(ignition::math::Vector3d(3, 2, 1), emitter.Size()); + emitter.SetSize(ignition::math::Vector3d(-3, -2, -1)); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), emitter.Size()); + + EXPECT_EQ(ignition::math::Vector3d::One, emitter.ParticleSize()); + emitter.SetParticleSize(ignition::math::Vector3d(4, 5, 6)); + EXPECT_EQ(ignition::math::Vector3d(4, 5, 6), emitter.ParticleSize()); + emitter.SetParticleSize(ignition::math::Vector3d(-4, -5, -6)); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), emitter.ParticleSize()); + + EXPECT_EQ(ignition::math::Color::White, emitter.ColorStart()); + emitter.SetColorStart(ignition::math::Color(0.1f, 0.2f, 0.3f, 1.0f)); + EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 1.0f), + emitter.ColorStart()); + + EXPECT_EQ(ignition::math::Color::White, emitter.ColorEnd()); + emitter.SetColorEnd(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0f)); + EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0f), emitter.ColorEnd()); + + EXPECT_TRUE(emitter.ColorRangeImage().empty()); + emitter.SetColorRangeImage("/test/string"); + EXPECT_EQ("/test/string", emitter.ColorRangeImage()); + + EXPECT_TRUE(emitter.Topic().empty()); + emitter.SetTopic("/test/topic"); + EXPECT_EQ("/test/topic", emitter.Topic()); + + EXPECT_EQ(ignition::math::Pose3d::Zero, emitter.RawPose()); + emitter.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 1.5707)); + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 1.5707), emitter.RawPose()); + + EXPECT_TRUE(emitter.PoseRelativeTo().empty()); + emitter.SetPoseRelativeTo("/test/relative"); + EXPECT_EQ("/test/relative", emitter.PoseRelativeTo()); +} diff --git a/src/Visual.cc b/src/Visual.cc index cb8488242..4ba701e7b 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -31,7 +31,7 @@ class sdf::VisualPrivate public: VisualPrivate() = default; /// \brief Copy constructor - /// \param[in] _visualPrivate Joint axis to move. + /// \param[in] _visualPrivate VisualPrivate to move. public: explicit VisualPrivate(const VisualPrivate &_visualPrivate); // Delete copy assignment so it is not accidentally used diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index e9cc14b14..5f2c1d15f 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -28,6 +28,7 @@ set(tests model_versions.cc nested_model.cc parser_error_detection.cc + particle_emitter_dom.cc plugin_attribute.cc plugin_bool.cc plugin_include.cc diff --git a/test/integration/particle_emitter_dom.cc b/test/integration/particle_emitter_dom.cc new file mode 100644 index 000000000..6071ac184 --- /dev/null +++ b/test/integration/particle_emitter_dom.cc @@ -0,0 +1,94 @@ +/* + * Copyright 2021 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/ParticleEmitter.hh" +#include "sdf/Pbr.hh" +#include "sdf/Filesystem.hh" +#include "test_config.h" + +////////////////////////////////////////////////// +TEST(DOMWorld, LoadParticleEmitter) +{ + 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); + ignition::math::Pose3d pose; + + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ(ignition::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(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE(model->SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), pose); + EXPECT_TRUE(model->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), pose); + + const sdf::Link *link = model->LinkByIndex(0); + ASSERT_NE(nullptr, link); + const sdf::ParticleEmitter *linkEmitter = link->ParticleEmitterByIndex(0); + ASSERT_NE(nullptr, linkEmitter); + EXPECT_EQ("emitter", linkEmitter->Name()); + EXPECT_TRUE(linkEmitter->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), pose); + + EXPECT_TRUE(linkEmitter->Emitting()); + EXPECT_EQ(ignition::math::Vector3d(10, 11, 12), linkEmitter->Size()); + EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), linkEmitter->ParticleSize()); + EXPECT_DOUBLE_EQ(25, linkEmitter->Lifetime()); + EXPECT_DOUBLE_EQ(0.1, linkEmitter->MinVelocity()); + EXPECT_DOUBLE_EQ(0.2, linkEmitter->MaxVelocity()); + EXPECT_DOUBLE_EQ(0.5, linkEmitter->ScaleRate()); + EXPECT_DOUBLE_EQ(5, linkEmitter->Rate()); + + sdf::Material *mat = linkEmitter->Material(); + ASSERT_NE(nullptr, mat); + EXPECT_EQ(ignition::math::Color(0.7f, 0.7f, 0.7f), mat->Diffuse()); + EXPECT_EQ(ignition::math::Color(1.0f, 1.0f, 1.0f), mat->Specular()); + + sdf::Pbr *pbr = mat->PbrMaterial(); + ASSERT_NE(nullptr, pbr); + sdf::PbrWorkflow *metal = pbr->Workflow(sdf::PbrWorkflowType::METAL); + ASSERT_NE(nullptr, metal); + EXPECT_EQ("materials/textures/fog.png", metal->AlbedoMap()); + + EXPECT_EQ("materials/textures/fogcolors.png", linkEmitter->ColorRangeImage()); +} diff --git a/test/sdf/world_complete.sdf b/test/sdf/world_complete.sdf index e0004de8e..955b4268d 100644 --- a/test/sdf/world_complete.sdf +++ b/test/sdf/world_complete.sdf @@ -70,6 +70,28 @@ 4 5 6 0 0 0 + + 7 8 9 0 0 0 + true + 10 11 12 + 1 2 3 + 25 + 0.1 + 0.2 + 0.5 + 5 + + 0.7 0.7 0.7 + 1.0 1.0 1.0 + + + materials/textures/fog.png + + + + materials/textures/fogcolors.png + + 7 8 9 0 0 0 true From 87a4992fc873a59584a4448a62e9e08fc2ee3821 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 6 Apr 2021 22:13:33 +0200 Subject: [PATCH 2/4] Fixed application of tags in lumped linkes during URDF conversion (#525) * Fixed application of tags in lumped linkes during URDF conversion. Fixed , too. Signed-off-by: Martin Pecka * Changelog Signed-off-by: Martin Pecka * Fixed float equality in test. Signed-off-by: Martin Pecka Co-authored-by: Nate Koenig --- Migration.md | 3 + src/parser_urdf.cc | 123 +++++++++++-- test/integration/urdf_gazebo_extensions.cc | 159 +++++++++++++++++ test/integration/urdf_gazebo_extensions.urdf | 178 +++++++++++++++++++ 4 files changed, 445 insertions(+), 18 deletions(-) diff --git a/Migration.md b/Migration.md index 5d92c43ee..ab4fbbe8d 100644 --- a/Migration.md +++ b/Migration.md @@ -18,6 +18,9 @@ but with improved human-readability.. 1. Fixed Atmosphere DOM class's temperature default value. Changed from -0.065 to -0.0065. * [Pull request 482](https://github.com/osrf/sdformat/pull/482) + +1. Fixed parsing of `` tags on lumped links when converting from URDF. + * [Pull request 525](https://github.com/osrf/sdformat/pull/525) ## SDFormat 9.x to 10.0 diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 85eebbaa9..a266ddb5f 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -2125,6 +2125,8 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem, for (auto blobIt = (*ge)->blobs.begin(); blobIt != (*ge)->blobs.end(); ++blobIt) { + // Be sure to always copy only the first element; code in + // ReduceSDFExtensionSensorTransformReduction depends in this behavior CopyBlob((*blobIt)->FirstChildElement(), _elem); } } @@ -3354,8 +3356,8 @@ void ReduceSDFExtensionSensorTransformReduction( std::vector::iterator _blobIt, ignition::math::Pose3d _reductionTransform) { - // overwrite and if they exist - if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "sensor") == 0) + auto sensorElement = (*_blobIt)->FirstChildElement(); + if ( strcmp(sensorElement->Name(), "sensor") == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform @@ -3369,16 +3371,58 @@ void ReduceSDFExtensionSensorTransformReduction( // sdfdbg << " " << streamIn.CStr() << "\n"; // } + auto sensorPose {ignition::math::Pose3d::Zero}; { - tinyxml2::XMLNode *oldPoseKey = (*_blobIt)->FirstChildElement("pose"); - /// @todo: FIXME: we should read xyz, rpy and aggregate it to - /// reductionTransform instead of just throwing the info away. + auto sensorPosText = "0 0 0 0 0 0"; + const auto& oldPoseKey = sensorElement->FirstChildElement("pose"); if (oldPoseKey) { - (*_blobIt)->DeleteChild(oldPoseKey); + const auto& poseElemXml = oldPoseKey->ToElement(); + if (poseElemXml->Attribute("relative_to")) + return; + + // see below for explanation; if a sibling element exists, it stores the + // original tag content + const auto& sibling = sensorElement->NextSibling(); + if (poseElemXml->GetText() && !sibling) + sensorPosText = poseElemXml->GetText(); + else if (sibling && sibling->ToElement()->GetText()) + sensorPosText = sibling->ToElement()->GetText(); + else + { + sdferr << "Unexpected case in sensor pose computation\n"; + return; + } + + // delete the tag, we'll add a new one at the end + sensorElement->DeleteChild(oldPoseKey); + } + + // parse the 6-tuple text into math::Pose3d + std::stringstream ss; + ss.imbue(std::locale::classic()); + ss << sensorPosText; + ss >> sensorPose; + if (ss.fail()) + { + sdferr << "Could not parse : [" << sensorPosText << "]\n"; + return; } + + // critical part: we store the original tag from actually + // as a sibling of ... only first element of the blob is processed + // further, so its siblings can be used as temporary storage; we store the + // original tag there so that we can use the tag + // for storing the reduced position + auto doc = (*_blobIt)->GetDocument(); + const auto& poseTxt = doc->NewText(sensorPosText); + auto poseKey = doc->NewElement("pose"); + poseKey->LinkEndChild(poseTxt); + (*_blobIt)->LinkEndChild(poseKey); } + _reductionTransform = _reductionTransform * sensorPose; + // convert reductionTransform to values urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), _reductionTransform.Pos().Y(), @@ -3403,7 +3447,7 @@ void ReduceSDFExtensionSensorTransformReduction( poseKey->LinkEndChild(poseTxt); - (*_blobIt)->LinkEndChild(poseKey); + sensorElement->LinkEndChild(poseKey); } } @@ -3412,8 +3456,8 @@ void ReduceSDFExtensionProjectorTransformReduction( std::vector::iterator _blobIt, ignition::math::Pose3d _reductionTransform) { - // overwrite (xyz/rpy) if it exists - if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "projector") == 0) + auto projectorElement = (*_blobIt)->FirstChildElement(); + if ( strcmp(projectorElement->Name(), "projector") == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform @@ -3426,16 +3470,59 @@ void ReduceSDFExtensionProjectorTransformReduction( // sdfdbg << " " << streamIn << "\n"; // } - // should read ... and agregate reductionTransform - tinyxml2::XMLNode *poseKey = (*_blobIt)->FirstChildElement("pose"); - // read pose and save it - - // remove the tag for now - if (poseKey) + auto projectorPose {ignition::math::Pose3d::Zero}; { - (*_blobIt)->DeleteChild(poseKey); + auto projectorPosText = "0 0 0 0 0 0"; + const auto& oldPoseKey = projectorElement->FirstChildElement("pose"); + if (oldPoseKey) + { + const auto& poseElemXml = oldPoseKey->ToElement(); + if (poseElemXml->Attribute("relative_to")) + return; + + // see below for explanation; if a sibling element exists, it stores the + // original tag content + const auto& sibling = projectorElement->NextSibling(); + if (poseElemXml->GetText() && !sibling) + projectorPosText = poseElemXml->GetText(); + else if (sibling && sibling->ToElement()->GetText()) + projectorPosText = sibling->ToElement()->GetText(); + else + { + sdferr << "Unexpected case in projector pose computation\n"; + return; + } + + // delete the tag, we'll add a new one at the end + projectorElement->DeleteChild(oldPoseKey); + } + + // parse the 6-tuple text into math::Pose3d + std::stringstream ss; + ss.imbue(std::locale::classic()); + ss << projectorPosText; + ss >> projectorPose; + if (ss.fail()) + { + sdferr << "Could not parse : [" + << projectorPosText << "]\n"; + return; + } + + // critical part: we store the original tag from + // actually as a sibling of ... only first element of the blob + // is processed further, so its siblings can be used as temporary storage; + // we store the original tag there so that we can use the + // tag for storing the reduced position + auto doc = (*_blobIt)->GetDocument(); + const auto& poseTxt = doc->NewText(projectorPosText); + auto poseKey = doc->NewElement("pose"); + poseKey->LinkEndChild(poseTxt); + (*_blobIt)->LinkEndChild(poseKey); } + _reductionTransform = _reductionTransform * projectorPose; + // convert reductionTransform to values urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), _reductionTransform.Pos().Y(), @@ -3456,10 +3543,10 @@ void ReduceSDFExtensionProjectorTransformReduction( auto* doc = (*_blobIt)->GetDocument(); tinyxml2::XMLText *poseTxt = doc->NewText(poseStream.str().c_str()); - poseKey = doc->NewElement("pose"); + auto poseKey = doc->NewElement("pose"); poseKey->LinkEndChild(poseTxt); - (*_blobIt)->LinkEndChild(poseKey); + projectorElement->LinkEndChild(poseKey); } } diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 6c51d6b69..45ba29698 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -99,4 +99,163 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_TRUE(physics->Get("implicit_spring_damper")); } } + + sdf::ElementPtr link0; + for (sdf::ElementPtr link = model->GetElement("link"); link; + link = link->GetNextElement("link")) + { + const auto& linkName = link->Get("name"); + if (linkName == "link0") + { + link0 = link; + break; + } + } + ASSERT_TRUE(link0); + + bool foundSensorNoPose {false}; + bool foundSensorPose {false}; + bool foundSensorPoseRelative {false}; + bool foundSensorPoseTwoLevel {false}; + bool foundIssue378Sensor {false}; + bool foundIssue67Sensor {false}; + + for (sdf::ElementPtr sensor = link0->GetElement("sensor"); sensor; + sensor = sensor->GetNextElement("sensor")) + { + const auto& sensorName = sensor->Get("name"); + if (sensorName == "sensorNoPose") + { + foundSensorNoPose = true; + ASSERT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 0.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2, 1e-5); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (sensorName == "sensorPose") + { + foundSensorPose = true; + ASSERT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 111.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (sensorName == "sensorPoseRelative") + { + foundSensorPoseRelative = true; + ASSERT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 111.0); + EXPECT_DOUBLE_EQ(pose.Y(), 0.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), -1, 1e-5); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (sensorName == "sensorPoseTwoLevel") + { + foundSensorPoseTwoLevel = true; + ASSERT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 111.0); + EXPECT_DOUBLE_EQ(pose.Z(), 222.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (sensorName == "issue378_sensor") + { + foundIssue378Sensor = true; + ASSERT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 1); + EXPECT_DOUBLE_EQ(pose.Y(), 2); + EXPECT_DOUBLE_EQ(pose.Z(), 3); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.1); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.2); + EXPECT_DOUBLE_EQ(pose.Yaw(), 0.3); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (sensorName == "issue67_sensor") + { + foundIssue67Sensor = true; + ASSERT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_GT(std::abs(pose.X() - (-0.20115)), 0.1); + EXPECT_GT(std::abs(pose.Y() - 0.42488), 0.1); + EXPECT_GT(std::abs(pose.Z() - 0.30943), 0.1); + EXPECT_GT(std::abs(pose.Roll() - 1.5708), 0.1); + EXPECT_GT(std::abs(pose.Pitch() - (-0.89012)), 0.1); + EXPECT_GT(std::abs(pose.Yaw() - 1.5708), 0.1); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + } + + EXPECT_TRUE(foundSensorNoPose); + EXPECT_TRUE(foundSensorPose); + EXPECT_TRUE(foundSensorPoseRelative); + EXPECT_TRUE(foundSensorPoseTwoLevel); + EXPECT_TRUE(foundIssue378Sensor); + EXPECT_TRUE(foundIssue67Sensor); } diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index 4703907db..558ca9357 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -137,4 +137,182 @@ + + + + + + + + + + + + + + + + + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + 1 + 400 + 1.0 2.0 3.0 0.1 0.2 0.3 + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 1 1 1 1.570796 1.570796 1.570796 + + + + + From f310d759b774d01ef4e4a40698971de068edf610 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 6 Apr 2021 16:46:43 -0700 Subject: [PATCH 3/4] 10.4.0 prep (#534) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- Changelog.md | 12 +++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fee2db3c7..51d563971 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ project (sdformat10) set (SDF_PROTOCOL_VERSION 1.7) set (SDF_MAJOR_VERSION 10) -set (SDF_MINOR_VERSION 3) +set (SDF_MINOR_VERSION 4) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) diff --git a/Changelog.md b/Changelog.md index 7f8d7e95f..faa51c61a 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,16 @@ ## libsdformat 10.X -### libsdformat 10.X.X (202X-XX-XX) +### libsdformat 10.4.0 (2021-04-06) + +1. Added particle emitter. + + [Pull request 528](https://github.com/osrf/sdformat/pull/528) + +1. Fixed application of tags in lumped linkes during URDF + conversion. + + [Pull request 525](https://github.com/osrf/sdformat/pull/525) + +1. Add camera type aliases to docs. + + [Pull request 514](https://github.com/osrf/sdformat/pull/514) ### libsdformat 10.3.0 (2021-02-18) From 7dfcd4e02bcad403092ac3947a9ad8abad6be9e0 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 19 Apr 2021 10:57:35 -0700 Subject: [PATCH 4/4] Update to latest SDF semantics * Copy particle_emitter.sdf to sdf/1.8 * Use IMPLPtr in ParticleEmitter DOM class * Update frame semantics Signed-off-by: Nate Koenig --- include/sdf/ParticleEmitter.hh | 30 +------ sdf/1.8/particle_emitter.sdf | 109 +++++++++++++++++++++++ src/ParticleEmitter.cc | 97 +++----------------- test/integration/particle_emitter_dom.cc | 6 +- 4 files changed, 129 insertions(+), 113 deletions(-) create mode 100755 sdf/1.8/particle_emitter.sdf diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh index 251b4ff60..c5fa6450e 100644 --- a/include/sdf/ParticleEmitter.hh +++ b/include/sdf/ParticleEmitter.hh @@ -32,7 +32,6 @@ namespace sdf // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // Forward declarations. - class ParticleEmitterPrivate; struct PoseRelativeToGraph; /// \enum ParticleEmitterType @@ -62,27 +61,6 @@ namespace sdf /// \brief Default constructor public: ParticleEmitter(); - /// \brief Copy constructor - /// \param[in] _emitter Particle emitter to copy. - public: ParticleEmitter(const ParticleEmitter &_emitter); - - /// \brief Move constructor - /// \param[in] _model Emitter to move. - public: ParticleEmitter(ParticleEmitter &&_emitter) noexcept; - - /// \brief Move assignment operator. - /// \param[in] _emitter Particle emitter to move. - /// \return Reference to this. - public: ParticleEmitter &operator=(ParticleEmitter &&_emitter); - - /// \brief Copy assignment operator. - /// \param[in] _emitter Particle emitter to copy. - /// \return Reference to this. - public: ParticleEmitter &operator=(const ParticleEmitter &_emitter); - - /// \brief Destructor - public: ~ParticleEmitter(); - /// \brief Load the particle emitter based on an element pointer. This is /// *not* the usual entry point. Typical usage of the SDF DOM is through /// the Root object. @@ -312,7 +290,7 @@ namespace sdf /// be a nullptr if material properties have not been set. /// \return Pointer to the emitter's material properties. Nullptr /// indicates that material properties have not been set. - public: sdf::Material *Material() const; + public: const sdf::Material *Material() const; /// \brief Set the emitter's material /// \param[in] _material The material of the particle emitter. @@ -335,9 +313,9 @@ namespace sdf /// \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 Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is @@ -345,7 +323,7 @@ namespace sdf friend class Link; /// \brief Private data pointer. - private: ParticleEmitterPrivate *dataPtr = nullptr; + IGN_UTILS_IMPL_PTR(dataPtr) }; } } diff --git a/sdf/1.8/particle_emitter.sdf b/sdf/1.8/particle_emitter.sdf new file mode 100755 index 000000000..428ed251e --- /dev/null +++ b/sdf/1.8/particle_emitter.sdf @@ -0,0 +1,109 @@ + + + A particle emitter that can be used to describe fog, smoke, and dust. + + + A unique name for the particle emitter. + + + + The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". + + + + True indicates that the particle emitter should generate particles when loaded + + + + The number of seconds the emitter is active. A value less than or equal to zero means infinite duration. + + + + + The size of the emitter where the particles are sampled. + Default value is (1, 1, 1). + Note that the interpretation of the emitter area varies + depending on the emmiter type: + - point: The area is ignored. + - box: The area is interpreted as width X height X depth. + - cylinder: The area is interpreted as the bounding box of the + cylinder. The cylinder is oriented along the Z-axis. + - ellipsoid: The area is interpreted as the bounding box of an + ellipsoid shaped area, i.e. a sphere or + squashed-sphere area. The parameters are again + identical to EM_BOX, except that the dimensions + describe the widest points along each of the axes. + + + + + The particle dimensions (width, height, depth). + + + + The number of seconds each particle will ’live’ for before being destroyed. This value must be greater than zero. + + + + The number of particles per second that should be emitted. + + + + Sets a minimum velocity for each particle (m/s). + + + + Sets a maximum velocity for each particle (m/s). + + + + Sets the amount by which to scale the particles in both x and y direction per second. + + + + + Sets the starting color for all particles emitted. + The actual color will be interpolated between this color + and the one set under color_end. + Color::White is the default color for the particles + unless a specific function is used. + To specify a color, RGB values should be passed in. + For example, to specify red, a user should enter: + 1 0 0 + Note that this function overrides the particle colors set + with color_range_image. + + + + + + Sets the end color for all particles emitted. + The actual color will be interpolated between this color + and the one set under color_start. + Color::White is the default color for the particles + unless a specific function is used (see color_start for + more information about defining custom colors with RGB + values). + Note that this function overrides the particle colors set + with color_range_image. + + + + + + Sets the path to the color image used as an affector. This affector modifies the color of particles in flight. The colors are taken from a specified image file. The range of color values begins from the left side of the image and moves to the right over the lifetime of the particle, therefore only the horizontal dimension of the image is used. Note that this function overrides the particle colors set with color_start and color_end. + + + + + + Topic used to update particle emitter properties at runtime. + The default topic is + /model/{model_name}/particle_emitter/{emitter_name} + Note that the emitter id and name may not be changed. + + + + + + diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc index e343077e3..782cee6ef 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -15,12 +15,16 @@ * */ #include +#include #include #include #include "sdf/ParticleEmitter.hh" #include "sdf/Error.hh" #include "sdf/Types.hh" + +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -35,20 +39,8 @@ const std::vector emitterTypeStrs = "ellipsoid", }; -class sdf::ParticleEmitterPrivate +class sdf::ParticleEmitter::Implementation { - /// \brief Default constructor - public: ParticleEmitterPrivate() = default; - - /// \brief Copy constructor - /// \param[in] _private ParticleEmitterPrivate to move. - public: explicit ParticleEmitterPrivate( - const ParticleEmitterPrivate &_private); - - // Delete copy assignment so it is not accidentally used - public: ParticleEmitterPrivate &operator=( - const ParticleEmitterPrivate &) = delete; - /// \brief Name of the particle emitter. public: std::string name = ""; @@ -103,13 +95,13 @@ class sdf::ParticleEmitterPrivate public: std::string poseRelativeTo = ""; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief Name of xml parent object. public: std::string xmlParentName = ""; /// \brief Pointer to the emitter's material properties. - public: std::unique_ptr material; + public: std::optional material; /// \brief The path to the file where this emitter was defined. public: std::string filePath = ""; @@ -118,73 +110,10 @@ class sdf::ParticleEmitterPrivate public: sdf::ElementPtr sdf; }; -///////////////////////////////////////////////// -ParticleEmitterPrivate::ParticleEmitterPrivate( - const ParticleEmitterPrivate &_private) - : name(_private.name), - type(_private.type), - emitting(_private.emitting), - duration(_private.duration), - lifetime(_private.lifetime), - rate(_private.rate), - scaleRate(_private.scaleRate), - minVelocity(_private.minVelocity), - maxVelocity(_private.maxVelocity), - size(_private.size), - particleSize(_private.particleSize), - colorStart(_private.colorStart), - colorEnd(_private.colorEnd), - colorRangeImage(_private.colorRangeImage), - topic(_private.topic), - pose(_private.pose), - poseRelativeTo(_private.poseRelativeTo), - xmlParentName(_private.xmlParentName), - filePath(_private.filePath), - sdf(_private.sdf) -{ - if (_private.material) - { - this->material = std::make_unique(*(_private.material)); - } -} - ///////////////////////////////////////////////// ParticleEmitter::ParticleEmitter() - : dataPtr(new ParticleEmitterPrivate) -{ -} - -///////////////////////////////////////////////// -ParticleEmitter::~ParticleEmitter() -{ - delete this->dataPtr; - this->dataPtr = nullptr; -} - -///////////////////////////////////////////////// -ParticleEmitter::ParticleEmitter(const ParticleEmitter &_particleEmitter) - : dataPtr(new ParticleEmitterPrivate(*_particleEmitter.dataPtr)) -{ -} - -///////////////////////////////////////////////// -ParticleEmitter::ParticleEmitter(ParticleEmitter &&_particleEmitter) noexcept - : dataPtr(std::exchange(_particleEmitter.dataPtr, nullptr)) -{ -} - -///////////////////////////////////////////////// -ParticleEmitter &ParticleEmitter::operator=( - const ParticleEmitter &_particleEmitter) -{ - return *this = ParticleEmitter(_particleEmitter); -} - -///////////////////////////////////////////////// -ParticleEmitter &ParticleEmitter::operator=(ParticleEmitter &&_particleEmitter) + : dataPtr(ignition::utils::MakeImpl()) { - std::swap(this->dataPtr, _particleEmitter.dataPtr); - return *this; } ///////////////////////////////////////////////// @@ -274,7 +203,7 @@ Errors ParticleEmitter::Load(ElementPtr _sdf) if (_sdf->HasElement("material")) { - this->dataPtr->material.reset(new sdf::Material()); + this->dataPtr->material.emplace(); Errors err = this->dataPtr->material->Load(_sdf->GetElement("material")); errors.insert(errors.end(), err.begin(), err.end()); } @@ -528,15 +457,15 @@ sdf::ElementPtr ParticleEmitter::Element() const } ///////////////////////////////////////////////// -sdf::Material *ParticleEmitter::Material() const +const sdf::Material *ParticleEmitter::Material() const { - return this->dataPtr->material.get(); + return optionalToPointer(this->dataPtr->material); } ///////////////////////////////////////////////// void ParticleEmitter::SetMaterial(const sdf::Material &_material) { - this->dataPtr->material.reset(new sdf::Material(_material)); + this->dataPtr->material = _material; } ////////////////////////////////////////////////// @@ -559,7 +488,7 @@ void ParticleEmitter::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void ParticleEmitter::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/test/integration/particle_emitter_dom.cc b/test/integration/particle_emitter_dom.cc index 6071ac184..05dba54a3 100644 --- a/test/integration/particle_emitter_dom.cc +++ b/test/integration/particle_emitter_dom.cc @@ -79,14 +79,14 @@ TEST(DOMWorld, LoadParticleEmitter) EXPECT_DOUBLE_EQ(0.5, linkEmitter->ScaleRate()); EXPECT_DOUBLE_EQ(5, linkEmitter->Rate()); - sdf::Material *mat = linkEmitter->Material(); + const sdf::Material *mat = linkEmitter->Material(); ASSERT_NE(nullptr, mat); EXPECT_EQ(ignition::math::Color(0.7f, 0.7f, 0.7f), mat->Diffuse()); EXPECT_EQ(ignition::math::Color(1.0f, 1.0f, 1.0f), mat->Specular()); - sdf::Pbr *pbr = mat->PbrMaterial(); + const sdf::Pbr *pbr = mat->PbrMaterial(); ASSERT_NE(nullptr, pbr); - sdf::PbrWorkflow *metal = pbr->Workflow(sdf::PbrWorkflowType::METAL); + const sdf::PbrWorkflow *metal = pbr->Workflow(sdf::PbrWorkflowType::METAL); ASSERT_NE(nullptr, metal); EXPECT_EQ("materials/textures/fog.png", metal->AlbedoMap());