diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 277bab16a..cb66f564a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -12,8 +12,6 @@ jobs: - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@bionic - with: - codecov-enabled: true focal-ci: runs-on: ubuntu-latest name: Ubuntu Focal CI @@ -28,3 +26,5 @@ jobs: - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@focal + with: + codecov-enabled: true diff --git a/include/sdf/Actor.hh b/include/sdf/Actor.hh index a8a537b53..a199627ca 100644 --- a/include/sdf/Actor.hh +++ b/include/sdf/Actor.hh @@ -27,6 +27,7 @@ #include "sdf/Types.hh" #include "sdf/Link.hh" #include "sdf/Joint.hh" +#include "sdf/Plugin.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" @@ -380,9 +381,28 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// actor. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated actor values. public: sdf::ElementPtr ToElement() const; + /// \brief Get the plugins attached to this object. + /// \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 object. + /// \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 object. + /// \param[in] _plugin Plugin to add. + public: void AddPlugin(const Plugin &_plugin); + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/AirPressure.hh b/include/sdf/AirPressure.hh index 50c3650e4..649e0edfa 100644 --- a/include/sdf/AirPressure.hh +++ b/include/sdf/AirPressure.hh @@ -86,6 +86,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// air pressure sensor. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Altimeter.hh b/include/sdf/Altimeter.hh index 0394ec3ed..35bc72977 100644 --- a/include/sdf/Altimeter.hh +++ b/include/sdf/Altimeter.hh @@ -78,6 +78,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// altimeter sensor. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Atmosphere.hh b/include/sdf/Atmosphere.hh index aaa9d5683..fa43d9fa9 100644 --- a/include/sdf/Atmosphere.hh +++ b/include/sdf/Atmosphere.hh @@ -97,6 +97,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// atmosphere. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated atmosphere values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 2ba1ee9fa..4c614fd1a 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -67,6 +67,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// box. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated box values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index b766d4089..993baf783 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -95,6 +95,22 @@ namespace sdf /// \param[in] _name Name of the sensor. public: void SetName(const std::string &_name); + /// \brief Get whether the camera is triggered by a topic. + /// \return True if the camera is triggered by a topic. + public: bool Triggered() const; + + /// \brief Set whether the camera should be triggered by a topic. + /// \param[in] _triggered True if the camera should be triggered by a topic. + public: void SetTriggered(bool _triggered); + + /// \brief Get the topic that will trigger the camera. + /// \return Topic for the camera trigger. + public: std::string TriggerTopic() const; + + /// \brief Set the topic that will trigger the camera. + /// \param[in] _triggerTopic Topic for the camera trigger. + public: void SetTriggerTopic(const std::string &_triggerTopic); + /// \brief Get the horizontal field of view in radians. /// \return The horizontal field of view in radians. public: ignition::math::Angle HorizontalFov() const; @@ -474,6 +490,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// camera. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated camera values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index 56ffbe6b3..045f68580 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -74,6 +74,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// capsule. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated capsule values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 5a12eede2..318742ead 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -117,6 +117,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// collision. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated collision values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 606399b21..f62189c4c 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -74,6 +74,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// cylinder. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated cylinder values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index fa18e9eed..cf8dbdad9 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -66,6 +66,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// ellipsoid. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated ellipsoid values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh index 97fafdee8..b47b9a342 100644 --- a/include/sdf/ForceTorque.hh +++ b/include/sdf/ForceTorque.hh @@ -156,6 +156,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// force torque sensor. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index b3d0a3b02..a6e444575 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -191,6 +191,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// geometry. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated geometry values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Gui.hh b/include/sdf/Gui.hh index 35f9f0b6a..bfc80a23f 100644 --- a/include/sdf/Gui.hh +++ b/include/sdf/Gui.hh @@ -64,6 +64,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// gui. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated gui values. public: sdf::ElementPtr ToElement() const; @@ -81,10 +83,20 @@ namespace sdf /// \brief Remove all plugins public: void ClearPlugins(); - /// \brief Add a plugin to the link. + /// \brief Add a plugin to this object. /// \param[in] _plugin Plugin to add. public: void AddPlugin(const Plugin &_plugin); + /// \brief Get the plugins attached to this object. + /// \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 object. + /// \return A vector of Plugin, which will be empty if there are no + /// plugins. + public: sdf::Plugins &Plugins(); + /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Heightmap.hh b/include/sdf/Heightmap.hh index a08a1706d..9af6064d8 100644 --- a/include/sdf/Heightmap.hh +++ b/include/sdf/Heightmap.hh @@ -216,6 +216,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// heightmap. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated heightmap values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index f530ffaaa..8801f4af2 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -255,6 +255,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// imu sensor. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index fdd59502e..f0567ad1d 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -227,6 +227,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// joint. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated joint values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index b8d41fb97..2e5e96cf7 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -223,6 +223,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// joint axis. + /// Note that parameter passing functionality is not captured with this + /// function. /// \param[in] _index Index of this joint axis /// \return SDF element pointer with updated joint values. public: sdf::ElementPtr ToElement(unsigned int _index = 0u) const; diff --git a/include/sdf/Lidar.hh b/include/sdf/Lidar.hh index 0beff0a1a..41ce9ebfd 100644 --- a/include/sdf/Lidar.hh +++ b/include/sdf/Lidar.hh @@ -234,6 +234,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// lidar. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index 281b051f5..4de83ce3a 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -126,6 +126,14 @@ namespace sdf /// \param[in] _cast True to indicate that the light casts shadows. public: void SetCastShadows(const bool _cast); + /// \brief Get if the light is on + /// \return True if the light is on. + public: bool LightOn() const; + + /// \brief Set if the light is ON/OFF + /// \param[in] _cast True to indicate that the light is on, False otherwise. + public: void SetLightOn(const bool _isLightOn); + /// \brief Get the light intensity /// \return The light intensity public: double Intensity() const; @@ -270,6 +278,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// light object. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated light values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 87964378f..bc86b1bc0 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -364,6 +364,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// link. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated link values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Magnetometer.hh b/include/sdf/Magnetometer.hh index 213d93207..3c469828d 100644 --- a/include/sdf/Magnetometer.hh +++ b/include/sdf/Magnetometer.hh @@ -87,6 +87,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// magnetometer. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index 68760de4f..b544574a0 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -195,6 +195,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// material. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated material values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 7636534d3..46a99884a 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -99,6 +99,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// mesh. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated mesh values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 0bb3f5adb..c24749a73 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -24,6 +24,7 @@ #include #include #include "sdf/Element.hh" +#include "sdf/Plugin.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -417,6 +418,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// model. + /// Note that parameter passing functionality is not captured with this + /// function. /// \param[in] _useIncludeTag When true, the model's URI is used to create /// an SDF `` rather than a ``. The model's URI must be /// first set using the `Model::SetUri` function. If the model's URI is @@ -428,6 +431,18 @@ namespace sdf /// \return SDF element pointer with updated model values. public: sdf::ElementPtr ToElement(bool _useIncludeTag = true) const; + /// \brief Check if a given name exists in the FrameAttachedTo graph at the + /// scope of the model. + /// \param[in] _name Name of the implicit or explicit frame to check. + /// To check for a frame in a nested model, prefix the frame name with + /// the sequence of nested models containing this frame, delimited by "::". + /// \return True if the frame name is found in the FrameAttachedTo graph. + /// False otherwise, or if the frame graph is invalid. + /// \note This function assumes the model has a valid FrameAttachedTo graph. + /// It will return false if the graph is invalid. + public: bool NameExistsInFrameAttachedToGraph( + const std::string &_name) const; + /// \brief Add a link to the model. /// \param[in] _link Link to add. /// \return True if successful, false if a link with the name already @@ -472,6 +487,23 @@ namespace sdf /// \param[in] _uri The model's URI. public: void SetUri(const std::string &_uri); + /// \brief Get the plugins attached to this object. + /// \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 object. + /// \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 object. + /// \param[in] _plugin Plugin to add. + public: void AddPlugin(const Plugin &_plugin); + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Root::Load or /// World::SetPoseRelativeToGraph if this is a standalone model and diff --git a/include/sdf/Noise.hh b/include/sdf/Noise.hh index 4c260d1d6..0b8756992 100644 --- a/include/sdf/Noise.hh +++ b/include/sdf/Noise.hh @@ -164,6 +164,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// noise. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated noise values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh index 95b68b202..37a8dfb9a 100644 --- a/include/sdf/ParticleEmitter.hh +++ b/include/sdf/ParticleEmitter.hh @@ -318,6 +318,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// particle emitter. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated particle emitter values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Physics.hh b/include/sdf/Physics.hh index c065cad66..931eaf182 100644 --- a/include/sdf/Physics.hh +++ b/include/sdf/Physics.hh @@ -116,6 +116,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// physics. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated physics values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Plane.hh b/include/sdf/Plane.hh index e02de22e1..3ba3d1b6e 100644 --- a/include/sdf/Plane.hh +++ b/include/sdf/Plane.hh @@ -82,6 +82,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// plane. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated plane values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Plugin.hh b/include/sdf/Plugin.hh index 2026b8ae9..56b33abea 100644 --- a/include/sdf/Plugin.hh +++ b/include/sdf/Plugin.hh @@ -23,6 +23,7 @@ #include #include +#include #include #include "sdf/sdf_config.h" #include "sdf/system_util.hh" @@ -109,6 +110,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// plugin. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated plugin values. public: sdf::ElementPtr ToElement() const; @@ -122,9 +125,44 @@ namespace sdf /// \return A reference to this plugin public: Plugin &operator=(Plugin &&_plugin) noexcept; + /// \brief Output stream operator for a Plugin. + /// \param[in] _out The output stream + /// \param[in] _plugin Plugin to output + public: friend std::ostream &operator<<(std::ostream& _out, + const sdf::Plugin &_plugin) + { + return _out << _plugin.ToElement()->ToString(""); + } + + /// \brief Input stream operator for a Plugin. + /// \param[in] _out The output stream + /// \param[in] _plugin Plugin to output + public: friend std::istream &operator>>(std::istream &_in, + sdf::Plugin &_plugin) + { + std::ostringstream stream; + stream << ""; + stream << std::string(std::istreambuf_iterator(_in), {}); + stream << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + bool result = sdf::readString(stream.str(), sdfParsed); + if (!result) + return _in; + + _plugin.ClearContents(); + _plugin.Load(sdfParsed->Root()->GetFirstElement()); + + return _in; + } + /// \brief Private data pointer. std::unique_ptr dataPtr; }; + + /// \brief A vector of Plugin. + using Plugins = std::vector; } } diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 9d6a2b324..65d63ae71 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -142,16 +142,31 @@ namespace sdf /// \return A pointer to the model, nullptr if it doesn't exist public: const sdf::Model *Model() const; + /// \brief Set the model object. This will override any existing model, + /// actor, and light object. + /// \param[in] _model The model to use. + public: void SetModel(const sdf::Model &_model); + /// \brief Get a pointer to the light object if it exists. /// /// \return A pointer to the light, nullptr if it doesn't exist public: const sdf::Light *Light() const; + /// \brief Set the light object. This will override any existing model, + /// actor, and light object. + /// \param[in] _light The light to use. + public: void SetLight(const sdf::Light &_light); + /// \brief Get a pointer to the actor object if it exists. /// /// \return A pointer to the actor, nullptr if it doesn't exist public: const sdf::Actor *Actor() const; + /// \brief Set the actor object. This will override any existing model, + /// actor, and light object. + /// \param[in] _actor The actor to use. + public: void SetActor(const sdf::Actor &_actor); + /// \brief Get a pointer to the SDF element that was generated during /// load. /// \return SDF element pointer. The value will be nullptr if Load has @@ -183,6 +198,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors UpdateGraphs(); + /// \brief Create and return an SDF element filled with data from this + /// root. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[in] _useIncludeTag This will pass the _useIncludeTag to + /// sdf::Model::ToElement. + /// \return SDF element pointer with updated root values. + public: sdf::ElementPtr ToElement(bool _useIncludeTag = true) const; + /// \brief Private data pointer IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index c4a09106e..1862f1ed5 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -99,6 +99,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// scene. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated scene values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 92b0c9cd0..6ee21ea6e 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -22,6 +22,7 @@ #include #include #include "sdf/Element.hh" +#include "sdf/Plugin.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -208,6 +209,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// sensor. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; @@ -398,6 +401,23 @@ namespace sdf /// \param[in] _lidar The lidar sensor. public: void SetLidarSensor(const Lidar &_lidar); + /// \brief Get the plugins attached to this object. + /// \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 object. + /// \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 object. + /// \param[in] _plugin Plugin to add. + public: void AddPlugin(const Plugin &_plugin); + /// \brief Give the name of the xml parent of this object, to be used /// for resolving poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. diff --git a/include/sdf/Sky.hh b/include/sdf/Sky.hh index e84693618..f9679f0e3 100644 --- a/include/sdf/Sky.hh +++ b/include/sdf/Sky.hh @@ -115,6 +115,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// sky. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sky values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 42f8da4c5..f60ee6e9a 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -67,6 +67,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// sphere. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated sphere values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 6312ffbd3..4eb335692 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -89,6 +89,8 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// surface. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated surface values. public: sdf::ElementPtr ToElement() const; diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index a3f00fb58..9875f5d7d 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -26,6 +26,7 @@ #include "sdf/Element.hh" #include "sdf/Material.hh" #include "sdf/Plane.hh" +#include "sdf/Plugin.hh" #include "sdf/SemanticPose.hh" #include "sdf/Sphere.hh" #include "sdf/Types.hh" @@ -162,9 +163,28 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// visual. + /// Note that parameter passing functionality is not captured with this + /// function. /// \return SDF element pointer with updated visual values. public: sdf::ElementPtr ToElement() const; + /// \brief Get the plugins attached to this object. + /// \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 object. + /// \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 object. + /// \param[in] _plugin Plugin to add. + public: void AddPlugin(const Plugin &_plugin); + /// \brief Give the name of the xml parent of this object, to be used /// for resolving poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 22a78ac9e..c3149b65e 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -27,6 +27,7 @@ #include "sdf/Atmosphere.hh" #include "sdf/Element.hh" #include "sdf/Gui.hh" +#include "sdf/Plugin.hh" #include "sdf/Scene.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -428,8 +429,29 @@ namespace sdf /// \brief Create and return an SDF element filled with data from this /// world. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[in] _useIncludeTag This parameter is passed through to the + /// Model::ToElement function. /// \return SDF element pointer with updated world values. - public: sdf::ElementPtr ToElement() const; + public: sdf::ElementPtr ToElement(bool _useIncludeTag = true) const; + + /// \brief Get the plugins attached to this object. + /// \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 object. + /// \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 object. + /// \param[in] _plugin Plugin to add. + public: void AddPlugin(const Plugin &_plugin); /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child /// entities for resolving poses. This is private and is intended to be diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index d5f9f5165..dae956840 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -444,6 +444,13 @@ namespace sdf /// \return True if the element should be validated SDFORMAT_VISIBLE bool shouldValidateElement(sdf::ElementPtr _elem); + + /// \brief Function to compute a merged model's proxy frame name + /// + /// \param [in] _modelName The merged model's name + /// \return The computed frame name + SDFORMAT_VISIBLE + std::string computeMergedModelProxyFrameName(const std::string &_modelName); } } #endif diff --git a/sdf/1.8/light.sdf b/sdf/1.8/light.sdf index c9e1f237b..a6939ac48 100644 --- a/sdf/1.8/light.sdf +++ b/sdf/1.8/light.sdf @@ -14,6 +14,10 @@ When true, the light will cast shadows. + + When true, the light is on. + + Scale factor to set the relative power of a light. diff --git a/sdf/1.9/camera.sdf b/sdf/1.9/camera.sdf index 67633cca2..c3cf5c5ab 100644 --- a/sdf/1.9/camera.sdf +++ b/sdf/1.9/camera.sdf @@ -5,6 +5,14 @@ An optional name for the camera. + + If the camera will be triggered by a topic + + + + Name of the topic that will trigger the camera if enabled + + Horizontal field of view diff --git a/sdf/1.9/light.sdf b/sdf/1.9/light.sdf index c9e1f237b..a6939ac48 100644 --- a/sdf/1.9/light.sdf +++ b/sdf/1.9/light.sdf @@ -14,6 +14,10 @@ When true, the light will cast shadows. + + When true, the light is on. + + Scale factor to set the relative power of a light. diff --git a/src/Actor.cc b/src/Actor.cc index 35872bf56..190092634 100644 --- a/src/Actor.cc +++ b/src/Actor.cc @@ -20,6 +20,7 @@ #include "sdf/Actor.hh" #include "sdf/Error.hh" #include "sdf/parser.hh" +#include "sdf/Plugin.hh" #include "Utils.hh" using namespace sdf; @@ -113,6 +114,9 @@ class sdf::Actor::Implementation /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; + + /// \brief Actor plugins. + public: std::vector plugins; }; ///////////////////////////////////////////////// @@ -454,6 +458,11 @@ Errors Actor::Load(ElementPtr _sdf) errors.insert(errors.end(), jointLoadErrors.begin(), jointLoadErrors.end()); + // Load the actor plugins + Errors pluginErrors = loadRepeated(_sdf, "plugin", + this->dataPtr->plugins); + errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); + return errors; } @@ -780,5 +789,33 @@ sdf::ElementPtr Actor::ToElement() const for (const sdf::Link &link : this->dataPtr->links) elem->InsertElement(link.ToElement(), true); + // Add in the plugins + for (const Plugin &plugin : this->dataPtr->plugins) + elem->InsertElement(plugin.ToElement(), true); + return elem; } + +///////////////////////////////////////////////// +const sdf::Plugins &Actor::Plugins() const +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +sdf::Plugins &Actor::Plugins() +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +void Actor::ClearPlugins() +{ + this->dataPtr->plugins.clear(); +} + +///////////////////////////////////////////////// +void Actor::AddPlugin(const Plugin &_plugin) +{ + this->dataPtr->plugins.push_back(_plugin); +} diff --git a/src/Actor_TEST.cc b/src/Actor_TEST.cc index 1e676ad0e..2a48c7959 100644 --- a/src/Actor_TEST.cc +++ b/src/Actor_TEST.cc @@ -18,6 +18,7 @@ #include #include #include "sdf/Actor.hh" +#include "sdf/Plugin.hh" ///////////////////////////////////////////////// sdf::Animation CreateDummyAnimation() @@ -561,6 +562,11 @@ TEST(DOMActor, ToElement) animation.SetScale(1.2); animation.SetInterpolateX(true); + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + actor.AddPlugin(plugin); + sdf::ElementPtr elem = actor.ToElement(); ASSERT_NE(nullptr, elem); @@ -598,4 +604,32 @@ TEST(DOMActor, ToElement) ASSERT_NE(nullptr, waypointB2); EXPECT_DOUBLE_EQ(waypointB.Time(), waypointB2->Time()); EXPECT_EQ(waypointB.Pose(), waypointB2->Pose()); + + ASSERT_EQ(1u, actor2.Plugins().size()); + EXPECT_EQ("name1", actor2.Plugins()[0].Name()); + EXPECT_EQ("filename1", actor2.Plugins()[0].Filename()); +} + +///////////////////////////////////////////////// +TEST(DOMActor, Plugins) +{ + sdf::Actor actor; + EXPECT_TRUE(actor.Plugins().empty()); + + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + + actor.AddPlugin(plugin); + ASSERT_EQ(1u, actor.Plugins().size()); + + plugin.SetName("name2"); + actor.AddPlugin(plugin); + ASSERT_EQ(2u, actor.Plugins().size()); + + EXPECT_EQ("name1", actor.Plugins()[0].Name()); + EXPECT_EQ("name2", actor.Plugins()[1].Name()); + + actor.ClearPlugins(); + EXPECT_TRUE(actor.Plugins().empty()); } diff --git a/src/Camera.cc b/src/Camera.cc index 52c2705cf..7a606d64f 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -55,6 +55,12 @@ class sdf::Camera::Implementation /// \brief Name of the camera. public: std::string name = ""; + /// \brief True if the camera is triggered by a topic. + public: bool triggered{false}; + + /// \brief Camera trigger topic. + public: std::string triggerTopic = ""; + /// \brief Horizontal fied of view. public: ignition::math::Angle hfov{1.047}; @@ -216,6 +222,12 @@ Errors Camera::Load(ElementPtr _sdf) // Read the camera's name loadName(_sdf, this->dataPtr->name); + this->dataPtr->triggered = _sdf->Get("triggered", + this->dataPtr->triggered).first; + + this->dataPtr->triggerTopic = _sdf->Get("trigger_topic", + this->dataPtr->triggerTopic).first; + this->dataPtr->hfov = _sdf->Get("horizontal_fov", this->dataPtr->hfov).first; @@ -402,6 +414,32 @@ void Camera::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +bool Camera::Triggered() const +{ + return this->dataPtr->triggered; +} + + +///////////////////////////////////////////////// +void Camera::SetTriggered(bool _triggered) +{ + this->dataPtr->triggered = _triggered; +} + +///////////////////////////////////////////////// +std::string Camera::TriggerTopic() const +{ + return this->dataPtr->triggerTopic; +} + + +///////////////////////////////////////////////// +void Camera::SetTriggerTopic(const std::string &_triggerTopic) +{ + this->dataPtr->triggerTopic = _triggerTopic; +} + ///////////////////////////////////////////////// ignition::math::Angle Camera::HorizontalFov() const { diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index ff2b74614..2e00db31e 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -26,6 +26,14 @@ TEST(DOMCamera, Construction) cam.SetName("my_camera"); EXPECT_EQ("my_camera", cam.Name()); + EXPECT_FALSE(cam.Triggered()); + cam.SetTriggered(true); + EXPECT_TRUE(cam.Triggered()); + + EXPECT_TRUE(cam.TriggerTopic().empty()); + cam.SetTriggerTopic("my_camera/trigger"); + EXPECT_EQ("my_camera/trigger", cam.TriggerTopic()); + EXPECT_DOUBLE_EQ(1.047, cam.HorizontalFov().Radian()); cam.SetHorizontalFov(1.45); EXPECT_DOUBLE_EQ(1.45, cam.HorizontalFov().Radian()); diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 47270bf3d..7bd475a30 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -514,7 +514,8 @@ struct ModelWrapper : public WrapperBase : WrapperBase{_ifaceModel.Name(), "Interface Model", _ifaceModel.Static() ? FrameType::STATIC_MODEL : FrameType::MODEL}, - rawPose(_ifaceModel.ModelFramePoseInParentFrame()), + rawPose(_nestedInclude.IncludeRawPose().value_or( + _ifaceModel.ModelFramePoseInParentFrame())), rawRelativeTo(_nestedInclude.IncludePoseRelativeTo().value_or("")), relativeTo(rawRelativeTo), canonicalLinkName(_ifaceModel.CanonicalLinkName()), diff --git a/src/Gui.cc b/src/Gui.cc index dc0a9e3e0..1ced2795f 100644 --- a/src/Gui.cc +++ b/src/Gui.cc @@ -133,3 +133,17 @@ void Gui::AddPlugin(const Plugin &_plugin) { this->dataPtr->plugins.push_back(_plugin); } + +///////////////////////////////////////////////// +const sdf::Plugins &Gui::Plugins() const +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +sdf::Plugins &Gui::Plugins() +{ + return this->dataPtr->plugins; +} + + diff --git a/src/Gui_TEST.cc b/src/Gui_TEST.cc index f068d5034..c7c9e77d7 100644 --- a/src/Gui_TEST.cc +++ b/src/Gui_TEST.cc @@ -144,8 +144,10 @@ TEST(DOMGui, ToElement) if (j == 0) { EXPECT_EQ(6u, gui.PluginCount()); + EXPECT_EQ(6u, gui.Plugins().size()); gui.ClearPlugins(); EXPECT_EQ(0u, gui.PluginCount()); + EXPECT_TRUE(gui.Plugins().empty()); } } diff --git a/src/Light.cc b/src/Light.cc index e2c8f6b0e..4551beb47 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -84,6 +84,9 @@ class sdf::Light::Implementation /// \brief Spot light falloff. public: double spotFalloff = 0.0; + + /// \brief Is light on ? + public: bool isLightOn = true; }; ///////////////////////////////////////////////// @@ -142,6 +145,9 @@ Errors Light::Load(ElementPtr _sdf) // Load the pose. Ignore the return value since the light pose is optional. loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); + this->dataPtr->isLightOn = _sdf->Get("light_on", + this->dataPtr->isLightOn).first; + this->dataPtr->castShadows = _sdf->Get("cast_shadows", this->dataPtr->castShadows).first; @@ -317,6 +323,18 @@ void Light::SetCastShadows(const bool _cast) this->dataPtr->castShadows = _cast; } +///////////////////////////////////////////////// +bool Light::LightOn() const +{ + return this->dataPtr->isLightOn; +} + +///////////////////////////////////////////////// +void Light::SetLightOn(const bool _isLightOn) +{ + this->dataPtr->isLightOn = _isLightOn; +} + ///////////////////////////////////////////////// ignition::math::Color Light::Diffuse() const { diff --git a/src/Light_TEST.cc b/src/Light_TEST.cc index 3ea74177c..b79ab85ac 100644 --- a/src/Light_TEST.cc +++ b/src/Light_TEST.cc @@ -55,6 +55,10 @@ TEST(DOMLight, DefaultConstruction) EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } + EXPECT_TRUE(light.LightOn()); + light.SetLightOn(false); + EXPECT_FALSE(light.LightOn()); + EXPECT_FALSE(light.CastShadows()); light.SetCastShadows(true); EXPECT_TRUE(light.CastShadows()); @@ -113,6 +117,7 @@ TEST(DOMLight, CopyConstructor) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); + light.SetLightOn(false); light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); @@ -131,6 +136,7 @@ TEST(DOMLight, CopyConstructor) EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); + EXPECT_FALSE(light2.LightOn()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); @@ -153,6 +159,7 @@ TEST(DOMLight, CopyAssignmentOperator) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); + light.SetLightOn(false); light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); @@ -172,6 +179,7 @@ TEST(DOMLight, CopyAssignmentOperator) EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); + EXPECT_FALSE(light2.LightOn()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); @@ -194,6 +202,7 @@ TEST(DOMLight, MoveConstructor) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); + light.SetLightOn(false); light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); @@ -212,6 +221,7 @@ TEST(DOMLight, MoveConstructor) EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); + EXPECT_FALSE(light2.LightOn()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); @@ -234,6 +244,7 @@ TEST(DOMLight, MoveAssignment) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); + light.SetLightOn(false); light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); @@ -253,6 +264,7 @@ TEST(DOMLight, MoveAssignment) EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); + EXPECT_FALSE(light2.LightOn()); EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); diff --git a/src/Model.cc b/src/Model.cc index 8d230e05f..a6337ba00 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -106,6 +106,26 @@ class sdf::Model::Implementation /// \brief Optional URI string that specifies where this model was or /// can be loaded from. public: std::string uri = ""; + + /// \brief All of the model plugins. + public: std::vector plugins; + + /// \brief The model plugins that were specified only in an tag. + /// This data structure is used by the ToElement() function to accurately + /// reproduce the tag. + /// + /// For example, a world could be: + /// + /// + /// test_model_with_plugin + /// + /// + /// + /// + /// and the included model could also have a plugin. We want the + /// ToElement(true) function to output only the plugin specified in the + /// tag. + public: std::vector includePlugins; }; ///////////////////////////////////////////////// @@ -379,6 +399,23 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) frameNames.insert(frameName); } + // Load the model plugins + Errors pluginErrors = loadRepeated(_sdf, "plugin", + this->dataPtr->plugins); + errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); + + // Check whether the model was loaded from an tag. If so, set + // the URI and capture the plugins. + if (_sdf->GetIncludeElement() && _sdf->GetIncludeElement()->HasElement("uri")) + { + sdf::ElementPtr includeElem = _sdf->GetIncludeElement(); + this->SetUri(includeElem->Get("uri")); + + Errors includePluginErrors = loadRepeated(includeElem, "plugin", + this->dataPtr->includePlugins); + errors.insert(errors.end(), includePluginErrors.begin(), + includePluginErrors.end()); + } return errors; } @@ -954,7 +991,15 @@ sdf::ElementPtr Model::ToElement(bool _useIncludeTag) const "relative_to")->Set(this->dataPtr->poseRelativeTo); } includeElem->GetElement("static")->Set(this->Static()); - includeElem->GetElement("placement_frame")->Set(this->PlacementFrameName()); + if (!this->dataPtr->placementFrameName.empty()) + { + includeElem->GetElement("placement_frame")->Set( + this->PlacementFrameName()); + } + + // Output the plugins + for (const Plugin &plugin : this->dataPtr->includePlugins) + includeElem->InsertElement(plugin.ToElement(), true); return includeElem; } @@ -1000,6 +1045,10 @@ sdf::ElementPtr Model::ToElement(bool _useIncludeTag) const for (const sdf::Model &model : this->dataPtr->models) elem->InsertElement(model.ToElement(_useIncludeTag), true); + // Add in the plugins + for (const Plugin &plugin : this->dataPtr->plugins) + elem->InsertElement(plugin.ToElement(), true); + return elem; } @@ -1062,3 +1111,37 @@ void Model::ClearFrames() { this->dataPtr->frames.clear(); } + +///////////////////////////////////////////////// +bool Model::NameExistsInFrameAttachedToGraph(const std::string &_name) const +{ + if (!this->dataPtr->frameAttachedToGraph) + return false; + + return this->dataPtr->frameAttachedToGraph.VertexIdByName(sdf::JoinName( + this->Name(), _name)) != ignition::math::graph::kNullId; +} + +///////////////////////////////////////////////// +const sdf::Plugins &Model::Plugins() const +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +sdf::Plugins &Model::Plugins() +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +void Model::ClearPlugins() +{ + this->dataPtr->plugins.clear(); +} + +///////////////////////////////////////////////// +void Model::AddPlugin(const Plugin &_plugin) +{ + this->dataPtr->plugins.push_back(_plugin); +} diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 3b84f7910..d619212d2 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -373,6 +373,11 @@ TEST(DOMModel, ToElement) model.ClearModels(); } + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + model.AddPlugin(plugin); + sdf::ElementPtr elem = model.ToElement(); ASSERT_NE(nullptr, elem); @@ -399,6 +404,10 @@ TEST(DOMModel, ToElement) EXPECT_EQ(model.ModelCount(), model2.ModelCount()); for (uint64_t i = 0; i < model2.ModelCount(); ++i) EXPECT_NE(nullptr, model2.ModelByIndex(i)); + + ASSERT_EQ(1u, model2.Plugins().size()); + EXPECT_EQ("name1", model2.Plugins()[0].Name()); + EXPECT_EQ("filename1", model2.Plugins()[0].Filename()); } ///////////////////////////////////////////////// @@ -610,3 +619,27 @@ TEST(DOMModel, MutableByName) EXPECT_FALSE(model.FrameNameExists("frame1")); EXPECT_TRUE(model.FrameNameExists("frame2")); } + +///////////////////////////////////////////////// +TEST(DOMModel, Plugins) +{ + sdf::Model model; + EXPECT_TRUE(model.Plugins().empty()); + + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + + model.AddPlugin(plugin); + ASSERT_EQ(1u, model.Plugins().size()); + + plugin.SetName("name2"); + model.AddPlugin(plugin); + ASSERT_EQ(2u, model.Plugins().size()); + + EXPECT_EQ("name1", model.Plugins()[0].Name()); + EXPECT_EQ("name2", model.Plugins()[1].Name()); + + model.ClearPlugins(); + EXPECT_TRUE(model.Plugins().empty()); +} diff --git a/src/ParamPassing.cc b/src/ParamPassing.cc index 2e56dca73..82bf01b02 100644 --- a/src/ParamPassing.cc +++ b/src/ParamPassing.cc @@ -466,8 +466,7 @@ void handleIndividualChildActions(const ParserConfig &_config, if (actionStr == "add") { - _elem->InsertElement(elemChild); - elemChild->SetParent(_elem); + _elem->InsertElement(elemChild->Clone(), true); } else if (actionStr == "replace") { @@ -511,8 +510,7 @@ void add(const ParserConfig &_config, const std::string &_source, if (xmlToSdf(_config, _source, _childXml, newElem, _errors)) { - _elem->InsertElement(newElem); - newElem->SetParent(_elem); + _elem->InsertElement(newElem, true); } else { diff --git a/src/Plugin_TEST.cc b/src/Plugin_TEST.cc index ca77bcede..0a0dd6e20 100644 --- a/src/Plugin_TEST.cc +++ b/src/Plugin_TEST.cc @@ -262,3 +262,59 @@ TEST(DOMPlugin, ToElement) EXPECT_EQ(1u, plugin2.Contents().size()); EXPECT_EQ("an-element", plugin2.Contents()[0]->GetName()); } + +///////////////////////////////////////////////// +TEST(DOMPlugin, OutputStreamOperator) +{ + sdf::Plugin plugin; + plugin.SetName("my-plugin"); + EXPECT_EQ("my-plugin", plugin.Name()); + + plugin.SetFilename("filename.so"); + EXPECT_EQ("filename.so", plugin.Filename()); + + sdf::ElementPtr content(new sdf::Element); + content->SetName("an-element"); + plugin.InsertContent(content); + EXPECT_EQ(1u, plugin.Contents().size()); + + sdf::ElementPtr elem = plugin.ToElement(); + ASSERT_NE(nullptr, elem); + std::string elemString = elem->ToString(""); + + std::ostringstream stream; + stream << plugin; + + EXPECT_EQ(elemString, stream.str()); + + // The expected plugin output string. + std::string expected = R"foo( + + +)foo"; + + EXPECT_EQ(expected, stream.str()); +} + +///////////////////////////////////////////////// +TEST(DOMPlugin, InputStreamOperator) +{ + // The provided plugin input string. + std::string input = R"foo( + + +)foo"; + std::istringstream stream(input); + + sdf::Plugin plugin; + stream >> plugin; + + EXPECT_EQ("my-plugin", plugin.Name()); + EXPECT_EQ("filename.so", plugin.Filename()); + EXPECT_EQ(1u, plugin.Contents().size()); + + sdf::ElementPtr elem = plugin.ToElement(); + ASSERT_NE(nullptr, elem); + std::string elemString = elem->ToString(""); + EXPECT_EQ(input, elemString); +} diff --git a/src/Root.cc b/src/Root.cc index d3ab5b2fc..9b0e4061b 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -48,7 +48,7 @@ class sdf::Root::Implementation public: void UpdateGraphs(sdf::Model &_model, sdf::Errors &_errors); /// \brief Version string - public: std::string version = ""; + public: std::string version = SDF_VERSION; /// \brief The worlds specified under the root SDF element public: std::vector worlds; @@ -210,7 +210,7 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) // Get the SDF version. std::pair versionPair = - this->dataPtr->sdf->Get("version", SDF_VERSION); + this->dataPtr->sdf->Get("version", this->dataPtr->version); // Check that the version exists. Exit if the version is missing. // readFile will fail if the version is missing, so this @@ -402,18 +402,37 @@ const Model *Root::Model() const return std::get_if(&this->dataPtr->modelLightOrActor); } +///////////////////////////////////////////////// +void Root::SetModel(const sdf::Model &_model) +{ + this->dataPtr->modelLightOrActor = _model; +} + ///////////////////////////////////////////////// const Light *Root::Light() const { return std::get_if(&this->dataPtr->modelLightOrActor); } + +///////////////////////////////////////////////// +void Root::SetLight(const sdf::Light &_light) +{ + this->dataPtr->modelLightOrActor = _light; +} + ///////////////////////////////////////////////// const Actor *Root::Actor() const { return std::get_if(&this->dataPtr->modelLightOrActor); } +///////////////////////////////////////////////// +void Root::SetActor(const sdf::Actor &_actor) +{ + this->dataPtr->modelLightOrActor = _actor; +} + ///////////////////////////////////////////////// sdf::ElementPtr Root::Element() const { @@ -504,3 +523,33 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model, this->modelPoseRelativeToGraph = createPoseRelativeToGraph(_model, _errors); _model.SetPoseRelativeToGraph(this->modelPoseRelativeToGraph); } + +///////////////////////////////////////////////// +sdf::ElementPtr Root::ToElement(bool _useIncludeTag) const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("root.sdf", elem); + + elem->GetAttribute("version")->Set(this->Version()); + + if (this->Model() != nullptr) + { + elem->InsertElement(this->Model()->ToElement(_useIncludeTag), true); + } + else if (this->Light() != nullptr) + { + elem->InsertElement(this->Light()->ToElement(), true); + } + else if (this->Actor() != nullptr) + { + elem->InsertElement(this->Actor()->ToElement(), true); + } + else + { + // Worlds + for (const sdf::World &world : this->dataPtr->worlds) + elem->InsertElement(world.ToElement(_useIncludeTag), true); + } + + return elem; +} diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 4be950e90..17dbf9486 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -32,7 +32,7 @@ TEST(DOMRoot, Construction) { sdf::Root root; EXPECT_EQ(nullptr, root.Element()); - EXPECT_EQ("", root.Version()); + EXPECT_EQ(SDF_VERSION, root.Version()); EXPECT_FALSE(root.WorldNameExists("default")); EXPECT_FALSE(root.WorldNameExists("")); EXPECT_EQ(0u, root.WorldCount()); @@ -203,7 +203,7 @@ TEST(DOMRoot, StringActorSdfParse) TEST(DOMRoot, Set) { sdf::Root root; - EXPECT_STREQ("", root.Version().c_str()); + EXPECT_STREQ(SDF_VERSION, root.Version().c_str()); root.SetVersion(SDF_PROTOCOL_VERSION); EXPECT_STREQ(SDF_PROTOCOL_VERSION, root.Version().c_str()); } @@ -339,3 +339,166 @@ TEST(DOMRoot, MutableByIndex) w->SetName("world2"); EXPECT_EQ("world2", root.WorldByIndex(0)->Name()); } + +///////////////////////////////////////////////// +TEST(DOMRoot, ToElementEmpty) +{ + sdf::Root root; + + sdf::ElementPtr elem = root.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Root root2; + root2.LoadSdfString(elem->ToString("")); + + EXPECT_EQ(SDF_VERSION, root2.Version()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, ToElementModel) +{ + sdf::Root root; + + sdf::Actor actor1; + actor1.SetName("actor1"); + root.SetActor(actor1); + + sdf::Light light1; + light1.SetName("light1"); + root.SetLight(light1); + + sdf::Model model1; + model1.SetName("model1"); + root.SetModel(model1); + + ASSERT_NE(nullptr, root.Model()); + ASSERT_EQ(nullptr, root.Light()); + ASSERT_EQ(nullptr, root.Actor()); + EXPECT_EQ(0u, root.WorldCount()); + + // Convert to sdf::ElementPtr + sdf::ElementPtr elem = root.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Root root2; + root2.LoadSdfString(elem->ToString("")); + + EXPECT_EQ(SDF_VERSION, root2.Version()); + + ASSERT_NE(nullptr, root2.Model()); + EXPECT_EQ("model1", root2.Model()->Name()); + + ASSERT_EQ(nullptr, root2.Actor()); + ASSERT_EQ(nullptr, root2.Light()); + EXPECT_EQ(0u, root2.WorldCount()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, ToElementLight) +{ + sdf::Root root; + + sdf::Model model1; + model1.SetName("model1"); + root.SetModel(model1); + + sdf::Actor actor1; + actor1.SetName("actor1"); + root.SetActor(actor1); + + sdf::Light light1; + light1.SetName("light1"); + root.SetLight(light1); + + ASSERT_NE(nullptr, root.Light()); + ASSERT_EQ(nullptr, root.Model()); + ASSERT_EQ(nullptr, root.Actor()); + EXPECT_EQ(0u, root.WorldCount()); + + // Convert to sdf::ElementPtr + sdf::ElementPtr elem = root.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Root root2; + root2.LoadSdfString(elem->ToString("")); + + EXPECT_EQ(SDF_VERSION, root2.Version()); + + ASSERT_NE(nullptr, root2.Light()); + EXPECT_EQ("light1", root2.Light()->Name()); + + ASSERT_EQ(nullptr, root2.Model()); + ASSERT_EQ(nullptr, root2.Actor()); + EXPECT_EQ(0u, root2.WorldCount()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, ToElementActor) +{ + sdf::Root root; + + sdf::Model model1; + model1.SetName("model1"); + root.SetModel(model1); + + sdf::Light light1; + light1.SetName("light1"); + root.SetLight(light1); + + sdf::Actor actor1; + actor1.SetName("actor1"); + root.SetActor(actor1); + + ASSERT_NE(nullptr, root.Actor()); + ASSERT_EQ(nullptr, root.Light()); + ASSERT_EQ(nullptr, root.Model()); + EXPECT_EQ(0u, root.WorldCount()); + + // Convert to sdf::ElementPtr + sdf::ElementPtr elem = root.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Root root2; + root2.LoadSdfString(elem->ToString("")); + + EXPECT_EQ(SDF_VERSION, root2.Version()); + + ASSERT_NE(nullptr, root2.Actor()); + EXPECT_EQ("actor1", root2.Actor()->Name()); + + ASSERT_EQ(nullptr, root2.Model()); + ASSERT_EQ(nullptr, root2.Light()); + EXPECT_EQ(0u, root2.WorldCount()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, ToElementWorld) +{ + sdf::Root root; + + sdf::World world1; + world1.SetName("world1"); + root.AddWorld(world1); + + sdf::World world2; + world2.SetName("world2"); + root.AddWorld(world2); + + EXPECT_EQ(2u, root.WorldCount()); + + // Convert to sdf::ElementPtr + sdf::ElementPtr elem = root.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Root root2; + root2.LoadSdfString(elem->ToString("")); + + EXPECT_EQ(SDF_VERSION, root2.Version()); + EXPECT_EQ(2u, root2.WorldCount()); + + ASSERT_NE(nullptr, root2.WorldByIndex(0)); + EXPECT_EQ("world1", root2.WorldByIndex(0)->Name()); + + ASSERT_NE(nullptr, root2.WorldByIndex(1)); + EXPECT_EQ("world2", root2.WorldByIndex(1)->Name()); +} diff --git a/src/Sensor.cc b/src/Sensor.cc index 264592aa1..348347c61 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -129,6 +129,9 @@ class sdf::Sensor::Implementation /// \brief The frequency at which the sensor data is generated. /// If left unspecified (0.0), the sensor will generate data every cycle. public: double updateRate = 0.0; + + /// \brief Sensor plugins. + public: std::vector plugins; }; ///////////////////////////////////////////////// @@ -398,6 +401,11 @@ Errors Sensor::Load(ElementPtr _sdf) // Load the pose. Ignore the return value since the sensor pose is optional. loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); + // Load the sensor plugins + Errors pluginErrors = loadRepeated(_sdf, "plugin", + this->dataPtr->plugins); + errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); + return errors; } @@ -755,5 +763,34 @@ sdf::ElementPtr Sensor::ToElement() const std::cout << "Conversion of sensor type: [" << this->TypeStr() << "] from " << "SDF DOM to Element is not supported yet." << std::endl; } + + // Add in the plugins + for (const Plugin &plugin : this->dataPtr->plugins) + elem->InsertElement(plugin.ToElement(), true); + return elem; } + +///////////////////////////////////////////////// +const sdf::Plugins &Sensor::Plugins() const +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +sdf::Plugins &Sensor::Plugins() +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +void Sensor::ClearPlugins() +{ + this->dataPtr->plugins.clear(); +} + +///////////////////////////////////////////////// +void Sensor::AddPlugin(const Plugin &_plugin) +{ + this->dataPtr->plugins.push_back(_plugin); +} diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 5873d44fa..7ac81742d 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -520,6 +520,11 @@ TEST(DOMSensor, ToElement) mag.SetXNoise(noise); sensor.SetMagnetometerSensor(mag); + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + sensor.AddPlugin(plugin); + sdf::ElementPtr sensorElem = sensor.ToElement(); EXPECT_NE(nullptr, sensorElem); EXPECT_EQ(nullptr, sensor.Element()); @@ -537,6 +542,10 @@ TEST(DOMSensor, ToElement) sensor2.MagnetometerSensor()->XNoise().Mean()); EXPECT_DOUBLE_EQ(0.123, sensor2.UpdateRate()); + ASSERT_EQ(1u, sensor2.Plugins().size()); + EXPECT_EQ("name1", sensor2.Plugins()[0].Name()); + EXPECT_EQ("filename1", sensor2.Plugins()[0].Filename()); + // make changes to DOM and verify ToElement produces updated values sensor2.SetUpdateRate(1.23); sdf::ElementPtr sensor2Elem = sensor2.ToElement(); @@ -545,3 +554,27 @@ TEST(DOMSensor, ToElement) sensor3.Load(sensor2Elem); EXPECT_DOUBLE_EQ(1.23, sensor3.UpdateRate()); } + +///////////////////////////////////////////////// +TEST(DOMSensor, Plugins) +{ + sdf::Sensor sensor; + EXPECT_TRUE(sensor.Plugins().empty()); + + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + + sensor.AddPlugin(plugin); + ASSERT_EQ(1u, sensor.Plugins().size()); + + plugin.SetName("name2"); + sensor.AddPlugin(plugin); + ASSERT_EQ(2u, sensor.Plugins().size()); + + EXPECT_EQ("name1", sensor.Plugins()[0].Name()); + EXPECT_EQ("name2", sensor.Plugins()[1].Name()); + + sensor.ClearPlugins(); + EXPECT_TRUE(sensor.Plugins().empty()); +} diff --git a/src/Utils.cc b/src/Utils.cc index 6ebbddd75..e37cda171 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -304,11 +304,5 @@ sdf::Errors loadIncludedInterfaceModels(sdf::ElementPtr _sdf, return allErrors; } - -///////////////////////////////////////////////// -std::string computeMergedModelProxyFrameName(const std::string &_modelName) -{ - return "_merged__" + _modelName + "__model__"; -} } } diff --git a/src/Utils.hh b/src/Utils.hh index 3d850aa0e..5efbb928d 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -229,11 +229,6 @@ namespace sdf return &_opt.value(); return nullptr; } - - /// \brief Function to compute a merged model's proxy frame name - /// \param [in] _modelName The merged model's name - /// \return The computed frame name - std::string computeMergedModelProxyFrameName(const std::string &_modelName); } } #endif diff --git a/src/Visual.cc b/src/Visual.cc index f49118c2e..61d8b93ce 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -68,6 +68,9 @@ class sdf::Visual::Implementation /// \brief Lidar reflective intensity public: double laserRetro = 0; + + /// \brief Visual plugins. + public: std::vector plugins; }; ///////////////////////////////////////////////// @@ -149,6 +152,11 @@ Errors Visual::Load(ElementPtr _sdf) this->SetLaserRetro(_sdf->Get("laser_retro")); } + // Load the visual plugins + Errors pluginErrors = loadRepeated(_sdf, "plugin", + this->dataPtr->plugins); + errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); + return errors; } @@ -332,5 +340,33 @@ sdf::ElementPtr Visual::ToElement() const elem->InsertElement(this->dataPtr->material->ToElement(), true); } + // Add in the plugins + for (const Plugin &plugin : this->dataPtr->plugins) + elem->InsertElement(plugin.ToElement(), true); + return elem; } + +///////////////////////////////////////////////// +const sdf::Plugins &Visual::Plugins() const +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +sdf::Plugins &Visual::Plugins() +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +void Visual::ClearPlugins() +{ + this->dataPtr->plugins.clear(); +} + +///////////////////////////////////////////////// +void Visual::AddPlugin(const Plugin &_plugin) +{ + this->dataPtr->plugins.push_back(_plugin); +} diff --git a/src/Visual_TEST.cc b/src/Visual_TEST.cc index 6119416b7..24eb475ef 100644 --- a/src/Visual_TEST.cc +++ b/src/Visual_TEST.cc @@ -304,6 +304,11 @@ TEST(DOMVisual, ToElement) sdf::Material mat; visual.SetMaterial(mat); + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + visual.AddPlugin(plugin); + sdf::ElementPtr elem = visual.ToElement(); ASSERT_NE(nullptr, elem); @@ -319,4 +324,32 @@ TEST(DOMVisual, ToElement) EXPECT_DOUBLE_EQ(visual.LaserRetro(), visual2.LaserRetro()); EXPECT_NE(nullptr, visual2.Geom()); EXPECT_NE(nullptr, visual2.Material()); + + ASSERT_EQ(1u, visual2.Plugins().size()); + EXPECT_EQ("name1", visual2.Plugins()[0].Name()); + EXPECT_EQ("filename1", visual2.Plugins()[0].Filename()); +} + +///////////////////////////////////////////////// +TEST(DOMVisual, Plugins) +{ + sdf::Visual visual; + EXPECT_TRUE(visual.Plugins().empty()); + + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + + visual.AddPlugin(plugin); + ASSERT_EQ(1u, visual.Plugins().size()); + + plugin.SetName("name2"); + visual.AddPlugin(plugin); + ASSERT_EQ(2u, visual.Plugins().size()); + + EXPECT_EQ("name1", visual.Plugins()[0].Name()); + EXPECT_EQ("name2", visual.Plugins()[1].Name()); + + visual.ClearPlugins(); + EXPECT_TRUE(visual.Plugins().empty()); } diff --git a/src/World.cc b/src/World.cc index 958383433..f5574669e 100644 --- a/src/World.cc +++ b/src/World.cc @@ -29,6 +29,7 @@ #include "sdf/Model.hh" #include "sdf/ParserConfig.hh" #include "sdf/Physics.hh" +#include "sdf/Plugin.hh" #include "sdf/Types.hh" #include "sdf/World.hh" #include "FrameSemantics.hh" @@ -105,8 +106,10 @@ class sdf::World::Implementation /// \brief Scoped Pose Relative-To graph that points to a graph owned by this /// world. public: sdf::ScopedGraph poseRelativeToGraph; -}; + /// \brief World plugins. + public: sdf::Plugins plugins; +}; ///////////////////////////////////////////////// World::World() @@ -311,6 +314,11 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) errors.insert(errors.end(), sceneLoadErrors.begin(), sceneLoadErrors.end()); } + // Load the world plugins + Errors pluginErrors = loadRepeated(_sdf, "plugin", + this->dataPtr->plugins); + errors.insert(errors.end(), pluginErrors.begin(), pluginErrors.end()); + return errors; } @@ -848,7 +856,7 @@ Errors World::Implementation::LoadSphericalCoordinates( } ///////////////////////////////////////////////// -sdf::ElementPtr World::ToElement() const +sdf::ElementPtr World::ToElement(bool _useIncludeTag) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("world.sdf", elem); @@ -866,7 +874,7 @@ sdf::ElementPtr World::ToElement() const // Models for (const sdf::Model &model : this->dataPtr->models) - elem->InsertElement(model.ToElement(), true); + elem->InsertElement(model.ToElement(_useIncludeTag), true); // Actors for (const sdf::Actor &actor : this->dataPtr->actors) @@ -910,6 +918,10 @@ sdf::ElementPtr World::ToElement() const if (this->dataPtr->audioDevice != "default") elem->GetElement("audio")->GetElement("device")->Set(this->AudioDevice()); + // Add in the plugins + for (const Plugin &plugin : this->dataPtr->plugins) + elem->InsertElement(plugin.ToElement(), true); + return elem; } @@ -991,3 +1003,27 @@ bool World::AddFrame(const Frame &_frame) return true; } + +///////////////////////////////////////////////// +const sdf::Plugins &World::Plugins() const +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +sdf::Plugins &World::Plugins() +{ + return this->dataPtr->plugins; +} + +///////////////////////////////////////////////// +void World::ClearPlugins() +{ + this->dataPtr->plugins.clear(); +} + +///////////////////////////////////////////////// +void World::AddPlugin(const Plugin &_plugin) +{ + this->dataPtr->plugins.push_back(_plugin); +} diff --git a/src/World_TEST.cc b/src/World_TEST.cc index eba434d50..41c4a1d51 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -529,6 +529,11 @@ TEST(DOMWorld, ToElement) world.ClearPhysics(); } + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + world.AddPlugin(plugin); + sdf::ElementPtr elem = world.ToElement(); ASSERT_NE(nullptr, elem); @@ -566,6 +571,10 @@ TEST(DOMWorld, ToElement) EXPECT_EQ(world.PhysicsCount(), world2.PhysicsCount()); for (uint64_t i = 0; i < world2.PhysicsCount(); ++i) EXPECT_NE(nullptr, world2.PhysicsByIndex(i)); + + ASSERT_EQ(1u, world2.Plugins().size()); + EXPECT_EQ("name1", world2.Plugins()[0].Name()); + EXPECT_EQ("filename1", world2.Plugins()[0].Filename()); } ///////////////////////////////////////////////// @@ -658,3 +667,27 @@ TEST(DOMWorld, MutableByName) EXPECT_FALSE(world.FrameByName("frame1")); EXPECT_TRUE(world.FrameByName("frame2")); } + +///////////////////////////////////////////////// +TEST(DOMWorld, Plugins) +{ + sdf::World world; + EXPECT_TRUE(world.Plugins().empty()); + + sdf::Plugin plugin; + plugin.SetName("name1"); + plugin.SetFilename("filename1"); + + world.AddPlugin(plugin); + ASSERT_EQ(1u, world.Plugins().size()); + + plugin.SetName("name2"); + world.AddPlugin(plugin); + ASSERT_EQ(2u, world.Plugins().size()); + + EXPECT_EQ("name1", world.Plugins()[0].Name()); + EXPECT_EQ("name2", world.Plugins()[1].Name()); + + world.ClearPlugins(); + EXPECT_TRUE(world.Plugins().empty()); +} diff --git a/src/parser.cc b/src/parser.cc index 9270c3fe9..b7cd3492b 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -2549,9 +2549,7 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) const std::string parentLocalName = sdf::SplitName(parentName).second; if (parentName != "world" && parentLocalName != "__model__" && - !_model->LinkNameExists(parentName) && - !_model->JointNameExists(parentName) && - !_model->FrameNameExists(parentName)) + !_model->NameExistsInFrameAttachedToGraph(parentName)) { errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, "parent frame with name[" + parentName + @@ -2568,10 +2566,8 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) joint->Name() + "] in model with name[" + _model->Name() + "]."}); } - if (childLocalName != "__model__" && !_model->LinkNameExists(childName) && - !_model->JointNameExists(childName) && - !_model->FrameNameExists(childName) && - !_model->ModelNameExists(childName)) + if (childLocalName != "__model__" && + !_model->NameExistsInFrameAttachedToGraph(childName)) { errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, "child frame with name[" + childName + @@ -2652,5 +2648,11 @@ bool shouldValidateElement(sdf::ElementPtr _elem) return true; } + +///////////////////////////////////////////////// +std::string computeMergedModelProxyFrameName(const std::string &_modelName) +{ + return "_merged__" + _modelName + "__model__"; +} } } diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index ed83d8e70..8a78389e5 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -15,6 +15,7 @@ set(tests force_torque_sensor.cc frame.cc geometry_dom.cc + gui_dom.cc include.cc includes.cc interface_api.cc @@ -47,6 +48,7 @@ set(tests sdf_basic.cc sdf_custom.cc sdf_dom_conversion.cc + sensor_dom.cc surface_dom.cc unknown.cc urdf_gazebo_extensions.cc diff --git a/test/integration/actor_dom.cc b/test/integration/actor_dom.cc index f4a53e6b0..ed86d4af8 100644 --- a/test/integration/actor_dom.cc +++ b/test/integration/actor_dom.cc @@ -159,3 +159,28 @@ TEST(DOMActor, CopySdfLoadedProperties) EXPECT_EQ(actor1.LinkCount(), actor2->LinkCount()); EXPECT_EQ(actor1.JointCount(), actor2->JointCount()); } + +////////////////////////////////////////////////// +TEST(DOMActor, ActorPlugins) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_complete.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(testFile, root.Element()->FilePath()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + const sdf::Actor *actor = world->ActorByIndex(0); + ASSERT_NE(nullptr, actor); + + ASSERT_EQ(2u, actor->Plugins().size()); + EXPECT_EQ("actor_plugin1", actor->Plugins()[0].Name()); + EXPECT_EQ("test/file/actor1", actor->Plugins()[0].Filename()); + EXPECT_EQ("actor_plugin2", actor->Plugins()[1].Name()); + EXPECT_EQ("test/file/actor2", actor->Plugins()[1].Filename()); +} diff --git a/test/integration/gui_dom.cc b/test/integration/gui_dom.cc new file mode 100644 index 000000000..33fea53c7 --- /dev/null +++ b/test/integration/gui_dom.cc @@ -0,0 +1,48 @@ +/* + * Copyright 2022 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 "sdf/Gui.hh" +#include "sdf/World.hh" +#include "test_config.h" +#include "test_utils.hh" + +////////////////////////////////////////////////// +TEST(DOMGui, GuiPlugins) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_complete.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(testFile, root.Element()->FilePath()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + const sdf::Gui *gui = world->Gui(); + ASSERT_NE(nullptr, gui); + + ASSERT_EQ(2u, gui->Plugins().size()); + EXPECT_EQ("gui_plugin1", gui->Plugins()[0].Name()); + EXPECT_EQ("test/file/gui1", gui->Plugins()[0].Filename()); + EXPECT_EQ("gui_plugin2", gui->Plugins()[1].Name()); + EXPECT_EQ("test/file/gui2", gui->Plugins()[1].Filename()); +} diff --git a/test/integration/interface_api.cc b/test/integration/interface_api.cc index 1041a21e4..884ad648f 100644 --- a/test/integration/interface_api.cc +++ b/test/integration/interface_api.cc @@ -83,8 +83,14 @@ sdf::InterfaceModelPtr parseModel(toml::Value &_doc, class CustomTomlParser { - public: CustomTomlParser(bool _supportsMergeInclude = true) - : supportsMergeInclude(_supportsMergeInclude) + /// \brief Constructor + /// \param[in] _supportsMergeInclude Whether the parser supports merge include + /// \param[in] _overridePoseInParser Whether the parser should apply pose + /// overrides from //include/pose + public: CustomTomlParser(bool _supportsMergeInclude = true, + bool _overridePoseInParser = true) + : supportsMergeInclude(_supportsMergeInclude), + overridePoseInParser(_overridePoseInParser) { } @@ -104,7 +110,7 @@ class CustomTomlParser param.Set(*_include.IsStatic()); doc["static"] = {param}; } - if (_include.IncludeRawPose().has_value()) + if (this->overridePoseInParser && _include.IncludeRawPose().has_value()) { // if //include/static is set, override the value in the inluded model sdf::Param poseParam("pose", "pose", "", false); @@ -120,6 +126,7 @@ class CustomTomlParser } public: bool supportsMergeInclude; + public: bool overridePoseInParser{true}; }; bool endsWith(const std::string &_str, const std::string &_suffix) @@ -552,16 +559,34 @@ TEST_F(InterfaceAPI, FrameSemantics) const std::string testFile = sdf::testing::TestFile( "sdf", "include_with_interface_api_frame_semantics.sdf"); this->config.RegisterCustomModelParser(this->customTomlParser); - sdf::Root root; - sdf::Errors errors = root.Load(testFile, config); - EXPECT_TRUE(errors.empty()) << errors; + { + sdf::Root root; + sdf::Errors errors = root.Load(testFile, config); + EXPECT_TRUE(errors.empty()) << errors; - const sdf::World *world = root.WorldByIndex(0); - ASSERT_NE(nullptr, world); - EXPECT_EQ(1u, world->InterfaceModelCount()); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ(1u, world->InterfaceModelCount()); - SCOPED_TRACE("InterfaceAPI.FrameSemantics"); - this->CheckFrameSemantics(world); + SCOPED_TRACE("InterfaceAPI.FrameSemantics"); + this->CheckFrameSemantics(world); + } + { + // Check without //include/pose override applied in parser. + sdf::Root root; + sdf::ParserConfig newConfig = this->config; + CustomTomlParser parserWithoutPoseOverride(true, false); + newConfig.RegisterCustomModelParser(parserWithoutPoseOverride); + sdf::Errors errors = root.Load(testFile, newConfig); + EXPECT_TRUE(errors.empty()) << errors; + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ(1u, world->InterfaceModelCount()); + + SCOPED_TRACE("InterfaceAPI.FrameSemantics_NoPoseOverrideInParser"); + this->CheckFrameSemantics(world); + } } ///////////////////////////////////////////////// @@ -1246,3 +1271,35 @@ TEST_F(InterfaceAPIMergeInclude, JointModelChild) EXPECT_EQ("base", body); } } + +///////////////////////////////////////////////// +TEST_F(InterfaceAPI, JointParentOrChildInNestedModel) +{ + this->config.RegisterCustomModelParser(customTomlParser); + + const std::string testSdf = R"( + + + + + + L1 + double_pendulum::base + + + double_pendulum.toml + double_pendulum + + + + double_pendulum::child_dp::base + L2 + + + + + )"; + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(testSdf, this->config); + EXPECT_TRUE(errors.empty()) << errors; +} diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 986e0034d..0050d6442 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -176,7 +176,7 @@ TEST(DOMJoint, LoadJointParentWorld) EXPECT_EQ(1u, model->LinkCount()); EXPECT_NE(nullptr, model->LinkByIndex(0)); EXPECT_EQ(nullptr, model->LinkByIndex(1)); - EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), model->RawPose()); EXPECT_EQ("", model->PoseRelativeTo()); ASSERT_TRUE(model->LinkNameExists("link")); diff --git a/test/integration/model/double_pendulum.toml b/test/integration/model/double_pendulum.toml index c80da60d7..02745e1e0 100644 --- a/test/integration/model/double_pendulum.toml +++ b/test/integration/model/double_pendulum.toml @@ -2,7 +2,7 @@ # very limited toml parser that is not capable of parsing the full toml syntax. # Specifically, arrays are not supported. name = "double_pendulum" -pose = "1 0 0 0 0 0" +pose = "1 2 3 0 0 0" canonical_link = "base" [links.base] diff --git a/test/integration/model/test_model_with_plugin/model.config b/test/integration/model/test_model_with_plugin/model.config new file mode 100644 index 000000000..ef5ed88dd --- /dev/null +++ b/test/integration/model/test_model_with_plugin/model.config @@ -0,0 +1,6 @@ + + + + test_model_with_plugin + model.sdf + diff --git a/test/integration/model/test_model_with_plugin/model.sdf b/test/integration/model/test_model_with_plugin/model.sdf new file mode 100644 index 000000000..74fe702c5 --- /dev/null +++ b/test/integration/model/test_model_with_plugin/model.sdf @@ -0,0 +1,38 @@ + + + + + + + + + + + meshes/mesh.dae + + my_submesh +
true
+ + 0.1 0.2 0.3 + + + + + + + + meshes/mesh.dae + + another_submesh +
false
+ + 1.2 2.3 3.4 + + + + + + + + + diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 69bc6cb89..88886cb2b 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -31,6 +31,12 @@ #include "test_config.h" #include "test_utils.hh" +///////////////////////////////////////////////// +std::string findFileCb(const std::string &_input) +{ + return sdf::testing::TestFile("integration", "model", _input); +} + ////////////////////////////////////////////////// TEST(DOMModel, NotAModel) { @@ -309,6 +315,23 @@ TEST(DOMRoot, MultiNestedModel) EXPECT_EQ(innerModel->FrameByIndex(0), outerModel->FrameByName(innerFrameNestedName)); EXPECT_NE(nullptr, outerModel->FrameByName(innerFrameNestedName)); + + + // Check that each implicit/explicit frame is in the frame attached to graph + EXPECT_TRUE(outerModel->NameExistsInFrameAttachedToGraph("outer_link")); + EXPECT_TRUE(outerModel->NameExistsInFrameAttachedToGraph("outer_joint")); + EXPECT_TRUE(outerModel->NameExistsInFrameAttachedToGraph("outer_frame")); + EXPECT_TRUE(outerModel->NameExistsInFrameAttachedToGraph("mid_model")); + + // Check that mid_link does not exist directly under outer_model, but can be + // accessed via its scoped name + EXPECT_FALSE(outerModel->NameExistsInFrameAttachedToGraph("mid_link")); + EXPECT_TRUE( + outerModel->NameExistsInFrameAttachedToGraph("mid_model::mid_link")); + + // Check multiple levels of nesting + EXPECT_TRUE(outerModel->NameExistsInFrameAttachedToGraph( + "mid_model::inner_model::inner_joint")); } ///////////////////////////////////////////////// @@ -628,3 +651,101 @@ TEST(DOMModel, LoadModelWithDuplicateChildNames) EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } } + +////////////////////////////////////////////////// +TEST(DOMModel, ModelPlugins) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_complete.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(testFile, root.Element()->FilePath()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ASSERT_EQ(2u, model->Plugins().size()); + EXPECT_EQ("model_plugin1", model->Plugins()[0].Name()); + EXPECT_EQ("test/file/model1", model->Plugins()[0].Filename()); + EXPECT_EQ("model_plugin2", model->Plugins()[1].Name()); + EXPECT_EQ("test/file/model2", model->Plugins()[1].Filename()); +} + +////////////////////////////////////////////////// +TEST(DOMModel, IncludeModelWithPlugin) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "include_model_with_plugin.sdf"); + + sdf::ParserConfig config; + config.SetFindCallback(findFileCb); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile, config); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(testFile, root.Element()->FilePath()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + // Test ToElement with useInclude == true + { + sdf::ElementPtr elem = model->ToElement(true); + + // There should be a uri + ASSERT_TRUE(elem->HasElement("uri")); + EXPECT_EQ("test_model_with_plugin", elem->Get("uri")); + + // There should be a plugin + ASSERT_TRUE(elem->HasElement("plugin")); + sdf::ElementPtr pluginElem = elem->GetElement("plugin"); + EXPECT_EQ("test_model_include_plugin", + pluginElem->Get("name")); + + EXPECT_EQ("include/test/model/plugin", + pluginElem->Get("filename")); + + // There should be only one plugin + ASSERT_EQ(nullptr, pluginElem->GetNextElement("plugin")); + } + + // Test ToElement with useInclude == false. This will result in the full + // model SDF which would have two plugins, one from the tag and + // one from the included model + { + sdf::ElementPtr elem = model->ToElement(false); + + // There should NOT be a uri + ASSERT_FALSE(elem->HasElement("uri")); + + // There should be a plugin + ASSERT_TRUE(elem->HasElement("plugin")); + sdf::ElementPtr pluginElem = elem->GetElement("plugin"); + EXPECT_EQ("test_model_plugin", + pluginElem->Get("name")); + + EXPECT_EQ("test/model/plugin", + pluginElem->Get("filename")); + + // There should be a second plugin with different information + pluginElem = pluginElem->GetNextElement("plugin"); + ASSERT_NE(nullptr, pluginElem); + EXPECT_EQ("test_model_include_plugin", + pluginElem->Get("name")); + EXPECT_EQ("include/test/model/plugin", + pluginElem->Get("filename")); + + // THere should not be third plugin. + ASSERT_EQ(nullptr, pluginElem->GetNextElement("plugin")); + } +} diff --git a/test/integration/param_passing.cc b/test/integration/param_passing.cc index 44a40ed1e..1ebda3c82 100644 --- a/test/integration/param_passing.cc +++ b/test/integration/param_passing.cc @@ -17,7 +17,11 @@ #include #include #include "sdf/Filesystem.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" #include "sdf/Root.hh" +#include "sdf/Visual.hh" +#include "sdf/World.hh" #include "test_config.h" void PrintErrors(sdf::Errors &_errors) @@ -163,3 +167,36 @@ TEST(ParamPassingTest, NestedInclude) << "ACTUAL:\n" << root.Element()->ToString("\t") << "\nEXPECTED:\n" << expectedRoot.Element()->ToString("\t"); } + +///////////////////////////////////////////////// +TEST(ParamPassingTest, CheckPluginClone) +{ + const std::string modelRootPath = + sdf::testing::TestFile("integration", "model"); + sdf::setFindCallback( + [&](const std::string &_file) + { + return sdf::filesystem::append(modelRootPath, _file); + }); + + // checks containing w/ correctly specified + // elements + sdf::Root root; + std::string testFile = sdf::testing::TestFile("integration", + "include_custom_model.sdf"); + sdf::Errors errors = root.Load(testFile); + PrintErrors(errors); + EXPECT_TRUE(errors.empty()); + + sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + sdf::Model *model = world->ModelByName("robot"); + ASSERT_NE(nullptr, model); + sdf::Model *nestedModel = model->ModelByName("vehicle"); + ASSERT_NE(nullptr, nestedModel); + sdf::Link *link = nestedModel->LinkByName("top"); + ASSERT_NE(nullptr, link); + sdf::Visual *visual = link->VisualByName("camera_visual"); + ASSERT_NE(nullptr, visual); + EXPECT_EQ(4u, visual->Plugins().size()); +} diff --git a/test/integration/sensor_dom.cc b/test/integration/sensor_dom.cc new file mode 100644 index 000000000..915b217e6 --- /dev/null +++ b/test/integration/sensor_dom.cc @@ -0,0 +1,56 @@ +/* + * Copyright 2022 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 "sdf/Link.hh" +#include "sdf/Model.hh" +#include "sdf/Sensor.hh" +#include "sdf/World.hh" +#include "test_config.h" +#include "test_utils.hh" + +////////////////////////////////////////////////// +TEST(DOMSensor, SensorPlugins) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_complete.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(testFile, root.Element()->FilePath()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + const sdf::Link *link = model->LinkByIndex(0); + ASSERT_NE(nullptr, link); + + const sdf::Sensor *sensor = link->SensorByIndex(0); + ASSERT_NE(nullptr, sensor); + + ASSERT_EQ(2u, sensor->Plugins().size()); + EXPECT_EQ("sensor_plugin1", sensor->Plugins()[0].Name()); + EXPECT_EQ("test/file/sensor1", sensor->Plugins()[0].Filename()); + EXPECT_EQ("sensor_plugin2", sensor->Plugins()[1].Name()); + EXPECT_EQ("test/file/sensor2", sensor->Plugins()[1].Filename()); +} diff --git a/test/integration/visual_dom.cc b/test/integration/visual_dom.cc index 5ff770751..330c778e2 100644 --- a/test/integration/visual_dom.cc +++ b/test/integration/visual_dom.cc @@ -28,6 +28,7 @@ #include "sdf/Root.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" +#include "sdf/World.hh" #include "test_config.h" ////////////////////////////////////////////////// @@ -436,3 +437,34 @@ TEST(DOMVisual, VisibilityFlags) EXPECT_EQ(0x00000001u, vis1->VisibilityFlags()); } + +////////////////////////////////////////////////// +TEST(DOMVisual, VisualPlugins) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_complete.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(testFile, root.Element()->FilePath()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + const sdf::Link *link = model->LinkByIndex(0); + ASSERT_NE(nullptr, link); + + const sdf::Visual *visual = link->VisualByIndex(0); + ASSERT_NE(nullptr, visual); + + ASSERT_EQ(2u, visual->Plugins().size()); + EXPECT_EQ("visual_plugin1", visual->Plugins()[0].Name()); + EXPECT_EQ("test/file/visual1", visual->Plugins()[0].Filename()); + EXPECT_EQ("visual_plugin2", visual->Plugins()[1].Name()); + EXPECT_EQ("test/file/visual2", visual->Plugins()[1].Filename()); +} diff --git a/test/integration/world_dom.cc b/test/integration/world_dom.cc index 21310cc2c..45a24af4e 100644 --- a/test/integration/world_dom.cc +++ b/test/integration/world_dom.cc @@ -358,3 +358,25 @@ TEST(DOMWorld, LoadWorldWithDuplicateChildNames) EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } } + +////////////////////////////////////////////////// +TEST(DOMWorld, WorldPlugins) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_complete.sdf"); + + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, root.Element()); + EXPECT_EQ(testFile, root.Element()->FilePath()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + ASSERT_EQ(2u, world->Plugins().size()); + EXPECT_EQ("world_plugin1", world->Plugins()[0].Name()); + EXPECT_EQ("test/file/world1", world->Plugins()[0].Filename()); + EXPECT_EQ("world_plugin2", world->Plugins()[1].Name()); + EXPECT_EQ("test/file/world2", world->Plugins()[1].Filename()); +} diff --git a/test/sdf/ball_prismatic_joint.sdf b/test/sdf/ball_prismatic_joint.sdf new file mode 100644 index 000000000..357aeec57 --- /dev/null +++ b/test/sdf/ball_prismatic_joint.sdf @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + 0 1 0 0 0 0 + link1 + link2 + + + 0 0 0 0 0 1 + link3 + link4 + + 0 0 1 + + + + + diff --git a/test/sdf/basic_shapes.sdf b/test/sdf/basic_shapes.sdf new file mode 100644 index 000000000..32e0b416f --- /dev/null +++ b/test/sdf/basic_shapes.sdf @@ -0,0 +1,159 @@ + + + + + true + + + + + 0 0 1 + 1.4 6.3 + + + + + + + + 0 0 1 + 2 4 + + + + + + + + 0 0 2.5 0 0 0 + + + + + 1 2 3 + + + + + 0xAB + + + + + + + + 1 2 3 + + + + + + + + 2 0 0 0 0 0 + + + + + 0.2 + 0.1 + + + + + + + + 0.2 + 0.1 + + + + + + + + 2 0 0 0 0 0 + + + + + 1.23 + + + + + + + + 1.23 + + + + + + + + 2 0 0 0 0 0 + + + + + 0.2 + 0.1 + + + + + + + + 0.2 + 0.1 + + + + 0 0.1 0.2 + 0.12 0.23 0.34 0.56 + + + + + + + 2 0 0 0 0 0 + + + + + file://box.dae + 0.1 0.2 0.3 + + + + + + + + file://box.dae + 1.2 2.3 3.4 + + + + 0.2 0.5 0.1 1.0 + 0.7 0.3 0.5 0.9 + + + albedo_map.png + normal_map.png + roughness_map.png + metalness_map.png + + + + + + + + diff --git a/test/sdf/box.dae b/test/sdf/box.dae new file mode 100644 index 000000000..6ace63dcb --- /dev/null +++ b/test/sdf/box.dae @@ -0,0 +1,98 @@ + + + + + Z_UP + + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.64 0.64 0.64 1 + + + 0.5 0.5 0.5 1 + + + 50 + + + 1 1 1 1 + + + 1 + + + 1 + + + + + + + + + + + + + + + + 1 1 -1 1 -1 -1 -1 -0.9999998 -1 -0.9999997 1 -1 1 0.9999995 1 0.9999994 -1.000001 1 -1 -0.9999997 1 -1 1 1 + + + + + + + + + + 0 0 -1 0 0 1 1 -2.83122e-7 0 -2.83122e-7 -1 0 -1 2.23517e-7 -1.3411e-7 2.38419e-7 1 2.08616e-7 + + + + + + + + + + + + + + + 4 4 4 4 4 4 +

0 0 1 0 2 0 3 0 4 1 7 1 6 1 5 1 0 2 4 2 5 2 1 2 1 3 5 3 6 3 2 3 2 4 6 4 7 4 3 4 4 5 0 5 3 5 7 5

+ + + 1 + + + + + + + + + + + + + + + + + + + diff --git a/test/sdf/double_pendulum.sdf b/test/sdf/double_pendulum.sdf index fd3d3b102..b6339f6e3 100644 --- a/test/sdf/double_pendulum.sdf +++ b/test/sdf/double_pendulum.sdf @@ -197,6 +197,12 @@ upper_link 1.0 0 0 + + 35 + + + 350 + @@ -205,6 +211,12 @@ lower_link 1.0 0 0 + + 35 + + + 350 + diff --git a/test/sdf/ellipsoid.sdf b/test/sdf/ellipsoid.sdf new file mode 100644 index 000000000..a0a29cb68 --- /dev/null +++ b/test/sdf/ellipsoid.sdf @@ -0,0 +1,62 @@ + + + + + true + + + + + 0 0 1 + 10 10 + + + + + + + 0 0 1 + 10 10 + + + + + + + + 0 3.0 0.5 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + + + + diff --git a/test/sdf/include_model_with_plugin.sdf b/test/sdf/include_model_with_plugin.sdf new file mode 100644 index 000000000..33392e249 --- /dev/null +++ b/test/sdf/include_model_with_plugin.sdf @@ -0,0 +1,8 @@ + + + + test_model_with_plugin + + + + diff --git a/test/sdf/joint_parent_world.sdf b/test/sdf/joint_parent_world.sdf index 85142e454..0ebb67202 100644 --- a/test/sdf/joint_parent_world.sdf +++ b/test/sdf/joint_parent_world.sdf @@ -1,6 +1,7 @@ + 1 0 0 0 0 0 0 0 1 0 0 0 diff --git a/test/sdf/usd_sensors.sdf b/test/sdf/usd_sensors.sdf new file mode 100644 index 000000000..eb25d97fc --- /dev/null +++ b/test/sdf/usd_sensors.sdf @@ -0,0 +1,144 @@ + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 100 + true + imu + true + + + + + + 4 0 1.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + + + + true + 4 0 0.5 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + lidar + 10 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 16 + 1 + -0.261799 + 0.261799 + + + + 0.08 + 10.0 + 0.01 + + + 1 + true + + + + + diff --git a/test/sdf/world_complete.sdf b/test/sdf/world_complete.sdf index 14a7e8008..4260d9076 100644 --- a/test/sdf/world_complete.sdf +++ b/test/sdf/world_complete.sdf @@ -27,6 +27,8 @@ 90 + + 0.3 0.4 0.5 @@ -111,7 +113,35 @@ 2.2 + + + 1 1 1 + + + + + + + + + + 0.1 + 0.2 + + + + + 2.3 + 4.5 + + + + + + + + @@ -148,6 +178,8 @@ + + @@ -225,5 +257,8 @@ + + + diff --git a/usd/include/sdf/usd/Conversions.hh b/usd/include/sdf/usd/Conversions.hh new file mode 100644 index 000000000..b1409a7af --- /dev/null +++ b/usd/include/sdf/usd/Conversions.hh @@ -0,0 +1,54 @@ +/* + * Copyright 2022 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_USD_CONVERSIONS_HH_ +#define SDF_USD_CONVERSIONS_HH_ + +#include + +#include + +#include "sdf/Material.hh" +#include "sdf/sdf_config.h" +#include "sdf/usd/Export.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Specialized conversion from an Ignition Common Material + /// to a SDF material + /// \param[in] _in Ignition Common Material. + /// \return SDF material. + IGNITION_SDFORMAT_USD_VISIBLE + sdf::Material convert(const ignition::common::Material *_in); + + /// \brief Specialized conversion from an SDF material to a Ignition Common + /// material. + /// \param[in] _in SDF material. + /// \param[out] _out The Ignition Common Material. + IGNITION_SDFORMAT_USD_VISIBLE + void convert(const sdf::Material &_in, ignition::common::Material &_out); + } + } +} + +#endif diff --git a/usd/include/sdf/usd/UsdError.hh b/usd/include/sdf/usd/UsdError.hh index 0da732696..28742e2d6 100644 --- a/usd/include/sdf/usd/UsdError.hh +++ b/usd/include/sdf/usd/UsdError.hh @@ -57,6 +57,18 @@ namespace sdf /// \brief A pxr API was not able to be applied to a USD prim. FAILED_PRIM_API_APPLY, + + /// \brief Attempt to define a USD prim or prim schema failed. + FAILED_USD_DEFINITION, + + /// \brief Failure to load a mesh and/or submesh + MESH_LOAD_FAILURE, + + /// \brief Invalid submesh primitive type + INVALID_SUBMESH_PRIMITIVE_TYPE, + + /// \brief Invalid material + INVALID_MATERIAL, }; class IGNITION_SDFORMAT_USD_VISIBLE UsdError diff --git a/usd/include/sdf/usd/sdf_parser/Geometry.hh b/usd/include/sdf/usd/sdf_parser/Geometry.hh new file mode 100644 index 000000000..524da8fa3 --- /dev/null +++ b/usd/include/sdf/usd/sdf_parser/Geometry.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2022 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_USD_SDF_PARSER_GEOMETRY_HH_ +#define SDF_USD_SDF_PARSER_GEOMETRY_HH_ + +#include + +// TODO(adlarkin) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Geometry.hh" +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse an SDF geometry into a USD stage. + /// \param[in] _geometry The SDF geometry to parse. + /// \param[in] _stage The stage that should contain the USD representation + /// of _geometry. + /// \param[in] _path The USD path of the parsed geometry in _stage, which + /// must be a valid USD path. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error. + UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfGeometry( + const sdf::Geometry &_geometry, pxr::UsdStageRefPtr &_stage, + const std::string &_path); + } + } +} + +#endif diff --git a/usd/include/sdf/usd/sdf_parser/Joint.hh b/usd/include/sdf/usd/sdf_parser/Joint.hh new file mode 100644 index 000000000..e452d6c51 --- /dev/null +++ b/usd/include/sdf/usd/sdf_parser/Joint.hh @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2022 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_USD_SDF_PARSER_JOINT_HH_ +#define SDF_USD_SDF_PARSER_JOINT_HH_ + +#include +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Joint.hh" +#include "sdf/Model.hh" +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse a SDF joint into a USD stage. + /// \param[in] _joint The SDF joint to parse. + /// \param[in] _stage The stage that should contain the USD representation + /// of _joint. This must be a valid, initialized stage. + /// \param[in] _path The USD path of the parsed joint in _stage, which must + /// be a valid USD path. + /// \param[in] _parentModel The model that is the parent of _joint + /// \param[in] _linkToUsdPath a map of a link's SDF name to the link's USD + /// path. This is used to determine which USD prims should be assigned as + /// the USD joint's relative links. + /// \param[in] _worldPath The USD path of the world prim. This is needed if + /// _joint's parent is the world. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no errors + /// occurred when parsing _joint to its USD representation. + UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfJoint( + const sdf::Joint &_joint, + pxr::UsdStageRefPtr &_stage, const std::string &_path, + const sdf::Model &_parentModel, + const std::unordered_map &_linkToUsdPath, + const pxr::SdfPath &_worldPath); + } + } +} + +#endif diff --git a/usd/include/sdf/usd/sdf_parser/Link.hh b/usd/include/sdf/usd/sdf_parser/Link.hh index dad8bd0a2..369600e83 100644 --- a/usd/include/sdf/usd/sdf_parser/Link.hh +++ b/usd/include/sdf/usd/sdf_parser/Link.hh @@ -20,7 +20,7 @@ #include -// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings // are reported using #pragma message so normal diagnostic flags cannot remove // them. This workaround requires this block to be used whenever usd is // included. diff --git a/usd/include/sdf/usd/sdf_parser/Material.hh b/usd/include/sdf/usd/sdf_parser/Material.hh new file mode 100644 index 000000000..429dd7b8e --- /dev/null +++ b/usd/include/sdf/usd/sdf_parser/Material.hh @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2022 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_USD_SDF_PARSER_MATERIALS_HH_ +#define SDF_USD_SDF_PARSER_MATERIALS_HH_ + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Material.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" +#include "sdf/sdf_config.h" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse an SDF material into a USD stage. + /// \param[in] _materialSdf The SDF material to parse. + /// \param[in] _stage The stage that should contain the USD representation + /// of _material. + /// \param[out] _materialPath USD Material path + /// \return UsdErrors, which is a list of UsdError objects. This list is + /// empty if no errors occurred when parsing _materialSdf its USD + /// representation + UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfMaterial( + const sdf::Material *_materialSdf, + pxr::UsdStageRefPtr &_stage, + pxr::SdfPath &_materialPath); + } + } +} + +#endif diff --git a/usd/include/sdf/usd/sdf_parser/Sensor.hh b/usd/include/sdf/usd/sdf_parser/Sensor.hh new file mode 100644 index 000000000..97529c173 --- /dev/null +++ b/usd/include/sdf/usd/sdf_parser/Sensor.hh @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2022 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_USD_SDF_PARSER_SENSOR_HH_ +#define SDF_USD_SDF_PARSER_SENSOR_HH_ + +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Sensor.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" +#include "sdf/sdf_config.h" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse an SDF sensor into a USD stage. + /// \param[in] _sensor The SDF sensor to parse. + /// \param[in] _stage The stage that should contain the USD representation + /// of _sensor. + /// \param[in] _path The USD path of the parsed sensor in _stage, which must + /// be a valid USD path. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error. + UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfSensor( + const sdf::Sensor &_sensor, + pxr::UsdStageRefPtr &_stage, + const std::string &_path); + } + } +} + +#endif diff --git a/usd/include/sdf/usd/sdf_parser/Visual.hh b/usd/include/sdf/usd/sdf_parser/Visual.hh new file mode 100644 index 000000000..3c5196ac1 --- /dev/null +++ b/usd/include/sdf/usd/sdf_parser/Visual.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2022 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_USD_SDF_PARSER_VISUAL_HH_ +#define SDF_USD_SDF_PARSER_VISUAL_HH_ + +#include + +// TODO(adlarkin) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Visual.hh" +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse an SDF visual into a USD stage. + /// \param[in] _visual The SDF visual to parse. + /// \param[in] _stage The stage that should contain the USD representation + /// of _visual. + /// \param[in] _path The USD path of the parsed visual in _stage, which must + /// be a valid USD path. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error. + UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfVisual( + const sdf::Visual &_visual, pxr::UsdStageRefPtr &_stage, + const std::string &_path); + } + } +} + +#endif diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 040ddb672..29b4ef4b5 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -1,8 +1,14 @@ set(sources + Conversions.cc UsdError.cc + sdf_parser/Geometry.cc + sdf_parser/Joint.cc sdf_parser/Light.cc sdf_parser/Link.cc + sdf_parser/Material.cc sdf_parser/Model.cc + sdf_parser/Sensor.cc + sdf_parser/Visual.cc sdf_parser/World.cc ) @@ -16,16 +22,23 @@ target_include_directories(${usd_target} target_link_libraries(${usd_target} PUBLIC ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-common${IGN_COMMON_VER}::graphics ${PXR_LIBRARIES} ) set(gtest_sources sdf_parser/sdf2usd_TEST.cc + sdf_parser/Geometry_Sdf2Usd_TEST.cc + sdf_parser/Joint_Sdf2Usd_TEST.cc sdf_parser/Light_Sdf2Usd_TEST.cc sdf_parser/Link_Sdf2Usd_TEST.cc + sdf_parser/Material_Sdf2Usd_TEST.cc # TODO(adlarkin) add a test for SDF -> USD models once model parsing # functionality is complete + sdf_parser/Sensors_Sdf2Usd_TEST.cc + sdf_parser/Visual_Sdf2Usd_TEST.cc sdf_parser/World_Sdf2Usd_TEST.cc + Conversions_TEST.cc UsdError_TEST.cc UsdUtils_TEST.cc ) diff --git a/usd/src/Conversions.cc b/usd/src/Conversions.cc new file mode 100644 index 000000000..5386893b3 --- /dev/null +++ b/usd/src/Conversions.cc @@ -0,0 +1,152 @@ +/* + * Copyright 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "sdf/usd/Conversions.hh" + +#include + +#include "sdf/Pbr.hh" + +namespace sdf +{ + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + sdf::Material convert(const ignition::common::Material *_in) + { + sdf::Material out; + out.SetEmissive(_in->Emissive()); + out.SetDiffuse(_in->Diffuse()); + out.SetSpecular(_in->Specular()); + out.SetAmbient(_in->Ambient()); + out.SetRenderOrder(_in->RenderOrder()); + out.SetLighting(_in->Lighting()); + out.SetDoubleSided(_in->TwoSidedEnabled()); + const ignition::common::Pbr * pbr = _in->PbrMaterial(); + if (pbr != nullptr) + { + out.SetNormalMap(pbr->NormalMap()); + sdf::Pbr pbrOut; + sdf::PbrWorkflow pbrWorkflow; + pbrWorkflow.SetAlbedoMap(pbr->AlbedoMap()); + pbrWorkflow.SetMetalnessMap(pbr->MetalnessMap()); + pbrWorkflow.SetEmissiveMap(pbr->EmissiveMap()); + pbrWorkflow.SetRoughnessMap(pbr->RoughnessMap()); + pbrWorkflow.SetSpecularMap(pbr->SpecularMap()); + pbrWorkflow.SetEnvironmentMap(pbr->EnvironmentMap()); + pbrWorkflow.SetAmbientOcclusionMap(pbr->AmbientOcclusionMap()); + pbrWorkflow.SetLightMap(pbr->LightMap()); + pbrWorkflow.SetRoughness(pbr->Roughness()); + pbrWorkflow.SetGlossiness(pbr->Glossiness()); + pbrWorkflow.SetMetalness(pbr->Metalness()); + + if (pbr->NormalMapType() == ignition::common::NormalMapSpace::TANGENT) + { + pbrWorkflow.SetNormalMap( + pbr->NormalMap(), sdf::NormalMapSpace::TANGENT); + } + else + { + pbrWorkflow.SetNormalMap( + pbr->NormalMap(), sdf::NormalMapSpace::OBJECT); + } + + if (pbr->Type() == ignition::common::PbrType::METAL) + { + pbrOut.SetWorkflow(sdf::PbrWorkflowType::METAL, pbrWorkflow); + } + else if (pbr->Type() == ignition::common::PbrType::SPECULAR) + { + pbrOut.SetWorkflow(sdf::PbrWorkflowType::SPECULAR, pbrWorkflow); + } + out.SetPbrMaterial(pbrOut); + } + else if (!_in->TextureImage().empty()) + { + sdf::Pbr pbrOut; + sdf::PbrWorkflow pbrWorkflow; + pbrWorkflow.SetAlbedoMap(_in->TextureImage()); + pbrOut.SetWorkflow(sdf::PbrWorkflowType::SPECULAR, pbrWorkflow); + out.SetPbrMaterial(pbrOut); + } + + return out; + } + + void convert(const sdf::Material &_in, ignition::common::Material &_out) + { + _out.SetEmissive(_in.Emissive()); + _out.SetDiffuse(_in.Diffuse()); + _out.SetSpecular(_in.Specular()); + _out.SetAmbient(_in.Ambient()); + _out.SetRenderOrder(_in.RenderOrder()); + _out.SetLighting(_in.Lighting()); + _out.SetAlphaFromTexture(false, 0.5, _in.DoubleSided()); + + const sdf::Pbr * pbr = _in.PbrMaterial(); + if (pbr != nullptr) + { + ignition::common::Pbr pbrOut; + + const sdf::PbrWorkflow * pbrWorkflow = + pbr->Workflow(sdf::PbrWorkflowType::METAL); + if (pbrWorkflow) + { + pbrOut.SetType(ignition::common::PbrType::METAL); + } + else + { + pbrWorkflow = pbr->Workflow(sdf::PbrWorkflowType::SPECULAR); + if (pbrWorkflow) + { + pbrOut.SetType(ignition::common::PbrType::SPECULAR); + } + } + if (pbrWorkflow != nullptr) + { + pbrOut.SetAlbedoMap(pbrWorkflow->AlbedoMap()); + pbrOut.SetMetalnessMap(pbrWorkflow->MetalnessMap()); + pbrOut.SetEmissiveMap(pbrWorkflow->EmissiveMap()); + pbrOut.SetRoughnessMap(pbrWorkflow->RoughnessMap()); + pbrOut.SetSpecularMap(pbrWorkflow->SpecularMap()); + pbrOut.SetEnvironmentMap(pbrWorkflow->EnvironmentMap()); + pbrOut.SetAmbientOcclusionMap(pbrWorkflow->AmbientOcclusionMap()); + pbrOut.SetLightMap(pbrWorkflow->LightMap()); + pbrOut.SetRoughness(pbrWorkflow->Roughness()); + pbrOut.SetGlossiness(pbrWorkflow->Glossiness()); + pbrOut.SetMetalness(pbrWorkflow->Metalness()); + + if (pbrWorkflow->NormalMapType() == sdf::NormalMapSpace::TANGENT) + { + pbrOut.SetNormalMap( + pbrWorkflow->NormalMap(), + ignition::common::NormalMapSpace::TANGENT); + } + else if (pbrWorkflow->NormalMapType() == sdf::NormalMapSpace::OBJECT) + { + pbrOut.SetNormalMap( + pbrWorkflow->NormalMap(), + ignition::common::NormalMapSpace::OBJECT); + } + } + _out.SetPbrMaterial(pbrOut); + } + } + } + } +} diff --git a/usd/src/Conversions_TEST.cc b/usd/src/Conversions_TEST.cc new file mode 100644 index 000000000..e5c092146 --- /dev/null +++ b/usd/src/Conversions_TEST.cc @@ -0,0 +1,161 @@ +/* + * Copyright 2022 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 + +#include "sdf/Material.hh" +#include "sdf/Pbr.hh" +#include "sdf/usd/Conversions.hh" + +///////////////////////////////////////////////// +TEST(Conversions, SdfToCommonMaterial) +{ + sdf::Material material; + material.SetEmissive(ignition::math::Color(1, 0.2, 0.2, 0.7)); + material.SetDiffuse(ignition::math::Color(0.1, 0.3, 0.4, 0.5)); + material.SetSpecular(ignition::math::Color(0.11, 0.22, 0.23, 0.77)); + material.SetAmbient(ignition::math::Color(0.25, 0.21, 0.28, 0.5)); + material.SetRenderOrder(5); + material.SetLighting(true); + material.SetDoubleSided(false); + + sdf::Pbr pbrSDF; + sdf::PbrWorkflow pbrWorkflow; + + pbrWorkflow.SetAlbedoMap("AlbedoMap"); + pbrWorkflow.SetMetalnessMap("MetalnessMap"); + pbrWorkflow.SetEmissiveMap("EmissiveMap"); + pbrWorkflow.SetRoughnessMap("RoughnessMap"); + pbrWorkflow.SetSpecularMap("SpecularMap"); + pbrWorkflow.SetEnvironmentMap("EnvironmentMap"); + pbrWorkflow.SetAmbientOcclusionMap("AmbientOcclusionMap"); + pbrWorkflow.SetLightMap("LightMap"); + pbrWorkflow.SetNormalMap( + "NormalMap", sdf::NormalMapSpace::TANGENT); + pbrWorkflow.SetRoughness(0.2); + pbrWorkflow.SetGlossiness(0.3); + pbrWorkflow.SetMetalness(0.55); + + pbrSDF.SetWorkflow(sdf::PbrWorkflowType::METAL, pbrWorkflow); + material.SetPbrMaterial(pbrSDF); + + ignition::common::Material materialCommon; + sdf::usd::convert(material, materialCommon); + const ignition::common::Pbr * pbrCommon = materialCommon.PbrMaterial(); + ASSERT_NE(nullptr, pbrCommon); + + EXPECT_EQ(material.Emissive(), materialCommon.Emissive()); + EXPECT_EQ(material.Diffuse(), materialCommon.Diffuse()); + EXPECT_EQ(material.Specular(), materialCommon.Specular()); + EXPECT_EQ(material.Ambient(), materialCommon.Ambient()); + EXPECT_FLOAT_EQ(material.RenderOrder(), + static_cast(materialCommon.RenderOrder())); + EXPECT_EQ(material.Lighting(), materialCommon.Lighting()); + EXPECT_EQ(material.DoubleSided(), materialCommon.TwoSidedEnabled()); + + EXPECT_EQ(pbrWorkflow.AlbedoMap(), pbrCommon->AlbedoMap()); + EXPECT_EQ(pbrWorkflow.MetalnessMap(), pbrCommon->MetalnessMap()); + EXPECT_EQ(pbrWorkflow.EmissiveMap(), pbrCommon->EmissiveMap()); + EXPECT_EQ(pbrWorkflow.RoughnessMap(), pbrCommon->RoughnessMap()); + EXPECT_EQ(pbrWorkflow.SpecularMap(), pbrCommon->SpecularMap()); + EXPECT_EQ(pbrWorkflow.EnvironmentMap(), pbrCommon->EnvironmentMap()); + EXPECT_EQ(pbrWorkflow.AmbientOcclusionMap(), + pbrCommon->AmbientOcclusionMap()); + EXPECT_EQ(pbrWorkflow.LightMap(), pbrCommon->LightMap()); + EXPECT_EQ(pbrWorkflow.NormalMap(), pbrCommon->NormalMap()); + + EXPECT_EQ(ignition::common::NormalMapSpace::TANGENT, + pbrCommon->NormalMapType()); + EXPECT_EQ(ignition::common::PbrType::METAL, pbrCommon->Type()); + + EXPECT_DOUBLE_EQ(pbrWorkflow.Roughness(), pbrCommon->Roughness()); + EXPECT_DOUBLE_EQ(pbrWorkflow.Glossiness(), pbrCommon->Glossiness()); + EXPECT_DOUBLE_EQ(pbrWorkflow.Metalness(), pbrCommon->Metalness()); +} + +TEST(Conversions, CommonToSdfMaterial) +{ + ignition::common::Material materialCommon; + materialCommon.SetEmissive(ignition::math::Color(1, 0.2, 0.2, 0.7)); + materialCommon.SetDiffuse(ignition::math::Color(0.1, 0.3, 0.4, 0.5)); + materialCommon.SetSpecular(ignition::math::Color(0.11, 0.22, 0.23, 0.77)); + materialCommon.SetAmbient(ignition::math::Color(0.25, 0.21, 0.28, 0.5)); + materialCommon.SetRenderOrder(5); + materialCommon.SetLighting(true); + materialCommon.SetAlphaFromTexture(false, 0.5, true); + + ignition::common::Pbr pbrCommon; + pbrCommon.SetType(ignition::common::PbrType::METAL); + + pbrCommon.SetAlbedoMap("AlbedoMap"); + pbrCommon.SetMetalnessMap("MetalnessMap"); + pbrCommon.SetEmissiveMap("EmissiveMap"); + pbrCommon.SetRoughnessMap("RoughnessMap"); + pbrCommon.SetSpecularMap("SpecularMap"); + pbrCommon.SetEnvironmentMap("EnvironmentMap"); + pbrCommon.SetAmbientOcclusionMap("AmbientOcclusionMap"); + pbrCommon.SetLightMap("LightMap"); + pbrCommon.SetNormalMap( + "NormalMap", ignition::common::NormalMapSpace::TANGENT); + pbrCommon.SetRoughness(0.2); + pbrCommon.SetGlossiness(0.3); + pbrCommon.SetMetalness(0.55); + + materialCommon.SetPbrMaterial(pbrCommon); + + const sdf::Material material = sdf::usd::convert(&materialCommon); + + const sdf::Pbr * pbr = material.PbrMaterial(); + ASSERT_NE(nullptr, pbr); + const sdf::PbrWorkflow * pbrWorkflow = + pbr->Workflow(sdf::PbrWorkflowType::METAL); + ASSERT_NE(nullptr, pbrWorkflow); + + EXPECT_EQ(material.Emissive(), materialCommon.Emissive()); + EXPECT_EQ(material.Diffuse(), materialCommon.Diffuse()); + EXPECT_EQ(material.Specular(), materialCommon.Specular()); + EXPECT_EQ(material.Ambient(), materialCommon.Ambient()); + EXPECT_FLOAT_EQ(material.RenderOrder(), + static_cast(materialCommon.RenderOrder())); + EXPECT_EQ(material.Lighting(), materialCommon.Lighting()); + EXPECT_EQ(material.DoubleSided(), materialCommon.TwoSidedEnabled()); + + EXPECT_EQ(pbrWorkflow->AlbedoMap(), pbrCommon.AlbedoMap()); + EXPECT_EQ(pbrWorkflow->MetalnessMap(), pbrCommon.MetalnessMap()); + EXPECT_EQ(pbrWorkflow->EmissiveMap(), pbrCommon.EmissiveMap()); + EXPECT_EQ(pbrWorkflow->RoughnessMap(), pbrCommon.RoughnessMap()); + EXPECT_EQ(pbrWorkflow->SpecularMap(), pbrCommon.SpecularMap()); + EXPECT_EQ(pbrWorkflow->EnvironmentMap(), pbrCommon.EnvironmentMap()); + EXPECT_EQ(pbrWorkflow->AmbientOcclusionMap(), + pbrCommon.AmbientOcclusionMap()); + EXPECT_EQ(pbrWorkflow->LightMap(), pbrCommon.LightMap()); + EXPECT_EQ(pbrWorkflow->NormalMap(), pbrCommon.NormalMap()); + + EXPECT_EQ(ignition::common::NormalMapSpace::TANGENT, + pbrCommon.NormalMapType()); + EXPECT_EQ(ignition::common::PbrType::METAL, pbrCommon.Type()); + + EXPECT_DOUBLE_EQ(pbrWorkflow->Roughness(), pbrCommon.Roughness()); + EXPECT_DOUBLE_EQ(pbrWorkflow->Glossiness(), pbrCommon.Glossiness()); + EXPECT_DOUBLE_EQ(pbrWorkflow->Metalness(), pbrCommon.Metalness()); +} diff --git a/usd/src/UsdTestUtils.hh b/usd/src/UsdTestUtils.hh index 48b0b4add..3224d76cd 100644 --- a/usd/src/UsdTestUtils.hh +++ b/usd/src/UsdTestUtils.hh @@ -17,11 +17,15 @@ #ifndef SDF_PARSER_USDTESTUTILS_HH_ #define SDF_PARSER_USDTESTUTILS_HH_ +#include + #include +#include +#include #include #include -// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings // are reported using #pragma message so normal diagnostic flags cannot remove // them. This workaround requires this block to be used whenever usd is // included. @@ -48,6 +52,33 @@ namespace usd { namespace testing { +/// \brief Callback for finding a file in the `test/sdf` directory +/// \param[in] _input The path to the file. This path should be relative to the +/// `test/sdf` directory +/// \return The full path to the requested file +std::string findFileCb(const std::string &_input) +{ + return sdf::testing::TestFile("sdf", _input); +} + +/// \brief This function is used by ignition::common::addFindFileURICallback to +/// find the resources defined in the URI +/// \param[in] _uri URI of the file to find +/// \return The full path to the uri. Empty +/// string is returned if the file could not be found. +std::string FindResourceUri(const ignition::common::URI &_uri) +{ + std::string prefix = _uri.Scheme(); + std::string suffix; + // Strip / + if (_uri.Path().IsAbsolute() && prefix != "file") + suffix += _uri.Path().Str().substr(1); + else + suffix += _uri.Path().Str(); + suffix += _uri.Query().Str(); + + return findFileCb(ignition::common::copyFromUnixPath(suffix)); +} /// \brief Compare the pose of a USD prim to a desired pose /// \param[in] _usdPrim The USD prim @@ -94,10 +125,6 @@ void CheckPrimPose(const pxr::UsdPrim &_usdPrim, { pxr::VtArray opNames; opOrderAttr.Get(&opNames); - // TODO(adlarkin) handle things like scale in the opOrder - // (checking for scale should be done elsehwere since prims aren't always - // scaled, but maybe what I can do here is make sure the opNames size is - // at least 2 and then make sure translate occurs before rotate) ASSERT_EQ(2u, opNames.size()); EXPECT_EQ(pxr::TfToken("xformOp:translate"), opNames[0]); EXPECT_EQ(pxr::TfToken("xformOp:rotateXYZ"), opNames[1]); @@ -106,6 +133,30 @@ void CheckPrimPose(const pxr::UsdPrim &_usdPrim, EXPECT_TRUE(checkedOpOrder); } +/// \brief Make sure a USD prim has a scale xFormOp applied to it +/// \param[in] _usdPrim The USD prim +void HasScaleXFormOp(const pxr::UsdPrim &_usdPrim) +{ + bool checkedScaleOp = false; + if (auto xFormOps = _usdPrim.GetAttribute(pxr::TfToken("xformOpOrder"))) + { + pxr::VtArray opNames; + xFormOps.Get(&opNames); + bool hasScaleOp = false; + for (const auto &tokenName : opNames) + { + if (tokenName == pxr::TfToken("xformOp:scale")) + { + hasScaleOp = true; + break; + } + } + EXPECT_TRUE(hasScaleOp); + checkedScaleOp = true; + } + EXPECT_TRUE(checkedScaleOp); +} + /// \brief Compare the Inertial of a USD prim to the desired values /// \param[in] _usdPrim The USD prim /// \param[in] _targetMass Mass of the link that _usdPrim should have diff --git a/usd/src/UsdUtils.hh b/usd/src/UsdUtils.hh index cb5d4b9aa..f989cda67 100644 --- a/usd/src/UsdUtils.hh +++ b/usd/src/UsdUtils.hh @@ -42,7 +42,6 @@ #include "sdf/SemanticPose.hh" #include "sdf/Visual.hh" #include "sdf/system_util.hh" -#include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" namespace sdf @@ -60,8 +59,7 @@ namespace sdf /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error. template - inline UsdErrors IGNITION_SDFORMAT_USD_VISIBLE - PoseWrtParent(const T &_obj, ignition::math::Pose3d &_pose) + inline UsdErrors PoseWrtParent(const T &_obj, ignition::math::Pose3d &_pose) { UsdErrors errors; const auto poseResolutionErrors = _obj.SemanticPose().Resolve(_pose, ""); @@ -85,8 +83,7 @@ namespace sdf /// pose modified to match _pose. /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error. - inline UsdErrors IGNITION_SDFORMAT_USD_VISIBLE SetPose( - const ignition::math::Pose3d &_pose, + inline UsdErrors SetPose(const ignition::math::Pose3d &_pose, pxr::UsdStageRefPtr &_stage, const pxr::SdfPath &_usdPath) { @@ -132,7 +129,7 @@ namespace sdf /// and one collision that have a plane geometry. False otherwise /// \note This method will no longer be needed when a pxr::USDGeomPlane /// class is created - inline bool SDFORMAT_VISIBLE IsPlane(const sdf::Model &_model) + inline bool IsPlane(const sdf::Model &_model) { if (!_model.Static() || _model.LinkCount() != 1u) return false; @@ -148,6 +145,10 @@ namespace sdf const auto &collision = link->CollisionByIndex(0u); return collision->Geom()->Type() == sdf::GeometryType::PLANE; } + + /// \brief Pre-defined USD plane thickness. This is a temporary variable + /// that will no longer be needed once USD supports their own plane class + static const double kPlaneThickness = 0.25; } } } diff --git a/usd/src/cmd/sdf2usd.cc b/usd/src/cmd/sdf2usd.cc index c6f03353a..5ca0d7131 100644 --- a/usd/src/cmd/sdf2usd.cc +++ b/usd/src/cmd/sdf2usd.cc @@ -17,11 +17,15 @@ #include -// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +#include +#include + +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings // are reported using #pragma message so normal diagnostic flags cannot remove // them. This workaround requires this block to be used whenever usd is // included. -#include #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED #include @@ -51,8 +55,164 @@ struct Options std::string outputFilename{"output.usd"}; }; +////////////////////////////////////////////////// +/// \brief Get the full path of a file +/// \param[in] _path Where to begin searching for the file +/// \param[in] _name The name of the file to find +/// \return The full path to the file named _name. Empty string is returned if +/// the file could not be found. +std::string findFileByName(const std::string &_path, const std::string &_name) +{ + for (ignition::common::DirIter file(_path); + file != ignition::common::DirIter(); ++file) + { + std::string current(*file); + + if (ignition::common::isDirectory(current)) + { + std::string result = findFileByName(current, _name); + if (result.empty()) + { + continue; + } + return result; + } + + if (!ignition::common::isFile(current)) + continue; + + auto fileName = ignition::common::basename(current); + + if (fileName == _name) + { + return current; + } + } + return ""; +} + +////////////////////////////////////////////////// +/// \brief Get the full path of a file based on the extension +/// \param[in] _path Where to begin searching for the file +/// \param[in] _extension The extension of the file +/// \param[in] _insertDirectories Whether subdirectories should be inserted as +/// needed when looking for the file (true) or not (false) +/// \return The full path to the file with an extension _extension. Empty +/// string is returned if the file could not be found. +std::string findFileByExtension( + const std::string &_path, const std::string &_extension, + bool _insertDirectories = false) +{ + if (_insertDirectories) + { + for (ignition::common::DirIter file(_path); + file != ignition::common::DirIter(); ++file) + { + std::string current(*file); + if (ignition::common::isDirectory(current)) + { + auto systemPaths = ignition::common::systemPaths(); + systemPaths->AddFilePaths(current); + findFileByExtension(current, "", true); + } + } + } + + for (ignition::common::DirIter file(_path); + file != ignition::common::DirIter(); ++file) + { + std::string current(*file); + + if (ignition::common::isDirectory(current)) + { + std::string result = findFileByExtension(current, _extension); + if (result.empty()) + { + continue; + } + return result; + } + + if (!ignition::common::isFile(current)) + continue; + + auto fileName = ignition::common::basename(current); + auto fileExtensionIndex = fileName.rfind("."); + auto fileExtension = fileName.substr(fileExtensionIndex + 1); + + if (fileExtension == _extension) + { + return current; + } + } + return ""; +} + +////////////////////////////////////////////////// +/// \brief This functions is used by sdf::setFindCallback to find +/// the resources defined in the URI +/// \param[in] _uri URI of the file to find +/// \return The full path to the uri. Empty +/// string is returned if the file could not be found. +std::string FindResources(const std::string &_uri) +{ + ignition::common::URI uri(_uri); + std::string path; + std::string home; + if (!ignition::common::env("HOME", home, false)) + { + std::cerr << "The HOME environment variable was not defined, " + << "so the resource [" << _uri << "] could not be found\n"; + return ""; + } + if (uri.Scheme() == "http" || uri.Scheme() == "https") + { + std::vector tokens = + ignition::common::split(uri.Path().Str(), "/"); + const std::string server = tokens[0]; + const std::string versionServer = tokens[1]; + const std::string owner = ignition::common::lowercase(tokens[2]); + const std::string type = ignition::common::lowercase(tokens[3]); + const std::string modelName = ignition::common::lowercase(tokens[4]); + path = ignition::common::joinPaths( + home, ".ignition", "fuel", server, owner, type, modelName); + } + else + { + path = ignition::common::joinPaths(home, ".ignition", "fuel"); + } + + auto fileName = ignition::common::basename(uri.Path().Str()); + auto fileExtensionIndex = fileName.rfind("."); + if (fileExtensionIndex == std::string::npos) + { + return findFileByExtension(path, "sdf", true); + } + else + { + return findFileByName(path, fileName); + } + return ""; +} + +////////////////////////////////////////////////// +/// \brief This function is used by ignition::common::addFindFileURICallback to +/// find the resources defined in the URI +/// \param[in] _uri URI of the file to find +/// \return The full path to the uri. Empty +/// string is returned if the file could not be found. +std::string FindResourceUri(const ignition::common::URI &_uri) +{ + return FindResources(_uri.Str()); +} + void runCommand(const Options &_opt) { + // Configure SDF to fetch assets from ignition fuel. + sdf::setFindCallback(std::bind(&FindResources, std::placeholders::_1)); + ignition::common::addFindFileURICallback( + std::bind(&FindResourceUri, std::placeholders::_1)); + sdf::Root root; auto errors = root.Load(_opt.inputFilename); if (!errors.empty()) @@ -85,8 +245,8 @@ void runCommand(const Options &_opt) if (!usdErrors.empty()) { std::cerr << "The following errors occurred when parsing world [" - << world->Name() << "]\n:"; - for (const auto &e : errors) + << world->Name() << "]:" << std::endl; + for (const auto &e : usdErrors) std::cout << e << "\n"; exit(-5); } diff --git a/usd/src/sdf_parser/Geometry.cc b/usd/src/sdf_parser/Geometry.cc new file mode 100644 index 000000000..775a1d592 --- /dev/null +++ b/usd/src/sdf_parser/Geometry.cc @@ -0,0 +1,641 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "sdf/usd/sdf_parser/Geometry.hh" + +#include + +#include +#include +#include +#include +#include + +#include + +// TODO(adlarkin) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Box.hh" +#include "sdf/Capsule.hh" +#include "sdf/Cylinder.hh" +#include "sdf/Ellipsoid.hh" +#include "sdf/Geometry.hh" +#include "sdf/Mesh.hh" +#include "sdf/Plane.hh" +#include "sdf/Sphere.hh" +#include "sdf/usd/Conversions.hh" +#include "sdf/usd/sdf_parser/Material.hh" + +#include "../UsdUtils.hh" + +namespace sdf +{ +// Inline bracke to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + /// \brief Parse a SDF box geometry into a USD box geometry + /// \param[in] _geometry The SDF box geometry + /// \param[in] _stage The stage that will contain the parsed USD equivalent + /// of _geometry + /// \param[in] _path Where the parsed USD equivalent of _geometry should exist + /// in _stage + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when creating the USD equivalent of _geometry + UsdErrors ParseSdfBoxGeometry(const sdf::Geometry &_geometry, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + const auto &sdfBox = _geometry.BoxShape(); + + // USD defines a cube (i.e., a box with all dimensions being equal), + // but not a box. So, we will take a 1x1x1 cube and scale it according + // to the SDF box's dimensions to achieve varying dimensions + auto usdCube = pxr::UsdGeomCube::Define(_stage, pxr::SdfPath(_path)); + if (!usdCube) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Unable to define a USD cube geometry at path [" + _path + "]")); + return errors; + } + + usdCube.CreateSizeAttr().Set(1.0); + pxr::GfVec3f endPoint(0.5); + pxr::VtArray extentBounds; + extentBounds.push_back(-1.0 * endPoint); + extentBounds.push_back(endPoint); + usdCube.CreateExtentAttr().Set(extentBounds); + + pxr::UsdGeomXformCommonAPI cubeXformAPI(usdCube); + cubeXformAPI.SetScale(pxr::GfVec3f( + sdfBox->Size().X(), + sdfBox->Size().Y(), + sdfBox->Size().Z())); + + return errors; + } + + /// \brief Parse a SDF cylinder geometry into a USD cylinder geometry + /// \param[in] _geometry The SDF cylinder geometry + /// \param[in] _stage The stage that will contain the parsed USD equivalent + /// of _geometry + /// \param[in] _path Where the parsed USD equivalent of _geometry should exist + /// in _stage + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when creating the USD equivalent of _geometry + UsdErrors ParseSdfCylinderGeometry(const sdf::Geometry &_geometry, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + const auto &sdfCylinder = _geometry.CylinderShape(); + + auto usdCylinder = + pxr::UsdGeomCylinder::Define(_stage, pxr::SdfPath(_path)); + if (!usdCylinder) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Unable to define a USD cylinder geometry at path [" + _path + "]")); + return errors; + } + + usdCylinder.CreateRadiusAttr().Set(sdfCylinder->Radius()); + usdCylinder.CreateHeightAttr().Set(sdfCylinder->Length()); + pxr::GfVec3f endPoint(sdfCylinder->Radius()); + endPoint[2] = sdfCylinder->Length() * 0.5; + pxr::VtArray extentBounds; + extentBounds.push_back(-1.0 * endPoint); + extentBounds.push_back(endPoint); + usdCylinder.CreateExtentAttr().Set(extentBounds); + + return errors; + } + + /// \brief Parse a SDF sphere geometry into a USD sphere geometry + /// \param[in] _geometry The SDF sphere geometry + /// \param[in] _stage The stage that will contain the parsed USD equivalent + /// of _geometry + /// \param[in] _path Where the parsed USD equivalent of _geometry should exist + /// in _stage + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when creating the USD equivalent of _geometry + UsdErrors ParseSdfSphereGeometry(const sdf::Geometry &_geometry, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + const auto &sdfSphere = _geometry.SphereShape(); + + auto usdSphere = pxr::UsdGeomSphere::Define(_stage, pxr::SdfPath(_path)); + if (!usdSphere) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Unable to define a USD sphere geometry at path [" + _path + "]")); + return errors; + } + + usdSphere.CreateRadiusAttr().Set(sdfSphere->Radius()); + pxr::VtArray extentBounds; + extentBounds.push_back(pxr::GfVec3f(-1.0 * sdfSphere->Radius())); + extentBounds.push_back(pxr::GfVec3f(sdfSphere->Radius())); + usdSphere.CreateExtentAttr().Set(extentBounds); + + return errors; + } + + /// \brief Parse a SDF mesh geometry into a USD mesh geometry + /// \param[in] _geometry The SDF mesh geometry + /// \param[in] _stage The stage that will contain the parsed USD equivalent + /// of _geometry + /// \param[in] _path Where the parsed USD equivalent of _geometry should exist + /// in _stage + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when creating the USD equivalent of _geometry + UsdErrors ParseSdfMeshGeometry(const sdf::Geometry &_geometry, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + ignition::common::URI uri(_geometry.MeshShape()->Uri()); + std::string fullName; + + if (uri.Scheme() == "https" || uri.Scheme() == "http") + { + fullName = + ignition::common::findFile(uri.Str()); + } + else + { + fullName = + ignition::common::findFile(_geometry.MeshShape()->Uri()); + } + + auto ignMesh = ignition::common::MeshManager::Instance()->Load( + fullName); + if (!ignMesh) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::MESH_LOAD_FAILURE, + "Unable to load mesh named [" + fullName + "]")); + return errors; + } + + // If a mesh has more than one submesh, only process the submesh who's name + // is a part of the USD path + // TODO(adlarkin) figure out if this workaround is actually required. There + // seemed to be a model that required this workaround, but ahcorde and I + // cannot remember the particular model that we tested. + // Most meshes only have one submesh, so this workaround will be ignored for + // most meshes + bool subMeshNameInUsdPath = false; + std::string targetSubMeshName = ""; + if (ignMesh->SubMeshCount() > 1) + { + for (unsigned int i = 0; i < ignMesh->SubMeshCount(); ++i) + { + auto subMesh = ignMesh->SubMeshByIndex(i).lock(); + if (!subMesh) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::MESH_LOAD_FAILURE, + "Unable to get a shared pointer to submesh at index [" + + std::to_string(i) + "] of parent mesh [" + ignMesh->Name() + + "]")); + return errors; + } + + std::string pathLowerCase = ignition::common::lowercase(_path); + std::string subMeshLowerCase = + ignition::common::lowercase(subMesh->Name()); + + if (pathLowerCase.find(subMeshLowerCase) != std::string::npos) + { + subMeshNameInUsdPath = true; + targetSubMeshName = subMesh->Name(); + break; + } + } + } + + for (unsigned int i = 0; i < ignMesh->SubMeshCount(); ++i) + { + pxr::VtArray meshPoints; + pxr::VtArray uvs; + pxr::VtArray normals; + pxr::VtArray faceVertexIndices; + pxr::VtArray faceVertexCounts; + + auto subMesh = ignMesh->SubMeshByIndex(i).lock(); + if (!subMesh) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::MESH_LOAD_FAILURE, + "Unable to get a shared pointer to submesh at index [" + + std::to_string(i) + "] of parent mesh [" + ignMesh->Name() + + "]")); + return errors; + } + + if (subMeshNameInUsdPath && (subMesh->Name() != targetSubMeshName)) + { + continue; + } + + // copy the submesh's vertices to the usd mesh's "points" array + for (unsigned int v = 0; v < subMesh->VertexCount(); ++v) + { + const auto &vertex = subMesh->Vertex(v); + meshPoints.push_back(pxr::GfVec3f(vertex.X(), vertex.Y(), vertex.Z())); + } + + // copy the submesh's indices to the usd mesh's "faceVertexIndices" array + for (unsigned int j = 0; j < subMesh->IndexCount(); ++j) + faceVertexIndices.push_back(subMesh->Index(j)); + + // copy the submesh's texture coordinates + for (unsigned int j = 0; j < subMesh->TexCoordCount(); ++j) + { + const auto &uv = subMesh->TexCoord(j); + uvs.push_back(pxr::GfVec2f(uv[0], 1 - uv[1])); + } + + // copy the submesh's normals + for (unsigned int j = 0; j < subMesh->NormalCount(); ++j) + { + const auto &normal = subMesh->Normal(j); + normals.push_back(pxr::GfVec3f(normal[0], normal[1], normal[2])); + } + + // set the usd mesh's "faceVertexCounts" array according to + // the submesh primitive type + // TODO(adlarkin) support all primitive types. The computations are more + // involved for LINESTRIPS, TRIFANS, and TRISTRIPS. I will need to spend + // some time deriving what the number of faces for these primitive types + // are, given the number of indices. The "faceVertexCounts" array will + // also not have the same value for every element in the array for these + // more complex primitive types (see the TODO note in the for loop below) + unsigned int verticesPerFace = 0; + unsigned int numFaces = 0; + switch (subMesh->SubMeshPrimitiveType()) + { + case ignition::common::SubMesh::PrimitiveType::POINTS: + verticesPerFace = 1; + numFaces = subMesh->IndexCount(); + break; + case ignition::common::SubMesh::PrimitiveType::LINES: + verticesPerFace = 2; + numFaces = subMesh->IndexCount() / 2; + break; + case ignition::common::SubMesh::PrimitiveType::TRIANGLES: + verticesPerFace = 3; + numFaces = subMesh->IndexCount() / 3; + break; + case ignition::common::SubMesh::PrimitiveType::LINESTRIPS: + case ignition::common::SubMesh::PrimitiveType::TRIFANS: + case ignition::common::SubMesh::PrimitiveType::TRISTRIPS: + default: + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::INVALID_SUBMESH_PRIMITIVE_TYPE, + "Submesh " + subMesh->Name() + " has a primitive type that is " + "not supported.")); + return errors; + } + // TODO(adlarkin) update this loop to allow for varying element + // values in the array (see TODO note above). Right now, the + // array only allows for all elements to have one value, which in + // this case is "verticesPerFace" + for (unsigned int n = 0; n < numFaces; ++n) + faceVertexCounts.push_back(verticesPerFace); + + std::string primName; + if (!subMesh->Name().empty()) + primName = _path + "/" + subMesh->Name(); + else + primName = _path + "/submesh_" + std::to_string(i); + + primName = ignition::common::replaceAll(primName, "-", "_"); + + auto usdMesh = pxr::UsdGeomMesh::Define(_stage, pxr::SdfPath(primName)); + if (!usdMesh) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Unable to define a USD mesh geometry at path [" + primName + "]")); + return errors; + } + usdMesh.CreatePointsAttr().Set(meshPoints); + usdMesh.CreateFaceVertexIndicesAttr().Set(faceVertexIndices); + usdMesh.CreateFaceVertexCountsAttr().Set(faceVertexCounts); + + auto coordinates = usdMesh.CreatePrimvar( + pxr::TfToken("st"), pxr::SdfValueTypeNames->Float2Array, + pxr::UsdGeomTokens->vertex); + coordinates.Set(uvs); + + usdMesh.CreateNormalsAttr().Set(normals); + usdMesh.SetNormalsInterpolation(pxr::TfToken("vertex")); + + usdMesh.CreateSubdivisionSchemeAttr(pxr::VtValue(pxr::TfToken("none"))); + + const auto &meshMin = ignMesh->Min(); + const auto &meshMax = ignMesh->Max(); + pxr::VtArray extentBounds; + extentBounds.push_back( + pxr::GfVec3f(meshMin.X(), meshMin.Y(), meshMin.Z())); + extentBounds.push_back( + pxr::GfVec3f(meshMax.X(), meshMax.Y(), meshMax.Z())); + usdMesh.CreateExtentAttr().Set(extentBounds); + + // TODO(adlarkin) update this call in sdf13 to avoid casting the index to + // an int: + // https://github.com/ignitionrobotics/ign-common/pull/319 + int materialIndex = subMesh->MaterialIndex(); + if (materialIndex != -1) + { + const auto material = ignMesh->MaterialByIndex(materialIndex).get(); + const sdf::Material materialSdf = sdf::usd::convert(material); + pxr::SdfPath materialPath; + UsdErrors materialErrors = ParseSdfMaterial( + &materialSdf, _stage, materialPath); + if (!materialErrors.empty()) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Unable to convert material [" + std::to_string(materialIndex) + + "] of submesh named [" + subMesh->Name() + + "] to a USD material.")); + return errors; + } + + auto materialPrim = _stage->GetPrimAtPath(materialPath); + if (!materialPrim) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Unable to get material prim at path [" + + materialPath.GetString() + + "], but a prim should exist at this path.")); + return errors; + } + + auto materialUSD = pxr::UsdShadeMaterial(materialPrim); + if (materialUSD && + (materialSdf.Emissive() != ignition::math::Color(0, 0, 0, 1) + || materialSdf.Specular() != ignition::math::Color(0, 0, 0, 1) + || materialSdf.PbrMaterial())) + { + pxr::UsdShadeMaterialBindingAPI(usdMesh).Bind(materialUSD); + } + else if (!materialUSD) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "The prim at path [" + materialPath.GetString() + + "] is not a pxr::UsdShadeMaterial object.")); + return errors; + } + } + + pxr::UsdGeomXformCommonAPI xform(usdMesh); + ignition::math::Vector3d scale = _geometry.MeshShape()->Scale(); + xform.SetScale(pxr::GfVec3f{ + static_cast(scale.X()), + static_cast(scale.Y()), + static_cast(scale.Z()), + }); + } + + return errors; + } + + /// \brief Parse a SDF capsule geometry into a USD capsule geometry + /// \param[in] _geometry The SDF capsule geometry + /// \param[in] _stage The stage that will contain the parsed USD equivalent + /// of _geometry + /// \param[in] _path Where the parsed USD equivalent of _geometry should exist + /// in _stage + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when creating the USD equivalent of _geometry + UsdErrors ParseSdfCapsuleGeometry(const sdf::Geometry &_geometry, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + const auto &sdfCapsule = _geometry.CapsuleShape(); + + auto usdCapsule = pxr::UsdGeomCapsule::Define(_stage, pxr::SdfPath(_path)); + if (!usdCapsule) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Unable to define a USD capsule geometry at path [" + _path + "]")); + return errors; + } + + usdCapsule.CreateRadiusAttr().Set(sdfCapsule->Radius()); + usdCapsule.CreateHeightAttr().Set(sdfCapsule->Length()); + pxr::GfVec3f endPoint(sdfCapsule->Radius()); + endPoint[2] += 0.5 * sdfCapsule->Length(); + pxr::VtArray extentBounds; + extentBounds.push_back(-1.0 * endPoint); + extentBounds.push_back(endPoint); + usdCapsule.CreateExtentAttr().Set(extentBounds); + + return errors; + } + + /// \brief Parse a SDF plane geometry into a USD plane geometry + /// \param[in] _geometry The SDF plane geometry + /// \param[in] _stage The stage that will contain the parsed USD equivalent + /// of _geometry + /// \param[in] _path Where the parsed USD equivalent of _geometry should exist + /// in _stage + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when creating the USD equivalent of _geometry + UsdErrors ParseSdfPlaneGeometry(const sdf::Geometry &_geometry, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + const auto &sdfPlane = _geometry.PlaneShape(); + + // Currently, there is no USDGeom plane class (it will be added in the + // future - see + // https://graphics.pixar.com/usd/release/wp_rigid_body_physics.html#plane-shapes), + // so the current workaround is to make a large, thin box. + // To keep things simple for now, a plane will only be parsed if its normal + // is the unit z vector (0, 0, 1). + // TODO(adlarkin) support plane normals other than the unit z vector + // (can update comment above once this functionality is added) + // TODO(adlarkin) update this to use the pxr::USDGeomPlane class when it's + // added + if (sdfPlane->Normal() != ignition::math::Vector3d::UnitZ) + { + errors.push_back(UsdError( + sdf::Error(sdf::ErrorCode::ATTRIBUTE_INCORRECT_TYPE, + "Only planes with a unit z vector normal are supported."))); + return errors; + } + + sdf::Box box; + box.SetSize(ignition::math::Vector3d( + sdfPlane->Size().X(), sdfPlane->Size().Y(), usd::kPlaneThickness)); + + sdf::Geometry planeBoxGeometry; + planeBoxGeometry.SetType(sdf::GeometryType::BOX); + planeBoxGeometry.SetBoxShape(box); + + return ParseSdfBoxGeometry(planeBoxGeometry, _stage, _path); + } + + /// \brief Parse a SDF ellipsoid geometry into a USD ellipsoid geometry + /// \param[in] _geometry The SDF ellipsoid geometry + /// \param[in] _stage The stage that will contain the parsed USD equivalent + /// of _geometry + /// \param[in] _path Where the parsed USD equivalent of _geometry should exist + /// in _stage + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when creating the USD equivalent of _geometry + UsdErrors ParseSdfEllipsoid(const sdf::Geometry &_geometry, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + const auto sdfEllipsoid = _geometry.EllipsoidShape(); + + auto usdEllipsoid = pxr::UsdGeomSphere::Define(_stage, pxr::SdfPath(_path)); + if (!usdEllipsoid) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Unable to define a USD ellipsoid geometry at path [" + _path + "]")); + return errors; + } + + usdEllipsoid.CreateRadiusAttr().Set(1.0); + pxr::UsdGeomXformCommonAPI xform(usdEllipsoid); + xform.SetScale(pxr::GfVec3f{ + static_cast(sdfEllipsoid->Radii().X()), + static_cast(sdfEllipsoid->Radii().Y()), + static_cast(sdfEllipsoid->Radii().Z()), + }); + // extents is the bounds before any transformation + pxr::VtArray extentBounds; + extentBounds.push_back(pxr::GfVec3f{-1.0f}); + extentBounds.push_back(pxr::GfVec3f{1.0f}); + usdEllipsoid.CreateExtentAttr().Set(extentBounds); + + return errors; + } + + UsdErrors ParseSdfGeometry(const sdf::Geometry &_geometry, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + switch (_geometry.Type()) + { + case sdf::GeometryType::BOX: + errors = ParseSdfBoxGeometry(_geometry, _stage, _path); + break; + case sdf::GeometryType::CYLINDER: + errors = ParseSdfCylinderGeometry(_geometry, _stage, _path); + break; + case sdf::GeometryType::SPHERE: + errors = ParseSdfSphereGeometry(_geometry, _stage, _path); + break; + case sdf::GeometryType::MESH: + errors = ParseSdfMeshGeometry(_geometry, _stage, _path); + break; + case sdf::GeometryType::CAPSULE: + errors = ParseSdfCapsuleGeometry(_geometry, _stage, _path); + break; + case sdf::GeometryType::PLANE: + errors = ParseSdfPlaneGeometry(_geometry, _stage, _path); + break; + case sdf::GeometryType::ELLIPSOID: + errors = ParseSdfEllipsoid(_geometry, _stage, _path); + break; + case sdf::GeometryType::HEIGHTMAP: + default: + errors.push_back(UsdError( + sdf::Error(sdf::ErrorCode::ATTRIBUTE_INCORRECT_TYPE, + "Geometry type is either invalid or not supported"))); + } + + // Set the collision for this geometry. + // In SDF, the collisions of a link are defined separately from + // the link's visual geometries (in case a user wants to define a simpler + // collision, for example). In USD, the collision can be attached to the + // geometry so that the collision shape is the geometry shape. + // The current approach that's taken here (attaching a collision to the + // geometry) assumes that for every visual in a link, the visual should have + // a collision that matches its geometry. This approach currently ignores + // collisions defined in a link since it instead creates collisions for a + // link that match a link's visual geometries. + // TODO(adlarkin) support the option of a different collision shape (i.e., + // don't ignore collisions defined in a link), + // especially for meshes - here's more information about how to do this: + // https://graphics.pixar.com/usd/release/wp_rigid_body_physics.html?highlight=collision#turning-meshes-into-shapes + if (errors.empty()) + { + auto geomPrim = _stage->GetPrimAtPath(pxr::SdfPath(_path)); + if (!geomPrim) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Internal error: unable to get prim at path [" + + _path + "], but a geom prim should exist at this path")); + return errors; + } + + if (!pxr::UsdPhysicsCollisionAPI::Apply(geomPrim)) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, + "Internal error: unable to apply a collision to the prim at path [" + + _path + "]")); + return errors; + } + } + + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc new file mode 100644 index 000000000..e167491e9 --- /dev/null +++ b/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc @@ -0,0 +1,296 @@ +/* + * Copyright 2022 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 + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/sdf_parser/World.hh" +#include "sdf/Root.hh" +#include "test_config.h" +#include "test_utils.hh" +#include "../UsdTestUtils.hh" + +///////////////////////////////////////////////// +// Fixture that creates a USD stage for each test case. +class UsdStageFixture : public::testing::Test +{ + public: UsdStageFixture() = default; + + /// \brief Check a USD geometry's extent. + /// \param[in] _extent The USD geometry extent + /// \param[in] _targetMin The desired minimum extent + /// \param[in] _targetMax The desired maximum extent + public: void CheckExtent(const pxr::VtArray &_extent, + const pxr::GfVec3f &_targetMin, + const pxr::GfVec3f &_targetMax) const + { + // make sure _extend has 2 points: min and max + ASSERT_EQ(2u, _extent.size()); + + const auto extentMin = _extent[0]; + EXPECT_FLOAT_EQ(extentMin[0], _targetMin[0]); + EXPECT_FLOAT_EQ(extentMin[1], _targetMin[1]); + EXPECT_FLOAT_EQ(extentMin[2], _targetMin[2]); + + const auto extentMax = _extent[1]; + EXPECT_FLOAT_EQ(extentMax[0], _targetMax[0]); + EXPECT_FLOAT_EQ(extentMax[1], _targetMax[1]); + EXPECT_FLOAT_EQ(extentMax[2], _targetMax[2]); + } + + protected: void SetUp() override + { + this->stage = pxr::UsdStage::CreateInMemory(); + ASSERT_TRUE(this->stage); + } + + public: pxr::UsdStageRefPtr stage; +}; + +///////////////////////////////////////////////// +TEST_F(UsdStageFixture, Ellipsoid) +{ + const auto testFile = sdf::testing::TestFile("sdf", "ellipsoid.sdf"); + + sdf::Root root; + ASSERT_TRUE(sdf::testing::LoadSdfFile(testFile, root)); + ASSERT_EQ(1u, root.WorldCount()); + const auto world = root.WorldByIndex(0u); + + const auto worldPath = std::string("/" + world->Name()); + const auto usdErrors = sdf::usd::ParseSdfWorld(*world, stage, worldPath); + EXPECT_TRUE(usdErrors.empty()); + + const auto usdSphere = pxr::UsdGeomSphere::Define( + this->stage, + pxr::SdfPath("/ellipsoid_world/ellipsoid/ellipsoid_link/" + "ellipsoid_visual/geometry")); + ASSERT_TRUE(usdSphere); + bool resetXformStack; + const auto xformOps = usdSphere.GetOrderedXformOps(&resetXformStack); + ASSERT_EQ(xformOps.size(), 1u); + EXPECT_FALSE(resetXformStack); + const auto scaleOp = xformOps[0]; + EXPECT_EQ(scaleOp.GetOpType(), pxr::UsdGeomXformOp::TypeScale); + pxr::GfVec3f scaleValue; + scaleOp.Get(&scaleValue); + EXPECT_FLOAT_EQ(scaleValue[0], 0.2); + EXPECT_FLOAT_EQ(scaleValue[1], 0.3); + EXPECT_FLOAT_EQ(scaleValue[2], 0.5); + + double radius; + EXPECT_TRUE(usdSphere.GetRadiusAttr().Get(&radius)); + EXPECT_DOUBLE_EQ(radius, 1); + + pxr::VtArray extent; + usdSphere.GetExtentAttr().Get(&extent); + ASSERT_EQ(extent.size(), 2u); + EXPECT_FLOAT_EQ(extent[0][0], -1.0f); + EXPECT_FLOAT_EQ(extent[0][1], -1.0f); + EXPECT_FLOAT_EQ(extent[0][2], -1.0f); + EXPECT_FLOAT_EQ(extent[1][0], 1.0f); + EXPECT_FLOAT_EQ(extent[1][1], 1.0f); + EXPECT_FLOAT_EQ(extent[1][2], 1.0f); +} + +///////////////////////////////////////////////// +TEST_F(UsdStageFixture, Geometry) +{ + sdf::setFindCallback(sdf::usd::testing::findFileCb); + ignition::common::addFindFileURICallback( + std::bind(&sdf::usd::testing::FindResourceUri, std::placeholders::_1)); + + const auto path = sdf::testing::TestFile("sdf", "basic_shapes.sdf"); + sdf::Root root; + + ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root)); + ASSERT_EQ(1u, root.WorldCount()); + const auto world = root.WorldByIndex(0u); + + const auto worldPath = std::string("/" + world->Name()); + const auto usdErrors = sdf::usd::ParseSdfWorld(*world, stage, worldPath); + EXPECT_TRUE(usdErrors.empty()); + + const auto worldPrim = this->stage->GetPrimAtPath(pxr::SdfPath(worldPath)); + ASSERT_TRUE(worldPrim); + + const std::string groundPlanePath = worldPath + "/ground_plane"; + const std::string groundPlaneLinkPath = groundPlanePath + "/link"; + const std::string groundPlaneVisualPath = groundPlaneLinkPath + "/visual"; + const std::string groundPlaneGeometryPath = + groundPlaneVisualPath + "/geometry"; + + pxr::GfVec3f scale; + pxr::VtArray extent; + double size, height, radius, length; + + const auto groundPlaneGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(groundPlaneGeometryPath)); + ASSERT_TRUE(groundPlaneGeometry); + EXPECT_TRUE(groundPlaneGeometry.HasAPI()); + const auto usdGroundPlane = pxr::UsdGeomCube(groundPlaneGeometry); + ASSERT_TRUE(usdGroundPlane); + usdGroundPlane.GetSizeAttr().Get(&size); + EXPECT_DOUBLE_EQ(1.0, size); + auto scaleAttr = + groundPlaneGeometry.GetAttribute(pxr::TfToken("xformOp:scale")); + ASSERT_TRUE(scaleAttr); + scaleAttr.Get(&scale); + EXPECT_EQ(pxr::GfVec3f(2, 4, 0.25), scale); + sdf::usd::testing::HasScaleXFormOp(groundPlaneGeometry); + usdGroundPlane.GetExtentAttr().Get(&extent); + this->CheckExtent(extent, pxr::GfVec3f(-0.5), pxr::GfVec3f(0.5)); + extent.clear(); + + const std::string boxPath = worldPath + "/box"; + const std::string boxLinkPath = boxPath + "/link"; + const std::string boxVisualPath = boxLinkPath + "/box_vis"; + const std::string boxGeometryPath = boxVisualPath + "/geometry"; + const auto boxGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(boxGeometryPath)); + ASSERT_TRUE(boxGeometry); + EXPECT_TRUE(boxGeometry.HasAPI()); + const auto usdCube = pxr::UsdGeomCube(boxGeometry); + ASSERT_TRUE(usdCube); + usdCube.GetSizeAttr().Get(&size); + EXPECT_DOUBLE_EQ(1.0, size); + scaleAttr = boxGeometry.GetAttribute(pxr::TfToken("xformOp:scale")); + ASSERT_TRUE(scaleAttr); + scaleAttr.Get(&scale); + EXPECT_EQ(pxr::GfVec3f(1, 2, 3), scale); + sdf::usd::testing::HasScaleXFormOp(boxGeometry); + usdCube.GetExtentAttr().Get(&extent); + this->CheckExtent(extent, pxr::GfVec3f(-0.5), pxr::GfVec3f(0.5)); + extent.clear(); + + const std::string cylinderPath = worldPath + "/cylinder"; + const std::string cylinderLinkPath = cylinderPath + "/link"; + const std::string cylinderVisualPath = cylinderLinkPath + "/visual"; + const std::string cylinderGeometryPath = cylinderVisualPath + "/geometry"; + const auto cylinderGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(cylinderGeometryPath)); + ASSERT_TRUE(cylinderGeometry); + EXPECT_TRUE(cylinderGeometry.HasAPI()); + const auto usdCylinder = pxr::UsdGeomCylinder(cylinderGeometry); + ASSERT_TRUE(usdCylinder); + usdCylinder.GetRadiusAttr().Get(&radius); + usdCylinder.GetHeightAttr().Get(&height); + EXPECT_DOUBLE_EQ(0.2, radius); + EXPECT_DOUBLE_EQ(0.1, height); + usdCylinder.GetExtentAttr().Get(&extent); + const pxr::GfVec3f cylinderExtent(0.2, 0.2, 0.05); + this->CheckExtent(extent, -1.0 * cylinderExtent, cylinderExtent); + extent.clear(); + + const std::string spherePath = worldPath + "/sphere"; + const std::string sphereLinkPath = spherePath + "/link"; + const std::string sphereVisualPath = sphereLinkPath + "/sphere_vis"; + const std::string sphereGeometryPath = sphereVisualPath + "/geometry"; + const auto sphereGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(sphereGeometryPath)); + ASSERT_TRUE(sphereGeometry); + EXPECT_TRUE(sphereGeometry.HasAPI()); + const auto usdSphere = pxr::UsdGeomSphere(sphereGeometry); + ASSERT_TRUE(usdSphere); + usdSphere.GetRadiusAttr().Get(&radius); + EXPECT_DOUBLE_EQ(1.23, radius); + usdSphere.GetExtentAttr().Get(&extent); + this->CheckExtent(extent, pxr::GfVec3f(-1.23), pxr::GfVec3f(1.23)); + extent.clear(); + + const std::string capsulePath = worldPath + "/capsule"; + const std::string capsuleLinkPath = capsulePath + "/link"; + const std::string capsuleVisualPath = capsuleLinkPath + "/visual"; + const std::string capsuleGeometryPath = capsuleVisualPath + "/geometry"; + const auto capsuleGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(capsuleGeometryPath)); + ASSERT_TRUE(capsuleGeometry); + EXPECT_TRUE(capsuleGeometry.HasAPI()); + const auto usdCapsule = pxr::UsdGeomCapsule(capsuleGeometry); + ASSERT_TRUE(usdCapsule); + usdCapsule.GetRadiusAttr().Get(&radius); + usdCapsule.GetHeightAttr().Get(&length); + EXPECT_DOUBLE_EQ(0.2, radius); + EXPECT_DOUBLE_EQ(0.1, length); + usdCapsule.GetExtentAttr().Get(&extent); + const pxr::GfVec3f capsuleExtent(0.2, 0.2, 0.25); + this->CheckExtent(extent, -1.0 * capsuleExtent, capsuleExtent); + extent.clear(); + + const std::string meshPath = worldPath + "/mesh"; + const std::string meshLinkPath = meshPath + "/link"; + const std::string meshVisualPath = meshLinkPath + "/visual"; + const std::string meshGeometryPath = meshVisualPath + "/geometry"; + const std::string meshGeometryMeshPath = meshGeometryPath + "/Cube"; + const auto meshGeometryParentPrim = this->stage->GetPrimAtPath( + pxr::SdfPath(meshGeometryPath)); + ASSERT_TRUE(meshGeometryParentPrim); + EXPECT_TRUE(meshGeometryParentPrim.HasAPI()); + const auto meshGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(meshGeometryMeshPath)); + ASSERT_TRUE(meshGeometry); + scaleAttr = meshGeometry.GetAttribute(pxr::TfToken("xformOp:scale")); + ASSERT_TRUE(scaleAttr); + scaleAttr.Get(&scale); + EXPECT_EQ(pxr::GfVec3f(1.2, 2.3, 3.4), scale); + sdf::usd::testing::HasScaleXFormOp(meshGeometry); + const auto usdMesh = pxr::UsdGeomMesh(meshGeometry); + ASSERT_TRUE(usdMesh); + usdMesh.GetExtentAttr().Get(&extent); + // there's a bit of floating point round of error in the mesh, so the + // target extent's minimum y coordinate is manually set to account for this + this->CheckExtent(extent, pxr::GfVec3f(-1, -1.000001, -1), pxr::GfVec3f(1)); + extent.clear(); + + pxr::VtArray normals; + pxr::VtArray points; + pxr::VtIntArray faceVertexIndices; + pxr::VtIntArray faceVertexCounts; + usdMesh.GetFaceVertexCountsAttr().Get(&faceVertexCounts); + usdMesh.GetFaceVertexIndicesAttr().Get(&faceVertexIndices); + usdMesh.GetNormalsAttr().Get(&normals); + usdMesh.GetPointsAttr().Get(&points); + EXPECT_EQ(24u, normals.size()); + EXPECT_EQ(24u, points.size()); + EXPECT_EQ(36u, faceVertexIndices.size()); + EXPECT_EQ(12u, faceVertexCounts.size()); + + EXPECT_EQ(pxr::TfToken("vertex"), usdMesh.GetNormalsInterpolation()); + pxr::TfToken subdivisionScheme; + usdMesh.GetSubdivisionSchemeAttr().Get(&subdivisionScheme); + EXPECT_EQ(pxr::TfToken("none"), subdivisionScheme); +} diff --git a/usd/src/sdf_parser/Joint.cc b/usd/src/sdf_parser/Joint.cc new file mode 100644 index 000000000..62a6b0ef6 --- /dev/null +++ b/usd/src/sdf_parser/Joint.cc @@ -0,0 +1,403 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "sdf/usd/sdf_parser/Joint.hh" + +#include + +#include +#include +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Error.hh" +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" +#include "../UsdUtils.hh" + +namespace sdf +{ +// Inline bracke to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + /// \brief Helper function for setting a USD joint's pose relative to the + /// joint's parent and child. + /// \param[in] _jointPrim The USD joint prim + /// \param[in] _joint The SDF representation of _jointPrim + /// \param[in] _parentModel The SDF model that is the parent of _joint + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when setting _joint's pose relative to _joint's parent and child + UsdErrors SetUSDJointPose(pxr::UsdPhysicsJoint &_jointPrim, + const sdf::Joint &_joint, const sdf::Model &_parentModel) + { + UsdErrors errors; + + ignition::math::Pose3d parentToJoint; + if (_joint.ParentLinkName() == "world") + { + ignition::math::Pose3d modelToJoint; + auto poseErrors = usd::PoseWrtParent(_joint, modelToJoint); + if (!poseErrors.empty()) + return poseErrors; + + // it is assumed the _parentModel's parent is the world because nested + // models are not yet supported (see issue #845) + ignition::math::Pose3d worldToModel; + poseErrors = usd::PoseWrtParent(_parentModel, worldToModel); + if (!poseErrors.empty()) + return poseErrors; + + parentToJoint = worldToModel * modelToJoint; + } + else + { + auto poseResolutionErrors = + _joint.SemanticPose().Resolve(parentToJoint, _joint.ParentLinkName()); + if (!poseResolutionErrors.empty()) + { + errors.push_back(UsdError( + sdf::Error(sdf::ErrorCode::POSE_RELATIVE_TO_INVALID, + "Unable to get the pose of joint [" + _joint.Name() + + "] w.r.t. its parent link [" + _joint.ParentLinkName() + "]."))); + for (const auto &e : poseResolutionErrors) + errors.push_back(UsdError(e)); + return errors; + } + } + + ignition::math::Pose3d childToJoint; + auto poseResolutionErrors = _joint.SemanticPose().Resolve(childToJoint, + _joint.ChildLinkName()); + if (!poseResolutionErrors.empty()) + { + errors.push_back(UsdError( + sdf::Error(sdf::ErrorCode::POSE_RELATIVE_TO_INVALID, + "Unable to get the pose of joint [" + _joint.Name() + + "] w.r.t. its child [" + _joint.ChildLinkName() + "]."))); + for (const auto &e : poseResolutionErrors) + errors.push_back(UsdError(e)); + return errors; + } + + auto parentRotation = pxr::GfQuatf( + parentToJoint.Rot().W(), + parentToJoint.Rot().X(), + parentToJoint.Rot().Y(), + parentToJoint.Rot().Z()); + + auto childRotation = pxr::GfQuatf( + childToJoint.Rot().W(), + childToJoint.Rot().X(), + childToJoint.Rot().Y(), + childToJoint.Rot().Z()); + + const auto axis = _joint.Axis(); + // TODO(anyone) Review this logic which converts a Y axis into a X axis. + if (axis && (axis->Xyz() == ignition::math::Vector3d::UnitY)) + { + if (auto jointRevolute = pxr::UsdPhysicsRevoluteJoint(_jointPrim)) + { + const ignition::math::Quaterniond fixRotation(0, 0, IGN_DTOR(90)); + ignition::math::Quaterniond parentRotationTmp = parentToJoint.Rot(); + ignition::math::Quaterniond childRotationTmp = childToJoint.Rot(); + + if (parentRotationTmp == ignition::math::Quaterniond::Identity) + { + parentRotationTmp = fixRotation * parentRotationTmp; + } + else + { + parentRotationTmp = ignition::math::Quaterniond(IGN_DTOR(-90), + IGN_PI, IGN_PI) * parentRotationTmp; + } + + childRotationTmp = fixRotation * childRotationTmp; + + parentRotation = pxr::GfQuatf( + parentRotationTmp.W(), + parentRotationTmp.X(), + parentRotationTmp.Y(), + parentRotationTmp.Z()); + childRotation = pxr::GfQuatf( + childRotationTmp.W(), + childRotationTmp.X(), + childRotationTmp.Y(), + childRotationTmp.Z()); + jointRevolute.CreateAxisAttr().Set(pxr::TfToken("X")); + } + } + + _jointPrim.CreateLocalPos0Attr().Set(pxr::GfVec3f( + parentToJoint.Pos().X(), + parentToJoint.Pos().Y(), + parentToJoint.Pos().Z())); + _jointPrim.CreateLocalRot0Attr().Set(parentRotation); + + _jointPrim.CreateLocalPos1Attr().Set(pxr::GfVec3f( + childToJoint.Pos().X(), + childToJoint.Pos().Y(), + childToJoint.Pos().Z())); + _jointPrim.CreateLocalRot1Attr().Set(childRotation); + + return errors; + } + + /// \brief Helper function to parse a SDF revolute joint to its USD + /// representation. + /// \param[in] _joint The SDF joint to parse + /// \param[in] _stage The stage that _joint should belong to. This must be a + /// valid, initialized stage. + /// \param[in] _path The path in _stage where the USD representation of _joint + /// will be defined. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when converting _joint to its USD representation. + UsdErrors ParseSdfRevoluteJoint(const sdf::Joint &_joint, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + auto usdJoint = + pxr::UsdPhysicsRevoluteJoint::Define(_stage, pxr::SdfPath(_path)); + + const auto axis = _joint.Axis(); + + if (axis->Xyz() == ignition::math::Vector3d::UnitX || + axis->Xyz() == -ignition::math::Vector3d::UnitX) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("X")); + } + else if (axis->Xyz() == ignition::math::Vector3d::UnitY || + axis->Xyz() == -ignition::math::Vector3d::UnitY) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("Y")); + } + else if (axis->Xyz() == ignition::math::Vector3d::UnitZ || + axis->Xyz() == -ignition::math::Vector3d::UnitZ) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("Z")); + } + else + { + std::stringstream axisStr; + axisStr << axis->Xyz(); + errors.push_back(UsdError(sdf::Error(sdf::ErrorCode::ELEMENT_INVALID, + "Revolute joint [" + _joint.Name() + "] has specified an axis [" + + axisStr.str() + "], but USD only supports integer values of [0, 1] " + "when specifying joint axis unit vectors."))); + return errors; + } + + // Revolute joint limits in SDF are in radians, but USD expects degrees + // of C++ type float + auto sdfLimitDegrees = static_cast(IGN_RTOD(axis->Lower())); + usdJoint.CreateLowerLimitAttr().Set(sdfLimitDegrees); + sdfLimitDegrees = static_cast(IGN_RTOD(axis->Upper())); + usdJoint.CreateUpperLimitAttr().Set(sdfLimitDegrees); + + pxr::UsdPrim usdJointPrim = _stage->GetPrimAtPath(pxr::SdfPath(_path)); + + auto drive = + pxr::UsdPhysicsDriveAPI::Apply(usdJointPrim, pxr::TfToken("angular")); + if (!drive) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, + "Internal error: unable to mark link at path [" + _path + + "] as a UsdPhysicsDriveAPI.")); + return errors; + } + + // TODO(ahcorde) Review damping and stiffness values + drive.CreateDampingAttr().Set(static_cast(axis->Damping())); + drive.CreateStiffnessAttr().Set(static_cast(axis->Stiffness())); + drive.CreateMaxForceAttr().Set(static_cast(axis->Effort())); + + return errors; + } + + /// \brief Helper function to parse a SDF prismatic joint to its USD + /// representation. + /// \param[in] _joint The SDF joint to parse + /// \param[in] _stage The stage that _joint should belong to. This must be a + /// valid, initialized stage. + /// \param[in] _path The path in _stage where the USD representation of _joint + /// will be defined. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when converting _joint to its USD representation. + UsdErrors ParseSdfPrismaticJoint(const sdf::Joint &_joint, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + auto usdJoint = + pxr::UsdPhysicsPrismaticJoint::Define(_stage, pxr::SdfPath(_path)); + + const auto axis = _joint.Axis(); + + if (axis->Xyz() == ignition::math::Vector3d::UnitX || + axis->Xyz() == -ignition::math::Vector3d::UnitX) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("X")); + } + else if (axis->Xyz() == ignition::math::Vector3d::UnitY || + axis->Xyz() == -ignition::math::Vector3d::UnitY) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("Y")); + } + else if (axis->Xyz() == ignition::math::Vector3d::UnitZ || + axis->Xyz() == -ignition::math::Vector3d::UnitZ) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("Z")); + } + else + { + std::stringstream axisStr; + axisStr << axis->Xyz(); + errors.push_back(UsdError(sdf::Error(sdf::ErrorCode::ELEMENT_INVALID, + "Prismatic joint [" + _joint.Name() + "] has specified an axis [" + + axisStr.str() + "], but USD only supports integer values of [0, 1] " + "when specifying joint axis unit vectors."))); + return errors; + } + + usdJoint.CreateLowerLimitAttr().Set( + static_cast(axis->Lower())); + usdJoint.CreateUpperLimitAttr().Set( + static_cast(axis->Upper())); + + return errors; + } + + UsdErrors ParseSdfJoint(const sdf::Joint &_joint, + pxr::UsdStageRefPtr &_stage, const std::string &_path, + const sdf::Model &_parentModel, + const std::unordered_map &_linkToUsdPath, + const pxr::SdfPath &_worldPath) + { + UsdErrors errors; + + // the joint's parent may be "world". If this is the case, the joint's + // parent should be set to the world prim, not a link + auto parentLinkPath = _worldPath; + if (_joint.ParentLinkName() != "world") + { + const auto it = _linkToUsdPath.find(_joint.ParentLinkName()); + if (it == _linkToUsdPath.end()) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Unable to find a USD path for link [" + _joint.ParentLinkName() + + "], which is the parent link of joint [" + _joint.Name() + "].")); + return errors; + } + parentLinkPath = it->second; + } + + const auto it = _linkToUsdPath.find(_joint.ChildLinkName()); + if (it == _linkToUsdPath.end()) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Unable to find a USD path for link [" + _joint.ParentLinkName() + + "], which is the child link of joint [" + _joint.Name() + "].")); + return errors; + } + const auto childLinkPath = it->second; + + UsdErrors parsingErrors; + switch (_joint.Type()) + { + case sdf::JointType::REVOLUTE: + parsingErrors = ParseSdfRevoluteJoint(_joint, _stage, _path); + break; + case sdf::JointType::BALL: + // While USD allows for cone limits that can restrict motion in a given + // range, SDF does not have limits for a ball joint. So, there's + // nothing to do after creating a UsdPhysicsSphericalJoint, since this + // joint by default has no limits (i.e., allows for circular motion) + // related issue https://github.com/ignitionrobotics/sdformat/issues/860 + pxr::UsdPhysicsSphericalJoint::Define(_stage, pxr::SdfPath(_path)); + break; + case sdf::JointType::FIXED: + pxr::UsdPhysicsFixedJoint::Define(_stage, pxr::SdfPath(_path)); + break; + case sdf::JointType::PRISMATIC: + parsingErrors = ParseSdfPrismaticJoint(_joint, _stage, _path); + break; + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::UNIVERSAL: + case sdf::JointType::INVALID: + default: + parsingErrors.push_back(UsdError( + sdf::Error(sdf::ErrorCode::ATTRIBUTE_INVALID, + "Joint type is either invalid or not supported."))); + } + + if (!parsingErrors.empty()) + { + errors.insert(errors.end(), parsingErrors.begin(), parsingErrors.end()); + return errors; + } + + auto jointPrim = pxr::UsdPhysicsJoint::Get(_stage, pxr::SdfPath(_path)); + if (!jointPrim) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Internal error: unable to get prim at path [" + _path + + "], but a joint prim should exist at this path.")); + return errors; + } + + // define the joint's parent/child links + jointPrim.CreateBody0Rel().AddTarget(parentLinkPath); + jointPrim.CreateBody1Rel().AddTarget(childLinkPath); + + const auto poseErrors = + SetUSDJointPose(jointPrim, _joint, _parentModel); + errors.insert(errors.end(), poseErrors.begin(), poseErrors.end()); + + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc new file mode 100644 index 000000000..8f44ccfcd --- /dev/null +++ b/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc @@ -0,0 +1,460 @@ +/* + * Copyright 2022 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 +#include +#include +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" +#include "sdf/Model.hh" +#include "sdf/Root.hh" +#include "sdf/usd/sdf_parser/Model.hh" +#include "test_config.h" +#include "test_utils.hh" + +///////////////////////////////////////////////// +// Fixture that creates a USD stage for each test case. +// This fixture also provides methods for performing joint validation checks. +class UsdJointStageFixture : public::testing::Test +{ + public: UsdJointStageFixture() : + worldPath("/world") + { + } + + protected: void SetUp() override + { + this->stage = pxr::UsdStage::CreateInMemory(); + ASSERT_TRUE(this->stage); + } + + /// \brief Parse the contents of a SDF file and convert to USD. + /// This method should be called at the beginning of a test case to set up + /// the USD contents that need to be checked/verified. + /// \param[in] _sdfFile The full path to the SDF file to parse + public: void GenerateUSD(const std::string &_sdfFile) + { + // load the world in the SDF file + ASSERT_TRUE(sdf::testing::LoadSdfFile(_sdfFile, this->root)); + this->model = const_cast(this->root.Model()); + ASSERT_NE(nullptr, this->model); + + this->modelPath = + std::string(this->worldPath.GetString() + "/" + this->model->Name()); + const auto errors = sdf::usd::ParseSdfModel(*(this->model), this->stage, + this->modelPath, this->worldPath); + EXPECT_TRUE(errors.empty()); + + // save the model's USD joint paths so that they can be verified + for (uint64_t i = 0; i < this->model->JointCount(); ++i) + { + const auto joint = this->model->JointByIndex(i); + const auto jointPath = this->modelPath + "/" + joint->Name(); + this->jointPathToSdf[jointPath] = joint; + } + EXPECT_EQ(this->model->JointCount(), this->jointPathToSdf.size()); + } + + /// \brief Verify that a USD joint is pointing to the correct parent link. + /// \param[in] _usdJoint The USD joint + /// \param[in] _parentLinkPath The parent link path that _usdJoint should + /// point to + public: void CheckParentLinkPath(const pxr::UsdPhysicsJoint *_usdJoint, + const std::string &_parentLinkPath) const + { + pxr::SdfPathVector jointTarget; + _usdJoint->GetBody0Rel().GetTargets(&jointTarget); + ASSERT_EQ(1u, jointTarget.size()); + EXPECT_EQ(_parentLinkPath, jointTarget[0].GetString()); + } + + /// \brief Verify that a USD joint is pointing to the correct child link. + /// \param[in] _usdJoint The USD joint + /// \param[in] _childLinkPath The child link path that _usdJoint should + /// point to + public: void CheckChildLinkPath(const pxr::UsdPhysicsJoint *_usdJoint, + const std::string &_childLinkPath) const + { + pxr::SdfPathVector jointTarget; + _usdJoint->GetBody1Rel().GetTargets(&jointTarget); + ASSERT_EQ(1u, jointTarget.size()); + EXPECT_EQ(_childLinkPath, jointTarget[0].GetString()); + } + + /// \brief Verify that a USD joint has a proper pose w.r.t. its parent and + /// child link. + /// \param[in] _usdJoint The USD joint + /// \param[in] _targetParentPose The pose _usdJoint should have w.r.t. its + /// parent link + /// \param[in] _targetChildPose The pose _usdJoint should have w.r.t. its + /// child link + public: void CheckRelativeLinkPoses(const pxr::UsdPhysicsJoint *_usdJoint, + const ignition::math::Pose3d &_targetParentPose, + const ignition::math::Pose3d &_targetChildPose) const + { + // helper function to compare USD position to ignition::math position + auto validatePos = + [](const pxr::GfVec3f &_usdPos, + const ignition::math::Vector3d &_targetPos) + { + EXPECT_FLOAT_EQ(_usdPos[0], static_cast(_targetPos.X())); + EXPECT_FLOAT_EQ(_usdPos[1], static_cast(_targetPos.Y())); + EXPECT_FLOAT_EQ(_usdPos[2], static_cast(_targetPos.Z())); + }; + + // helper function to compare USD rotation to ignition::math quaternion + auto validateRot = + [](const pxr::GfQuatf &_usdRot, + const ignition::math::Quaterniond &_targetRot) + { + EXPECT_FLOAT_EQ(_usdRot.GetReal(), + static_cast(_targetRot.W())); + EXPECT_FLOAT_EQ(_usdRot.GetImaginary()[0], + static_cast(_targetRot.X())); + EXPECT_FLOAT_EQ(_usdRot.GetImaginary()[1], + static_cast(_targetRot.Y())); + EXPECT_FLOAT_EQ(_usdRot.GetImaginary()[2], + static_cast(_targetRot.Z())); + }; + + pxr::GfVec3f usdPos; + pxr::GfQuatf usdRot; + + EXPECT_TRUE(_usdJoint->GetLocalPos0Attr().Get(&usdPos)); + validatePos(usdPos, _targetParentPose.Pos()); + + EXPECT_TRUE(_usdJoint->GetLocalRot0Attr().Get(&usdRot)); + validateRot(usdRot, _targetParentPose.Rot()); + + EXPECT_TRUE(_usdJoint->GetLocalPos1Attr().Get(&usdPos)); + validatePos(usdPos, _targetChildPose.Pos()); + + EXPECT_TRUE(_usdJoint->GetLocalRot1Attr().Get(&usdRot)); + validateRot(usdRot, _targetChildPose.Rot()); + } + + /// \brief Verify that a USD joint has a proper axis (this is not required + /// by all USD joint types, but is required by revolute and prismatic joints, + /// for example) + /// \param[in] _usdJoint The USD joint + /// \param[in] _targetAxis The axis _usdJoint should have. This should be + /// "X", "Y", or "Z" + /// \tparam JointTypeT A USD joint type that has a GetAxisAttr() method, which + /// returns a pxr::UsdAttribute that stores the axis in a pxr::TfToken object + public: template + void VerifyJointAxis(const JointTypeT &_usdJoint, + const std::string &_targetAxis) const + { + pxr::TfToken usdAxis; + EXPECT_TRUE(_usdJoint.GetAxisAttr().Get(&usdAxis)); + EXPECT_EQ(_targetAxis, usdAxis.GetString()); + } + + /// \brief Verify that a USD joint has the proper limits (this is not required + /// by all USD joint types, but is required by revolute and prismatic joints, + /// for example) + /// \param[in] _usdJoint The USD joint + /// \param[in] _targetLower The lower limit _usdJoint should have. For + /// revolute joints, USD interprets this as an angle, but for prismatic + /// joints, USD interprets this as a distance + /// \param[in] _targetUpper The upper limit _usdJoint should have. For + /// revolute joints, USD interprets this as an angle, but for prismatic + /// joints, USD interprets this as a distance + /// \param[in] _convertToDeg Whether _targetLower and _targetUpper need to be + /// converted to degrees (true) or not (false). If _usdJoint is a prismatic + /// joint, this value should be set to false since prismatic joint limits are + /// interpreted as a distance instead of an angle + /// \tparam JointTypeT A USD joint type that has GetLowerLimitAttr() and + /// GetUpperLimitAttr() methods. Both of these methods should return a + /// pxr::UsdAttribute that stores the limit as a float + public: template + void VerifyJointLimits(const JointTypeT &_usdJoint, + float _targetLower, float _targetUpper, bool _convertToDeg) const + { + if (_convertToDeg) + { + _targetLower = IGN_RTOD(_targetLower); + _targetUpper = IGN_RTOD(_targetUpper); + } + + float usdLowerLimit; + EXPECT_TRUE(_usdJoint.GetLowerLimitAttr().Get(&usdLowerLimit)); + EXPECT_FLOAT_EQ(usdLowerLimit, _targetLower); + + float usdUpperLimit; + EXPECT_TRUE(_usdJoint.GetUpperLimitAttr().Get(&usdUpperLimit)); + EXPECT_FLOAT_EQ(usdUpperLimit, _targetUpper); + } + + /// \brief The USD stage + public: pxr::UsdStageRefPtr stage; + + /// \brief The SDF model with joints to be parsed to USD + public: sdf::Model *model{nullptr}; + + /// \brief The USD path of the SDF world + public: const pxr::SdfPath worldPath; + + /// \brief The string representation of this->model's USD path + public: std::string modelPath; + + /// \brief Mapping of a joint's USD path to the corresponding SDF joint + public: std::unordered_map jointPathToSdf; + + /// \brief The root object of the SDF file that has been loaded. A reference + /// to the root must be kept so that pointer objects extracted from the root + /// (sdf::Model and sdf::Joint, for example) remain valid throughout the test + /// (destroying the sdf::Root object early invalidates referenced pointers) + private: sdf::Root root; +}; + +///////////////////////////////////////////////// +TEST_F(UsdJointStageFixture, RevoluteJoints) +{ + this->GenerateUSD(sdf::testing::TestFile("sdf", "double_pendulum.sdf")); + + // validate USD joints + int checkedJoints = 0; + for (const auto & prim : this->stage->Traverse()) + { + if (!prim.IsA()) + continue; + + auto iter = this->jointPathToSdf.find(prim.GetPath().GetString()); + ASSERT_NE(this->jointPathToSdf.end(), iter); + const auto sdfJoint = iter->second; + + // the double pendulum model only has revolute joints + EXPECT_TRUE(prim.IsA()); + const auto usdRevoluteJoint = + pxr::UsdPhysicsRevoluteJoint::Get(this->stage, prim.GetPath()); + ASSERT_TRUE(usdRevoluteJoint); + + // make sure joint is pointing to the proper parent/child links + this->CheckParentLinkPath(&usdRevoluteJoint, + this->modelPath + "/" + sdfJoint->ParentLinkName()); + this->CheckChildLinkPath(&usdRevoluteJoint, + this->modelPath + "/" + sdfJoint->ChildLinkName()); + + // check joint's pose w.r.t. parent and child links + ignition::math::Pose3d parentToJointPose; + auto poseErrors = sdfJoint->SemanticPose().Resolve(parentToJointPose, + sdfJoint->ParentLinkName()); + EXPECT_TRUE(poseErrors.empty()); + poseErrors.clear(); + ignition::math::Pose3d childToJointPose; + poseErrors = sdfJoint->SemanticPose().Resolve(childToJointPose, + sdfJoint->ChildLinkName()); + EXPECT_TRUE(poseErrors.empty()); + this->CheckRelativeLinkPoses(&usdRevoluteJoint, parentToJointPose, + childToJointPose); + + // check the joint's axis + this->VerifyJointAxis(usdRevoluteJoint, "X"); + + // check the joint limits + this->VerifyJointLimits(usdRevoluteJoint, + static_cast(sdfJoint->Axis()->Lower()), + static_cast(sdfJoint->Axis()->Upper()), + true); + + // revolute joints should have a UsdPhysicsDriveAPI + EXPECT_TRUE(prim.HasAPI(pxr::TfToken("angular"))); + const auto driveApiAttrPrefix = std::string("drive:angular:physics:"); + + // check damping + const auto dampingAttr = + prim.GetAttribute(pxr::TfToken(driveApiAttrPrefix + "damping")); + ASSERT_TRUE(dampingAttr); + float usdDamping; + EXPECT_TRUE(dampingAttr.Get(&usdDamping)); + EXPECT_FLOAT_EQ(usdDamping, + static_cast(sdfJoint->Axis()->Damping())); + + // check stiffness + const auto stiffnessAttr = + prim.GetAttribute(pxr::TfToken(driveApiAttrPrefix + "stiffness")); + ASSERT_TRUE(stiffnessAttr); + float usdStiffness; + EXPECT_TRUE(stiffnessAttr.Get(&usdStiffness)); + EXPECT_FLOAT_EQ(usdStiffness, + static_cast(sdfJoint->Axis()->Stiffness())); + + // check max force/effort + const auto maxForceAttr = + prim.GetAttribute(pxr::TfToken(driveApiAttrPrefix + "maxForce")); + ASSERT_TRUE(maxForceAttr); + float usdForce; + EXPECT_TRUE(maxForceAttr.Get(&usdForce)); + EXPECT_FLOAT_EQ(usdForce, static_cast(sdfJoint->Axis()->Effort())); + + checkedJoints++; + } + EXPECT_EQ(checkedJoints, 2); +} + +///////////////////////////////////////////////// +TEST_F(UsdJointStageFixture, JointParentIsWorld) +{ + this->GenerateUSD(sdf::testing::TestFile("sdf", "joint_parent_world.sdf")); + + // validate USD joints + int checkedJoints = 0; + for (const auto & prim : this->stage->Traverse()) + { + if (!prim.IsA()) + continue; + + auto iter = this->jointPathToSdf.find(prim.GetPath().GetString()); + ASSERT_NE(this->jointPathToSdf.end(), iter); + const auto sdfJoint = iter->second; + + // the only joint type in this test file is a fixed joint + EXPECT_TRUE(prim.IsA()); + const auto usdFixedJoint = + pxr::UsdPhysicsFixedJoint::Get(this->stage, prim.GetPath()); + ASSERT_TRUE(usdFixedJoint); + + // make sure joint is pointing to the proper parent/child links. + // The parent in this test should be the world + this->CheckParentLinkPath(&usdFixedJoint, this->worldPath.GetString()); + this->CheckChildLinkPath(&usdFixedJoint, + this->modelPath + "/" + sdfJoint->ChildLinkName()); + + // check joint's pose w.r.t. parent and child links. For this test case, + // we need to get the joint pose w.r.t. the world + ignition::math::Pose3d modelToJointPose; + auto poseErrors = sdfJoint->SemanticPose().Resolve(modelToJointPose); + EXPECT_TRUE(poseErrors.empty()); + poseErrors.clear(); + ignition::math::Pose3d worldToModelPose; + poseErrors = this->model->SemanticPose().Resolve(worldToModelPose); + const auto worldToJointPose = worldToModelPose * modelToJointPose; + EXPECT_TRUE(poseErrors.empty()); + poseErrors.clear(); + ignition::math::Pose3d childToJointPose; + poseErrors = sdfJoint->SemanticPose().Resolve(childToJointPose, + sdfJoint->ChildLinkName()); + EXPECT_TRUE(poseErrors.empty()); + this->CheckRelativeLinkPoses(&usdFixedJoint, worldToJointPose, + childToJointPose); + + checkedJoints++; + } + EXPECT_EQ(checkedJoints, 1); +} + +///////////////////////////////////////////////// +TEST_F(UsdJointStageFixture, BallPrismaticJoint) +{ + this->GenerateUSD(sdf::testing::TestFile("sdf", "ball_prismatic_joint.sdf")); + + // validate USD joints + int checkedBallJoints = 0; + int checkedPrismaticJoints = 0; + for (const auto & prim : this->stage->Traverse()) + { + if (!prim.IsA()) + continue; + + auto iter = this->jointPathToSdf.find(prim.GetPath().GetString()); + ASSERT_NE(this->jointPathToSdf.end(), iter); + const auto sdfJoint = iter->second; + + if (prim.IsA()) + { + checkedBallJoints++; + } + else if (prim.IsA()) + { + checkedPrismaticJoints++; + + const auto usdPrismaticJoint = + pxr::UsdPhysicsPrismaticJoint::Get(this->stage, prim.GetPath()); + ASSERT_TRUE(usdPrismaticJoint); + + // check the joint's axis + this->VerifyJointAxis(usdPrismaticJoint, "Z"); + + // check the joint limits + this->VerifyJointLimits(usdPrismaticJoint, + static_cast(sdfJoint->Axis()->Lower()), + static_cast(sdfJoint->Axis()->Upper()), + false); + } + else + { + continue; + } + + const auto usdJoint = + pxr::UsdPhysicsJoint::Get(this->stage, prim.GetPath()); + ASSERT_TRUE(usdJoint); + + // make sure joint is pointing to the proper parent/child links + this->CheckParentLinkPath(&usdJoint, + this->modelPath + "/" + sdfJoint->ParentLinkName()); + this->CheckChildLinkPath(&usdJoint, + this->modelPath + "/" + sdfJoint->ChildLinkName()); + + // check joint's pose w.r.t. parent and child links + ignition::math::Pose3d parentToJointPose; + auto poseErrors = sdfJoint->SemanticPose().Resolve(parentToJointPose, + sdfJoint->ParentLinkName()); + EXPECT_TRUE(poseErrors.empty()); + poseErrors.clear(); + ignition::math::Pose3d childToJointPose; + poseErrors = sdfJoint->SemanticPose().Resolve(childToJointPose, + sdfJoint->ChildLinkName()); + EXPECT_TRUE(poseErrors.empty()); + this->CheckRelativeLinkPoses(&usdJoint, parentToJointPose, + childToJointPose); + } + EXPECT_EQ(checkedBallJoints, 1); + EXPECT_EQ(checkedPrismaticJoints, 1); +} + +// TODO(adlarkin) Add a test case for a revolute joint with the axis being "y". +// This is a special case; see the sdf::usd::SetUSDJointPose method in +// usd/src/sdf_parser/Joint.cc for how this is handled diff --git a/usd/src/sdf_parser/Link.cc b/usd/src/sdf_parser/Link.cc index 8478a3a44..973cd5bf1 100644 --- a/usd/src/sdf_parser/Link.cc +++ b/usd/src/sdf_parser/Link.cc @@ -37,6 +37,8 @@ #include "sdf/Link.hh" #include "sdf/usd/sdf_parser/Light.hh" +#include "sdf/usd/sdf_parser/Sensor.hh" +#include "sdf/usd/sdf_parser/Visual.hh" #include "../UsdUtils.hh" namespace sdf @@ -123,6 +125,38 @@ namespace usd centerOfMass.Pos().Z())); } + // parse all of the link's visuals and convert them to USD + for (uint64_t i = 0; i < _link.VisualCount(); ++i) + { + const auto visual = *(_link.VisualByIndex(i)); + const auto visualPath = std::string(_path + "/" + visual.Name()); + auto errorsLink = ParseSdfVisual(visual, _stage, visualPath); + if (!errorsLink.empty()) + { + errors.insert(errors.end(), errorsLink.begin(), errorsLink.end()); + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing visual [" + visual.Name() + "]")); + return errors; + } + } + + // convert the link's sensors + for (uint64_t i = 0; i < _link.SensorCount(); ++i) + { + const auto sensor = *(_link.SensorByIndex(i)); + const auto sensorPath = std::string(_path + "/" + sensor.Name()); + UsdErrors errorsSensor = ParseSdfSensor(sensor, _stage, sensorPath); + if (!errorsSensor.empty()) + { + errors.push_back( + UsdError(sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing sensor [" + sensor.Name() + "]")); + errors.insert(errors.end(), errorsSensor.begin(), errorsSensor.end()); + return errors; + } + } + // links can have lights attached to them for (uint64_t i = 0; i < _link.LightCount(); ++i) { diff --git a/usd/src/sdf_parser/Link_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Link_Sdf2Usd_TEST.cc index 9c211b8c5..7919f25cc 100644 --- a/usd/src/sdf_parser/Link_Sdf2Usd_TEST.cc +++ b/usd/src/sdf_parser/Link_Sdf2Usd_TEST.cc @@ -35,6 +35,8 @@ #include #pragma pop_macro ("__DEPRECATED") +#include + #include "sdf/usd/sdf_parser/World.hh" #include "sdf/Root.hh" #include "test_config.h" @@ -59,7 +61,11 @@ class UsdStageFixture : public::testing::Test ///////////////////////////////////////////////// TEST_F(UsdStageFixture, Link) { - const auto path = sdf::testing::TestFile("sdf", "shapes_world.sdf"); + sdf::setFindCallback(sdf::usd::testing::findFileCb); + ignition::common::addFindFileURICallback( + std::bind(&sdf::usd::testing::FindResourceUri, std::placeholders::_1)); + + const auto path = sdf::testing::TestFile("sdf", "basic_shapes.sdf"); sdf::Root root; ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root)); diff --git a/usd/src/sdf_parser/Material.cc b/usd/src/sdf_parser/Material.cc new file mode 100644 index 000000000..0a1c789a8 --- /dev/null +++ b/usd/src/sdf_parser/Material.cc @@ -0,0 +1,586 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "sdf/usd/sdf_parser/Material.hh" + +#include +#include + +#include +#include +#include + +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Pbr.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + /// \brief Copy a file in a directory + /// \param[in] _path path where the copy will be located + /// \param[in] _fullPath name of the file to copy + /// \return True if the file at _fullPath was copied to _path. False otherwise + bool copyMaterial(const std::string &_path, const std::string &_fullPath) + { + if (!_path.empty() && !_fullPath.empty()) + { + auto fileName = ignition::common::basename(_path); + auto filePathIndex = _path.rfind(fileName); + auto filePath = _path.substr(0, filePathIndex); + if (!filePath.empty()) + { + ignition::common::createDirectories(filePath); + } + return ignition::common::copyFile(_fullPath, _path); + } + return false; + } + + /// \brief Get the path to copy the material to + /// \param[in] _uri full path of the file to copy + /// \return A relative path to save the material. The path looks like: + /// materials/textures/ + std::string getMaterialCopyPath(const std::string &_uri) + { + return ignition::common::joinPaths( + "materials", + "textures", + ignition::common::basename(_uri)); + } + + /// \brief Fill Material shader attributes and properties + /// \param[in] _prim USD primitive + /// \param[in] _name Name of the field attribute or property + /// \param[in] _vType Type of the field + /// \param[in] _value Value of the field + /// \param[in] _customData Custom data to set the field + /// \param[in] _displayName Display name + /// \param[in] _displayGroup Display group + /// \param[in] _doc Documentation of the field + /// \param[in] _colorSpace if the material is a texture, we can specify the + /// color space of the image + /// \return UsdErrors, which is a list of UsdError objects. This list is empty + /// if no errors occurred when creating the material input. + template + UsdErrors CreateMaterialInput( + const pxr::UsdPrim &_prim, + const std::string &_name, + const pxr::SdfValueTypeName &_vType, + const T &_value, + const std::map &_customData, + const pxr::TfToken &_displayName, + const pxr::TfToken &_displayGroup, + const std::string &_doc, + const pxr::TfToken &_colorSpace = pxr::TfToken("")) + { + UsdErrors errors; + auto shader = pxr::UsdShadeShader(_prim); + if (!shader) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Unable to convert the prim to a UsdShadeShader")); + return errors; + } + + auto existingInput = shader.GetInput(pxr::TfToken(_name)); + pxr::SdfValueTypeName vTypeName; + if (_vType.IsScalar()) + { + vTypeName = _vType.GetScalarType(); + } + else if (_vType.IsArray()) + { + vTypeName = _vType.GetArrayType(); + } + auto surfaceInput = shader.CreateInput( + pxr::TfToken(_name), vTypeName); + surfaceInput.Set(_value); + auto attr = surfaceInput.GetAttr(); + + for (const auto &[key, customValue] : _customData) + { + attr.SetCustomDataByKey(key, customValue); + } + if (!_displayName.GetString().empty()) + { + attr.SetDisplayName(_displayName); + } + if (!_displayGroup.GetString().empty()) + { + attr.SetDisplayGroup(_displayGroup); + } + if (!_doc.empty()) + { + attr.SetDocumentation(_doc); + } + if (!_colorSpace.GetString().empty()) + { + attr.SetColorSpace(_colorSpace); + } + return errors; + } + + UsdErrors ParseSdfMaterial(const sdf::Material *_materialSdf, + pxr::UsdStageRefPtr &_stage, pxr::SdfPath &_materialPath) + { + UsdErrors errors; + + const auto looksPath = pxr::SdfPath("/Looks"); + auto looksPrim = _stage->GetPrimAtPath(looksPath); + if (!looksPrim) + { + looksPrim = _stage->DefinePrim(looksPath, pxr::TfToken("Scope")); + } + + // This variable will increase with every new material to avoid collision + // with the names of the materials + static int i = 0; + + _materialPath = pxr::SdfPath("/Looks/Material_" + std::to_string(i)); + i++; + + pxr::UsdShadeMaterial materialUsd; + auto usdMaterialPrim = _stage->GetPrimAtPath(_materialPath); + if (!usdMaterialPrim) + { + materialUsd = pxr::UsdShadeMaterial::Define(_stage, _materialPath); + } + else + { + materialUsd = pxr::UsdShadeMaterial(usdMaterialPrim); + } + + const auto shaderPath = pxr::SdfPath(_materialPath.GetString() + "/Shader"); + auto usdShader = pxr::UsdShadeShader::Define(_stage, shaderPath); + auto shaderPrim = _stage->GetPrimAtPath(shaderPath); + if (!shaderPrim) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Not able to cast the UsdShadeShader at path [" + shaderPath.GetString() + + "] to a Prim")); + } + + auto shaderOut = pxr::UsdShadeConnectableAPI(shaderPrim).CreateOutput( + pxr::TfToken("out"), pxr::SdfValueTypeNames->Token); + const auto mdlToken = pxr::TfToken("mdl"); + materialUsd.CreateSurfaceOutput(mdlToken).ConnectToSource(shaderOut); + materialUsd.CreateVolumeOutput(mdlToken).ConnectToSource(shaderOut); + materialUsd.CreateDisplacementOutput(mdlToken).ConnectToSource(shaderOut); + usdShader.GetImplementationSourceAttr().Set( + pxr::UsdShadeTokens->sourceAsset); + usdShader.SetSourceAsset(pxr::SdfAssetPath("OmniPBR.mdl"), mdlToken); + usdShader.SetSourceAssetSubIdentifier(pxr::TfToken("OmniPBR"), mdlToken); + + const std::map customDataDiffuse = + { + {pxr::TfToken("default"), pxr::VtValue(pxr::GfVec3f(0.2, 0.2, 0.2))}, + {pxr::TfToken("range:max"), + pxr::VtValue(pxr::GfVec3f(100000, 100000, 100000))}, + {pxr::TfToken("range:min"), pxr::VtValue(pxr::GfVec3f(0, 0, 0))} + }; + const ignition::math::Color diffuse = _materialSdf->Diffuse(); + auto errorsMaterialDiffuseColorConstant = CreateMaterialInput( + shaderPrim, + "diffuse_color_constant", + pxr::SdfValueTypeNames->Color3f, + pxr::GfVec3f(diffuse.R(), diffuse.G(), diffuse.B()), + customDataDiffuse, + pxr::TfToken("Base Color"), + pxr::TfToken("Albedo"), + "This is the base color"); + + if (!errorsMaterialDiffuseColorConstant.empty()) + { + errors.insert( + errors.end(), + errorsMaterialDiffuseColorConstant.begin(), + errorsMaterialDiffuseColorConstant.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the base color of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + + const std::map customDataEmissive = + { + {pxr::TfToken("default"), pxr::VtValue(pxr::GfVec3f(1, 0.1, 0.1))}, + {pxr::TfToken("range:max"), + pxr::VtValue(pxr::GfVec3f(100000, 100000, 100000))}, + {pxr::TfToken("range:min"), pxr::VtValue(pxr::GfVec3f(0, 0, 0))} + }; + ignition::math::Color emissive = _materialSdf->Emissive(); + auto errorsMaterialEmissiveColor = CreateMaterialInput( + shaderPrim, + "emissive_color", + pxr::SdfValueTypeNames->Color3f, + pxr::GfVec3f(emissive.R(), emissive.G(), emissive.B()), + customDataEmissive, + pxr::TfToken("Emissive Color"), + pxr::TfToken("Emissive"), + "The emission color"); + + if (!errorsMaterialEmissiveColor.empty()) + { + errors.insert( + errors.end(), + errorsMaterialEmissiveColor.begin(), + errorsMaterialEmissiveColor.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the emission color of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + + const std::map customDataEnableEmission = + { + {pxr::TfToken("default"), pxr::VtValue(0)} + }; + + auto errorsMaterialEnableEmission = CreateMaterialInput( + shaderPrim, + "enable_emission", + pxr::SdfValueTypeNames->Bool, + emissive.A() > 0, + customDataEnableEmission, + pxr::TfToken("Enable Emissive"), + pxr::TfToken("Emissive"), + "Enables the emission of light from the material"); + + if (!errorsMaterialEnableEmission.empty()) + { + errors.insert( + errors.end(), + errorsMaterialEnableEmission.begin(), + errorsMaterialEnableEmission.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the emissive enabled propery of the material at path" + " [" + _materialPath.GetString() + "]")); + return errors; + } + + const std::map customDataIntensity = + { + {pxr::TfToken("default"), pxr::VtValue(40)}, + {pxr::TfToken("range:max"), pxr::VtValue(100000)}, + {pxr::TfToken("range:min"), pxr::VtValue(0)} + }; + auto errorsMaterialEmissiveIntensity = CreateMaterialInput( + shaderPrim, + "emissive_intensity", + pxr::SdfValueTypeNames->Float, + emissive.A(), + customDataIntensity, + pxr::TfToken("Emissive Intensity"), + pxr::TfToken("Emissive"), + "Intensity of the emission"); + + if (!errorsMaterialEmissiveIntensity.empty()) + { + errors.insert( + errors.end(), + errorsMaterialEmissiveIntensity.begin(), + errorsMaterialEmissiveIntensity.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the emissive intensity of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + + const sdf::Pbr * pbr = _materialSdf->PbrMaterial(); + if (pbr) + { + const sdf::PbrWorkflow * pbrWorkflow = + pbr->Workflow(sdf::PbrWorkflowType::METAL); + if (!pbrWorkflow) + { + pbrWorkflow = pbr->Workflow(sdf::PbrWorkflowType::SPECULAR); + } + + if (pbrWorkflow) + { + const std::map customDataMetallicConstant = + { + {pxr::TfToken("default"), pxr::VtValue(0.5)}, + {pxr::TfToken("range:max"), pxr::VtValue(1)}, + {pxr::TfToken("range:min"), pxr::VtValue(0)} + }; + auto errorsMaterialMetallicConstant = CreateMaterialInput( + shaderPrim, + "metallic_constant", + pxr::SdfValueTypeNames->Float, + pbrWorkflow->Metalness(), + customDataMetallicConstant, + pxr::TfToken("Metallic Amount"), + pxr::TfToken("Reflectivity"), + "Metallic Material"); + if (!errorsMaterialMetallicConstant.empty()) + { + errors.insert( + errors.end(), + errorsMaterialMetallicConstant.begin(), + errorsMaterialMetallicConstant.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the metallic constant of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + const std::map customDataRoughnessConstant = + { + {pxr::TfToken("default"), pxr::VtValue(0.5)}, + {pxr::TfToken("range:max"), pxr::VtValue(1)}, + {pxr::TfToken("range:min"), pxr::VtValue(0)} + }; + auto errorsMaterialReflectionRoughnessConstant = + CreateMaterialInput( + shaderPrim, + "reflection_roughness_constant", + pxr::SdfValueTypeNames->Float, + pbrWorkflow->Roughness(), + customDataRoughnessConstant, + pxr::TfToken("Roughness Amount"), + pxr::TfToken("Reflectivity"), + "Higher roughness values lead to more blurry reflections"); + if (!errorsMaterialReflectionRoughnessConstant.empty()) + { + errors.insert( + errors.end(), + errorsMaterialReflectionRoughnessConstant.begin(), + errorsMaterialReflectionRoughnessConstant.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the roughness constant of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + + const std::map customDefaultSdfAssetPath = + { + {pxr::TfToken("default"), pxr::VtValue(pxr::SdfAssetPath())}, + }; + + if (!pbrWorkflow->AlbedoMap().empty()) + { + std::string copyPath = getMaterialCopyPath(pbrWorkflow->AlbedoMap()); + + std::string fullnameAlbedoMap = + ignition::common::findFile( + ignition::common::basename(pbrWorkflow->AlbedoMap())); + + if (fullnameAlbedoMap.empty()) + { + fullnameAlbedoMap = pbrWorkflow->AlbedoMap(); + } + + copyMaterial(copyPath, fullnameAlbedoMap); + + auto errorsMaterialDiffuseTexture = + CreateMaterialInput( + shaderPrim, + "diffuse_texture", + pxr::SdfValueTypeNames->Asset, + pxr::SdfAssetPath(copyPath), + customDefaultSdfAssetPath, + pxr::TfToken("Base Map"), + pxr::TfToken("Albedo"), + "", + pxr::TfToken("auto")); + if (!errorsMaterialDiffuseTexture.empty()) + { + errors.insert( + errors.end(), + errorsMaterialDiffuseTexture.begin(), + errorsMaterialDiffuseTexture.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the albedo of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + } + if (!pbrWorkflow->MetalnessMap().empty()) + { + std::string copyPath = + getMaterialCopyPath(pbrWorkflow->MetalnessMap()); + + std::string fullnameMetallnessMap = + ignition::common::findFile( + ignition::common::basename(pbrWorkflow->MetalnessMap())); + + if (fullnameMetallnessMap.empty()) + { + fullnameMetallnessMap = pbrWorkflow->MetalnessMap(); + } + + copyMaterial(copyPath, fullnameMetallnessMap); + + auto errorsMaterialMetallicTexture = + CreateMaterialInput( + shaderPrim, + "metallic_texture", + pxr::SdfValueTypeNames->Asset, + pxr::SdfAssetPath(copyPath), + customDefaultSdfAssetPath, + pxr::TfToken("Metallic Map"), + pxr::TfToken("Reflectivity"), + "", + pxr::TfToken("raw")); + if (!errorsMaterialMetallicTexture.empty()) + { + errors.insert( + errors.end(), + errorsMaterialMetallicTexture.begin(), + errorsMaterialMetallicTexture.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the reflectivity of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + } + if (!pbrWorkflow->NormalMap().empty()) + { + std::string copyPath = getMaterialCopyPath(pbrWorkflow->NormalMap()); + + std::string fullnameNormalMap = + ignition::common::findFile( + ignition::common::basename(pbrWorkflow->NormalMap())); + + if (fullnameNormalMap.empty()) + { + fullnameNormalMap = pbrWorkflow->NormalMap(); + } + + copyMaterial(copyPath, fullnameNormalMap); + + auto errorsMaterialNormalMapTexture = + CreateMaterialInput( + shaderPrim, + "normalmap_texture", + pxr::SdfValueTypeNames->Asset, + pxr::SdfAssetPath(copyPath), + customDefaultSdfAssetPath, + pxr::TfToken("Normal Map"), + pxr::TfToken("Normal"), + "", + pxr::TfToken("raw")); + if (!errorsMaterialNormalMapTexture.empty()) + { + errors.insert( + errors.end(), + errorsMaterialNormalMapTexture.begin(), + errorsMaterialNormalMapTexture.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the normal map of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + } + if (!pbrWorkflow->RoughnessMap().empty()) + { + std::string copyPath = + getMaterialCopyPath(pbrWorkflow->RoughnessMap()); + + std::string fullnameRoughnessMap = + ignition::common::findFile( + ignition::common::basename(pbrWorkflow->RoughnessMap())); + + if (fullnameRoughnessMap.empty()) + { + fullnameRoughnessMap = pbrWorkflow->RoughnessMap(); + } + + copyMaterial(copyPath, fullnameRoughnessMap); + + auto errorsMaterialReflectionRoughnessTexture = + CreateMaterialInput( + shaderPrim, + "reflectionroughness_texture", + pxr::SdfValueTypeNames->Asset, + pxr::SdfAssetPath(copyPath), + customDefaultSdfAssetPath, + pxr::TfToken("RoughnessMap Map"), + pxr::TfToken("RoughnessMap"), + "", + pxr::TfToken("raw")); + if (!errorsMaterialReflectionRoughnessTexture.empty()) + { + errors.insert( + errors.end(), + errorsMaterialReflectionRoughnessTexture.begin(), + errorsMaterialReflectionRoughnessTexture.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the roughness map of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + + const std::map + customDataRoughnessTextureInfluence = + { + {pxr::TfToken("default"), pxr::VtValue(0)}, + {pxr::TfToken("range:max"), pxr::VtValue(1)}, + {pxr::TfToken("range:min"), pxr::VtValue(0)} + }; + + auto errorsMaterialReflectionRoughnessTextureInfluence = + CreateMaterialInput( + shaderPrim, + "reflection_roughness_texture_influence", + pxr::SdfValueTypeNames->Bool, + true, + customDataRoughnessTextureInfluence, + pxr::TfToken("Roughness Map Influence"), + pxr::TfToken("Reflectivity"), + "", + pxr::TfToken("raw")); + if (!errorsMaterialReflectionRoughnessTextureInfluence.empty()) + { + errors.insert( + errors.end(), + errorsMaterialReflectionRoughnessTextureInfluence.begin(), + errorsMaterialReflectionRoughnessTextureInfluence.end()); + errors.push_back(UsdError(UsdErrorCode::INVALID_MATERIAL, + "Unable to set the reflectivity of the material at path [" + + _materialPath.GetString() + "]")); + return errors; + } + } + } + } + + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Material_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Material_Sdf2Usd_TEST.cc new file mode 100644 index 000000000..657c5360e --- /dev/null +++ b/usd/src/sdf_parser/Material_Sdf2Usd_TEST.cc @@ -0,0 +1,243 @@ +/* + * Copyright 2022 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 + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/sdf_parser/World.hh" +#include "sdf/Root.hh" +#include "test_config.h" +#include "test_utils.hh" +#include "../UsdTestUtils.hh" + +void CheckMaterial( + const pxr::UsdPrim &_prim, + const pxr::GfVec3f &_diffuseColor, + const pxr::GfVec3f &_emissiveColor, + bool _hasPbr, + const std::string &_albedoName = "", + const std::string &_normalName = "", + const std::string &_roughnessName = "", + const std::string &_metallicName = "") +{ + auto variantshader = pxr::UsdShadeShader(_prim); + ASSERT_TRUE(variantshader); + + std::vector inputs = variantshader.GetInputs(); + pxr::GfVec3f diffuseColor {0, 0, 0}; + pxr::GfVec3f emissiveColor {0, 0, 0}; + + // PBR-specific values + const float _metallicConstant = 0.5; + const bool _enableEmission = true; + + bool checkedDiffuse = false; + bool checkedEmissive = false; + bool checkedAlbedo = false; + bool checkedNormal = false; + bool checkedRoughness = false; + bool checkedMetallicName = false; + bool checkedMetallicConstant = false; + bool checkedEnableEmission = false; + for (auto &input : inputs) + { + if (_hasPbr && input.GetBaseName() == "diffuse_texture") + { + pxr::SdfAssetPath materialPathUSD; + pxr::UsdShadeInput diffuseTextureShaderInput = + variantshader.GetInput(pxr::TfToken("diffuse_texture")); + diffuseTextureShaderInput.Get(&materialPathUSD); + EXPECT_EQ(_albedoName, + materialPathUSD.GetAssetPath()); + checkedAlbedo = true; + } + else if (_hasPbr && input.GetBaseName() == "metallic_constant") + { + pxr::UsdShadeInput metallicConstantShaderInput = + variantshader.GetInput(pxr::TfToken("metallic_constant")); + float metallicConstant; + metallicConstantShaderInput.Get(&metallicConstant); + EXPECT_FLOAT_EQ(_metallicConstant, metallicConstant); + checkedMetallicConstant = true; + } + else if (input.GetBaseName() == "enable_emission") + { + bool enableEmission; + pxr::UsdShadeInput enableEmissiveShaderInput = + variantshader.GetInput(pxr::TfToken("enable_emission")); + enableEmissiveShaderInput.Get(&enableEmission); + EXPECT_EQ(_enableEmission, enableEmission); + checkedEnableEmission = true; + } + else if (_hasPbr && input.GetBaseName() == "normalmap_texture") + { + pxr::SdfAssetPath materialPath; + pxr::UsdShadeInput normalTextureShaderInput = + variantshader.GetInput(pxr::TfToken("normalmap_texture")); + normalTextureShaderInput.Get(&materialPath); + EXPECT_EQ(_normalName, materialPath.GetAssetPath()); + checkedNormal = true; + } + else if (_hasPbr && input.GetBaseName() == "reflectionroughness_texture") + { + pxr::SdfAssetPath materialPath; + pxr::UsdShadeInput roughnessTextureShaderInput = + variantshader.GetInput(pxr::TfToken("reflectionroughness_texture")); + roughnessTextureShaderInput.Get(&materialPath); + EXPECT_EQ(_roughnessName, materialPath.GetAssetPath()); + checkedRoughness = true; + } + else if (_hasPbr && input.GetBaseName() == "metallic_texture") + { + pxr::SdfAssetPath materialPath; + pxr::UsdShadeInput metallicTextureShaderInput = + variantshader.GetInput(pxr::TfToken("metallic_texture")); + metallicTextureShaderInput.Get(&materialPath); + EXPECT_EQ(_metallicName, materialPath.GetAssetPath()); + checkedMetallicName = true; + } + else if (input.GetBaseName() == "emissive_color") + { + pxr::UsdShadeInput emissiveColorShaderInput = + variantshader.GetInput(pxr::TfToken("emissive_color")); + if (emissiveColorShaderInput.Get(&emissiveColor)) + { + EXPECT_EQ(_emissiveColor, emissiveColor); + } + checkedEmissive = true; + } + else if (input.GetBaseName() == "diffuse_color_constant") + { + auto sourceInfoV = input.GetConnectedSources(); + if (sourceInfoV.size() > 0) + { + pxr::UsdShadeInput connectedInput = + sourceInfoV[0].source.GetInput(sourceInfoV[0].sourceName); + + const pxr::SdfPath& thisAttrPath = connectedInput.GetAttr().GetPath(); + auto connectedPrim = _prim.GetStage()->GetPrimAtPath( + thisAttrPath.GetPrimPath()); + if (connectedPrim) + { + connectedPrim.GetAttribute( + pxr::TfToken("inputs:diffuse_color_constant")).Get(&diffuseColor); + } + } + else + { + pxr::UsdShadeInput diffuseShaderInput = + variantshader.GetInput(pxr::TfToken("diffuse_color_constant")); + diffuseShaderInput.Get(&diffuseColor); + } + EXPECT_EQ(_diffuseColor, diffuseColor); + checkedDiffuse = true; + } + } + EXPECT_TRUE(checkedDiffuse); + EXPECT_TRUE(checkedEmissive); + EXPECT_TRUE(checkedEnableEmission); + EXPECT_EQ(_hasPbr, checkedAlbedo); + EXPECT_EQ(_hasPbr, checkedNormal); + EXPECT_EQ(_hasPbr, checkedRoughness); + EXPECT_EQ(_hasPbr, checkedMetallicName); + EXPECT_EQ(_hasPbr, checkedMetallicConstant); +} + +///////////////////////////////////////////////// +// Fixture that creates a USD stage for each test case. +class UsdStageFixture : public::testing::Test +{ + public: UsdStageFixture() = default; + + protected: void SetUp() override + { + this->stage = pxr::UsdStage::CreateInMemory(); + ASSERT_TRUE(this->stage); + } + + public: pxr::UsdStageRefPtr stage; +}; + +///////////////////////////////////////////////// +TEST_F(UsdStageFixture, Material) +{ + sdf::setFindCallback(sdf::usd::testing::findFileCb); + ignition::common::addFindFileURICallback( + std::bind(&sdf::usd::testing::FindResourceUri, std::placeholders::_1)); + + const auto path = sdf::testing::TestFile("sdf", "basic_shapes.sdf"); + sdf::Root root; + + ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root)); + ASSERT_EQ(1u, root.WorldCount()); + auto world = root.WorldByIndex(0u); + + const auto worldPath = std::string("/" + world->Name()); + auto usdErrors = sdf::usd::ParseSdfWorld(*world, stage, worldPath); + EXPECT_TRUE(usdErrors.empty()); + + auto worldPrim = this->stage->GetPrimAtPath(pxr::SdfPath(worldPath)); + ASSERT_TRUE(worldPrim); + + const std::string meshGeometryPath = worldPath + "/mesh/link/visual/geometry"; + ASSERT_TRUE(this->stage->GetPrimAtPath(pxr::SdfPath(meshGeometryPath))); + + { + const std::string materialPath = "/Looks/Material_2"; + ASSERT_TRUE(this->stage->GetPrimAtPath(pxr::SdfPath(materialPath))); + + const std::string materialshaderPath = materialPath + "/Shader"; + const auto materialShaderPrim = this->stage->GetPrimAtPath( + pxr::SdfPath(materialshaderPath)); + ASSERT_TRUE(materialShaderPrim); + + CheckMaterial(materialShaderPrim, + pxr::GfVec3f(0.2, 0.5, 0.1), + pxr::GfVec3f(0, 0, 0), + true, + "materials/textures/albedo_map.png", + "materials/textures/normal_map.png", + "materials/textures/roughness_map.png", + "materials/textures/metalness_map.png"); + } + + { + const std::string materialPath = "/Looks/Material_0"; + ASSERT_TRUE(this->stage->GetPrimAtPath(pxr::SdfPath(materialPath))); + + const std::string materialshaderPath = materialPath + "/Shader"; + const auto materialShaderPrim = this->stage->GetPrimAtPath( + pxr::SdfPath(materialshaderPath)); + ASSERT_TRUE(materialShaderPrim); + + CheckMaterial(materialShaderPrim, + pxr::GfVec3f(0, 0.1, 0.2), + pxr::GfVec3f(0.12, 0.23, 0.34), + false); + } +} diff --git a/usd/src/sdf_parser/Model.cc b/usd/src/sdf_parser/Model.cc index 348a40a68..9130921bf 100644 --- a/usd/src/sdf_parser/Model.cc +++ b/usd/src/sdf_parser/Model.cc @@ -35,6 +35,7 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/Model.hh" +#include "sdf/usd/sdf_parser/Joint.hh" #include "sdf/usd/sdf_parser/Link.hh" #include "../UsdUtils.hh" @@ -46,7 +47,7 @@ inline namespace SDF_VERSION_NAMESPACE { namespace usd { UsdErrors ParseSdfModel(const sdf::Model &_model, pxr::UsdStageRefPtr &_stage, - const std::string &_path, const pxr::SdfPath &/*_worldPath*/) + const std::string &_path, const pxr::SdfPath &_worldPath) { UsdErrors errors; @@ -68,7 +69,6 @@ namespace usd // https://graphics.pixar.com/usd/release/wp_rigid_body_physics.html#plane-shapes if (usd::IsPlane(_model)) { - static const double kPlaneThickness = 0.25; ignition::math::Vector3d planePosition( _model.RawPose().X(), _model.RawPose().Y(), @@ -130,6 +130,23 @@ namespace usd } } + // Parse all of the model's joints and convert them to USD. + for (uint64_t i = 0; i < _model.JointCount(); ++i) + { + const auto joint = *(_model.JointByIndex(i)); + const auto jointPath = std::string(_path + "/" + joint.Name()); + const auto jointErrors = ParseSdfJoint(joint, _stage, jointPath, _model, + sdfLinkToUSDPath, _worldPath); + if (!jointErrors.empty()) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing joint [" + joint.Name() + "].")); + errors.insert(errors.end(), jointErrors.begin(), jointErrors.end()); + return errors; + } + } + // TODO(adlarkin) finish parsing model return errors; diff --git a/usd/src/sdf_parser/Sensor.cc b/usd/src/sdf_parser/Sensor.cc new file mode 100644 index 000000000..728b503d4 --- /dev/null +++ b/usd/src/sdf_parser/Sensor.cc @@ -0,0 +1,246 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "sdf/usd/sdf_parser/Sensor.hh" + +#include + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Camera.hh" +#include "sdf/Lidar.hh" +#include "sdf/Sensor.hh" +#include "../UsdUtils.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + /// \brief Create a USD camera from a SDF camera object. + /// \param[in] _sensor The SDF camera object + /// \param[in] _stage The stage that contains the definition of the USD + /// camera + /// \param[in] _path The path where the USD representation of _sensor should + /// be defined in _stage + /// \return UsdErrors, which is a list of UsdError objects. An empty list + /// means there were no issues defining the USD representation of _sensor in + /// _stage + UsdErrors ParseSdfCameraSensor(const sdf::Sensor &_sensor, + pxr::UsdStageRefPtr &_stage, const pxr::SdfPath &_path) + { + UsdErrors errors; + + auto usdCamera = pxr::UsdGeomCamera::Define(_stage, _path); + if (!usdCamera) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Unable to define a USD camera at path [" + _path.GetString() + "]")); + return errors; + } + + const auto sdfCamera = _sensor.CameraSensor(); + + // When then focal length is not defined in SDF, the default value is 1 + if (!ignition::math::equal(sdfCamera->LensFocalLength(), 1.0)) + { + usdCamera.CreateFocalLengthAttr().Set( + static_cast(sdfCamera->LensFocalLength())); + } + else + { + // The default value in USD is 50, but something more + // similar to ignition Gazebo is 40. + usdCamera.CreateFocalLengthAttr().Set( + static_cast(40.0f)); + } + usdCamera.CreateClippingRangeAttr().Set(pxr::GfVec2f( + static_cast(sdfCamera->NearClip()), + static_cast(sdfCamera->FarClip()))); + + usdCamera.CreateHorizontalApertureAttr().Set( + static_cast(sdfCamera->HorizontalFov().Degree())); + return errors; + } + + /// \brief Create a USD lidar sensor from a SDF lidar sensor object. + /// \param[in] _sensor The SDF lidar sensor object + /// \param[in] _stage The stage that contains the definition of the USD + /// lidar sensor + /// \param[in] _path The path where the USD representation of _sensor should + /// be defined in _stage + /// \return UsdErrors, which is a list of UsdError objects. An empty list + /// means there were no issues defining the USD representation of _sensor in + /// _stage + UsdErrors ParseSdfLidarSensor(const sdf::Sensor &_sensor, + pxr::UsdStageRefPtr &_stage, const pxr::SdfPath &_path) + { + UsdErrors errors; + + pxr::UsdGeomXform::Define(_stage, _path); + auto lidarPrim = _stage->GetPrimAtPath(_path); + if (!lidarPrim) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Unable to find a lidar sensor prim at path [" + + _path.GetString() + "]")); + return errors; + } + + const auto sdfLidar = _sensor.LidarSensor(); + + lidarPrim.SetTypeName(pxr::TfToken("Lidar")); + lidarPrim.CreateAttribute(pxr::TfToken("minRange"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(sdfLidar->RangeMin())); + lidarPrim.CreateAttribute(pxr::TfToken("maxRange"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(sdfLidar->RangeMax())); + const auto horizontalFov = sdfLidar->HorizontalScanMaxAngle() - + sdfLidar->HorizontalScanMinAngle(); + lidarPrim.CreateAttribute(pxr::TfToken("horizontalFov"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(horizontalFov.Degree())); + const auto verticalFov = sdfLidar->VerticalScanMaxAngle() - + sdfLidar->VerticalScanMinAngle(); + lidarPrim.CreateAttribute(pxr::TfToken("verticalFov"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(verticalFov.Degree())); + lidarPrim.CreateAttribute(pxr::TfToken("horizontalResolution"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(sdfLidar->HorizontalScanResolution())); + lidarPrim.CreateAttribute(pxr::TfToken("verticalResolution"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(sdfLidar->VerticalScanResolution())); + + // TODO(adlarkin) incorporate SDF lidar's horizontal/samples and + // vertical/samples values somehow? There is a "rotationRate" + // attribute for the USD sensor, which might be related + + return errors; + } + + /// \brief Create a USD IMU sensor + /// \param[in] _stage The stage that contains the definition of the USD + /// IMU sensor + /// \param[in] _path The path where the USD IMU should be defined in _stage + /// \return UsdErrors, which is a list of UsdError objects. An empty list + /// means there were no issues defining the USD IMU sensor in _stage + UsdErrors ParseSdfImuSensor(pxr::UsdStageRefPtr &_stage, pxr::SdfPath &_path) + { + UsdErrors errors; + + // for now, IMUs are defined as a cube geometry named "imu" + // (there will be an IMU schema released upstream in the future). + // It should be noted that the Carter robot example from isaac sim sample + // assets has its IMU prim labeled with a "kind = model", but + // https://graphics.pixar.com/usd/release/glossary.html#usdglossary-kind + // says, “'model' is considered an abstract type and should not be assigned + // as any prim’s kind." So, "kind = model" is not applied to the IMU prim + // here. + // TODO(adlarkin) update this code when an IMU schema is released + _path = _path.ReplaceName(pxr::TfToken("imu")); + auto usdCube = pxr::UsdGeomCube::Define(_stage, pxr::SdfPath(_path)); + if (!usdCube) + { + errors.push_back( + UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Unable to define an IMU at path [" + _path.GetString() + "]")); + return errors; + } + // Set the size of the box very small + usdCube.CreateSizeAttr().Set(0.001); + + return errors; + } + + UsdErrors ParseSdfSensor(const sdf::Sensor &_sensor, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + pxr::SdfPath sdfSensorPath(_path); + + switch (_sensor.Type()) + { + case sdf::SensorType::CAMERA: + errors = ParseSdfCameraSensor(_sensor, _stage, sdfSensorPath); + break; + case sdf::SensorType::LIDAR: + case sdf::SensorType::GPU_LIDAR: + errors = ParseSdfLidarSensor(_sensor, _stage, sdfSensorPath); + break; + case sdf::SensorType::IMU: + errors = ParseSdfImuSensor(_stage, sdfSensorPath); + break; + case sdf::SensorType::CONTACT: + // TODO(adlarkin) figure out how to convert contact sensor. I found the + // following docs, but they seem to require isaac sim specific packages: + // https://docs.omniverse.nvidia.com/py/isaacsim/source/extensions/omni.isaac.contact_sensor/docs/index.html + default: + errors.push_back( + UsdError(sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "This type of sensor [" + _sensor.TypeStr() + + "] is not supported")); + } + + if (errors.empty()) + { + ignition::math::Pose3d pose; + auto poseErrors = sdf::usd::PoseWrtParent(_sensor, pose); + if (!poseErrors.empty()) + { + errors.insert(errors.end(), poseErrors.begin(), poseErrors.end()); + return errors; + } + + if (_sensor.Type() == sdf::SensorType::CAMERA) + { + // Camera sensors are upAxis equal to "Y", we need to rotate the camera + // properly. + const ignition::math::Pose3d poseCamera( + 0, 0, 0, IGN_PI_2, 0, -IGN_PI_2); + usd::SetPose( + pose * poseCamera, _stage, sdfSensorPath); + } + else + { + usd::SetPose(pose, _stage, sdfSensorPath); + } + } + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Sensors_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Sensors_Sdf2Usd_TEST.cc new file mode 100644 index 000000000..4eefdf32e --- /dev/null +++ b/usd/src/sdf_parser/Sensors_Sdf2Usd_TEST.cc @@ -0,0 +1,128 @@ +/* + * Copyright 2022 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 + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include + +#include "sdf/usd/sdf_parser/World.hh" +#include "sdf/Root.hh" +#include "test_config.h" +#include "test_utils.hh" +#include "../UsdTestUtils.hh" + +///////////////////////////////////////////////// +// Fixture that creates a USD stage for each test case. +class UsdStageFixture : public::testing::Test +{ + public: UsdStageFixture() = default; + + protected: void SetUp() override + { + this->stage = pxr::UsdStage::CreateInMemory(); + ASSERT_TRUE(this->stage); + } + + public: pxr::UsdStageRefPtr stage; +}; + +///////////////////////////////////////////////// +TEST_F(UsdStageFixture, Sensors) +{ + sdf::setFindCallback(sdf::usd::testing::findFileCb); + ignition::common::addFindFileURICallback( + std::bind(&sdf::usd::testing::FindResourceUri, std::placeholders::_1)); + + const auto path = sdf::testing::TestFile("sdf", "usd_sensors.sdf"); + sdf::Root root; + + ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root)); + ASSERT_EQ(1u, root.WorldCount()); + auto world = root.WorldByIndex(0u); + + const auto worldPath = std::string("/" + world->Name()); + auto usdErrors = sdf::usd::ParseSdfWorld(*world, stage, worldPath); + EXPECT_TRUE(usdErrors.empty()); + + auto worldPrim = this->stage->GetPrimAtPath(pxr::SdfPath(worldPath)); + ASSERT_TRUE(worldPrim); + + const std::string cameraPath = worldPath + "/model_with_camera"; + const std::string cameraLinkPath = cameraPath + "/link"; + const pxr::SdfPath cameraSensorPath(cameraLinkPath + "/camera"); + const auto usdCamera = pxr::UsdGeomCamera::Get(this->stage, cameraSensorPath); + ASSERT_TRUE(usdCamera); + + float focalLength; + pxr::GfVec2f clippingRange; + float horizontalAperture; + usdCamera.GetFocalLengthAttr().Get(&focalLength); + usdCamera.GetClippingRangeAttr().Get(&clippingRange); + usdCamera.GetHorizontalApertureAttr().Get(&horizontalAperture); + EXPECT_FLOAT_EQ(40.0f, focalLength); + EXPECT_EQ(pxr::GfVec2f(0.1, 100), clippingRange); + EXPECT_FLOAT_EQ(59.98868, horizontalAperture); + + const std::string lidarPath = worldPath + "/model_with_lidar"; + const std::string lidarLinkPath = lidarPath + "/link"; + const std::string lidarSensorPath = lidarLinkPath + "/gpu_lidar"; + const auto lidarSensor = this->stage->GetPrimAtPath( + pxr::SdfPath(lidarSensorPath)); + ASSERT_TRUE(lidarSensor); + float hFOV; + float hResolution; + float vFOV; + float vResolution; + float minRange; + float maxRange; + lidarSensor.GetAttribute(pxr::TfToken("minRange")).Get(&minRange); + lidarSensor.GetAttribute(pxr::TfToken("maxRange")).Get(&maxRange); + lidarSensor.GetAttribute(pxr::TfToken("horizontalFov")).Get(&hFOV); + lidarSensor.GetAttribute(pxr::TfToken("horizontalResolution")).Get(&hResolution); + lidarSensor.GetAttribute(pxr::TfToken("verticalFov")).Get(&vFOV); + lidarSensor.GetAttribute(pxr::TfToken("verticalResolution")).Get(&vResolution); + EXPECT_FLOAT_EQ(10.0f, maxRange); + EXPECT_FLOAT_EQ(0.08f, minRange); + EXPECT_FLOAT_EQ(159.99995f, hFOV); + EXPECT_FLOAT_EQ(1.0f, hResolution); + EXPECT_FLOAT_EQ(29.999956f, vFOV); + EXPECT_FLOAT_EQ(1.0f, vResolution); + + const std::string imuPath = worldPath + "/model_with_imu"; + const std::string imuLinkPath = imuPath + "/link"; + const pxr::SdfPath imuSensorPath(imuLinkPath + "/imu"); + const auto usdIMUCube = pxr::UsdGeomCube::Get(this->stage, imuSensorPath); + ASSERT_TRUE(usdIMUCube); + double imuSize; + usdIMUCube.GetSizeAttr().Get(&imuSize); + EXPECT_DOUBLE_EQ(0.001, imuSize); +} diff --git a/usd/src/sdf_parser/Visual.cc b/usd/src/sdf_parser/Visual.cc new file mode 100644 index 000000000..3dbbb889c --- /dev/null +++ b/usd/src/sdf_parser/Visual.cc @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "sdf/usd/sdf_parser/Visual.hh" + +#include + +#include + +// TODO(adlarkin) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Visual.hh" +#include "sdf/usd/sdf_parser/Geometry.hh" +#include "sdf/usd/sdf_parser/Material.hh" +#include "../UsdUtils.hh" + +namespace sdf +{ +// Inline bracke to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + UsdErrors ParseSdfVisual(const sdf::Visual &_visual, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + const pxr::SdfPath sdfVisualPath(_path); + auto usdVisualXform = pxr::UsdGeomXform::Define(_stage, sdfVisualPath); + if (!usdVisualXform) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Not able to define a Geom Xform at path [" + _path + "]")); + return errors; + } + + ignition::math::Pose3d pose; + auto poseErrors = usd::PoseWrtParent(_visual, pose); + if (!poseErrors.empty()) + { + for (const auto &e : poseErrors) + errors.push_back(e); + return errors; + } + + poseErrors = usd::SetPose(pose, _stage, sdfVisualPath); + if (!poseErrors.empty()) + { + for (const auto &e : poseErrors) + errors.push_back(e); + errors.push_back(UsdError(UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Unable to set the pose of the link prim corresponding to the " + "SDF visual named [" + _visual.Name() + "]")); + return errors; + } + + const auto geometry = *(_visual.Geom()); + const auto geometryPath = std::string(_path + "/geometry"); + auto geomErrors = ParseSdfGeometry(geometry, _stage, geometryPath); + if (!geomErrors.empty()) + { + errors.insert(errors.end(), geomErrors.begin(), geomErrors.end()); + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing geometry attached to visual [" + _visual.Name() + "]")); + return errors; + } + + if (auto geomPrim = _stage->GetPrimAtPath(pxr::SdfPath(geometryPath))) + { + if (_visual.Material()) + { + pxr::SdfPath materialPath; + UsdErrors materialErrors = ParseSdfMaterial( + _visual.Material(), _stage, materialPath); + if (!materialErrors.empty()) + { + errors.insert(errors.end(), materialErrors.begin(), + materialErrors.end()); + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing material attached to visual [" + + _visual.Name() + "]")); + return errors; + } + + auto materialUSD = + pxr::UsdShadeMaterial(_stage->GetPrimAtPath(materialPath)); + if (!materialUSD) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Unable to convert prim at path [" + materialPath.GetString() + + "] to a pxr::UsdShadeMaterial.")); + return errors; + } + pxr::UsdShadeMaterialBindingAPI(geomPrim).Bind(materialUSD); + } + } + else + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Internal error: no geometry prim exists at path [" + + geometryPath + "]")); + return errors; + } + + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Visual_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Visual_Sdf2Usd_TEST.cc new file mode 100644 index 000000000..c80b71ef7 --- /dev/null +++ b/usd/src/sdf_parser/Visual_Sdf2Usd_TEST.cc @@ -0,0 +1,157 @@ +/* + * Copyright 2022 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 + +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/sdf_parser/World.hh" +#include "sdf/Root.hh" +#include "test_config.h" +#include "test_utils.hh" +#include "../UsdTestUtils.hh" + +///////////////////////////////////////////////// +// Fixture that creates a USD stage for each test case. +class UsdStageFixture : public::testing::Test +{ + public: UsdStageFixture() = default; + + protected: void SetUp() override + { + this->stage = pxr::UsdStage::CreateInMemory(); + ASSERT_TRUE(this->stage); + } + + public: pxr::UsdStageRefPtr stage; +}; + +///////////////////////////////////////////////// +TEST_F(UsdStageFixture, Visual) +{ + sdf::setFindCallback(sdf::usd::testing::findFileCb); + ignition::common::addFindFileURICallback( + std::bind(&sdf::usd::testing::FindResourceUri, std::placeholders::_1)); + + const auto path = sdf::testing::TestFile("sdf", "basic_shapes.sdf"); + sdf::Root root; + + ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root)); + ASSERT_EQ(1u, root.WorldCount()); + const auto world = root.WorldByIndex(0u); + + const auto worldPath = std::string("/" + world->Name()); + const auto usdErrors = sdf::usd::ParseSdfWorld(*world, stage, worldPath); + EXPECT_TRUE(usdErrors.empty()); + + const auto worldPrim = this->stage->GetPrimAtPath(pxr::SdfPath(worldPath)); + ASSERT_TRUE(worldPrim); + + const auto targetPose = ignition::math::Pose3d( + ignition::math::Vector3d(0, 0, 0), + ignition::math::Quaterniond(0, 0, 0)); + + const std::string groundPlanePath = worldPath + "/ground_plane"; + const auto groundPlane = + this->stage->GetPrimAtPath(pxr::SdfPath(groundPlanePath)); + ASSERT_TRUE(groundPlane); + const std::string groundPlaneLinkPath = groundPlanePath + "/link"; + const auto groundPlaneLink = this->stage->GetPrimAtPath( + pxr::SdfPath(groundPlaneLinkPath)); + ASSERT_TRUE(groundPlaneLink); + const std::string groundPlaneVisualPath = groundPlaneLinkPath + "/visual"; + const auto groundPlaneVisual = this->stage->GetPrimAtPath( + pxr::SdfPath(groundPlaneVisualPath)); + ASSERT_TRUE(groundPlaneVisual); + sdf::usd::testing::CheckPrimPose(groundPlaneVisual, targetPose); + + const std::string boxPath = worldPath + "/box"; + const auto box = this->stage->GetPrimAtPath(pxr::SdfPath(boxPath)); + ASSERT_TRUE(box); + const std::string boxLinkPath = boxPath + "/link"; + const auto boxLink = this->stage->GetPrimAtPath(pxr::SdfPath(boxLinkPath)); + ASSERT_TRUE(boxLink); + const std::string boxVisualPath = boxLinkPath + "/box_vis"; + const auto boxVisual = this->stage->GetPrimAtPath( + pxr::SdfPath(boxVisualPath)); + ASSERT_TRUE(boxVisual); + sdf::usd::testing::CheckPrimPose(boxVisual, targetPose); + + const std::string cylinderPath = worldPath + "/cylinder"; + auto cylinder = this->stage->GetPrimAtPath(pxr::SdfPath(cylinderPath)); + ASSERT_TRUE(cylinder); + std::string cylinderLinkPath = cylinderPath + "/link"; + const auto cylinderLink = + this->stage->GetPrimAtPath(pxr::SdfPath(cylinderLinkPath)); + ASSERT_TRUE(cylinderLink); + const std::string cylinderVisualPath = cylinderLinkPath + "/visual"; + const auto cylinderVisual = this->stage->GetPrimAtPath( + pxr::SdfPath(cylinderVisualPath)); + ASSERT_TRUE(cylinderVisual); + sdf::usd::testing::CheckPrimPose(cylinderVisual, targetPose); + + const std::string spherePath = worldPath + "/sphere"; + const auto sphere = this->stage->GetPrimAtPath(pxr::SdfPath(spherePath)); + ASSERT_TRUE(sphere); + const std::string sphereLinkPath = spherePath + "/link"; + const auto sphereLink = + this->stage->GetPrimAtPath(pxr::SdfPath(sphereLinkPath)); + ASSERT_TRUE(sphereLink); + const std::string sphereVisualPath = sphereLinkPath + "/sphere_vis"; + const auto sphereVisual = this->stage->GetPrimAtPath( + pxr::SdfPath(sphereVisualPath)); + ASSERT_TRUE(sphereVisual); + sdf::usd::testing::CheckPrimPose(sphereVisual, targetPose); + + const std::string capsulePath = worldPath + "/capsule"; + const auto capsule = this->stage->GetPrimAtPath(pxr::SdfPath(capsulePath)); + ASSERT_TRUE(capsule); + const std::string capsuleLinkPath = capsulePath + "/link"; + const auto capsuleLink = + this->stage->GetPrimAtPath(pxr::SdfPath(capsuleLinkPath)); + ASSERT_TRUE(capsuleLink); + const std::string capsuleVisualPath = capsuleLinkPath + "/visual"; + const auto capsuleVisual = this->stage->GetPrimAtPath( + pxr::SdfPath(capsuleVisualPath)); + ASSERT_TRUE(capsuleVisual); + sdf::usd::testing::CheckPrimPose(capsuleVisual, targetPose); + + const std::string meshPath = worldPath + "/mesh"; + const auto mesh = this->stage->GetPrimAtPath(pxr::SdfPath(meshPath)); + ASSERT_TRUE(mesh); + const std::string meshLinkPath = meshPath + "/link"; + const auto meshLink = this->stage->GetPrimAtPath(pxr::SdfPath(meshLinkPath)); + ASSERT_TRUE(meshLink); + const std::string meshVisualPath = meshLinkPath + "/visual"; + const auto meshVisual = this->stage->GetPrimAtPath( + pxr::SdfPath(meshVisualPath)); + ASSERT_TRUE(meshVisual); + sdf::usd::testing::CheckPrimPose(meshVisual, targetPose); +} diff --git a/usd/src/sdf_parser/sdf2usd_TEST.cc b/usd/src/sdf_parser/sdf2usd_TEST.cc index d532172dc..a50654396 100644 --- a/usd/src/sdf_parser/sdf2usd_TEST.cc +++ b/usd/src/sdf_parser/sdf2usd_TEST.cc @@ -69,7 +69,7 @@ TEST(check_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a good SDF file { std::string path = ignition::common::joinPaths(pathBase, - "shapes_world.sdf"); + "lights.sdf"); const auto outputUsdFilePath = ignition::common::joinPaths(tmp, "shapes.usd"); EXPECT_FALSE(ignition::common::isFile(outputUsdFilePath)); @@ -81,7 +81,7 @@ TEST(check_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // that functionality isn't complete) // make sure that a shapes.usd file was generated - EXPECT_TRUE(ignition::common::isFile(outputUsdFilePath)); + EXPECT_TRUE(ignition::common::isFile(outputUsdFilePath)) << output; // TODO(ahcorde): Check the contents of outputUsdFilePath when the parser // is implemented