From 310b6fdf89c69f0c734c7292cba522436d09418f Mon Sep 17 00:00:00 2001 From: Aditya Agrawal Date: Fri, 16 Aug 2024 11:55:15 +0800 Subject: [PATCH 01/23] added changes Signed-off-by: Aditya Agrawal --- include/sdf/Actor.hh | 3 ++ include/sdf/AirPressure.hh | 3 ++ include/sdf/AirSpeed.hh | 3 ++ include/sdf/Altimeter.hh | 3 ++ include/sdf/Atmosphere.hh | 3 ++ include/sdf/Camera.hh | 3 ++ include/sdf/Collision.hh | 3 ++ include/sdf/ForceTorque.hh | 2 + include/sdf/Frame.hh | 3 ++ include/sdf/Geometry.hh | 3 ++ include/sdf/Gui.hh | 3 ++ include/sdf/Imu.hh | 3 ++ include/sdf/Joint.hh | 3 ++ include/sdf/Lidar.hh | 3 ++ include/sdf/Light.hh | 3 ++ include/sdf/Link.hh | 3 ++ include/sdf/Magnetometer.hh | 3 ++ include/sdf/Material.hh | 3 ++ include/sdf/Model.hh | 3 ++ include/sdf/NavSat.hh | 3 ++ include/sdf/Noise.hh | 3 ++ include/sdf/ParticleEmitter.hh | 3 ++ include/sdf/Physics.hh | 3 ++ include/sdf/Plugin.hh | 3 ++ include/sdf/Projector.hh | 3 ++ include/sdf/Root.hh | 3 ++ include/sdf/Scene.hh | 3 ++ include/sdf/Sensor.hh | 3 ++ include/sdf/Surface.hh | 3 ++ include/sdf/Visual.hh | 3 ++ include/sdf/World.hh | 3 ++ src/Actor.cc | 8 ++++ src/AirPressure.cc | 8 ++++ src/AirSpeed.cc | 8 ++++ src/Altimeter.cc | 8 ++++ src/Atmosphere.cc | 8 ++++ src/Camera.cc | 8 ++++ src/Collision.cc | 8 ++++ src/ForceTorque.cc | 8 ++++ src/Frame.cc | 8 ++++ src/Geometry.cc | 8 ++++ src/Gui.cc | 8 ++++ src/Imu.cc | 8 ++++ src/Joint.cc | 8 ++++ src/Lidar.cc | 8 ++++ src/Light.cc | 8 ++++ src/Link.cc | 8 ++++ src/Magnetometer.cc | 8 ++++ src/Material.cc | 8 ++++ src/Model.cc | 8 ++++ src/NavSat.cc | 8 ++++ src/Noise.cc | 8 ++++ src/ParticleEmitter.cc | 8 ++++ src/Physics.cc | 8 ++++ src/Plugin.cc | 8 ++++ src/Projector.cc | 8 ++++ src/Root.cc | 8 ++++ src/Scene.cc | 8 ++++ src/Sensor.cc | 8 ++++ src/Surface.cc | 8 ++++ src/Visual.cc | 8 ++++ src/World.cc | 8 ++++ tools/format_schema.py | 67 ++++++++++++++++++++++++++++++++++ 63 files changed, 407 insertions(+) create mode 100644 tools/format_schema.py diff --git a/include/sdf/Actor.hh b/include/sdf/Actor.hh index 509ae58a9..57c5c0fc9 100644 --- a/include/sdf/Actor.hh +++ b/include/sdf/Actor.hh @@ -193,6 +193,9 @@ namespace sdf /// \brief Default constructor public: Actor(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the actor based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/AirPressure.hh b/include/sdf/AirPressure.hh index 798474dd5..b8fae845e 100644 --- a/include/sdf/AirPressure.hh +++ b/include/sdf/AirPressure.hh @@ -36,6 +36,9 @@ namespace sdf /// \brief Default constructor public: AirPressure(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the airPressure based on an element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. diff --git a/include/sdf/AirSpeed.hh b/include/sdf/AirSpeed.hh index 90a1be025..050c7fac6 100644 --- a/include/sdf/AirSpeed.hh +++ b/include/sdf/AirSpeed.hh @@ -36,6 +36,9 @@ namespace sdf /// \brief Default constructor public: AirSpeed(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the air speed based on an element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. diff --git a/include/sdf/Altimeter.hh b/include/sdf/Altimeter.hh index c27a6daf1..d6ac7a962 100644 --- a/include/sdf/Altimeter.hh +++ b/include/sdf/Altimeter.hh @@ -35,6 +35,9 @@ namespace sdf /// \brief Default constructor public: Altimeter(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the altimeter based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Atmosphere.hh b/include/sdf/Atmosphere.hh index ed4042128..3efb86813 100644 --- a/include/sdf/Atmosphere.hh +++ b/include/sdf/Atmosphere.hh @@ -47,6 +47,9 @@ namespace sdf /// \brief Default constructor public: Atmosphere(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the atmosphere based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 5c46827c8..6b25af210 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -62,6 +62,9 @@ namespace sdf /// \brief Constructor public: Camera(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Return true if both Camera objects contain the same values. /// \param[_in] _alt Camera value to compare. /// \returen True if 'this' == _alt. diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 2ab4114ce..f49700f59 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -52,6 +52,9 @@ namespace sdf /// \brief Default constructor public: Collision(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the collision based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh index 2a9a693ed..c84cd693f 100644 --- a/include/sdf/ForceTorque.hh +++ b/include/sdf/ForceTorque.hh @@ -66,6 +66,8 @@ namespace sdf /// \brief Default constructor public: ForceTorque(); + public: static inline std::string_view SchemaFile(); + /// \brief Load the force torque sensor based on an element pointer. This is /// *not* the usual entry point. Typical usage of the SDF DOM is through the /// Root object. diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index cecb63ea9..90e9d0d45 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -44,6 +44,9 @@ namespace sdf /// \brief Default constructor public: Frame(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the frame based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 35fd1e1cd..800dfb9a6 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -90,6 +90,9 @@ namespace sdf /// \brief Default constructor public: Geometry(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the geometry based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Gui.hh b/include/sdf/Gui.hh index 67435fdb0..a66ab4d41 100644 --- a/include/sdf/Gui.hh +++ b/include/sdf/Gui.hh @@ -33,6 +33,9 @@ namespace sdf /// \brief Default constructor public: Gui(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the gui based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index e3ced1181..83ce17a40 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -35,6 +35,9 @@ namespace sdf /// \brief Default constructor public: Imu(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the IMU based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 26c4b226f..1147ad4e2 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -87,6 +87,9 @@ namespace sdf /// \brief Default constructor public: Joint(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the joint based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Lidar.hh b/include/sdf/Lidar.hh index 89cfa612b..dac6bb4ab 100644 --- a/include/sdf/Lidar.hh +++ b/include/sdf/Lidar.hh @@ -107,6 +107,9 @@ namespace sdf /// \brief Default constructor public: Lidar(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the lidar based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index fc19e02f1..21f31e763 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -65,6 +65,9 @@ namespace sdf /// \brief Default constructor public: Light(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the light based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 3e5260e73..291fa27b0 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -53,6 +53,9 @@ namespace sdf /// \brief Default constructor public: Link(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the link based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Magnetometer.hh b/include/sdf/Magnetometer.hh index 3f6d9ae74..db1d4f697 100644 --- a/include/sdf/Magnetometer.hh +++ b/include/sdf/Magnetometer.hh @@ -36,6 +36,9 @@ namespace sdf /// \brief Default constructor public: Magnetometer(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the magnetometer based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index 6891a8899..32115d9b6 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -48,6 +48,9 @@ namespace sdf /// \brief Default constructor public: Material(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the material based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 86cc3d422..163121965 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -56,6 +56,9 @@ namespace sdf /// \brief Default constructor public: Model(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the model based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/NavSat.hh b/include/sdf/NavSat.hh index 9b1677d77..e85b6d0f6 100644 --- a/include/sdf/NavSat.hh +++ b/include/sdf/NavSat.hh @@ -82,6 +82,9 @@ namespace sdf /// \param[in] _sdf The SDF Element pointer /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error. + + public: static inline std::string_view SchemaFile(); + public: Errors Load(ElementPtr _sdf); /// \brief Get a pointer to the SDF element that was used during diff --git a/include/sdf/Noise.hh b/include/sdf/Noise.hh index 361e12f7e..202f57efb 100644 --- a/include/sdf/Noise.hh +++ b/include/sdf/Noise.hh @@ -49,6 +49,9 @@ namespace sdf /// \brief Default constructor public: Noise(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Return true if both Noise objects contain the same values. /// \param[_in] _noise Noise value to compare. /// \return True if 'this' == _noise. diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh index bf592de24..d20534db1 100644 --- a/include/sdf/ParticleEmitter.hh +++ b/include/sdf/ParticleEmitter.hh @@ -62,6 +62,9 @@ namespace sdf /// \brief Default constructor public: ParticleEmitter(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the particle emitter based on an element pointer. This is /// *not* the usual entry point. Typical usage of the SDF DOM is through /// the Root object. diff --git a/include/sdf/Physics.hh b/include/sdf/Physics.hh index 0fa5ea5ef..6bd63e54b 100644 --- a/include/sdf/Physics.hh +++ b/include/sdf/Physics.hh @@ -38,6 +38,9 @@ namespace sdf /// \brief Default constructor public: Physics(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the physics based on an element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Plugin.hh b/include/sdf/Plugin.hh index def211593..bc49a4f0d 100644 --- a/include/sdf/Plugin.hh +++ b/include/sdf/Plugin.hh @@ -47,6 +47,9 @@ namespace sdf /// \brief Default constructor public: Plugin(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Default destructor public: ~Plugin(); diff --git a/include/sdf/Projector.hh b/include/sdf/Projector.hh index b13fd8797..a4c8bdd81 100644 --- a/include/sdf/Projector.hh +++ b/include/sdf/Projector.hh @@ -45,6 +45,9 @@ namespace sdf /// \brief Default constructor public: Projector(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the projector based on an element pointer. This is /// *not* the usual entry point. Typical usage of the SDF DOM is through /// the Root object. diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 9ad5f1632..a8c823cf2 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -59,6 +59,9 @@ namespace sdf /// \brief Default constructor public: Root(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Get the name of the world without loading the entire world /// Users shouldn't normally need to use this API. /// This doesn't load the world, it might return the world name even if the diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index e11f51596..5c33334c3 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -35,6 +35,9 @@ namespace sdf /// \brief Default constructor public: Scene(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the scene based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 7b62572ec..38c3f1fce 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -142,6 +142,9 @@ namespace sdf /// \brief Default constructor public: Sensor(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the sensor based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 02fd98be5..ad855d3a4 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -161,6 +161,9 @@ namespace sdf /// \brief Default constructor public: Surface(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the surface based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index ff9da957f..571cfcdd9 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -50,6 +50,9 @@ namespace sdf /// \brief Default constructor public: Visual(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the visual based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 7e449abab..4d68a3dcc 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -60,6 +60,9 @@ namespace sdf /// \brief Default constructor public: World(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the world based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/src/Actor.cc b/src/Actor.cc index 0a0eebb93..595ef1f8b 100644 --- a/src/Actor.cc +++ b/src/Actor.cc @@ -819,3 +819,11 @@ void Actor::AddPlugin(const Plugin &_plugin) { this->dataPtr->plugins.push_back(_plugin); } + +///////////////////////////////////////////////// +inline std::string_view Actor::SchemaFile() +{ + static char kSchemaFile[] = "actor.sdf"; + return kSchemaFile; +} + diff --git a/src/AirPressure.cc b/src/AirPressure.cc index 9cc58a8a0..fdbdc5b35 100644 --- a/src/AirPressure.cc +++ b/src/AirPressure.cc @@ -139,3 +139,11 @@ sdf::ElementPtr AirPressure::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view AirPressure::SchemaFile() +{ + static char kSchemaFile[] = "air_pressure.sdf"; + return kSchemaFile; +} + diff --git a/src/AirSpeed.cc b/src/AirSpeed.cc index 2ca0e16c9..0045f6ac4 100644 --- a/src/AirSpeed.cc +++ b/src/AirSpeed.cc @@ -106,3 +106,11 @@ sdf::ElementPtr AirSpeed::ToElement() const return elem; } + +///////////////////////////////////////////////// +inline std::string_view AirSpeed::SchemaFile() +{ + static char kSchemaFile[] = "air_speed.sdf"; + return kSchemaFile; +} + diff --git a/src/Altimeter.cc b/src/Altimeter.cc index 83fcad037..638ebcc88 100644 --- a/src/Altimeter.cc +++ b/src/Altimeter.cc @@ -169,3 +169,11 @@ sdf::ElementPtr Altimeter::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Altimeter::SchemaFile() +{ + static char kSchemaFile[] = "altimeter.sdf"; + return kSchemaFile; +} + diff --git a/src/Atmosphere.cc b/src/Atmosphere.cc index 597504658..9ff3e14b4 100644 --- a/src/Atmosphere.cc +++ b/src/Atmosphere.cc @@ -172,3 +172,11 @@ sdf::ElementPtr Atmosphere::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Atmosphere::SchemaFile() +{ + static char kSchemaFile[] = "atmosphere.sdf"; + return kSchemaFile; +} + diff --git a/src/Camera.cc b/src/Camera.cc index 2f2ea7a2b..78ed66de7 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -1337,3 +1337,11 @@ sdf::ElementPtr Camera::ToElement() const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Camera::SchemaFile() +{ + static char kSchemaFile[] = "camera.sdf"; + return kSchemaFile; +} + diff --git a/src/Collision.cc b/src/Collision.cc index ca944712d..e78481e60 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -392,3 +392,11 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Collision::SchemaFile() +{ + static char kSchemaFile[] = "collision.sdf"; + return kSchemaFile; +} + diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index 4386629c1..e48e0c39b 100644 --- a/src/ForceTorque.cc +++ b/src/ForceTorque.cc @@ -367,3 +367,11 @@ sdf::ElementPtr ForceTorque::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view ForceTorque::SchemaFile() +{ + static char kSchemaFile[] = "forcetorque.sdf"; + return kSchemaFile; +} + diff --git a/src/Frame.cc b/src/Frame.cc index de9870915..a12e1f7bc 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -250,3 +250,11 @@ sdf::ElementPtr Frame::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Frame::SchemaFile() +{ + static char kSchemaFile[] = "frame.sdf"; + return kSchemaFile; +} + diff --git a/src/Geometry.cc b/src/Geometry.cc index 9afb20212..d4f73508b 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -421,3 +421,11 @@ sdf::ElementPtr Geometry::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Geometry::SchemaFile() +{ + static char kSchemaFile[] = "geometry.sdf"; + return kSchemaFile; +} + diff --git a/src/Gui.cc b/src/Gui.cc index f64fd61ca..f48b7dd8c 100644 --- a/src/Gui.cc +++ b/src/Gui.cc @@ -147,3 +147,11 @@ sdf::Plugins &Gui::Plugins() } + +///////////////////////////////////////////////// +inline std::string_view Gui::SchemaFile() +{ + static char kSchemaFile[] = "gui.sdf"; + return kSchemaFile; +} + diff --git a/src/Imu.cc b/src/Imu.cc index 65ea2e6e6..644ea095c 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -454,3 +454,11 @@ sdf::ElementPtr Imu::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Imu::SchemaFile() +{ + static char kSchemaFile[] = "imu.sdf"; + return kSchemaFile; +} + diff --git a/src/Joint.cc b/src/Joint.cc index 2b8fd8282..ae4a79398 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -630,3 +630,11 @@ void Joint::ClearSensors() { this->dataPtr->sensors.clear(); } + +///////////////////////////////////////////////// +inline std::string_view Joint::SchemaFile() +{ + static char kSchemaFile[] = "joint.sdf"; + return kSchemaFile; +} + diff --git a/src/Lidar.cc b/src/Lidar.cc index 56803c695..314501a2b 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -450,3 +450,11 @@ sdf::ElementPtr Lidar::ToElement() const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Lidar::SchemaFile() +{ + static char kSchemaFile[] = "lidar.sdf"; + return kSchemaFile; +} + diff --git a/src/Light.cc b/src/Light.cc index 02fb48001..fde792633 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -556,3 +556,11 @@ sdf::ElementPtr Light::ToElement(sdf::Errors &_errors) const _errors, this->SpotFalloff()); return elem; } + +///////////////////////////////////////////////// +inline std::string_view Light::SchemaFile() +{ + static char kSchemaFile[] = "light.sdf"; + return kSchemaFile; +} + diff --git a/src/Link.cc b/src/Link.cc index 085feab93..2d88af046 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -1060,3 +1060,11 @@ sdf::ElementPtr Link::ToElement() const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Link::SchemaFile() +{ + static char kSchemaFile[] = "link.sdf"; + return kSchemaFile; +} + diff --git a/src/Magnetometer.cc b/src/Magnetometer.cc index 80498158d..452d6acd1 100644 --- a/src/Magnetometer.cc +++ b/src/Magnetometer.cc @@ -155,3 +155,11 @@ sdf::ElementPtr Magnetometer::ToElement() const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Magnetometer::SchemaFile() +{ + static char kSchemaFile[] = "magnetometer.sdf"; + return kSchemaFile; +} + diff --git a/src/Material.cc b/src/Material.cc index 6a0456686..30939e0b5 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -528,3 +528,11 @@ sdf::ElementPtr Material::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Material::SchemaFile() +{ + static char kSchemaFile[] = "material.sdf"; + return kSchemaFile; +} + diff --git a/src/Model.cc b/src/Model.cc index 944e50b87..2d846b56b 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -1414,3 +1414,11 @@ sdf::Frame Model::PrepareForMerge(sdf::Errors &_errors, return proxyFrame; } + +///////////////////////////////////////////////// +inline std::string_view Model::SchemaFile() +{ + static char kSchemaFile[] = "model.sdf"; + return kSchemaFile; +} + diff --git a/src/NavSat.cc b/src/NavSat.cc index 8ab7d7e64..7372ff994 100644 --- a/src/NavSat.cc +++ b/src/NavSat.cc @@ -191,3 +191,11 @@ bool NavSat::operator!=(const NavSat &_navsat) const { return !(*this == _navsat); } + +///////////////////////////////////////////////// +inline std::string_view NavSat::SchemaFile() +{ + static char kSchemaFile[] = "navsat.sdf"; + return kSchemaFile; +} + diff --git a/src/Noise.cc b/src/Noise.cc index b4507e957..a971b672c 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -300,3 +300,11 @@ sdf::ElementPtr Noise::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Noise::SchemaFile() +{ + static char kSchemaFile[] = "noise.sdf"; + return kSchemaFile; +} + diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc index 605b58812..dfc36aea8 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -573,3 +573,11 @@ sdf::ElementPtr ParticleEmitter::ToElement(sdf::Errors &_errors) const } + +///////////////////////////////////////////////// +inline std::string_view ParticleEmitter::SchemaFile() +{ + static char kSchemaFile[] = "particle_emitter.sdf"; + return kSchemaFile; +} + diff --git a/src/Physics.cc b/src/Physics.cc index b9dd69317..1af423242 100644 --- a/src/Physics.cc +++ b/src/Physics.cc @@ -232,3 +232,11 @@ sdf::ElementPtr Physics::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Physics::SchemaFile() +{ + static char kSchemaFile[] = "physics.sdf"; + return kSchemaFile; +} + diff --git a/src/Plugin.cc b/src/Plugin.cc index c85563519..d0b5d7572 100644 --- a/src/Plugin.cc +++ b/src/Plugin.cc @@ -311,3 +311,11 @@ bool Plugin::operator!=(const Plugin &_plugin) const { return !(*this == _plugin); } + +///////////////////////////////////////////////// +inline std::string_view Plugin::SchemaFile() +{ + static char kSchemaFile[] = "plugin.sdf"; + return kSchemaFile; +} + diff --git a/src/Projector.cc b/src/Projector.cc index b8c1f7b98..7b851e973 100644 --- a/src/Projector.cc +++ b/src/Projector.cc @@ -331,3 +331,11 @@ sdf::ElementPtr Projector::ToElement() const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Projector::SchemaFile() +{ + static char kSchemaFile[] = "projector.sdf"; + return kSchemaFile; +} + diff --git a/src/Root.cc b/src/Root.cc index 03349d2af..0d3210684 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -644,3 +644,11 @@ sdf::ElementPtr Root::ToElement(const OutputConfig &_config) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Root::SchemaFile() +{ + static char kSchemaFile[] = "root.sdf"; + return kSchemaFile; +} + diff --git a/src/Scene.cc b/src/Scene.cc index 429d16aee..52e53373b 100644 --- a/src/Scene.cc +++ b/src/Scene.cc @@ -212,3 +212,11 @@ sdf::ElementPtr Scene::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Scene::SchemaFile() +{ + static char kSchemaFile[] = "scene.sdf"; + return kSchemaFile; +} + diff --git a/src/Sensor.cc b/src/Sensor.cc index 78ac7087b..15fcdd455 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -848,3 +848,11 @@ void Sensor::AddPlugin(const Plugin &_plugin) { this->dataPtr->plugins.push_back(_plugin); } + +///////////////////////////////////////////////// +inline std::string_view Sensor::SchemaFile() +{ + static char kSchemaFile[] = "sensor.sdf"; + return kSchemaFile; +} + diff --git a/src/Surface.cc b/src/Surface.cc index 5bc84970d..22b48e198 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -431,3 +431,11 @@ sdf::ElementPtr Surface::ToElement(sdf::Errors &_errors) const return elem; } + +///////////////////////////////////////////////// +inline std::string_view Surface::SchemaFile() +{ + static char kSchemaFile[] = "surface.sdf"; + return kSchemaFile; +} + diff --git a/src/Visual.cc b/src/Visual.cc index 5ffdf3d33..685c26e9b 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -378,3 +378,11 @@ void Visual::AddPlugin(const Plugin &_plugin) { this->dataPtr->plugins.push_back(_plugin); } + +///////////////////////////////////////////////// +inline std::string_view Visual::SchemaFile() +{ + static char kSchemaFile[] = "visual.sdf"; + return kSchemaFile; +} + diff --git a/src/World.cc b/src/World.cc index d94ff685a..fa1f939ce 100644 --- a/src/World.cc +++ b/src/World.cc @@ -1271,3 +1271,11 @@ void World::AddPlugin(const Plugin &_plugin) { this->dataPtr->plugins.push_back(_plugin); } + +///////////////////////////////////////////////// +inline std::string_view World::SchemaFile() +{ + static char kSchemaFile[] = "world.sdf"; + return kSchemaFile; +} + diff --git a/tools/format_schema.py b/tools/format_schema.py new file mode 100644 index 000000000..ebaa97faa --- /dev/null +++ b/tools/format_schema.py @@ -0,0 +1,67 @@ +import os + +lines = os.listdir('./sdf/1.11/') +# Only keep the files that end with .sdf +lines = [line for line in lines if line.endswith('.sdf')] + +for line in lines: + # Remove .sdf extension + snake_case = line.strip().split('.')[0] + + # Convert from snake_case to CamelCase + camel_case = ''.join([word.capitalize() for word in snake_case.split('_')]) + + addHeader = """ + /// \\brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + """ + + addImpl = f""" +///////////////////////////////////////////////// +inline std::string_view {camel_case}::SchemaFile() +{{ + static const char kSchemaFile[] = "{line}"; + return kSchemaFile; +}}\n\n""" + + # Debug print statements + # print(addHeader) + # print(addImpl) + # print(addTest) + # print("\n") + + # Edit './include/sdf/{camel_case}.hh' + # Find the line with 'class SDFFORMAT_VISIBLE {camel_case}` + try: + f = open('./include/sdf/' + camel_case + '.hh', 'r') + lines = f.readlines() + f.close() + line_number = 0 + for i, line in enumerate(lines): + if line == " class SDFORMAT_VISIBLE " + camel_case + '\n': + line_number = i+3 # After the class line there is `\n{\n` and then the constructor + break + if line_number == 0: + print("Error: Could not find class declaration in " + camel_case + ".hh") + exit(1) + + try: + with open('./include/sdf/' + camel_case + '.hh', 'w') as file: + for i, line in enumerate(lines): + file.write(line) + if i == line_number: + file.write(addHeader) + except: + print("Unexpected error while writing to: " + camel_case + ".hh.") + except: + print("Error while writing to: " + camel_case + ".hh." + " Check if file exists.") + + # Edit './src/{camel_case}.cc' if it exists + # Add implementation to end of document + if os.path.exists('./src/' + camel_case + '.cc'): + with open('./src/' + camel_case + '.cc', 'a') as file: + file.write(addImpl) + else: + print("Error: Could not find " + camel_case + ".cc") + + print("Changes written to source successfully") From f8e22dc59883cce01430e8eccc4d7559f0f3d1f6 Mon Sep 17 00:00:00 2001 From: Aditya Agrawal Date: Fri, 16 Aug 2024 11:55:38 +0800 Subject: [PATCH 02/23] string to string_view, refactoring Signed-off-by: Aditya Agrawal --- format_schema.py | 83 ++++++++++++++++++++++++++++++++++++++++++ src/Actor.cc | 2 +- src/AirPressure.cc | 2 +- src/AirSpeed.cc | 2 +- src/Altimeter.cc | 2 +- src/Atmosphere.cc | 2 +- src/Camera.cc | 2 +- src/Collision.cc | 2 +- src/ForceTorque.cc | 2 +- src/Frame.cc | 2 +- src/Geometry.cc | 2 +- src/Gui.cc | 2 +- src/Imu.cc | 2 +- src/Joint.cc | 2 +- src/Lidar.cc | 2 +- src/Light.cc | 2 +- src/Link.cc | 2 +- src/Magnetometer.cc | 2 +- src/Material.cc | 2 +- src/Model.cc | 2 +- src/Noise.cc | 2 +- src/ParticleEmitter.cc | 2 +- src/Physics.cc | 2 +- src/Plugin.cc | 2 +- src/Projector.cc | 2 +- src/Root.cc | 2 +- src/Scene.cc | 2 +- src/Sensor.cc | 2 +- src/Surface.cc | 2 +- src/Visual.cc | 2 +- src/World.cc | 2 +- tools/format_schema.py | 67 ---------------------------------- 32 files changed, 113 insertions(+), 97 deletions(-) create mode 100644 format_schema.py delete mode 100644 tools/format_schema.py diff --git a/format_schema.py b/format_schema.py new file mode 100644 index 000000000..b7ae61978 --- /dev/null +++ b/format_schema.py @@ -0,0 +1,83 @@ +import os + +lines = os.listdir('./sdf/1.11/') +# Only keep the files that end with .sdf +lines = [line for line in lines if line.endswith('.sdf')] +for line in lines: + # Remove .sdf extension + snake_case = line.strip().split('.')[0] + + # Convert from snake_case to CamelCase + camel_case = ''.join([word.capitalize() for word in snake_case.split('_')]) + + addHeader = """ + /// \\brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + """ + + addImpl = f""" +///////////////////////////////////////////////// +inline std::string_view {camel_case}::SchemaFile() +{{ + static const char kSchemaFile[] = "{line}"; + return kSchemaFile; +}}\n\n""" + + # Debug print statements + # print(addHeader) + # print(addImpl) + # print(addTest) + # print("\n") + + print(line) + + # # Edit './include/sdf/{camel_case}.hh' + # # Find the line with 'class SDFFORMAT_VISIBLE {camel_case}` + # try: + # f = open('./include/sdf/' + camel_case + '.hh', 'r') + # read_lines = f.readlines() + # f.close() + # line_number = 0 + # for i, line_raw in enumerate(read_lines): + # if line_raw == " class SDFORMAT_VISIBLE " + camel_case + '\n': + # line_number = i+3 # After the class line there is `\n{\n` and then the constructor + # break + # if line_number == 0: + # print("Error: Could not find class declaration in " + camel_case + ".hh") + # exit(1) + # try: + # with open('./include/sdf/' + camel_case + '.hh', 'w') as file: + # for i, line_raw in enumerate(read_lines): + # file.write(line_raw) + # if i == line_number: + # file.write(addHeader) + # except: + # print("Unexpected error while writing to: " + camel_case + ".hh.") + # except: + # print("Error while writing to: " + camel_case + ".hh." + " Check if file exists.") + + # Replace all instances of `line` with `std::string(this->SchemaFile())` + if (os.path.exists('./src/' + camel_case + '.cc')): + try: + f = open('./src/' + camel_case + '.cc', 'r') + read_lines = f.readlines() + f.close() + + with open('./src/' + camel_case + '.cc', 'w') as file: + for i, line_raw in enumerate(read_lines): + if "kSchemaFile[]" not in line_raw: + file.write(line_raw.replace(f'"{line}"', 'std::string(this->SchemaFile())')) + else: + file.write(line_raw) + except: + print("Unexpected error while reading from: " + camel_case + ".cc.") + + # # Edit './src/{camel_case}.cc' if it exists + # # Add implementation to end of document + # if os.path.exists('./src/' + camel_case + '.cc'): + # with open('./src/' + camel_case + '.cc', 'a') as file: + # file.write(addImpl) + # else: + # print("Error: Could not find " + camel_case + ".cc") + + print("Changes written to source successfully") diff --git a/src/Actor.cc b/src/Actor.cc index 595ef1f8b..0ca834536 100644 --- a/src/Actor.cc +++ b/src/Actor.cc @@ -726,7 +726,7 @@ void Actor::ClearJoints() sdf::ElementPtr Actor::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("actor.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name()); // Set pose diff --git a/src/AirPressure.cc b/src/AirPressure.cc index fdbdc5b35..1f576ed59 100644 --- a/src/AirPressure.cc +++ b/src/AirPressure.cc @@ -129,7 +129,7 @@ sdf::ElementPtr AirPressure::ToElement() const sdf::ElementPtr AirPressure::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("air_pressure.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetElement("reference_altitude", _errors)->Set( _errors, this->ReferenceAltitude()); diff --git a/src/AirSpeed.cc b/src/AirSpeed.cc index 0045f6ac4..fd0dec211 100644 --- a/src/AirSpeed.cc +++ b/src/AirSpeed.cc @@ -98,7 +98,7 @@ void AirSpeed::SetPressureNoise(const Noise &_noise) sdf::ElementPtr AirSpeed::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("air_speed.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); sdf::ElementPtr pressureElem = elem->GetElement("pressure"); sdf::ElementPtr noiseElem = pressureElem->GetElement("noise"); diff --git a/src/Altimeter.cc b/src/Altimeter.cc index 638ebcc88..f7556c485 100644 --- a/src/Altimeter.cc +++ b/src/Altimeter.cc @@ -151,7 +151,7 @@ sdf::ElementPtr Altimeter::ToElement() const sdf::ElementPtr Altimeter::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("altimeter.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); sdf::ElementPtr verticalPosElem = elem->GetElement( "vertical_position", _errors); diff --git a/src/Atmosphere.cc b/src/Atmosphere.cc index 9ff3e14b4..1b4459430 100644 --- a/src/Atmosphere.cc +++ b/src/Atmosphere.cc @@ -161,7 +161,7 @@ sdf::ElementPtr Atmosphere::ToElement() const sdf::ElementPtr Atmosphere::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("atmosphere.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("type")->Set("adiabatic", _errors); elem->GetElement("temperature", _errors)->Set( diff --git a/src/Camera.cc b/src/Camera.cc index 78ed66de7..fceb6f1f2 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -1213,7 +1213,7 @@ bool Camera::HasLensProjection() const sdf::ElementPtr Camera::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("camera.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name()); sdf::ElementPtr poseElem = elem->GetElement("pose"); diff --git a/src/Collision.cc b/src/Collision.cc index e78481e60..5a3fde531 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -364,7 +364,7 @@ sdf::ElementPtr Collision::ToElement() const sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("collision.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name(), _errors); diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index e48e0c39b..e55f56c06 100644 --- a/src/ForceTorque.cc +++ b/src/ForceTorque.cc @@ -296,7 +296,7 @@ sdf::ElementPtr ForceTorque::ToElement() const sdf::ElementPtr ForceTorque::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("forcetorque.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); std::string frame; switch (this->Frame()) diff --git a/src/Frame.cc b/src/Frame.cc index a12e1f7bc..bf5f4d243 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -232,7 +232,7 @@ sdf::ElementPtr Frame::ToElement() const sdf::ElementPtr Frame::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("frame.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->dataPtr->name, _errors); diff --git a/src/Geometry.cc b/src/Geometry.cc index d4f73508b..75bcca03f 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -377,7 +377,7 @@ sdf::ElementPtr Geometry::ToElement() const sdf::ElementPtr Geometry::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("geometry.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); switch (this->dataPtr->type) { diff --git a/src/Gui.cc b/src/Gui.cc index f48b7dd8c..6f61d738e 100644 --- a/src/Gui.cc +++ b/src/Gui.cc @@ -97,7 +97,7 @@ sdf::ElementPtr Gui::Element() const sdf::ElementPtr Gui::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("gui.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("fullscreen")->Set(this->dataPtr->fullscreen); diff --git a/src/Imu.cc b/src/Imu.cc index 644ea095c..f239bcf1a 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -389,7 +389,7 @@ sdf::ElementPtr Imu::ToElement() const sdf::ElementPtr Imu::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("imu.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); sdf::ElementPtr orientationRefFrameElem = elem->GetElement("orientation_reference_frame", _errors); diff --git a/src/Joint.cc b/src/Joint.cc index ae4a79398..4532b5b41 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -538,7 +538,7 @@ Sensor *Joint::SensorByName(const std::string &_name) sdf::ElementPtr Joint::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("joint.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name()); sdf::ElementPtr poseElem = elem->GetElement("pose"); diff --git a/src/Lidar.cc b/src/Lidar.cc index 314501a2b..98b5caffe 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -395,7 +395,7 @@ bool Lidar::operator!=(const Lidar &_lidar) const sdf::ElementPtr Lidar::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("lidar.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); sdf::ElementPtr scanElem = elem->GetElement("scan"); sdf::ElementPtr horElem = scanElem->GetElement("horizontal"); diff --git a/src/Light.cc b/src/Light.cc index fde792633..ca44f3db1 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -500,7 +500,7 @@ sdf::ElementPtr Light::ToElement() const sdf::ElementPtr Light::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("light.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); std::string lightTypeStr = "point"; switch (this->Type()) diff --git a/src/Link.cc b/src/Link.cc index 2d88af046..cc6522966 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -959,7 +959,7 @@ void Link::ClearProjectors() sdf::ElementPtr Link::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("link.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name()); diff --git a/src/Magnetometer.cc b/src/Magnetometer.cc index 452d6acd1..580b41129 100644 --- a/src/Magnetometer.cc +++ b/src/Magnetometer.cc @@ -136,7 +136,7 @@ bool Magnetometer::operator==(const Magnetometer &_mag) const sdf::ElementPtr Magnetometer::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("magnetometer.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); sdf::ElementPtr magnetometerXElem = elem->GetElement("x"); sdf::ElementPtr magnetometerXNoiseElem = diff --git a/src/Material.cc b/src/Material.cc index 30939e0b5..0d908aa1e 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -411,7 +411,7 @@ sdf::ElementPtr Material::ToElement() const sdf::ElementPtr Material::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("material.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetElement("ambient", _errors)->Set(_errors, this->Ambient()); elem->GetElement("diffuse", _errors)->Set(_errors, this->Diffuse()); diff --git a/src/Model.cc b/src/Model.cc index 2d846b56b..0cb50476a 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -1115,7 +1115,7 @@ sdf::ElementPtr Model::ToElement(const OutputConfig &_config) const } sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("model.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name()); if (!this->dataPtr->canonicalLink.empty()) diff --git a/src/Noise.cc b/src/Noise.cc index a971b672c..4f6c8dc11 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -265,7 +265,7 @@ sdf::ElementPtr Noise::ToElement() const sdf::ElementPtr Noise::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("noise.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); std::string noiseType; switch (this->Type()) diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc index dfc36aea8..86bb64ab7 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -533,7 +533,7 @@ sdf::ElementPtr ParticleEmitter::ToElement() const sdf::ElementPtr ParticleEmitter::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("particle_emitter.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); // Set pose sdf::ElementPtr poseElem = elem->GetElement("pose", _errors); diff --git a/src/Physics.cc b/src/Physics.cc index 1af423242..c1ca03488 100644 --- a/src/Physics.cc +++ b/src/Physics.cc @@ -215,7 +215,7 @@ sdf::ElementPtr Physics::ToElement() const sdf::ElementPtr Physics::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("physics.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name(), _errors); elem->GetAttribute("default")->Set(this->IsDefault(), _errors); diff --git a/src/Plugin.cc b/src/Plugin.cc index d0b5d7572..637f12334 100644 --- a/src/Plugin.cc +++ b/src/Plugin.cc @@ -178,7 +178,7 @@ sdf::ElementPtr Plugin::ToElement() const sdf::ElementPtr Plugin::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("plugin.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name(), _errors); elem->GetAttribute("filename")->Set(this->Filename(), _errors); diff --git a/src/Projector.cc b/src/Projector.cc index 7b851e973..8dbbf6457 100644 --- a/src/Projector.cc +++ b/src/Projector.cc @@ -307,7 +307,7 @@ void Projector::AddPlugin(const Plugin &_plugin) sdf::ElementPtr Projector::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("projector.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); // Set pose sdf::ElementPtr poseElem = elem->GetElement("pose"); diff --git a/src/Root.cc b/src/Root.cc index 0d3210684..2449b5a49 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -619,7 +619,7 @@ void Root::ResolveAutoInertials(sdf::Errors &_errors, sdf::ElementPtr Root::ToElement(const OutputConfig &_config) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("root.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("version")->Set(this->Version()); diff --git a/src/Scene.cc b/src/Scene.cc index 52e53373b..cf4fcfcd9 100644 --- a/src/Scene.cc +++ b/src/Scene.cc @@ -198,7 +198,7 @@ sdf::ElementPtr Scene::ToElement() const sdf::ElementPtr Scene::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("scene.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetElement("ambient", _errors)->Set(_errors, this->Ambient()); elem->GetElement("background", _errors)->Set(_errors, this->Background()); diff --git a/src/Sensor.cc b/src/Sensor.cc index 15fcdd455..4a2ad4864 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -738,7 +738,7 @@ sdf::ElementPtr Sensor::ToElement() const sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("sensor.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("type")->Set(this->TypeStr(), _errors); elem->GetAttribute("name")->Set(this->Name(), _errors); diff --git a/src/Surface.cc b/src/Surface.cc index 22b48e198..e8958a0da 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -409,7 +409,7 @@ sdf::ElementPtr Surface::ToElement() const sdf::ElementPtr Surface::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("surface.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); sdf::ElementPtr contactElem = elem->GetElement("contact", _errors); contactElem->GetElement("collide_bitmask", _errors)->Set( diff --git a/src/Visual.cc b/src/Visual.cc index 685c26e9b..9682439ec 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -322,7 +322,7 @@ void Visual::SetPoseRelativeToGraph( sdf::ElementPtr Visual::ToElement() const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("visual.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name()); diff --git a/src/World.cc b/src/World.cc index fa1f939ce..4a884f6fb 100644 --- a/src/World.cc +++ b/src/World.cc @@ -1067,7 +1067,7 @@ Errors World::Implementation::LoadSphericalCoordinates( sdf::ElementPtr World::ToElement(const OutputConfig &_config) const { sdf::ElementPtr elem(new sdf::Element); - sdf::initFile("world.sdf", elem); + sdf::initFile(std::string(this->SchemaFile()), elem); elem->GetAttribute("name")->Set(this->Name()); elem->GetElement("gravity")->Set(this->Gravity()); diff --git a/tools/format_schema.py b/tools/format_schema.py deleted file mode 100644 index ebaa97faa..000000000 --- a/tools/format_schema.py +++ /dev/null @@ -1,67 +0,0 @@ -import os - -lines = os.listdir('./sdf/1.11/') -# Only keep the files that end with .sdf -lines = [line for line in lines if line.endswith('.sdf')] - -for line in lines: - # Remove .sdf extension - snake_case = line.strip().split('.')[0] - - # Convert from snake_case to CamelCase - camel_case = ''.join([word.capitalize() for word in snake_case.split('_')]) - - addHeader = """ - /// \\brief Get the schema file name accessor - public: static inline std::string_view SchemaFile(); - """ - - addImpl = f""" -///////////////////////////////////////////////// -inline std::string_view {camel_case}::SchemaFile() -{{ - static const char kSchemaFile[] = "{line}"; - return kSchemaFile; -}}\n\n""" - - # Debug print statements - # print(addHeader) - # print(addImpl) - # print(addTest) - # print("\n") - - # Edit './include/sdf/{camel_case}.hh' - # Find the line with 'class SDFFORMAT_VISIBLE {camel_case}` - try: - f = open('./include/sdf/' + camel_case + '.hh', 'r') - lines = f.readlines() - f.close() - line_number = 0 - for i, line in enumerate(lines): - if line == " class SDFORMAT_VISIBLE " + camel_case + '\n': - line_number = i+3 # After the class line there is `\n{\n` and then the constructor - break - if line_number == 0: - print("Error: Could not find class declaration in " + camel_case + ".hh") - exit(1) - - try: - with open('./include/sdf/' + camel_case + '.hh', 'w') as file: - for i, line in enumerate(lines): - file.write(line) - if i == line_number: - file.write(addHeader) - except: - print("Unexpected error while writing to: " + camel_case + ".hh.") - except: - print("Error while writing to: " + camel_case + ".hh." + " Check if file exists.") - - # Edit './src/{camel_case}.cc' if it exists - # Add implementation to end of document - if os.path.exists('./src/' + camel_case + '.cc'): - with open('./src/' + camel_case + '.cc', 'a') as file: - file.write(addImpl) - else: - print("Error: Could not find " + camel_case + ".cc") - - print("Changes written to source successfully") From 339a4dc2c866146b852398fba7959ba6e04cf7cc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 8 Apr 2024 00:46:11 -0700 Subject: [PATCH 03/23] Param_TEST: Check return values of Param::Get/Set (#1394) Signed-off-by: Steve Peters --- src/Param_TEST.cc | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index 56f30b4e0..378a088bc 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -46,45 +46,45 @@ TEST(Param, Bool) { sdf::Param boolParam("key", "bool", "true", false, "description"); bool value = true; - boolParam.Get(value); + EXPECT_TRUE(boolParam.Get(value)); EXPECT_TRUE(value); - boolParam.Set(false); - boolParam.Get(value); + EXPECT_TRUE(boolParam.Set(false)); + EXPECT_TRUE(boolParam.Get(value)); EXPECT_FALSE(value); // String parameter that represents a boolean. sdf::Param strParam("key", "string", "true", false, "description"); - strParam.Get(value); + EXPECT_TRUE(strParam.Get(value)); EXPECT_TRUE(value); - strParam.Set("false"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("false")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_FALSE(value); - strParam.Set("1"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("1")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_TRUE(value); - strParam.Set("0"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("0")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_FALSE(value); - strParam.Set("True"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("True")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_TRUE(value); - strParam.Set("TRUE"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("TRUE")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_TRUE(value); // Anything other than 1 or true is treated as a false value - strParam.Set("%"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("%")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_FALSE(value); - boolParam.Set(true); + EXPECT_TRUE(boolParam.Set(true)); std::any anyValue; EXPECT_TRUE(boolParam.GetAny(anyValue)); try From ceee6ff7f6ca817bfb948a37fdd873618be40e81 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Mar 2024 17:41:37 -0700 Subject: [PATCH 04/23] backport - Add mesh optimization attribute to #1382 Signed-off-by: Ian Chen --- include/sdf/Mesh.hh | 72 ++++++++++++++ sdf/1.11/mesh_shape.sdf | 14 +++ src/Mesh.cc | 163 ++++++++++++++++++++++++++++++- src/Mesh_TEST.cc | 74 ++++++++++++++ test/integration/geometry_dom.cc | 6 ++ test/sdf/shapes.sdf | 5 +- 6 files changed, 332 insertions(+), 2 deletions(-) diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index f19d98818..7cc48c955 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -37,6 +37,47 @@ namespace sdf // Forward declarations. class ParserConfig; + /// \brief Mesh optimization method + enum class MeshOptimization + { + /// \brief No mesh optimization + NONE, + /// \brief Convex hull + CONVEX_HULL, + /// \brief Convex decomposition + CONVEX_DECOMPOSITION + }; + + /// \brief Convex decomposition + class SDFORMAT_VISIBLE ConvexDecomposition + { + /// \brief Default constructor + public: ConvexDecomposition(); + + /// \brief Load the contact based on a element pointer. This is *not* the + /// usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the maximum number of convex hulls that can be generated. + public: unsigned int MaxConvexHulls() const; + + /// \brief Set the maximum number of convex hulls that can be generated. + public: void SetMaxConvexHulls(unsigned int _maxConvexHulls); + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + /// \brief Mesh represents a mesh shape, and is usually accessed through a /// Geometry. class SDFORMAT_VISIBLE Mesh @@ -61,6 +102,37 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the mesh's optimization method + /// \return The mesh optimization method. + /// MeshOptimization::NONE if no mesh simplificaton is done. + public: MeshOptimization Optimization() const; + + /// \brief Get the mesh's optimization method + /// \return The mesh optimization method. + /// Empty string if no mesh simplificaton is done. + public: std::string OptimizationStr() const; + + /// \brief Set the mesh optimization method. + /// \param[in] _optimization The mesh optimization method. + public: void SetOptimization(MeshOptimization _optimization); + + /// \brief Set the mesh optimization method. + /// \param[in] _optimization The mesh optimization method. + /// \return True if the _optimizationStr parameter matched a known + /// mesh optimization method. False if the mesh optimization method + /// could not be set. + public: bool SetOptimization(const std::string &_optimizationStr); + + /// \brief Get the associated ConvexDecomposition object + /// \returns Pointer to the associated ConvexDecomposition object, + /// nullptr if the Mesh doesn't contain a ConvexDecomposition element. + public: const sdf::ConvexDecomposition *ConvexDecomposition() const; + + /// \brief Set the associated ConvexDecomposition object. + /// \param[in] _convexDecomposition The ConvexDecomposition object. + public: void SetConvexDecomposition( + const sdf::ConvexDecomposition &_convexDecomposition); + /// \brief Get the mesh's URI. /// \return The URI of the mesh data. public: std::string Uri() const; diff --git a/sdf/1.11/mesh_shape.sdf b/sdf/1.11/mesh_shape.sdf index 61bce76dd..a59300d46 100644 --- a/sdf/1.11/mesh_shape.sdf +++ b/sdf/1.11/mesh_shape.sdf @@ -1,5 +1,19 @@ Mesh shape + + + + Set whether to optimize the mesh using one of the specified methods. Values include: "convex_hull" - a single convex hull that encapsulates the mesh, "convex_decomposition" - decompose the mesh into multiple convex hull meshes. Default value is an empty string which means no mesh optimization. + + + + + Convex decomposition parameters. Applicable if the mesh optimization attribute is set to convex_decomposition + + Maximum number of convex hulls to decompose into. If the input mesh has multiple submeshes, this limit is applied when decomposing each submesh + + + Mesh uri diff --git a/src/Mesh.cc b/src/Mesh.cc index 9dfddcf2b..f026e2d63 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -14,8 +14,10 @@ * limitations under the License. * */ +#include #include #include +#include #include #include "sdf/CustomInertiaCalcProperties.hh" @@ -27,9 +29,35 @@ using namespace sdf; -// Private data class +/// Mesh Optimization method strings. These should match the data in +/// `enum class MeshOptimization` located in Mesh.hh, and the size +/// template parameter should match the number of elements as well. +constexpr std::array kMeshOptimizationStrs = +{ + "", + "convex_hull", + "convex_decomposition" +}; + +// Private data class for ConvexDecomposition +class sdf::ConvexDecomposition::Implementation +{ + /// \brief Maximum number of convex hulls to generate. + public: unsigned int maxConvexHulls{16u}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf = nullptr; +}; + +// Private data class for Mesh class sdf::Mesh::Implementation { + /// \brief Mesh optimization method + public: MeshOptimization optimization = MeshOptimization::NONE; + + /// \brief Optional convex decomposition. + public: std::optional convexDecomposition; + /// \brief The mesh's URI. public: std::string uri = ""; @@ -49,6 +77,62 @@ class sdf::Mesh::Implementation public: sdf::ElementPtr sdf = nullptr; }; +///////////////////////////////////////////////// +ConvexDecomposition::ConvexDecomposition() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors ConvexDecomposition::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load convex decomposition, " + "but the provided SDF element is null."}); + return errors; + } + + // We need a convex_decomposition element + if (_sdf->GetName() != "convex_decomposition") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load convex decomposition, but the provided SDF " + "element is not ."}); + return errors; + } + + this->dataPtr->maxConvexHulls = _sdf->Get( + errors, "max_convex_hulls", + this->dataPtr->maxConvexHulls).first; + + return errors; +} + +///////////////////////////////////////////////// +sdf::ElementPtr ConvexDecomposition::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +unsigned int ConvexDecomposition::MaxConvexHulls() const +{ + return this->dataPtr->maxConvexHulls; +} + +///////////////////////////////////////////////// +void ConvexDecomposition::SetMaxConvexHulls(unsigned int _maxConvexHulls) +{ + this->dataPtr->maxConvexHulls = _maxConvexHulls; +} + ///////////////////////////////////////////////// Mesh::Mesh() : dataPtr(gz::utils::MakeImpl()) @@ -61,6 +145,7 @@ Errors Mesh::Load(ElementPtr _sdf) return this->Load(_sdf, ParserConfig::GlobalConfig()); } + ///////////////////////////////////////////////// Errors Mesh::Load(ElementPtr _sdf, const ParserConfig &_config) { @@ -87,6 +172,20 @@ Errors Mesh::Load(ElementPtr _sdf, const ParserConfig &_config) return errors; } + // Optimization + if (_sdf->HasAttribute("optimization")) + { + this->SetOptimization(_sdf->Get("optimization", "").first); + } + + if (_sdf->HasElement("convex_decomposition")) + { + this->dataPtr->convexDecomposition.emplace(); + Errors err = this->dataPtr->convexDecomposition->Load( + _sdf->GetElement("convex_decomposition", errors)); + errors.insert(errors.end(), err.begin(), err.end()); + } + if (_sdf->HasElement("uri")) { std::unordered_set paths; @@ -140,6 +239,56 @@ sdf::ElementPtr Mesh::Element() const return this->dataPtr->sdf; } +////////////////////////////////////////////////// +MeshOptimization Mesh::Optimization() const +{ + return this->dataPtr->optimization; +} + +////////////////////////////////////////////////// +std::string Mesh::OptimizationStr() const +{ + size_t index = static_cast(this->dataPtr->optimization); + if (index < kMeshOptimizationStrs.size()) + return std::string(kMeshOptimizationStrs[index]); + return ""; +} + +////////////////////////////////////////////////// +bool Mesh::SetOptimization(const std::string &_optimizationStr) +{ + for (size_t i = 0; i < kMeshOptimizationStrs.size(); ++i) + { + if (_optimizationStr == kMeshOptimizationStrs[i]) + { + this->dataPtr->optimization = static_cast(i); + return true; + } + } + return false; +} + +////////////////////////////////////////////////// +void Mesh::SetOptimization(MeshOptimization _optimization) +{ + this->dataPtr->optimization = _optimization; +} + +////////////////////////////////////////////////// +const sdf::ConvexDecomposition *Mesh::ConvexDecomposition() const +{ + if (this->dataPtr->convexDecomposition.has_value()) + return &this->dataPtr->convexDecomposition.value(); + return nullptr; +} + +////////////////////////////////////////////////// + void Mesh::SetConvexDecomposition( + const sdf::ConvexDecomposition &_convexDecomposition) +{ + this->dataPtr->convexDecomposition = _convexDecomposition; +} + ////////////////////////////////////////////////// std::string Mesh::Uri() const { @@ -244,6 +393,18 @@ sdf::ElementPtr Mesh::ToElement(sdf::Errors &_errors) const sdf::ElementPtr elem(new sdf::Element); sdf::initFile("mesh_shape.sdf", elem); + // Optimization + elem->GetAttribute("optimization")->Set( + this->OptimizationStr()); + + if (this->dataPtr->convexDecomposition.has_value()) + { + sdf::ElementPtr convexDecomp = elem->GetElement("convex_decomposition", + _errors); + convexDecomp->GetElement("max_convex_hulls")->Set( + this->dataPtr->convexDecomposition->MaxConvexHulls()); + } + // Uri sdf::ElementPtr uriElem = elem->GetElement("uri", _errors); uriElem->Set(_errors, this->Uri()); diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 51c1751f4..29ef70d85 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -34,6 +34,9 @@ TEST(DOMMesh, Construction) sdf::Mesh mesh; EXPECT_EQ(nullptr, mesh.Element()); + EXPECT_EQ(std::string(), mesh.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::NONE, mesh.Optimization()); + EXPECT_EQ(nullptr, mesh.ConvexDecomposition()); EXPECT_EQ(std::string(), mesh.FilePath()); EXPECT_EQ(std::string(), mesh.Uri()); EXPECT_EQ(std::string(), mesh.Submesh()); @@ -45,24 +48,37 @@ TEST(DOMMesh, Construction) TEST(DOMMesh, MoveConstructor) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_decomposition")); mesh.SetUri("banana"); mesh.SetSubmesh("watermelon"); mesh.SetCenterSubmesh(true); mesh.SetScale({0.5, 0.6, 0.7}); mesh.SetFilePath("/pear"); + sdf::ConvexDecomposition convexDecomp; + EXPECT_EQ(nullptr, convexDecomp.Element()); + convexDecomp.SetMaxConvexHulls(10u); + mesh.SetConvexDecomposition(convexDecomp); + sdf::Mesh mesh2(std::move(mesh)); + EXPECT_EQ("convex_decomposition", mesh2.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_DECOMPOSITION, mesh2.Optimization()); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); EXPECT_TRUE(mesh2.CenterSubmesh()); EXPECT_EQ("/pear", mesh2.FilePath()); + + auto convexDecomp2 = mesh2.ConvexDecomposition(); + ASSERT_NE(nullptr, convexDecomp2); + EXPECT_EQ(10u, convexDecomp2->MaxConvexHulls()); } ///////////////////////////////////////////////// TEST(DOMMesh, CopyConstructor) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); mesh.SetUri("banana"); mesh.SetSubmesh("watermelon"); mesh.SetCenterSubmesh(true); @@ -70,6 +86,9 @@ TEST(DOMMesh, CopyConstructor) mesh.SetFilePath("/pear"); sdf::Mesh mesh2(mesh); + EXPECT_EQ("convex_hull", mesh2.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_HULL, mesh2.Optimization()); + EXPECT_EQ(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); @@ -81,6 +100,7 @@ TEST(DOMMesh, CopyConstructor) TEST(DOMMesh, CopyAssignmentOperator) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); mesh.SetUri("banana"); mesh.SetSubmesh("watermelon"); mesh.SetCenterSubmesh(true); @@ -89,6 +109,9 @@ TEST(DOMMesh, CopyAssignmentOperator) sdf::Mesh mesh2; mesh2 = mesh; + EXPECT_EQ("convex_hull", mesh2.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_HULL, mesh2.Optimization()); + EXPECT_EQ(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); @@ -100,6 +123,7 @@ TEST(DOMMesh, CopyAssignmentOperator) TEST(DOMMesh, MoveAssignmentOperator) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); mesh.SetUri("banana"); mesh.SetSubmesh("watermelon"); mesh.SetCenterSubmesh(true); @@ -108,6 +132,9 @@ TEST(DOMMesh, MoveAssignmentOperator) sdf::Mesh mesh2; mesh2 = std::move(mesh); + EXPECT_EQ("convex_hull", mesh2.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_HULL, mesh2.Optimization()); + EXPECT_EQ(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); @@ -140,6 +167,29 @@ TEST(DOMMesh, Set) sdf::Mesh mesh; EXPECT_EQ(nullptr, mesh.Element()); + EXPECT_EQ(std::string(), mesh.OptimizationStr()); + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); + EXPECT_EQ("convex_hull", mesh.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_HULL, mesh.Optimization()); + mesh.SetOptimization(sdf::MeshOptimization::CONVEX_DECOMPOSITION); + EXPECT_EQ("convex_decomposition", mesh.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_DECOMPOSITION, + mesh.Optimization()); + // check invalid inputs + EXPECT_FALSE(mesh.SetOptimization("invalid")); + { + auto invalidMeshOpt = static_cast(99); + mesh.SetOptimization(invalidMeshOpt); + EXPECT_EQ(invalidMeshOpt, mesh.Optimization()); + EXPECT_EQ("", mesh.OptimizationStr()); + } + + sdf::ConvexDecomposition convexDecomp; + convexDecomp.SetMaxConvexHulls(10u); + mesh.SetConvexDecomposition(convexDecomp); + ASSERT_NE(nullptr, mesh.ConvexDecomposition()); + EXPECT_EQ(10u, mesh.ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ(std::string(), mesh.Uri()); mesh.SetUri("http://myuri.com"); EXPECT_EQ("http://myuri.com", mesh.Uri()); @@ -165,6 +215,7 @@ TEST(DOMMesh, Set) TEST(DOMMesh, Load) { sdf::Mesh mesh; + sdf::ConvexDecomposition convexDecomp; sdf::Errors errors; // Null element name @@ -173,6 +224,11 @@ TEST(DOMMesh, Load) EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); EXPECT_EQ(nullptr, mesh.Element()); + errors = convexDecomp.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, convexDecomp.Element()); + // Bad element name sdf::ElementPtr sdf(new sdf::Element()); sdf->SetName("bad"); @@ -181,6 +237,11 @@ TEST(DOMMesh, Load) EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); EXPECT_NE(nullptr, mesh.Element()); + errors = convexDecomp.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, convexDecomp.Element()); + // Missing element sdf->SetName("mesh"); errors = mesh.Load(sdf); @@ -296,21 +357,30 @@ TEST(DOMMesh, ToElement) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_decomposition")); mesh.SetUri("mesh-uri"); mesh.SetScale(gz::math::Vector3d(1, 2, 3)); mesh.SetSubmesh("submesh"); mesh.SetCenterSubmesh(false); + sdf::ConvexDecomposition convexDecomp; + convexDecomp.SetMaxConvexHulls(10u); + mesh.SetConvexDecomposition(convexDecomp); + sdf::ElementPtr elem = mesh.ToElement(); ASSERT_NE(nullptr, elem); sdf::Mesh mesh2; mesh2.Load(elem); + EXPECT_EQ(mesh.OptimizationStr(), mesh2.OptimizationStr()); + EXPECT_EQ(mesh.Optimization(), mesh2.Optimization()); EXPECT_EQ(mesh.Uri(), mesh2.Uri()); EXPECT_EQ(mesh.Scale(), mesh2.Scale()); EXPECT_EQ(mesh.Submesh(), mesh2.Submesh()); EXPECT_EQ(mesh.CenterSubmesh(), mesh2.CenterSubmesh()); + ASSERT_NE(nullptr, mesh2.ConvexDecomposition()); + EXPECT_EQ(10u, mesh2.ConvexDecomposition()->MaxConvexHulls()); } ///////////////////////////////////////////////// @@ -332,6 +402,7 @@ TEST(DOMMesh, ToElementErrorOutput) sdf::Mesh mesh; sdf::Errors errors; + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); mesh.SetUri("mesh-uri"); mesh.SetScale(gz::math::Vector3d(1, 2, 3)); mesh.SetSubmesh("submesh"); @@ -345,6 +416,9 @@ TEST(DOMMesh, ToElementErrorOutput) errors = mesh2.Load(elem); EXPECT_TRUE(errors.empty()); + EXPECT_EQ(mesh.OptimizationStr(), mesh2.OptimizationStr()); + EXPECT_EQ(mesh.Optimization(), mesh2.Optimization()); + EXPECT_EQ(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ(mesh.Uri(), mesh2.Uri()); EXPECT_EQ(mesh.Scale(), mesh2.Scale()); EXPECT_EQ(mesh.Submesh(), mesh2.Submesh()); diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 07d50d570..297d1f0af 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -179,6 +179,12 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::MESH, meshCol->Geom()->Type()); const sdf::Mesh *meshColGeom = meshCol->Geom()->MeshShape(); ASSERT_NE(nullptr, meshColGeom); + EXPECT_EQ("convex_decomposition", meshColGeom->OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_DECOMPOSITION, + meshColGeom->Optimization()); + ASSERT_NE(nullptr, meshColGeom->ConvexDecomposition()); + EXPECT_EQ(4u, meshColGeom->ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ("https://fuel.gazebosim.org/1.0/an_org/models/a_model/mesh/" "mesh.dae", meshColGeom->Uri()); EXPECT_TRUE(gz::math::Vector3d(0.1, 0.2, 0.3) == diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 1f5f4fa27..f411afa37 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -120,7 +120,10 @@ - + + + 4 + https://fuel.gazebosim.org/1.0/an_org/models/a_model/mesh/mesh.dae my_submesh From 2028a963c75131f8f6b837b3d13368e543eeab36 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 26 Mar 2024 14:00:35 -0700 Subject: [PATCH 05/23] backport - Update max_convex_hulls description #1386 Signed-off-by: Ian Chen --- sdf/1.11/mesh_shape.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sdf/1.11/mesh_shape.sdf b/sdf/1.11/mesh_shape.sdf index a59300d46..1ed64ca44 100644 --- a/sdf/1.11/mesh_shape.sdf +++ b/sdf/1.11/mesh_shape.sdf @@ -10,7 +10,7 @@ Convex decomposition parameters. Applicable if the mesh optimization attribute is set to convex_decomposition - Maximum number of convex hulls to decompose into. If the input mesh has multiple submeshes, this limit is applied when decomposing each submesh + Maximum number of convex hulls to decompose into. This sets the maximum number of submeshes that the final decomposed mesh will contain. From 2276134919109ff63299356b03319caefbcdb1d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 9 Apr 2024 22:45:58 +0200 Subject: [PATCH 06/23] Added Python wrapper to ConvexDecomposition (#1398) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Ian Chen --- python/CMakeLists.txt | 1 + python/src/sdf/_gz_sdformat_pybind11.cc | 2 + python/src/sdf/pyConvexDecomposition.cc | 50 +++++++++++++++++++++++++ python/src/sdf/pyConvexDecomposition.hh | 41 ++++++++++++++++++++ python/src/sdf/pyMesh.cc | 22 +++++++++++ python/test/pyMesh_TEST.py | 44 +++++++++++++++++++++- 6 files changed, 159 insertions(+), 1 deletion(-) create mode 100644 python/src/sdf/pyConvexDecomposition.cc create mode 100644 python/src/sdf/pyConvexDecomposition.hh diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 62078d8db..f9cc0917d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -52,6 +52,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/sdf/pyCamera.cc src/sdf/pyCapsule.cc src/sdf/pyCollision.cc + src/sdf/pyConvexDecomposition.cc src/sdf/pyCylinder.cc src/sdf/pyElement.cc src/sdf/pyEllipsoid.cc diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index d982e13f4..298cd1362 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -26,6 +26,7 @@ #include "pyCamera.hh" #include "pyCapsule.hh" #include "pyCollision.hh" +#include "pyConvexDecomposition.hh" #include "pyCylinder.hh" #include "pyElement.hh" #include "pyEllipsoid.hh" @@ -85,6 +86,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineCamera(m); sdf::python::defineCapsule(m); sdf::python::defineCollision(m); + sdf::python::defineConvexDecomposition(m); sdf::python::defineContact(m); sdf::python::defineCylinder(m); // PrintConfig has to be defined before Param and Element because it's used as diff --git a/python/src/sdf/pyConvexDecomposition.cc b/python/src/sdf/pyConvexDecomposition.cc new file mode 100644 index 000000000..aacd7ee42 --- /dev/null +++ b/python/src/sdf/pyConvexDecomposition.cc @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2024 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 "pyMesh.hh" + +#include + +#include "sdf/ParserConfig.hh" +#include "sdf/Mesh.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineConvexDecomposition(pybind11::object module) +{ + pybind11::class_(module, "ConvexDecomposition") + .def(pybind11::init<>()) + .def("max_convex_hulls", &sdf::ConvexDecomposition::MaxConvexHulls, + "Get the maximum number of convex hulls that can be generated.") + .def("set_max_convex_hulls", &sdf::ConvexDecomposition::SetMaxConvexHulls, + "Set the maximum number of convex hulls that can be generated.") + .def("__copy__", [](const sdf::ConvexDecomposition &self) { + return sdf::ConvexDecomposition(self); + }) + .def("__deepcopy__", [](const sdf::ConvexDecomposition &self, pybind11::dict) { + return sdf::ConvexDecomposition(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyConvexDecomposition.hh b/python/src/sdf/pyConvexDecomposition.hh new file mode 100644 index 000000000..78a58840f --- /dev/null +++ b/python/src/sdf/pyConvexDecomposition.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2024 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 SDFORMAT_PYTHON_CONVEX_DECOMPOSITION_HH_ +#define SDFORMAT_PYTHON_CONVEX_DECOMPOSITION_HH_ + +#include + +#include "sdf/Mesh.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::ConvexDecomposition +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineConvexDecomposition(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_CONVEX_DECOMPOSITION_HH_ diff --git a/python/src/sdf/pyMesh.cc b/python/src/sdf/pyMesh.cc index 85705f874..b9fdafd17 100644 --- a/python/src/sdf/pyMesh.cc +++ b/python/src/sdf/pyMesh.cc @@ -35,6 +35,23 @@ void defineMesh(pybind11::object module) pybind11::class_(module, "Mesh") .def(pybind11::init<>()) .def(pybind11::init()) + .def("optimization", &sdf::Mesh::Optimization, + "Get the mesh's optimization method.") + .def("optimization_str", &sdf::Mesh::OptimizationStr, + "Get the mesh's optimization method") + .def("set_optimization", + pybind11::overload_cast( + &sdf::Mesh::SetOptimization), + "Set the mesh optimization method.") + .def("set_optimization", + pybind11::overload_cast( + &sdf::Mesh::SetOptimization), + "Set the mesh optimization method.") + .def("convex_decomposition", &sdf::Mesh::ConvexDecomposition, + pybind11::return_value_policy::reference_internal, + "Get the associated ConvexDecomposition object") + .def("set_convex_decomposition", &sdf::Mesh::SetConvexDecomposition, + "Set the associated ConvexDecomposition object.") .def("uri", &sdf::Mesh::Uri, "Get the mesh's URI.") .def("set_uri", &sdf::Mesh::SetUri, @@ -67,6 +84,11 @@ void defineMesh(pybind11::object module) .def("__deepcopy__", [](const sdf::Mesh &self, pybind11::dict) { return sdf::Mesh(self); }, "memo"_a); + + pybind11::enum_(module, "MeshOptimization") + .value("NONE", sdf::MeshOptimization::NONE) + .value("CONVEX_HULL", sdf::MeshOptimization::CONVEX_HULL) + .value("CONVEX_DECOMPOSITION", sdf::MeshOptimization::CONVEX_DECOMPOSITION); } } // namespace python } // namespace SDF_VERSION_NAMESPACE diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index 64d89a57a..ac9840ea6 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -13,8 +13,9 @@ # limitations under the License. import copy -from gz_test_deps.sdformat import Mesh +from gz_test_deps.sdformat import Mesh, ConvexDecomposition from gz_test_deps.math import Vector3d +import gz_test_deps.sdformat as sdf import unittest @@ -23,6 +24,9 @@ class MeshTEST(unittest.TestCase): def test_default_construction(self): mesh = Mesh() + self.assertEqual("", mesh.optimization_str()) + self.assertEqual(sdf.MeshOptimization.NONE, mesh.optimization()) + self.assertEqual(None, mesh.convex_decomposition()) self.assertEqual("", mesh.file_path()) self.assertEqual("", mesh.uri()) self.assertEqual("", mesh.submesh()) @@ -32,19 +36,29 @@ def test_default_construction(self): def test_assigment(self): mesh = Mesh() + self.assertTrue(mesh.set_optimization("convex_decomposition")) mesh.set_uri("banana") mesh.set_submesh("watermelon") mesh.set_center_submesh(True) mesh.set_scale(Vector3d(0.5, 0.6, 0.7)) mesh.set_file_path("/pear") + convexDecomp = ConvexDecomposition() + convexDecomp.set_max_convex_hulls(10) + mesh.set_convex_decomposition(convexDecomp) + mesh2 = mesh + self.assertEqual("convex_decomposition", mesh2.optimization_str()) + self.assertEqual(sdf.MeshOptimization.CONVEX_DECOMPOSITION, mesh2.optimization()) self.assertEqual("banana", mesh2.uri()) self.assertEqual("watermelon", mesh2.submesh()) self.assertEqual(Vector3d(0.5, 0.6, 0.7), mesh2.scale()) self.assertTrue(mesh2.center_submesh()) self.assertEqual("/pear", mesh2.file_path()) + convexDecomp2 = mesh2.convex_decomposition() + self.assertEqual(10, convexDecomp2.max_convex_hulls()) + mesh.set_file_path("/apple") self.assertEqual("/apple", mesh2.file_path()) @@ -63,19 +77,29 @@ def test_assigment(self): def test_deepcopy_construction(self): mesh = Mesh() + self.assertTrue(mesh.set_optimization("convex_decomposition")) mesh.set_uri("banana") mesh.set_submesh("watermelon") mesh.set_center_submesh(True) mesh.set_scale(Vector3d(0.5, 0.6, 0.7)) mesh.set_file_path("/pear") + convexDecomp = ConvexDecomposition() + convexDecomp.set_max_convex_hulls(10) + mesh.set_convex_decomposition(convexDecomp) + mesh2 = copy.deepcopy(mesh) + self.assertEqual("convex_decomposition", mesh2.optimization_str()) + self.assertEqual(sdf.MeshOptimization.CONVEX_DECOMPOSITION, mesh2.optimization()) self.assertEqual("banana", mesh2.uri()) self.assertEqual("watermelon", mesh2.submesh()) self.assertEqual(Vector3d(0.5, 0.6, 0.7), mesh2.scale()) self.assertTrue(mesh2.center_submesh()) self.assertEqual("/pear", mesh2.file_path()) + convexDecomp2 = mesh2.convex_decomposition() + self.assertEqual(10, convexDecomp2.max_convex_hulls()) + mesh.set_file_path("/apple") mesh.set_scale(Vector3d(0.3, 0.2, 0.4)) mesh.set_center_submesh(False) @@ -92,6 +116,24 @@ def test_deepcopy_construction(self): def test_set(self): mesh = Mesh() + self.assertEqual("", mesh.optimization_str()) + self.assertTrue(mesh.set_optimization("convex_hull")) + self.assertEqual("convex_hull", mesh.optimization_str()) + self.assertEqual(sdf.MeshOptimization.CONVEX_HULL, mesh.optimization()) + mesh.set_optimization(sdf.MeshOptimization.CONVEX_DECOMPOSITION) + self.assertEqual("convex_decomposition", mesh.optimization_str()) + self.assertEqual(sdf.MeshOptimization.CONVEX_DECOMPOSITION, mesh.optimization()) + + self.assertFalse(mesh.set_optimization("invalid")) + mesh.set_optimization(sdf.MeshOptimization(99)) + self.assertEqual(sdf.MeshOptimization(99), mesh.optimization()) + self.assertEqual("", mesh.optimization_str()) + + convexDecomp = ConvexDecomposition() + convexDecomp.set_max_convex_hulls(10) + mesh.set_convex_decomposition(convexDecomp) + self.assertEqual(10, mesh.convex_decomposition().max_convex_hulls()) + self.assertEqual("", mesh.uri()) mesh.set_uri("http://myuri.com") self.assertEqual("http://myuri.com", mesh.uri()) From 5590a1440cf0932829d1cf2e132f08686284674f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 19 Apr 2024 15:13:50 -0500 Subject: [PATCH 07/23] Add package.xml, fix `gz sdf` tests on Windows (#1374) The `gz sdf` tests are fixed by * fixing dll path issue with Ruby on windows * Setting home path --------- Signed-off-by: Addisu Z. Taddese --- .github/workflows/package_xml.yml | 11 ++++++ package.xml | 32 +++++++++++++++++ src/Console.cc | 6 ---- src/cmd/CMakeLists.txt | 8 ++++- src/cmd/cmdsdformat.rb.in | 16 ++++++--- src/gz_TEST.cc | 58 +++++++++++++++++-------------- 6 files changed, 94 insertions(+), 37 deletions(-) create mode 100644 .github/workflows/package_xml.yml create mode 100644 package.xml diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 000000000..4bd4a9aa0 --- /dev/null +++ b/.github/workflows/package_xml.yml @@ -0,0 +1,11 @@ +name: Validate package.xml + +on: + pull_request: + +jobs: + package-xml: + runs-on: ubuntu-latest + name: Validate package.xml + steps: + - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..33d829ad8 --- /dev/null +++ b/package.xml @@ -0,0 +1,32 @@ + + + sdformat14 + 14.1.1 + SDFormat is an XML file format that describes environments, objects, and robots +in a manner suitable for robotic applications + + Addisu Z. Taddese + Steve Peters + + Apache License 2.0 + + https://github.com/gazebosim/sdformat + + cmake + gz-cmake3 + gz-math7 + gz-utils2 + tinyxml2 + liburdfdom-dev + pybind11-dev + + gz-tools2 + + libxml2-utils + python3-psutil + python3-pytest + + + cmake + + diff --git a/src/Console.cc b/src/Console.cc index d9ec8d044..558f10d87 100644 --- a/src/Console.cc +++ b/src/Console.cc @@ -34,13 +34,7 @@ using namespace sdf; static std::shared_ptr myself; static std::mutex g_instance_mutex; -/// \todo Output disabled for windows, to allow tests to pass. We should -/// disable output just for tests on windows. -#ifndef _WIN32 static bool g_quiet = false; -#else -static bool g_quiet = true; -#endif static Console::ConsoleStream g_NullStream(nullptr); diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index a1cf707c1..c1c91078b 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -31,7 +31,13 @@ set(cmd_script_configured "${CMAKE_CURRENT_BINARY_DIR}/cmd${PROJECT_NAME}.rb.con # Set the library_location variable to the relative path to the library file # within the install directory structure. -set(library_location "../../../${CMAKE_INSTALL_LIBDIR}/$") +if (MSVC) + set(library_location_prefix "${CMAKE_INSTALL_BINDIR}") +else() + set(library_location_prefix "${CMAKE_INSTALL_LIBDIR}") +endif() + +set(library_location "../../../${library_location_prefix}/$") configure_file( "cmd${PROJECT_NAME_NO_VERSION_LOWER}.rb.in" diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 7c93d7680..c4bd35f4c 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -26,6 +26,8 @@ else end require 'optparse' +require 'pathname' + # Constants. LIBRARY_NAME = '@library_location@' @@ -174,9 +176,7 @@ class Cmd # puts options # Read the plugin that handles the command. - if LIBRARY_NAME[0] == '/' - # If the first character is a slash, we'll assume that we've been given an - # absolute path to the library. This is only used during test mode. + if Pathname.new(LIBRARY_NAME).absolute? plugin = LIBRARY_NAME else # We're assuming that the library path is relative to the current @@ -185,10 +185,18 @@ class Cmd end conf_version = LIBRARY_VERSION + if defined? RubyInstaller + # RubyInstaller does not search for dlls in PATH or the directory that tests are running from, + # so we'll add the parent directory of the plugin to the search path. + # https://github.com/oneclick/rubyinstaller2/wiki/For-gem-developers#-dll-loading + RubyInstaller::Runtime.add_dll_directory(File.dirname(plugin)) + end + begin Importer.dlload plugin - rescue DLError + rescue DLError => error puts "Library error: [#{plugin}] not found." + puts "DLError: #{error.message}" exit(-1) end diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 816882f56..2e16d19db 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -73,7 +73,7 @@ std::string custom_exec_str(std::string _cmd) } ///////////////////////////////////////////////// -TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(checkUnrecognizedElements, SDF) { // Check an SDFormat file with unrecognized elements { @@ -120,7 +120,7 @@ TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check, SDF) { // Check a good SDF file { @@ -1011,7 +1011,7 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check_shapes_sdf, SDF) { { const auto path = @@ -1035,7 +1035,7 @@ TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check_model_sdf, SDF) { // Check a good SDF file by passing the absolute path { @@ -1062,7 +1062,7 @@ TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(describe, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(describe, SDF) { // Get the description std::string output = @@ -1074,7 +1074,7 @@ TEST(describe, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print, SDF) { // Check a good SDF file { @@ -1103,7 +1103,7 @@ TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_degrees, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_degrees.sdf"); @@ -1171,7 +1171,7 @@ TEST(print_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_radians, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_radians.sdf"); @@ -1239,7 +1239,7 @@ TEST(print_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_quaternions, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_quaternions, SDF) { const auto path = sdf::testing::TestFile( "sdf", "rotations_in_quaternions.sdf"); @@ -1308,7 +1308,7 @@ TEST(print_rotations_in_quaternions, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_degrees, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1379,7 +1379,7 @@ TEST(print_includes_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_radians, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1450,8 +1450,7 @@ TEST(print_includes_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_quaternions, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_quaternions, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1523,8 +1522,7 @@ TEST(print_includes_rotations_in_quaternions, } ///////////////////////////////////////////////// -TEST(print_rotations_in_unnormalized_degrees, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_unnormalized_degrees, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_degrees.sdf"); @@ -1595,8 +1593,7 @@ TEST(print_rotations_in_unnormalized_degrees, } ///////////////////////////////////////////////// -TEST(print_rotations_in_unnormalized_radians, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_unnormalized_radians, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); @@ -1664,7 +1661,7 @@ TEST(print_rotations_in_unnormalized_radians, } ///////////////////////////////////////////////// -TEST(shuffled_cmd_flags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(shuffled_cmd_flags, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); @@ -1713,8 +1710,7 @@ TEST(shuffled_cmd_flags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_snap_to_degrees_tolerance_too_high, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_snap_to_degrees_tolerance_too_high, SDF) { const std::string path = sdf::testing::TestFile( "sdf", @@ -1731,7 +1727,7 @@ TEST(print_snap_to_degrees_tolerance_too_high, } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) +TEST(GraphCmd, WorldPoseRelativeTo) { // world pose relative_to graph const std::string path = @@ -1780,7 +1776,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) +TEST(GraphCmd, ModelPoseRelativeTo) { const auto path = sdf::testing::TestFile("sdf", "model_relative_to_nested_reference.sdf"); @@ -1857,7 +1853,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) +TEST(GraphCmd, WorldFrameAttachedTo) { const auto path = sdf::testing::TestFile("sdf", "world_nested_frame_attached_to.sdf"); @@ -1903,7 +1899,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) +TEST(GraphCmd, ModelFrameAttachedTo) { const auto path = sdf::testing::TestFile("sdf", "model_nested_frame_attached_to.sdf"); @@ -1955,7 +1951,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) // Disable on arm #if !defined __ARM_ARCH ///////////////////////////////////////////////// -TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(inertial_stats, SDF) { std::string expectedOutput = "Inertial statistics for model: test_model\n" @@ -2043,7 +2039,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ////////////////////////////////////////////////// /// \brief Check help message and bash completion script for consistent flags -TEST(HelpVsCompletionFlags, SDF) +TEST(HelpVsCompletionFlags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { // Flags in help message std::string helpOutput = custom_exec_str(GzCommand() + " sdf --help"); @@ -2098,6 +2094,16 @@ int main(int argc, char **argv) gz::utils::setenv("LD_LIBRARY_PATH", testLibraryPath); #endif + // temporarily set HOME + std::string homeDir; + sdf::testing::TestTmpPath(homeDir); + +#ifdef _WIN32 + gz::utils::setenv("HOMEPATH", homeDir); +#else + gz::utils::setenv("HOME", homeDir); +#endif + ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From 67bf992b8e062fc0eb1977f9c910aee51a49e108 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 22 Apr 2024 19:32:34 +0200 Subject: [PATCH 08/23] Fix trivial warning on 24.04 for JointAxis_TEST.cc (#1402) * Fix trivial warning on 24.04 for JointAxis_TEST.cc --------- Signed-off-by: Jose Luis Rivero Co-authored-by: Steve Peters --- src/JointAxis_TEST.cc | 50 +++++++++++++++++++++---------------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index 0e04be6fd..bc612c689 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -226,58 +226,58 @@ TEST(DOMJointAxis, ToElement) sdf::ElementPtr dynElem = elem->GetElement("dynamics", errors); ASSERT_TRUE(errors.empty()); - double damping = 0; - damping = dynElem->Get(errors, "damping", damping).first; + double damping; + damping = dynElem->Get(errors, "damping", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(0.2, damping); - double friction = 0; - friction = dynElem->Get(errors, "friction", friction).first; + double friction; + friction = dynElem->Get(errors, "friction", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(1.3, friction); - double springReference = 0; + double springReference; springReference = dynElem->Get( - errors, "spring_reference", springReference).first; + errors, "spring_reference", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(2.4, springReference); - double springStiffness = 0; + double springStiffness; springStiffness = dynElem->Get( - errors, "spring_stiffness", springStiffness).first; + errors, "spring_stiffness", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(-1.2, springStiffness); // Check //axis/limit sdf::ElementPtr limitElem = elem->GetElement("limit", errors); - double lower = 0; - lower = limitElem->Get(errors, "lower", lower).first; + double lower; + lower = limitElem->Get(errors, "lower", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(-10.8, lower); - double upper = 0; - upper = limitElem->Get(errors, "upper", upper).first; + double upper; + upper = limitElem->Get(errors, "upper", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(123.4, upper); - double effort = 0; - effort = limitElem->Get(errors, "effort", effort).first; + double effort; + effort = limitElem->Get(errors, "effort", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(3.2, effort); - double maxVel = 0; - maxVel = limitElem->Get(errors, "velocity", maxVel).first; + double maxVel; + maxVel = limitElem->Get(errors, "velocity", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(54.2, maxVel); - double stiffness = 0; - stiffness = limitElem->Get(errors, "stiffness", stiffness).first; + double stiffness; + stiffness = limitElem->Get(errors, "stiffness", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(1e2, stiffness); - double dissipation = 0; + double dissipation; dissipation = limitElem->Get( - errors, "dissipation", dissipation).first; + errors, "dissipation", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(1.5, dissipation); @@ -286,31 +286,31 @@ TEST(DOMJointAxis, ToElement) ASSERT_NE(nullptr, mimicElem); std::string mimicJointName; mimicJointName = mimicElem->Get( - errors, "joint", mimicJointName).first; + errors, "joint", "").first; ASSERT_TRUE(errors.empty()); EXPECT_EQ("test_joint", mimicJointName); std::string mimicAxisName; mimicAxisName = mimicElem->Get( - errors, "axis", mimicAxisName).first; + errors, "axis", "").first; ASSERT_TRUE(errors.empty()); EXPECT_EQ("axis2", mimicAxisName); double multiplier; multiplier = mimicElem->Get( - errors, "multiplier", multiplier).first; + errors, "multiplier", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(5.0, multiplier); double offset; offset = mimicElem->Get( - errors, "offset", offset).first; + errors, "offset", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(1.0, offset); double reference; reference = mimicElem->Get( - errors, "reference", reference).first; + errors, "reference", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(2.0, reference); From 8631b3f10deba9da22833d90da5507c7fddf8097 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 23 Apr 2024 10:54:04 -0700 Subject: [PATCH 09/23] Prepare for 14.2.0 release (#1405) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 16 ++++++++++++++++ package.xml | 2 +- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f18d73f91..f771a9c65 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0004 NEW) endif(COMMAND CMAKE_POLICY) -project (sdformat14 VERSION 14.1.1) +project (sdformat14 VERSION 14.2.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software diff --git a/Changelog.md b/Changelog.md index c41c7af4e..ecb491ea1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,21 @@ ## libsdformat 14.X +### libsdformat 14.2.0 (2024-04-23) + +1. Fix trivial warning on 24.04 for JointAxis_TEST.cc + * [Pull request #1402](https://github.com/gazebosim/sdformat/pull/1402) + +1. Add package.xml, fix `gz sdf` tests on Windows + * [Pull request #1374](https://github.com/gazebosim/sdformat/pull/1374) + +1. Backport mesh optimization feature + * [Pull request #1398](https://github.com/gazebosim/sdformat/pull/1398) + * [Pull request #1386](https://github.com/gazebosim/sdformat/pull/1386) + * [Pull request #1382](https://github.com/gazebosim/sdformat/pull/1382) + +1. Param_TEST: Check return values of Param::Get/Set + * [Pull request #1394](https://github.com/gazebosim/sdformat/pull/1394) + ### libsdformat 14.1.1 (2024-03-28) 1. Fix warning with pybind11 2.12 diff --git a/package.xml b/package.xml index 33d829ad8..3132f5f8f 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ sdformat14 - 14.1.1 + 14.2.0 SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications From 74aca7e74f2d3c5f99415a548c3bdd443ca6f997 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 14 May 2024 05:10:51 -0300 Subject: [PATCH 10/23] (Backport) Enable 24.04 CI, remove distutils dependency (#1413) * Enable 24.04 CI, remove distutils dependency (#1408) distutils is no longer required since this branch requires a new enough version of cmake. Signed-off-by: Steve Peters Signed-off-by: Jorge Perez Co-authored-by: Steve Peters --- .github/ci/packages.apt | 1 - .github/workflows/ci.yml | 9 +++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index acee87feb..1c65edcc2 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -7,7 +7,6 @@ libtinyxml2-dev liburdfdom-dev libxml2-utils python3-dev -python3-distutils python3-gz-math7 python3-psutil python3-pybind11 diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index fe275a3cb..8a0a9947a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -21,3 +21,12 @@ jobs: codecov-enabled: true cppcheck-enabled: true cpplint-enabled: true + noble-ci: + runs-on: ubuntu-latest + name: Ubuntu Noble CI + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Compile and test + id: ci + uses: gazebo-tooling/action-gz-ci@noble From 0a61cc1444851fcc7d1c08748d51438ceeb4e961 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 May 2024 23:00:20 +0200 Subject: [PATCH 11/23] Allow empty strings in plugin and custom attributes (#1407) Currently errors are generated when adding an attribute containing an empty string to a block or as a custom attribute. This adds failing tests to confirm the bug and fixes the errors in by setting `_required == 0` when calling Element::AddAttribute. This also changes Element::ToString to print empty custom attributes. Fixes #725. Signed-off-by: Steve Peters --- src/Element.cc | 3 ++- src/Utils.cc | 3 ++- src/parser.cc | 3 ++- test/integration/custom_elems_attrs.sdf | 2 +- test/integration/plugin_attribute.cc | 4 +++- test/integration/sdf_custom.cc | 14 ++++++++++++++ 6 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/Element.cc b/src/Element.cc index 9ded14004..fd8950d7f 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -671,7 +671,8 @@ void ElementPrivate::PrintAttributes(sdf::Errors &_errors, // better separation of concerns if the conversion process set the // required attributes with their default values. if ((*aiter)->GetSet() || (*aiter)->GetRequired() || - _includeDefaultAttributes) + _includeDefaultAttributes || + ((*aiter)->GetKey().find(':') != std::string::npos)) { const std::string key = (*aiter)->GetKey(); const auto it = attributeExceptions.find(key); diff --git a/src/Utils.cc b/src/Utils.cc index c6a89e867..a76315e0f 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -366,7 +366,8 @@ void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute(); attribute; attribute = attribute->Next()) { - element->AddAttribute(attribute->Name(), "string", "", 1, ""); + // Add with required == 0 to allow unrecognized attribute to be empty + element->AddAttribute(attribute->Name(), "string", "", 0, ""); element->GetAttribute(attribute->Name())->SetFromString( attribute->Value()); } diff --git a/src/parser.cc b/src/parser.cc index 84a7593d6..5a914a948 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1440,7 +1440,8 @@ static bool readAttributes(tinyxml2::XMLElement *_xml, ElementPtr _sdf, // attribute is found if (std::strchr(attribute->Name(), ':') != nullptr) { - _sdf->AddAttribute(attribute->Name(), "string", "", 1, ""); + // Add with required == 0 to allow custom attribute to be empty + _sdf->AddAttribute(attribute->Name(), "string", "", 0, ""); _sdf->GetAttribute(attribute->Name())->SetFromString(attribute->Value()); attribute = attribute->Next(); continue; diff --git a/test/integration/custom_elems_attrs.sdf b/test/integration/custom_elems_attrs.sdf index 8f051fc79..54c9f9c9e 100644 --- a/test/integration/custom_elems_attrs.sdf +++ b/test/integration/custom_elems_attrs.sdf @@ -4,7 +4,7 @@ Description of this world - + L1 L2 diff --git a/test/integration/plugin_attribute.cc b/test/integration/plugin_attribute.cc index 02af21046..7f4f9f4cb 100644 --- a/test/integration/plugin_attribute.cc +++ b/test/integration/plugin_attribute.cc @@ -30,7 +30,7 @@ std::string get_sdf_string() << "" << " " << " " - << " " + << " " << " value" << " " << "" @@ -54,6 +54,8 @@ TEST(PluginAttribute, ParseAttributes) EXPECT_TRUE(user->HasAttribute("attribute")); EXPECT_EQ(user->GetAttribute("attribute")->GetAsString(), std::string("attribute")); + EXPECT_TRUE(user->HasAttribute("empty_attribute")); + EXPECT_EQ(user->GetAttribute("empty_attribute")->GetAsString(), ""); } { sdf::ElementPtr value = plugin->GetElement("value"); diff --git a/test/integration/sdf_custom.cc b/test/integration/sdf_custom.cc index 53ecedcec..cdf1be1ee 100644 --- a/test/integration/sdf_custom.cc +++ b/test/integration/sdf_custom.cc @@ -60,6 +60,20 @@ TEST(SDFParser, CustomElements) auto customAttrInt = link1Element->Get("mysim:custom_attr_int"); EXPECT_EQ(5, customAttrInt); + // Check empty attribute in link L2 + const sdf::Link *link2 = model->LinkByName("L2"); + ASSERT_TRUE(link2 != nullptr); + sdf::ElementPtr link2Element = link2->Element(); + ASSERT_TRUE(link2Element != nullptr); + EXPECT_TRUE(link2Element->HasAttribute("mysim:empty_attr")); + auto emptyAttrStr = link2Element->Get("mysim:empty_attr"); + EXPECT_EQ("", emptyAttrStr); + + // Ensure that mysim:empty_attr appears when calling ToString("") + auto rootToString = link2Element->ToString(""); + EXPECT_NE(std::string::npos, rootToString.find("mysim:empty_attr=''")) + << rootToString; + // Use of sdf::Model::Element() to obtain an sdf::ElementPtr object sdf::ElementPtr modelElement = model->Element(); From 186f7b176f752958821da4ec68f18aa9926fbdab Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Tue, 28 May 2024 01:43:22 -0700 Subject: [PATCH 12/23] Update default camera instrinsics skew to 0, which matches spec (#1423) Signed-off-by: Shameek Ganguly --- python/test/pyCamera_TEST.py | 2 +- src/Camera.cc | 2 +- src/Camera_TEST.cc | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index 6daca4cbe..c2a33312e 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -191,7 +191,7 @@ def test_default_construction(self): cam.set_lens_intrinsics_cy(123) self.assertAlmostEqual(123, cam.lens_intrinsics_cy()) - self.assertAlmostEqual(1.0, cam.lens_intrinsics_skew()) + self.assertAlmostEqual(0.0, cam.lens_intrinsics_skew()) cam.set_lens_intrinsics_skew(2.3) self.assertAlmostEqual(2.3, cam.lens_intrinsics_skew()) diff --git a/src/Camera.cc b/src/Camera.cc index fceb6f1f2..884ce36e1 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -208,7 +208,7 @@ class sdf::Camera::Implementation public: double lensProjectionTy{0.0}; /// \brief lens instrinsics s. - public: double lensIntrinsicsS{1.0}; + public: double lensIntrinsicsS{0.0}; /// \brief True if this camera has custom intrinsics values public: bool hasIntrinsics = false; diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 68b61beac..c68609b48 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -214,7 +214,7 @@ TEST(DOMCamera, Construction) cam.SetLensProjectionTy(2); EXPECT_DOUBLE_EQ(2, cam.LensProjectionTy()); - EXPECT_DOUBLE_EQ(1.0, cam.LensIntrinsicsSkew()); + EXPECT_DOUBLE_EQ(0, cam.LensIntrinsicsSkew()); cam.SetLensIntrinsicsSkew(2.3); EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); From 2489af176d4c19edd66dc9858ab31bbfcf7c8859 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 30 May 2024 15:36:47 +0900 Subject: [PATCH 13/23] Add note in migration guide on camera skew value change (#1425) Signed-off-by: Ian Chen --- Migration.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Migration.md b/Migration.md index be560d702..1f859ee5f 100644 --- a/Migration.md +++ b/Migration.md @@ -22,6 +22,9 @@ but with improved human-readability.. ### Modifications +1. The default camera lens intrinsics skew value in the Camera DOM class changed + from `1` to `0` to match the SDF specification. + 1. World class only renames frames with name collisions if original file version is 1.6 or earlier. Name collisions in newer files will cause `DUPLICATE_NAME` errors, which now matches the behavior of the Model class. From 0127bb8ba61afe5ae7e9cc991c5a140c2a614c49 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 30 May 2024 13:15:05 +0200 Subject: [PATCH 14/23] =?UTF-8?q?Added=20Automatic=20Moment=20of=20Inertia?= =?UTF-8?q?=20Calculations=20for=20Basic=20Shapes=20Pytho=E2=80=A6=20(#142?= =?UTF-8?q?4)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- python/src/sdf/pyBox.cc | 3 + python/src/sdf/pyCapsule.cc | 3 + python/src/sdf/pyCollision.cc | 7 ++ python/src/sdf/pyCylinder.cc | 3 + python/src/sdf/pyEllipsoid.cc | 3 + python/src/sdf/pyGeometry.cc | 3 + python/src/sdf/pyLink.cc | 10 ++ python/src/sdf/pyModel.cc | 2 + python/src/sdf/pyParserConfig.cc | 10 ++ python/src/sdf/pyRoot.cc | 4 +- python/src/sdf/pySphere.cc | 3 + python/src/sdf/pyWorld.cc | 2 + python/test/pyBox_TEST.py | 38 ++++++- python/test/pyCapsule_TEST.py | 54 +++++++-- python/test/pyCollision_TEST.py | 140 ++++++++++++++++++++++- python/test/pyCylinder_TEST.py | 47 +++++++- python/test/pyEllipsoid_TEST.py | 46 +++++++- python/test/pyGeometry_TEST.py | 174 ++++++++++++++++++++++++++++- python/test/pyLink_TEST.py | 172 ++++++++++++++++++++++++++-- python/test/pyParserConfig_TEST.py | 6 +- python/test/pyRoot_TEST.py | 37 +++++- python/test/pySphere_TEST.py | 97 +++++++++++----- python/test/pyWorld_TEST.py | 56 +++++++++- 23 files changed, 846 insertions(+), 74 deletions(-) diff --git a/python/src/sdf/pyBox.cc b/python/src/sdf/pyBox.cc index 31d9d8f08..453438119 100644 --- a/python/src/sdf/pyBox.cc +++ b/python/src/sdf/pyBox.cc @@ -16,6 +16,7 @@ #include "pyBox.hh" #include +#include #include "sdf/Box.hh" @@ -42,6 +43,8 @@ void defineBox(pybind11::object module) pybind11::overload_cast<>(&sdf::Box::Shape, pybind11::const_), pybind11::return_value_policy::reference, "Get a mutable Gazebo Math representation of this Box.") + .def("calculate_inertial", &sdf::Box::CalculateInertial, + "Calculate and return the Inertial values for the Box.") .def("__copy__", [](const sdf::Box &self) { return sdf::Box(self); }) diff --git a/python/src/sdf/pyCapsule.cc b/python/src/sdf/pyCapsule.cc index ac25cd917..50ca4337d 100644 --- a/python/src/sdf/pyCapsule.cc +++ b/python/src/sdf/pyCapsule.cc @@ -16,6 +16,7 @@ #include "pyCapsule.hh" #include +#include #include "sdf/Capsule.hh" @@ -41,6 +42,8 @@ void defineCapsule(pybind11::object module) "Get the capsule's length in meters.") .def("set_length", &sdf::Capsule::SetLength, "Set the capsule's length in meters.") + .def("calculate_inertial", &sdf::Capsule::CalculateInertial, + "Calculate and return the Inertial values for the Capsule.") .def( "shape", pybind11::overload_cast<>(&sdf::Capsule::Shape, pybind11::const_), diff --git a/python/src/sdf/pyCollision.cc b/python/src/sdf/pyCollision.cc index c7305063e..d01f6fba3 100644 --- a/python/src/sdf/pyCollision.cc +++ b/python/src/sdf/pyCollision.cc @@ -17,6 +17,7 @@ #include "pyCollision.hh" #include +#include #include "sdf/Collision.hh" #include "sdf/Geometry.hh" @@ -37,6 +38,12 @@ void defineCollision(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("set_density", &sdf::Collision::SetDensity, "Set the density of the collision.") + .def("density", &sdf::Collision::Density, "Get the density of the collision.") + .def("calculate_inertial", + pybind11::overload_cast( + &sdf::Collision::CalculateInertial), + "Calculate and return the MassMatrix for the collision.") .def("name", &sdf::Collision::Name, "Get the name of the collision. " "The name of the collision must be unique within the scope of a Link.") diff --git a/python/src/sdf/pyCylinder.cc b/python/src/sdf/pyCylinder.cc index 83af7dda4..d8515ddc6 100644 --- a/python/src/sdf/pyCylinder.cc +++ b/python/src/sdf/pyCylinder.cc @@ -16,6 +16,7 @@ #include "pyCylinder.hh" #include +#include #include "sdf/Cylinder.hh" @@ -33,6 +34,8 @@ void defineCylinder(pybind11::object module) pybind11::class_(module, "Cylinder") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Cylinder::CalculateInertial, + "Calculate and return the Inertial values for the Cylinder.") .def("radius", &sdf::Cylinder::Radius, "Get the cylinder's radius in meters.") .def("set_radius", &sdf::Cylinder::SetRadius, diff --git a/python/src/sdf/pyEllipsoid.cc b/python/src/sdf/pyEllipsoid.cc index 89fdb1454..47ea60c96 100644 --- a/python/src/sdf/pyEllipsoid.cc +++ b/python/src/sdf/pyEllipsoid.cc @@ -16,6 +16,7 @@ #include "pyEllipsoid.hh" #include +#include #include "sdf/Ellipsoid.hh" @@ -33,6 +34,8 @@ void defineEllipsoid(pybind11::object module) pybind11::class_(module, "Ellipsoid") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Ellipsoid::CalculateInertial, + "Calculate and return the Inertial values for the Ellipsoid.") .def("radii", &sdf::Ellipsoid::Radii, "Get the ellipsoid's radii in meters.") .def("set_radii", &sdf::Ellipsoid::SetRadii, diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index 84a146c07..ffcbabbe5 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -17,6 +17,7 @@ #include "pyGeometry.hh" #include +#include #include "sdf/ParserConfig.hh" @@ -45,6 +46,8 @@ void defineGeometry(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Geometry::CalculateInertial, + "Calculate and return the Mass Matrix values for the Geometry") .def("type", &sdf::Geometry::Type, "Get the type of geometry.") .def("set_type", &sdf::Geometry::SetType, diff --git a/python/src/sdf/pyLink.cc b/python/src/sdf/pyLink.cc index 3f77912fa..a101d5512 100644 --- a/python/src/sdf/pyLink.cc +++ b/python/src/sdf/pyLink.cc @@ -41,6 +41,16 @@ void defineLink(pybind11::object module) pybind11::class_(module, "Link") .def(pybind11::init<>()) .def(pybind11::init()) + .def("resolve_auto_inertials", &sdf::Link::ResolveAutoInertials, + "Calculate & set inertial values for the link") + .def("auto_inertia", &sdf::Link::AutoInertia, + "Check if the automatic calculation for the link inertial is enabled or not.") + .def("set_auto_inertia", &sdf::Link::SetAutoInertia, + "Enable automatic inertial calculations by setting autoInertia to true") + .def("auto_inertia_saved", &sdf::Link::AutoInertiaSaved, + "Check if the inertial values for this link were saved.") + .def("set_auto_inertia_saved", &sdf::Link::SetAutoInertiaSaved, + "Set the autoInertiaSaved() values") .def("name", &sdf::Link::Name, "Get the name of the link.") .def("set_name", &sdf::Link::SetName, diff --git a/python/src/sdf/pyModel.cc b/python/src/sdf/pyModel.cc index fce94d0b9..1127945de 100644 --- a/python/src/sdf/pyModel.cc +++ b/python/src/sdf/pyModel.cc @@ -46,6 +46,8 @@ void defineModel(pybind11::object module) }, "Check that the FrameAttachedToGraph and PoseRelativeToGraph " "are valid.") + .def("resolve_auto_inertials", &sdf::Model::ResolveAutoInertials, + "Calculate and set the inertials for all the links belonging to the model object") .def("name", &sdf::Model::Name, "Get the name of model.") .def("set_name", &sdf::Model::SetName, diff --git a/python/src/sdf/pyParserConfig.cc b/python/src/sdf/pyParserConfig.cc index 73d8b1990..2e58422fe 100644 --- a/python/src/sdf/pyParserConfig.cc +++ b/python/src/sdf/pyParserConfig.cc @@ -46,6 +46,12 @@ void defineParserConfig(pybind11::object module) .def("find_file_callback", &sdf::ParserConfig::FindFileCallback, "Get the find file callback function") + .def("calculate_inertial_configuration", + &sdf::ParserConfig::CalculateInertialConfiguration, + "Get the current configuration for the CalculateInertial() function") + .def("set_calculate_inertial_configuration", + &sdf::ParserConfig::SetCalculateInertialConfiguration, + "Set the configuration for the CalculateInertial() function") .def("set_find_callback", &sdf::ParserConfig::SetFindCallback, "Set the callback to use when libsdformat can't find a file.") @@ -96,6 +102,10 @@ void defineParserConfig(pybind11::object module) .value("WARN", sdf::EnforcementPolicy::WARN) .value("LOG", sdf::EnforcementPolicy::LOG); + pybind11::enum_( + module, "ConfigureResolveAutoInertials") + .value("SKIP_CALCULATION_IN_LOAD", sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD) + .value("SAVE_CALCULATION", sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); } } // namespace python } // namespace SDF_VERSION_NAMESPACE diff --git a/python/src/sdf/pyRoot.cc b/python/src/sdf/pyRoot.cc index 42c21907a..2da83830a 100644 --- a/python/src/sdf/pyRoot.cc +++ b/python/src/sdf/pyRoot.cc @@ -38,6 +38,8 @@ void defineRoot(pybind11::object module) { pybind11::class_(module, "Root") .def(pybind11::init<>()) + .def("resolve_auto_inertials", &sdf::Root::ResolveAutoInertials, + "Calculate and set the inertial properties") .def("load", [](Root &self, const std::string &_filename) { @@ -46,7 +48,7 @@ void defineRoot(pybind11::object module) "Parse the given SDF file, and generate objects based on types " "specified in the SDF file.") .def("load", - [](Root &self, const std::string &_filename, + [](Root &self, const std::string &_filename, const ParserConfig &_config) { ThrowIfErrors(self.Load(_filename, _config)); diff --git a/python/src/sdf/pySphere.cc b/python/src/sdf/pySphere.cc index 06e991df7..ee6197b38 100644 --- a/python/src/sdf/pySphere.cc +++ b/python/src/sdf/pySphere.cc @@ -16,6 +16,7 @@ #include "pySphere.hh" #include +#include #include "sdf/Sphere.hh" @@ -33,6 +34,8 @@ void defineSphere(pybind11::object module) pybind11::class_(module, "Sphere") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Sphere::CalculateInertial, + "Calculate and return the Inertial values for the Sphere.") .def("radius", &sdf::Sphere::Radius, "Get the sphere's radius in meters.") .def("set_radius", &sdf::Sphere::SetRadius, diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index 41b930fc4..108708ae4 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -48,6 +48,8 @@ void defineWorld(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("resolve_auto_inertials", &sdf::World::ResolveAutoInertials, + "Calculate and set the inertials for all the models in the world object") .def("validate_graphs", &sdf::World::ValidateGraphs, "Check that the FrameAttachedToGraph and PoseRelativeToGraph " "are valid.") diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index d98440de5..84ce40bee 100644 --- a/python/test/pyBox_TEST.py +++ b/python/test/pyBox_TEST.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz_test_deps.math import Vector3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Box import unittest @@ -33,7 +33,7 @@ def test_assignment(self): box = Box() box.set_size(size) - box2 = box; + box2 = box self.assertEqual(size, box2.size()) self.assertEqual(1 * 2 * 3, box2.shape().volume()) @@ -55,5 +55,39 @@ def test_shape(self): box.shape().set_size(Vector3d(1, 2, 3)) self.assertEqual(Vector3d(1, 2, 3), box.size()) + def test_calculate_inertial(self): + box = Box() + density = 2710 + + box.set_size(Vector3d(-1, 1, 0)) + invalidBoxInertial = box.calculate_inertial(density) + self.assertEqual(None, invalidBoxInertial) + + l = 2.0 + w = 2.0 + h = 2.0 + box.set_size(Vector3d(l, w, h)) + + expectedMass = box.shape().volume() * density + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + boxInertial = box.calculate_inertial(density) + self.assertEqual(box.shape().material().density(), density) + self.assertNotEqual(None, boxInertial) + self.assertEqual(expectedInertial, boxInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + boxInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + boxInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 52472ed91..976e53c7b 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -16,6 +16,7 @@ import math +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Capsule import unittest @@ -56,7 +57,7 @@ def test_move_construction(self): def test_copy_construction(self): - capsule = Capsule(); + capsule = Capsule() capsule.set_radius(0.2) capsule.set_length(3.0) @@ -65,29 +66,29 @@ def test_copy_construction(self): self.assertEqual(3.0, capsule2.length()) def test_copy_construction(self): - capsule = Capsule(); + capsule = Capsule() capsule.set_radius(0.2) capsule.set_length(3.0) - capsule2 = copy.deepcopy(capsule); + capsule2 = copy.deepcopy(capsule) self.assertEqual(0.2, capsule2.radius()) self.assertEqual(3.0, capsule2.length()) def test_assignment_after_move(self): - capsule1 = Capsule(); + capsule1 = Capsule() capsule1.set_radius(0.2) capsule1.set_length(3.0) - capsule2 = Capsule(); + capsule2 = Capsule() capsule2.set_radius(2) capsule2.set_length(30.0) # This is similar to what std::swap does except it uses std::move for each # assignment tmp = capsule1 - capsule1 = copy.deepcopy(capsule2); - capsule2 = tmp; + capsule1 = copy.deepcopy(capsule2) + capsule2 = tmp self.assertEqual(2, capsule1.radius()) self.assertEqual(30, capsule1.length()) @@ -97,7 +98,7 @@ def test_assignment_after_move(self): def test_shape(self): - capsule = Capsule(); + capsule = Capsule() self.assertEqual(0.5, capsule.radius()) self.assertEqual(1.0, capsule.length()) @@ -106,6 +107,43 @@ def test_shape(self): self.assertEqual(0.123, capsule.radius()) self.assertEqual(0.456, capsule.length()) + def test_calculate_inertial(self): + capsule = Capsule() + + # density of aluminium + density = 2710 + l = 2.0 + r = 0.1 + + capsule.set_length(l) + capsule.set_radius(r) + + expectedMass = capsule.shape().volume() * density + cylinderVolume = math.pi * r * r * l + sphereVolume = math.pi * 4. / 3. * r * r * r + volume = cylinderVolume + sphereVolume + cylinderMass = expectedMass * cylinderVolume / volume + sphereMass = expectedMass * sphereVolume / volume + + ixxIyy = (1 / 12.0) * cylinderMass * (3 * r * r + l * l) + sphereMass * ( + 0.4 * r * r + 0.375 * r * l + 0.25 * l * l) + izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixxIyy, ixxIyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + capsuleInertial = capsule.calculate_inertial(density) + self.assertEqual(capsule.shape().material().density(), density) + self.assertNotEqual(None, capsuleInertial) + self.assertEqual(expectedInertial, capsuleInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + capsuleInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + capsuleInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index 9e92f521b..8d8d96735 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz_test_deps.math import Pose3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error, - Geometry, Plane, Surface, Sphere, + Geometry, ParserConfig, Plane, Root, Surface, Sphere, SDFErrorsException) import gz_test_deps.sdformat as sdf import unittest @@ -26,6 +26,7 @@ class CollisionTEST(unittest.TestCase): def test_default_construction(self): collision = Collision() self.assertTrue(not collision.name()) + self.assertEqual(collision.density(), 1000.0) collision.set_name("test_collison") self.assertEqual(collision.name(), "test_collison") @@ -40,6 +41,9 @@ def test_default_construction(self): with self.assertRaises(SDFErrorsException): semanticPose.resolve() + collision.set_density(1240.0) + self.assertAlmostEqual(collision.density(), 1240.0) + collision.set_raw_pose(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi)) self.assertEqual(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi), collision.raw_pose()) @@ -130,5 +134,137 @@ def test_set_surface(self): self.assertEqual(collision.surface().contact().collide_bitmask(), 0x2) + def test_incorrect_box_collision_calculate_inertial(self): + collision = Collision() + self.assertAlmostEqual(1000.0, collision.density()) + + # sdf::ElementPtr sdf(new sdf::Element()) + # collision.Load(sdf) + + collisionInertial = Inertiald() + sdfParserConfig = ParserConfig() + geom = Geometry() + box = Box() + + # Invalid Inertial test + box.set_size(Vector3d(-1, 1, 0)) + geom.set_type(sdf.GeometryType.BOX) + geom.set_box_shape(box) + collision.set_geometry(geom) + + errors = [] + + collision.calculate_inertial(errors, collisionInertial, sdfParserConfig) + self.assertFalse(len(errors)) + + + def test_correct_box_collision_calculate_inertial(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + model = root.model() + link = model.link_by_index(0) + collision = link.collision_by_index(0) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * collision.density() + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(len(inertialErr), 0) + self.assertAlmostEqual(1240.0, collision.density()) + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(expectedInertial.mass_matrix(), link.inertial().mass_matrix()) + self.assertEqual(expectedInertial.pose(), link.inertial().pose()) + + def test_calculate_inertial_pose_not_relative_to_link(self): + + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " 0 0 1 0 0 0" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0 0 -1 0 0 0" + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + # self.assertNotEqual(None, root.Element()) + + model = root.model() + link = model.link_by_index(0) + collision = link.collision_by_index(0) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * collision.density() + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(len(inertialErr), 0) + self.assertAlmostEqual(1240.0, collision.density()) + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(expectedInertial.mass_matrix(), link.inertial().mass_matrix()) + self.assertEqual(expectedInertial.pose(), link.inertial().pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index 99bcf7823..b5387dd0b 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -16,6 +16,7 @@ import math +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Cylinder import unittest @@ -64,7 +65,7 @@ def test_assignment(self): def test_copy_construction(self): - cylinder = Cylinder(); + cylinder = Cylinder() cylinder.set_radius(0.2) cylinder.set_length(3.0) @@ -81,11 +82,11 @@ def test_copy_construction(self): self.assertEqual(3.0, cylinder2.length()) def test_deepcopy(self): - cylinder = Cylinder(); + cylinder = Cylinder() cylinder.set_radius(0.2) cylinder.set_length(3.0) - cylinder2 = copy.deepcopy(cylinder); + cylinder2 = copy.deepcopy(cylinder) self.assertEqual(0.2, cylinder2.radius()) self.assertEqual(3.0, cylinder2.length()) @@ -98,7 +99,7 @@ def test_deepcopy(self): self.assertEqual(3.0, cylinder2.length()) def test_shape(self): - cylinder = Cylinder(); + cylinder = Cylinder() self.assertEqual(0.5, cylinder.radius()) self.assertEqual(1.0, cylinder.length()) @@ -107,6 +108,44 @@ def test_shape(self): self.assertEqual(0.123, cylinder.radius()) self.assertEqual(0.456, cylinder.length()) + def test_calculate_interial(self): + cylinder = Cylinder() + + # density of aluminium + density = 2170 + + # Invalid dimensions leading to None return in + # CalculateInertial() + cylinder.set_length(-1) + cylinder.set_radius(0) + invalidCylinderInertial = cylinder.calculate_inertial(density) + self.assertEqual(None, invalidCylinderInertial) + + l = 2.0 + r = 0.1 + + cylinder.set_length(l) + cylinder.set_radius(r) + + expectedMass = cylinder.shape().volume() * density + ixxIyy = (1 / 12.0) * expectedMass * (3 * r * r + l * l) + izz = 0.5 * expectedMass * r * r + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixxIyy, ixxIyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + cylinderInertial = cylinder.calculate_inertial(density) + self.assertEqual(cylinder.shape().mat().density(), density) + self.assertNotEqual(None, cylinderInertial) + self.assertEqual(expectedInertial, cylinderInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + cylinderInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + cylinderInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index a102a42ad..c12690ffe 100644 --- a/python/test/pyEllipsoid_TEST.py +++ b/python/test/pyEllipsoid_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz_test_deps.math import Vector3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d import math from gz_test_deps.sdformat import Ellipsoid import unittest @@ -51,7 +51,7 @@ def test_deepcopy(self): def test_deepcopy_after_assignment(self): - ellipsoid1 = Ellipsoid(); + ellipsoid1 = Ellipsoid() expectedradii1 = Vector3d(1.0, 2.0, 3.0) ellipsoid1.set_radii(expectedradii1) @@ -62,8 +62,8 @@ def test_deepcopy_after_assignment(self): # This is similar to what std::swap does except it uses std::move for each # assignment tmp = copy.deepcopy(ellipsoid1) - ellipsoid1 = ellipsoid2; - ellipsoid2 = tmp; + ellipsoid1 = ellipsoid2 + ellipsoid2 = tmp self.assertEqual(expectedradii1, ellipsoid2.shape().radii()) self.assertEqual(expectedradii2, ellipsoid1.shape().radii()) @@ -77,6 +77,44 @@ def test_shape(self): ellipsoid.shape().set_radii(expectedradii) self.assertEqual(expectedradii, ellipsoid.radii()) + def test_calculate_inertial(self): + ellipsoid = Ellipsoid() + + # density of aluminium + density = 2170 + + # Invalid dimensions leading to std::nullopt return in + # CalculateInertial() + ellipsoid.set_radii(Vector3d(-1, 2, 0)) + invalidEllipsoidInertial = ellipsoid.calculate_inertial(density) + self.assertEqual(None, invalidEllipsoidInertial) + + a = 1.0 + b = 10.0 + c = 100.0 + + ellipsoid.set_radii(Vector3d(a, b, c)) + + expectedMass = ellipsoid.shape().volume() * density + ixx = (expectedMass / 5.0) * (b * b + c * c) + iyy = (expectedMass / 5.0) * (a * a + c * c) + izz = (expectedMass / 5.0) * (a * a + b * b) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + ellipsoidInertial = ellipsoid.calculate_inertial(density) + self.assertEqual(ellipsoid.shape().material().density(), density) + self.assertNotEqual(None, ellipsoidInertial) + self.assertEqual(expectedInertial, ellipsoidInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + ellipsoidInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + ellipsoidInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index a1f4ec65b..c4cd9c511 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,10 +13,11 @@ # limitations under the License. import copy -from gz_test_deps.sdformat import (Geometry, Box, Capsule, Cylinder, Ellipsoid, - Mesh, Plane, Sphere) -from gz_test_deps.math import Vector3d, Vector2d +from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cylinder, Ellipsoid, + Mesh, ParserConfig, Plane, Sphere) +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf +import math import unittest @@ -185,5 +186,172 @@ def test_plane(self): self.assertEqual(Vector2d(9, 8), geom.plane_shape().size()) + def test_calculate_inertial(self): + geom = Geometry() + + # Density of Aluminimum + density = 2170.0 + expectedMass = 0 + expectedMassMat = MassMatrix3d() + expectedInertial = Inertiald() + sdfParserConfig = ParserConfig() + errors = [] + + # Not supported geom type + geom.set_type(sdf.GeometryType.EMPTY) + element = Element() + notSupportedInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + self.assertEqual(notSupportedInertial, None) + + box = Box() + l = 2.0 + w = 2.0 + h = 2.0 + box.set_size(Vector3d(l, w, h)) + + expectedMass = box.shape().volume() * density + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixx, iyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.BOX) + geom.set_box_shape(box) + boxInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, boxInertial) + self.assertEqual(expectedInertial, boxInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + + # Capsule + capsule = Capsule() + l = 2.0 + r = 0.1 + capsule.set_length(l) + capsule.set_radius(r) + + expectedMass = capsule.shape().volume() * density + cylinderVolume = math.pi * r * r * l + sphereVolume = math.pi * 4. / 3. * r * r * r + volume = cylinderVolume + sphereVolume + cylinderMass = expectedMass * cylinderVolume / volume + sphereMass = expectedMass * sphereVolume / volume + ixxIyy = (1 / 12.0) * cylinderMass * (3 * r *r + l * l) + sphereMass * ( + 0.4 * r * r + 0.375 * r * l + 0.25 * l * l) + izz = r * r * (0.5 * cylinderMass + 0.4 * sphereMass) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixxIyy, ixxIyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.CAPSULE) + geom.set_capsule_shape(capsule) + capsuleInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, capsuleInertial) + self.assertEqual(expectedInertial, capsuleInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) + + # Cylinder + cylinder = Cylinder() + l = 2.0 + r = 0.1 + + cylinder.set_length(l) + cylinder.set_radius(r) + + expectedMass = cylinder.shape().volume() * density + ixxIyy = (1/12.0) * expectedMass * (3*r*r + l*l) + izz = 0.5 * expectedMass * r * r + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixxIyy, ixxIyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.CYLINDER) + geom.set_cylinder_shape(cylinder) + cylinderInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, cylinderInertial) + self.assertEqual(expectedInertial, cylinderInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) + + # Ellipsoid + ellipsoid = Ellipsoid() + + a = 1.0 + b = 10.0 + c = 100.0 + + ellipsoid.set_radii(Vector3d(a, b, c)) + + expectedMass = ellipsoid.shape().volume() * density + ixx = (expectedMass / 5.0) * (b*b + c*c) + iyy = (expectedMass / 5.0) * (a*a + c*c) + izz = (expectedMass / 5.0) * (a*a + b*b) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixx, iyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.ELLIPSOID) + geom.set_ellipsoid_shape(ellipsoid) + ellipsoidInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, ellipsoidInertial) + self.assertEqual(expectedInertial, ellipsoidInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) + + # Sphere + sphere = Sphere() + r = 0.1 + + sphere.set_radius(r) + + expectedMass = sphere.shape().volume() * density + ixxIyyIzz = 0.4 * expectedMass * r * r + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments( + Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.SPHERE) + geom.set_sphere_shape(sphere) + sphereInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, sphereInertial) + self.assertEqual(expectedInertial, sphereInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index e4ad505a4..0f2c8615f 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -14,8 +14,8 @@ import copy from gz_test_deps.math import Pose3d, Inertiald, MassMatrix3d, Vector3d -from gz_test_deps.sdformat import (Collision, Light, Link, Projector, Sensor, - Visual, SDFErrorsException) +from gz_test_deps.sdformat import (Collision, Light, Link, ParserConfig, Projector, Sensor, + Visual, Root, SDFErrorsException) import unittest import math @@ -61,6 +61,14 @@ def test_default_construction(self): link.set_enable_wind(True) self.assertTrue(link.enable_wind()) + self.assertFalse(link.auto_inertia_saved()) + link.set_auto_inertia_saved(True) + self.assertTrue(link.auto_inertia_saved()) + + self.assertFalse(link.auto_inertia()) + link.set_auto_inertia(True) + self.assertTrue(link.auto_inertia()) + self.assertEqual(0, link.sensor_count()) self.assertEqual(None, link.sensor_by_index(0)) self.assertEqual(None, link.sensor_by_index(1)) @@ -313,8 +321,8 @@ def test_mutable_by_index(self): pr = link.projector_by_index(0) self.assertNotEqual(None, pr) self.assertEqual("projector1", pr.name()) - pr.set_name("projector2"); - self.assertEqual("projector2", link.projector_by_index(0).name()); + pr.set_name("projector2") + self.assertEqual("projector2", link.projector_by_index(0).name()) def test_mutable_by_name(self): link = Link() @@ -376,7 +384,7 @@ def test_mutable_by_name(self): self.assertFalse(link.sensor_name_exists("sensor1")) self.assertTrue(link.sensor_name_exists("sensor2")) - # // Modify the particle emitter + # # Modify the particle emitter # sdf::ParticleEmitter *p = link.ParticleEmitterByName("pe1") # self.assertNotEqual(None, p) # self.assertEqual("pe1", p.name()) @@ -385,12 +393,154 @@ def test_mutable_by_name(self): # self.assertTrue(link.particle_emitter_name_exists("pe2")) # Modify the projector - pr = link.projector_by_name("projector1"); - self.assertNotEqual(None, pr); - self.assertEqual("projector1", pr.name()); - pr.set_name("projector2"); - self.assertFalse(link.projector_name_exists("projector1")); - self.assertTrue(link.projector_name_exists("projector2")); + pr = link.projector_by_name("projector1") + self.assertNotEqual(None, pr) + self.assertEqual("projector1", pr.name()) + pr.set_name("projector2") + self.assertFalse(link.projector_name_exists("projector1")) + self.assertTrue(link.projector_name_exists("projector2")) + + def test_resolveauto_inertialsWithNoCollisionsInLink(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + with self.assertRaises(SDFErrorsException): + errors = root.load_sdf_string(sdf, sdfParserConfig) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Default Inertial values set during load + self.assertEqual(1.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d.ONE, + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsWithMultipleCollisions(self): + sdf = "" + \ + "" + \ + " " + \ + " 0 0 1.0 0 0 0" + \ + " " + \ + " " + \ + " " + \ + " 0 0 -0.5 0 0 0" + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0 0 0.5 0 0 0" + \ + " 4" + \ + " " + \ + " " + \ + " 0.5" + \ + " 1.0" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Mass of cube(volume * density) + mass of cylinder(volume * density) + expectedMass = 1.0 * 2.0 + math.pi * 0.5 * 0.5 * 1 * 4.0 + + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d(2.013513, 2.013513, 0.72603), + link.inertial().mass_matrix().diagonal_moments()) + + def test_inertial_values_given_with_auto_set_to_true(self): + sdf = "" + \ + "" + \ + " " + \ + " " + \ + " " + \ + " 4.0" + \ + " 1 1 1 2 2 2" + \ + " " + \ + " 1" + \ + " 1" + \ + " 1" + \ + " " + \ + " " + \ + " " + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + self.assertNotEqual(4.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Pose3d.ZERO, link.inertial().pose()) + self.assertEqual(Vector3d(0.33333, 0.33333, 0.33333), + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsCalledWithAutoFalse(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Default Inertial values set during load + self.assertEqual(1.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d.ONE, + link.inertial().mass_matrix().diagonal_moments()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyParserConfig_TEST.py b/python/test/pyParserConfig_TEST.py index a56bfc405..52917dd30 100644 --- a/python/test/pyParserConfig_TEST.py +++ b/python/test/pyParserConfig_TEST.py @@ -26,7 +26,7 @@ def test_construction(self): self.assertFalse(config.uri_path_map()) self.assertFalse(config.find_file_callback()) - testDir = source_file(); + testDir = source_file() config.add_uri_path("file://", testDir) self.assertTrue(config.uri_path_map()) @@ -35,7 +35,7 @@ def test_construction(self): self.assertEqual(it[0], testDir) def testFunc(argument): - return "test/dir2"; + return "test/dir2" config.set_find_callback(testFunc) self.assertTrue(config.find_file_callback()) @@ -102,7 +102,7 @@ def test_copy(self): # so we'll use the source path testDir1 = source_file() - config1 = ParserConfig(); + config1 = ParserConfig() config1.add_uri_path("file://", testDir1) it = config1.uri_path_map().get("file://") diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index 0df1d3023..0a41bf667 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.math import Pose3d -from gz_test_deps.sdformat import (Error, Model, Light, Root, SDF_VERSION, +from gz_test_deps.sdformat import (ConfigureResolveAutoInertials, Error, Model, ParserConfig, Light, Root, SDF_VERSION, SDFErrorsException, SDF_PROTOCOL_VERSION, World) import gz_test_deps.sdformat as sdf @@ -121,7 +121,7 @@ def test_string_light_sdf_parse(self): # ///////////////////////////////////////////////// # TEST(DOMRoot, StringActorSdfParse) # { - # std::string sdf = "" + # sdf = "" # " " # " " # " 0 0 1.0 0 0 0" @@ -324,6 +324,39 @@ def test_element_to_light(self): # ASSERT_EQ(None, root2.Actor()) self.assertEqual(0, root2.world_count()) + def test_resolve_auto_inertials_with_save_calculation_configuration(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + model = root.model() + link = model.link_by_index(0) + + sdfParserConfig.set_calculate_inertial_configuration( + ConfigureResolveAutoInertials.SAVE_CALCULATION) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + self.assertEqual(len(inertialErr), 0) + self.assertTrue(link.auto_inertia_saved()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index 0636d602b..f687f6815 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -15,66 +15,101 @@ import copy import math from gz_test_deps.sdformat import Sphere +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d import unittest class SphereTEST(unittest.TestCase): def test_default_construction(self): - sphere = Sphere(); - self.assertEqual(1.0, sphere.radius()); - sphere.set_radius(0.5); - self.assertEqual(0.5, sphere.radius()); + sphere = Sphere() + self.assertEqual(1.0, sphere.radius()) + sphere.set_radius(0.5) + self.assertEqual(0.5, sphere.radius()) def test_assignment(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = sphere; - self.assertEqual(0.2, sphere2.radius()); + sphere2 = sphere + self.assertEqual(0.2, sphere2.radius()) def test_copy_construction(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = Sphere(sphere); - self.assertEqual(0.2, sphere2.radius()); + sphere2 = Sphere(sphere) + self.assertEqual(0.2, sphere2.radius()) - self.assertEqual(4.0/3.0*math.pi*math.pow(0.2, 3), sphere2.shape().volume()); - self.assertEqual(0.2, sphere2.shape().radius()); + self.assertEqual(4.0/3.0*math.pi*math.pow(0.2, 3), sphere2.shape().volume()) + self.assertEqual(0.2, sphere2.shape().radius()) def test_deepcopy_construction(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = copy.deepcopy(sphere); - self.assertEqual(0.2, sphere2.radius()); + sphere2 = copy.deepcopy(sphere) + self.assertEqual(0.2, sphere2.radius()) def test_deepcopy_after_assignment(self): - sphere1 = Sphere(); - sphere1.set_radius(0.1); + sphere1 = Sphere() + sphere1.set_radius(0.1) - sphere2 = Sphere(); - sphere2.set_radius(0.2); + sphere2 = Sphere() + sphere2.set_radius(0.2) # This is similar to what swap does - tmp = copy.deepcopy(sphere1); - sphere1 = sphere2; - sphere2 = tmp; + tmp = copy.deepcopy(sphere1) + sphere1 = sphere2 + sphere2 = tmp - self.assertEqual(0.2, sphere1.radius()); - self.assertEqual(0.1, sphere2.radius()); + self.assertEqual(0.2, sphere1.radius()) + self.assertEqual(0.1, sphere2.radius()) def test_shape(self): - sphere = Sphere(); - self.assertEqual(1.0, sphere.radius()); + sphere = Sphere() + self.assertEqual(1.0, sphere.radius()) - sphere.shape().set_radius(0.123); - self.assertEqual(0.123, sphere.radius()); + sphere.shape().set_radius(0.123) + self.assertEqual(0.123, sphere.radius()) + + def test_calculate_inertial(self): + sphere = Sphere() + + # density of aluminium + density = 2170 + + sphere.set_radius(-2) + invalidSphereInertial = sphere.calculate_inertial(density) + self.assertEqual(None, invalidSphereInertial) + + r = 0.1 + + sphere.set_radius(r) + + expectedMass = sphere.shape().volume() * density + ixxIyyIzz = 0.4 * expectedMass * r * r + + expectedMassMat = MassMatrix3d( + expectedMass, Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + sphereInertial = sphere.calculate_inertial(density) + self.assertEqual(sphere.shape().material().density(), density) + self.assertNotEqual(None, sphereInertial) + self.assertEqual(expectedInertial, sphereInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + sphereInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + sphereInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) if __name__ == '__main__': diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index f77fcdc8e..f8f5f9d0c 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -14,8 +14,9 @@ import copy from gz_test_deps.math import Color, Vector3d, SphericalCoordinates +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import (Atmosphere, Gui, Physics, Plugin, Error, - Frame, Joint, Light, Model, Scene, World) + Frame, Joint, Light, Model, ParserConfig, Root, Scene, World) import gz_test_deps.sdformat as sdf import unittest import math @@ -87,8 +88,8 @@ def test_default_construction(self): "PoseRelativeToGraph error: scope does not point to a valid graph.") # world doesn't have graphs, so no names should exist in graphs - self.assertFalse(world.name_exists_in_frame_attached_to_graph("")); - self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")); + self.assertFalse(world.name_exists_in_frame_attached_to_graph("")) + self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")) def test_copy_construction(self): @@ -443,6 +444,55 @@ def test_plugins(self): world.clear_plugins() self.assertEqual(0, len(world.plugins())) + def test_resolve_auto_inertials(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + world = root.world_by_index(0) + model = world.model_by_index(0) + link = model.link_by_index(0) + + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * 1240.0 + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(0, len(errors)) + self.assertEqual(expectedInertial, link.inertial()) if __name__ == '__main__': unittest.main() From faf94a01252cd8a1f37b61e91c34054730d68d75 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Mar 2024 01:58:37 -0700 Subject: [PATCH 15/23] Add bullet and torsional friction DOM (#1351) Signed-off-by: Ian Chen --- include/sdf/Surface.hh | 145 ++++++++++++ src/Surface.cc | 343 +++++++++++++++++++++++++++- src/Surface_TEST.cc | 383 +++++++++++++++++++++++++++++--- test/integration/surface_dom.cc | 18 ++ test/sdf/shapes.sdf | 25 ++- 5 files changed, 879 insertions(+), 35 deletions(-) diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index ad855d3a4..09590aef6 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -17,6 +17,7 @@ #ifndef SDF_SURFACE_HH_ #define SDF_SURFACE_HH_ +#include #include #include "sdf/Element.hh" #include "sdf/Types.hh" @@ -122,6 +123,132 @@ namespace sdf GZ_UTILS_IMPL_PTR(dataPtr) }; + /// \brief BulletFriction information for a friction. + class SDFORMAT_VISIBLE BulletFriction + { + /// \brief Default constructor + public: BulletFriction(); + + /// \brief Load BulletFriction friction based on a element pointer. This is + /// *not* the usual entry point. Typical usage of the SDF DOM is through + /// the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the friction coefficient in first friction pyramid direction. + /// \returns Friction coefficient + public: double Friction() const; + + /// \brief Set friction coefficient in first friction pyramid direction. + /// \param[in] _fricton Friction coefficient + public: void SetFriction(double _friction); + + /// \brief Get the friction coefficient in second friction pyramid + /// direction. + /// \return Second friction coefficient + public: double Friction2() const; + + /// \brief Set friction coefficient in second friction pyramid direction. + /// \param[in] _fricton Friction coefficient + public: void SetFriction2(double _friction); + + /// \brief Get the first friction pyramid direction in collision-fixed + /// reference + /// \return First friction pyramid direction. + public: const gz::math::Vector3d &Fdir1() const; + + /// \brief Set the first friction pyramid direction in collision-fixed + /// reference + /// \param[in] _fdir First friction pyramid direction. + public: void SetFdir1(const gz::math::Vector3d &_fdir); + + /// \brief Get the rolling friction coefficient + /// \return Rolling friction coefficient + public: double RollingFriction() const; + + /// \brief Set the rolling friction coefficient + /// \param[in] _slip1 Rolling friction coefficient + public: void SetRollingFriction(double _friction); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + + /// \brief Torsional friction + class SDFORMAT_VISIBLE Torsional + { + /// \brief Default constructor + public: Torsional(); + + /// \brief Load torsional friction based on a element pointer. This is *not* + /// the usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the torsional friction coefficient. + /// \return Torsional friction coefficient + public: double Coefficient() const; + + /// \brief Set the torsional friction coefficient. + /// \param[in] _fricton Torsional friction coefficient + public: void SetCoefficient(double _coefficient); + + /// \brief Get whether the patch radius is used to calculate torsional + /// friction. + /// \return True if patch radius is used. + public: bool UsePatchRadius() const; + + /// \brief Set whether to use patch radius for torsional friction + /// calculation. + /// \param[in] _usePatchRadius True to use patch radius. + /// False to use surface radius. + public: void SetUsePatchRadius(bool _usePatchRadius); + + /// \brief Get the radius of contact patch surface. + /// \return Patch radius + public: double PatchRadius() const; + + /// \brief Set the radius of contact patch surface. + /// \param[in] _radius Patch radius + public: void SetPatchRadius(double _radius); + + /// \brief Get the surface radius on the contact point + /// \return Surface radius + public: double SurfaceRadius() const; + + /// \brief Set the surface radius on the contact point. + /// \param[in] _radius Surface radius + public: void SetSurfaceRadius(double _radius); + + /// \brief Get the ODE force dependent slip for torsional friction + /// \return Force dependent slip for torsional friction. + public: double ODESlip() const; + + /// \brief Set the ODE force dependent slip for torsional friction + /// \param[in] _slip Force dependent slip for torsional friction. + public: void SetODESlip(double _slip); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + /// \brief Friction information for a surface. class SDFORMAT_VISIBLE Friction { @@ -145,6 +272,24 @@ namespace sdf /// \param[in] _ode The ODE object. public: void SetODE(const sdf::ODE &_ode); + /// \brief Get the associated BulletFriction object + /// \return Pointer to the associated BulletFriction object, + /// nullptr if the Surface doesn't contain a BulletFriction element. + public: const sdf::BulletFriction *BulletFriction() const; + + /// \brief Set the associated BulletFriction object. + /// \param[in] _bullet The BulletFriction object. + public: void SetBulletFriction(const sdf::BulletFriction &_bullet); + + /// \brief Get the torsional friction + /// \return Pointer to the torsional friction + /// nullptr if the Surface doesn't contain a torsional friction element. + public: const sdf::Torsional *Torsional() const; + + /// \brief Set the torsional friction + /// \param[in] _torsional The torsional friction. + public: void SetTorsional(const sdf::Torsional &_torsional); + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/src/Surface.cc b/src/Surface.cc index e8958a0da..7772583e6 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -15,6 +15,9 @@ * */ +#include +#include + #include "sdf/Element.hh" #include "sdf/parser.hh" #include "sdf/Surface.hh" @@ -64,18 +67,74 @@ class sdf::ODE::Implementation public: double slip2 = 0.0; }; +class sdf::BulletFriction::Implementation +{ + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; + + /// \brief Coefficient of friction in first friction pyramid direction, + /// the unitless maximum ratio of force in first friction pyramid + /// direction to normal force. + public: double friction{1.0}; + + /// \brief Coefficient of friction in second friction pyramid direction, + /// the unitless maximum ratio of force in second friction pyramid + /// direction to normal force. + public: double friction2{1.0}; + + /// \brief Unit vector specifying first friction pyramid direction in + /// collision-fixed reference frame. + public: gz::math::Vector3d fdir1{0, 0, 0}; + + /// \brief Rolling friction coefficient. + public: double rollingFriction{1.0}; +}; + +class sdf::Torsional::Implementation +{ + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; + + /// \brief Torsional friction coefficient. Uunitless maximum ratio of + /// tangential stress to normal stress. + public: double coefficient{1.0}; + + /// \brief If this flag is true, torsional friction is calculated using the + /// "patch_radius" parameter. If this flag is set to false, + /// "surface_radius" (R) and contact depth (d) are used to compute the patch + /// radius as sqrt(R*d). + public: bool usePatchRadius{true}; + + /// \brief Radius of contact patch surface. + public: double patchRadius{0.0}; + + /// \brief Surface radius on the point of contact. + public: double surfaceRadius{0.0}; + + /// \brief Force dependent slip for torsional friction. + /// equivalent to inverse of viscous damping coefficient with units of + /// rad/s/(Nm). A slip value of 0 is infinitely viscous. + public: double odeSlip{0.0}; +}; + class sdf::Friction::Implementation { - /// \brief The object storing contact parameters + /// \brief The object storing ode parameters public: sdf::ODE ode; + /// \brief The object storing bullet friction parameters + public: std::optional bullet; + + /// \brief The object storing torsional parameters + public: std::optional torsional; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf{nullptr}; }; class sdf::Surface::Implementation { - /// \brief The object storing contact parameters + /// \brief The object storing friction parameters public: sdf::Friction friction; /// \brief The object storing contact parameters @@ -85,6 +144,219 @@ class sdf::Surface::Implementation public: sdf::ElementPtr sdf{nullptr}; }; +///////////////////////////////////////////////// +Torsional::Torsional() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Torsional::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a BulletFriction, but the provided SDF " + "element is null."}); + return errors; + } + + // Check that the provided SDF element is a + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "torsional") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a BulletFriction, but the provided SDF element " + "is not a ."}); + return errors; + } + + this->dataPtr->coefficient = _sdf->Get( + "coefficient", this->dataPtr->coefficient).first; + this->dataPtr->usePatchRadius = _sdf->Get( + "use_patch_radius", this->dataPtr->usePatchRadius).first; + this->dataPtr->patchRadius = _sdf->Get( + "patch_radius", this->dataPtr->patchRadius).first; + this->dataPtr->surfaceRadius = _sdf->Get( + "surface_radius", this->dataPtr->surfaceRadius).first; + + if (_sdf->HasElement("ode")) + { + this->dataPtr->odeSlip = _sdf->GetElement("ode")->Get( + "slip", this->dataPtr->odeSlip).first; + } + + return errors; +} + +///////////////////////////////////////////////// +double Torsional::Coefficient() const +{ + return this->dataPtr->coefficient; +} + +///////////////////////////////////////////////// +void Torsional::SetCoefficient(double _coefficient) +{ + this->dataPtr->coefficient = _coefficient; +} + +///////////////////////////////////////////////// +bool Torsional::UsePatchRadius() const +{ + return this->dataPtr->usePatchRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetUsePatchRadius(bool _usePatchRadius) +{ + this->dataPtr->usePatchRadius = _usePatchRadius; +} + +///////////////////////////////////////////////// +double Torsional::PatchRadius() const +{ + return this->dataPtr->patchRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetPatchRadius(double _radius) +{ + this->dataPtr->patchRadius = _radius; +} + +///////////////////////////////////////////////// +double Torsional::SurfaceRadius() const +{ + return this->dataPtr->surfaceRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetSurfaceRadius(double _radius) +{ + this->dataPtr->surfaceRadius = _radius; +} + +///////////////////////////////////////////////// +double Torsional::ODESlip() const +{ + return this->dataPtr->odeSlip; +} + +///////////////////////////////////////////////// +void Torsional::SetODESlip(double _slip) +{ + this->dataPtr->odeSlip = _slip; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Torsional::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +BulletFriction::BulletFriction() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors BulletFriction::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a BulletFriction, but the provided SDF " + "element is null."}); + return errors; + } + + // Check that the provided SDF element is a + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "bullet") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a BulletFriction, but the provided SDF element " + "is not a ."}); + return errors; + } + + this->dataPtr->friction = _sdf->Get( + "friction", this->dataPtr->friction).first; + this->dataPtr->friction2 = _sdf->Get( + "friction2", this->dataPtr->friction2).first; + this->dataPtr->fdir1 = _sdf->Get("fdir1", + this->dataPtr->fdir1).first; + this->dataPtr->rollingFriction = _sdf->Get( + "rolling_friction", this->dataPtr->rollingFriction).first; + + return errors; +} + +///////////////////////////////////////////////// +double BulletFriction::Friction() const +{ + return this->dataPtr->friction; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFriction(double _friction) +{ + this->dataPtr->friction = _friction; +} + +///////////////////////////////////////////////// +double BulletFriction::Friction2() const +{ + return this->dataPtr->friction2; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFriction2(double _friction2) +{ + this->dataPtr->friction2 = _friction2; +} + +///////////////////////////////////////////////// +const gz::math::Vector3d &BulletFriction::Fdir1() const +{ + return this->dataPtr->fdir1; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFdir1(const gz::math::Vector3d &_fdir) +{ + this->dataPtr->fdir1 = _fdir; +} + +///////////////////////////////////////////////// +double BulletFriction::RollingFriction() const +{ + return this->dataPtr->rollingFriction; +} + +///////////////////////////////////////////////// +void BulletFriction::SetRollingFriction(double _rollingFriction) +{ + this->dataPtr->rollingFriction = _rollingFriction; +} + +///////////////////////////////////////////////// +sdf::ElementPtr BulletFriction::Element() const +{ + return this->dataPtr->sdf; +} ///////////////////////////////////////////////// ODE::ODE() @@ -235,6 +507,20 @@ Errors Friction::Load(ElementPtr _sdf) errors.insert(errors.end(), err.begin(), err.end()); } + if (_sdf->HasElement("bullet")) + { + this->dataPtr->bullet.emplace(); + Errors err = this->dataPtr->bullet->Load(_sdf->GetElement("bullet")); + errors.insert(errors.end(), err.begin(), err.end()); + } + + if (_sdf->HasElement("torsional")) + { + this->dataPtr->torsional.emplace(); + Errors err = this->dataPtr->torsional->Load(_sdf->GetElement("torsional")); + errors.insert(errors.end(), err.begin(), err.end()); + } + return errors; } @@ -256,6 +542,30 @@ const sdf::ODE *Friction::ODE() const return &this->dataPtr->ode; } +///////////////////////////////////////////////// +void Friction::SetBulletFriction(const sdf::BulletFriction &_bullet) +{ + this->dataPtr->bullet = _bullet; +} + +///////////////////////////////////////////////// +const sdf::BulletFriction *Friction::BulletFriction() const +{ + return optionalToPointer(this->dataPtr->bullet); +} + +///////////////////////////////////////////////// +void Friction::SetTorsional(const sdf::Torsional &_torsional) +{ + this->dataPtr->torsional = _torsional; +} + +///////////////////////////////////////////////// +const sdf::Torsional *Friction::Torsional() const +{ + return optionalToPointer(this->dataPtr->torsional); +} + ///////////////////////////////////////////////// Contact::Contact() : dataPtr(gz::utils::MakeImpl()) @@ -429,6 +739,35 @@ sdf::ElementPtr Surface::ToElement(sdf::Errors &_errors) const ode->GetElement("fdir1", _errors)->Set( _errors, this->dataPtr->friction.ODE()->Fdir1()); + if (this->dataPtr->friction.BulletFriction()) + { + sdf::ElementPtr bullet = frictionElem->GetElement("bullet"); + bullet->GetElement("friction")->Set( + this->dataPtr->friction.BulletFriction()->Friction()); + bullet->GetElement("friction2")->Set( + this->dataPtr->friction.BulletFriction()->Friction2()); + bullet->GetElement("fdir1")->Set( + this->dataPtr->friction.BulletFriction()->Fdir1()); + bullet->GetElement("rolling_friction")->Set( + this->dataPtr->friction.BulletFriction()->RollingFriction()); + } + + if (this->dataPtr->friction.Torsional()) + { + sdf::ElementPtr torsional = frictionElem->GetElement("torsional"); + torsional->GetElement("coefficient")->Set( + this->dataPtr->friction.Torsional()->Coefficient()); + torsional->GetElement("use_patch_radius")->Set( + this->dataPtr->friction.Torsional()->UsePatchRadius()); + torsional->GetElement("patch_radius")->Set( + this->dataPtr->friction.Torsional()->PatchRadius()); + torsional->GetElement("surface_radius")->Set( + this->dataPtr->friction.Torsional()->SurfaceRadius()); + + torsional->GetElement("ode")->GetElement("slip")->Set( + this->dataPtr->friction.Torsional()->ODESlip()); + } + return elem; } diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index 8498be8f1..a7b2341d6 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include "sdf/Surface.hh" #include "test_utils.hh" @@ -150,6 +151,19 @@ TEST(DOMsurface, ToElement) ode.SetSlip2(4); ode.SetFdir1(gz::math::Vector3d(1, 2, 3)); friction.SetODE(ode); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction.SetBulletFriction(bullet); + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction.SetTorsional(torsional); contact.SetCollideBitmask(0x12); surface1.SetContact(contact); surface1.SetFriction(friction); @@ -168,6 +182,18 @@ TEST(DOMsurface, ToElement) EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + EXPECT_DOUBLE_EQ(0.3, surface2.Friction()->BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, surface2.Friction()->BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), + surface2.Friction()->BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, + surface2.Friction()->BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, surface2.Friction()->Torsional()->Coefficient()); + EXPECT_FALSE(surface2.Friction()->Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, surface2.Friction()->Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, surface2.Friction()->Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, surface2.Friction()->Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -287,13 +313,39 @@ TEST(DOMfriction, SetFriction) ode1.SetFdir1(gz::math::Vector3d(1, 2, 3)); sdf::Friction friction1; friction1.SetODE(ode1); - EXPECT_EQ(nullptr, friction1.Element()); + + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 0.2); EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 3); EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 4); EXPECT_EQ(friction1.ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction1.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction1.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction1.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction1.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction1.Torsional()->Coefficient()); + EXPECT_FALSE(friction1.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction1.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction1.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction1.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -308,6 +360,21 @@ TEST(DOMfriction, CopyOperator) sdf::Friction friction1; friction1.SetODE(ode1); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + sdf::Friction friction2(friction1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); @@ -315,6 +382,17 @@ TEST(DOMfriction, CopyOperator) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -329,6 +407,21 @@ TEST(DOMfriction, CopyAssignmentOperator) sdf::Friction friction1; friction1.SetODE(ode1); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + sdf::Friction friction2 = friction1; EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); @@ -336,6 +429,17 @@ TEST(DOMfriction, CopyAssignmentOperator) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -359,6 +463,36 @@ TEST(DOMfriction, CopyAssignmentAfterMove) sdf::Friction friction2; friction2.SetODE(ode2); + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.3); + bullet1.SetFriction2(0.5); + bullet1.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet1.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet1); + + sdf::BulletFriction bullet2; + bullet2.SetFriction(0.1); + bullet2.SetFriction2(0.2); + bullet2.SetFdir1(gz::math::Vector3d(3, 4, 5)); + bullet2.SetRollingFriction(3.1); + friction2.SetBulletFriction(bullet2); + + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.5); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.1); + torsional1.SetSurfaceRadius(0.3); + torsional1.SetODESlip(0.2); + friction1.SetTorsional(torsional1); + + sdf::Torsional torsional2; + torsional2.SetCoefficient(1.5); + torsional2.SetUsePatchRadius(true); + torsional2.SetPatchRadius(1.1); + torsional2.SetSurfaceRadius(3.3); + torsional2.SetODESlip(2.2); + friction2.SetTorsional(torsional2); + sdf::Friction tmp = std::move(friction1); friction1 = friction2; friction2 = tmp; @@ -375,34 +509,26 @@ TEST(DOMfriction, CopyAssignmentAfterMove) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); -} - -///////////////////////////////////////////////// -TEST(DOMfriction, Set) -{ - sdf::ODE ode1; - sdf::Friction friction1; - - EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 1.0); - EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 1.0); - EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 0); - EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 0); - EXPECT_EQ(friction1.ODE()->Fdir1(), - gz::math::Vector3d(0, 0, 0)); - ode1.SetMu(0.1); - ode1.SetMu2(0.2); - ode1.SetSlip1(3); - ode1.SetSlip2(4); - ode1.SetFdir1(gz::math::Vector3d(1, 2, 3)); - friction1.SetODE(ode1); - - EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 0.1); - EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 0.2); - EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 3); - EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 4); - EXPECT_EQ(friction1.ODE()->Fdir1(), - gz::math::Vector3d(1, 2, 3)); + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + EXPECT_DOUBLE_EQ(0.1, friction1.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.2, friction1.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(3, 4, 5), friction1.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(3.1, friction1.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); + EXPECT_DOUBLE_EQ(1.5, friction1.Torsional()->Coefficient()); + EXPECT_TRUE(friction1.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.1, friction1.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(3.3, friction1.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(2.2, friction1.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -515,3 +641,204 @@ TEST(DOMode, Set) EXPECT_EQ(ode1.Fdir1(), gz::math::Vector3d(1, 2, 3)); } + +///////////////////////////////////////////////// +TEST(DOMbullet, DefaultValues) +{ + sdf::BulletFriction bullet; + EXPECT_EQ(nullptr, bullet.Element()); + EXPECT_DOUBLE_EQ(1.0, bullet.Friction()); + EXPECT_DOUBLE_EQ(1.0, bullet.Friction2()); + EXPECT_EQ(gz::math::Vector3d(0, 0, 0), bullet.Fdir1()); + EXPECT_DOUBLE_EQ(1.0, bullet.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyOperator) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2(bullet1); + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyAssignmentOperator) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2 = bullet1; + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyAssignmentAfterMove) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2; + bullet2.SetFriction(0.2); + bullet2.SetFriction2(0.1); + bullet2.SetFdir1(gz::math::Vector3d(3, 2, 1)); + bullet2.SetRollingFriction(3.0); + + sdf::BulletFriction tmp = std::move(bullet1); + bullet1 = bullet2; + bullet2 = tmp; + + EXPECT_DOUBLE_EQ(0.2, bullet1.Friction()); + EXPECT_DOUBLE_EQ(0.1, bullet1.Friction2()); + EXPECT_EQ(gz::math::Vector3d(3, 2, 1), bullet1.Fdir1()); + EXPECT_DOUBLE_EQ(3.0, bullet1.RollingFriction()); + + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, Set) +{ + sdf::BulletFriction bullet1; + + EXPECT_DOUBLE_EQ(bullet1.Friction(), 1.0); + EXPECT_DOUBLE_EQ(bullet1.Friction2(), 1.0); + EXPECT_EQ(bullet1.Fdir1(), + gz::math::Vector3d(0, 0, 0)); + EXPECT_DOUBLE_EQ(bullet1.RollingFriction(), 1.0); + + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4); + + EXPECT_DOUBLE_EQ(0.1, bullet1.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet1.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet1.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet1.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, DefaultValues) +{ + sdf::Torsional torsional; + EXPECT_EQ(nullptr, torsional.Element()); + EXPECT_DOUBLE_EQ(1.0, torsional.Coefficient()); + EXPECT_TRUE(torsional.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.PatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyOperator) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2(torsional1); + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyAssignmentOperator) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2 = torsional1; + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyAssignmentAfterMove) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2; + torsional2.SetCoefficient(1.1); + torsional2.SetUsePatchRadius(true); + torsional2.SetPatchRadius(1.2); + torsional2.SetSurfaceRadius(4.1); + torsional2.SetODESlip(1.1); + + sdf::Torsional tmp = std::move(torsional1); + torsional1 = torsional2; + torsional2 = tmp; + + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); + + EXPECT_DOUBLE_EQ(1.1, torsional1.Coefficient()); + EXPECT_TRUE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.2, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(4.1, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.1, torsional1.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, Set) +{ + sdf::Torsional torsional1; + + EXPECT_DOUBLE_EQ(1.0, torsional1.Coefficient()); + EXPECT_TRUE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.ODESlip()); + + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4); + torsional1.SetODESlip(1.0); + + EXPECT_DOUBLE_EQ(0.1, torsional1.Coefficient()); + EXPECT_FALSE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional1.ODESlip()); +} diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index 9b3cf83aa..f1f7faf94 100644 --- a/test/integration/surface_dom.cc +++ b/test/integration/surface_dom.cc @@ -56,4 +56,22 @@ TEST(DOMSurface, Shapes) EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Slip2(), 5); EXPECT_EQ(boxCol->Surface()->Friction()->ODE()->Fdir1(), gz::math::Vector3d(1.2, 3.4, 5.6)); + + EXPECT_DOUBLE_EQ(1.6, + boxCol->Surface()->Friction()->BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(1.7, + boxCol->Surface()->Friction()->BulletFriction()->Friction2()); + EXPECT_EQ(boxCol->Surface()->Friction()->BulletFriction()->Fdir1(), + gz::math::Vector3d(6.5, 4.3, 2.1)); + EXPECT_DOUBLE_EQ(1.5, + boxCol->Surface()->Friction()->BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(5.1, + boxCol->Surface()->Friction()->Torsional()->Coefficient()); + EXPECT_FALSE(boxCol->Surface()->Friction()->Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.3, + boxCol->Surface()->Friction()->Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(3.7, + boxCol->Surface()->Friction()->Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(3.1, boxCol->Surface()->Friction()->Torsional()->ODESlip()); } diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index f411afa37..036348ef2 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -14,12 +14,27 @@ - 0.6 - 0.7 - 4 - 5 - 1.2 3.4 5.6 + 0.6 + 0.7 + 4 + 5 + 1.2 3.4 5.6 + + 1.6 + 1.7 + 6.5 4.3 2.1 + 1.5 + + + 5.1 + false + 1.3 + 3.7 + + 3.1 + + From 6d4141ef9a9d878f78413112008281a0443ee2e9 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 31 May 2024 02:12:00 +0900 Subject: [PATCH 16/23] Add python bindings for bullet and torsional friction (#1427) Signed-off-by: Ian Chen --- python/src/sdf/_gz_sdformat_pybind11.cc | 2 + python/src/sdf/pySurface.cc | 72 +++++++++ python/src/sdf/pySurface.hh | 12 ++ python/test/pySurface_TEST.py | 202 +++++++++++++++++++++++- 4 files changed, 287 insertions(+), 1 deletion(-) diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 298cd1362..0a2c9fff4 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -83,6 +83,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineAltimeter(m); sdf::python::defineAtmosphere(m); sdf::python::defineBox(m); + sdf::python::defineBulletFriction(m); sdf::python::defineCamera(m); sdf::python::defineCapsule(m); sdf::python::defineCollision(m); @@ -134,6 +135,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineSky(m); sdf::python::defineSphere(m); sdf::python::defineSurface(m); + sdf::python::defineTorsional(m); sdf::python::defineVisual(m); sdf::python::defineWorld(m); diff --git a/python/src/sdf/pySurface.cc b/python/src/sdf/pySurface.cc index dc7be622b..ed8545e95 100644 --- a/python/src/sdf/pySurface.cc +++ b/python/src/sdf/pySurface.cc @@ -55,6 +55,16 @@ void defineFriction(pybind11::object module) "Get the ODE object.") .def("set_ode", &sdf::Friction::SetODE, "Set the ODE object.") + .def("bullet_friction", &sdf::Friction::BulletFriction, + pybind11::return_value_policy::reference_internal, + "Get the bullet friction object.") + .def("set_bullet_friction", &sdf::Friction::SetBulletFriction, + "Set the bullet friction object.") + .def("torsional", &sdf::Friction::Torsional, + pybind11::return_value_policy::reference_internal, + "Get the torsional friction object.") + .def("set_torsional", &sdf::Friction::SetTorsional, + "Set the torsional friction object.") .def("__copy__", [](const sdf::Friction &self) { return sdf::Friction(self); }) @@ -118,6 +128,68 @@ void defineSurface(pybind11::object module) return sdf::Surface(self); }, "memo"_a); } +///////////////////////////////////////////////// +void defineBulletFriction(pybind11::object module) +{ + pybind11::class_(module, "BulletFriction") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("friction", &sdf::BulletFriction::Friction, + "Get the friction parameter.") + .def("set_friction", &sdf::BulletFriction::SetFriction, + "Set the friction parameter.") + .def("friction2", &sdf::BulletFriction::Friction2, + "Get the friction parameter.") + .def("set_friction2", &sdf::BulletFriction::SetFriction2, + "Set the friction2 parameter.") + .def("fdir1", &sdf::BulletFriction::Fdir1, + "Get the fdir1 parameter.") + .def("set_fdir1", &sdf::BulletFriction::SetFdir1, + "Set the fdir1 parameter.") + .def("rolling_friction", &sdf::BulletFriction::RollingFriction, + "Get the rolling fricion parameter.") + .def("set_rolling_friction", &sdf::BulletFriction::SetRollingFriction, + "Set the rolling friction parameter.") + .def("__copy__", [](const sdf::BulletFriction &self) { + return sdf::BulletFriction(self); + }) + .def("__deepcopy__", [](const sdf::BulletFriction &self, pybind11::dict) { + return sdf::BulletFriction(self); + }, "memo"_a); +} +///////////////////////////////////////////////// +void defineTorsional(pybind11::object module) +{ + pybind11::class_(module, "Torsional") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("coefficient", &sdf::Torsional::Coefficient, + "Get the coefficient parameter.") + .def("set_coefficient", &sdf::Torsional::SetCoefficient, + "Set the coefficient parameter.") + .def("use_patch_radius", &sdf::Torsional::UsePatchRadius, + "Get whether to use patch radius for torsional friction calculation.") + .def("set_use_patch_radius", &sdf::Torsional::SetUsePatchRadius, + "Set whether to use patch radius for torsional friction calculation.") + .def("patch_radius", &sdf::Torsional::PatchRadius, + "Get the radius of contact patch surface.") + .def("set_patch_radius", &sdf::Torsional::SetPatchRadius, + "Set the radius of contact patch surface.") + .def("surface_radius", &sdf::Torsional::SurfaceRadius, + "Get the surface radius on the contact point.") + .def("set_surface_radius", &sdf::Torsional::SetSurfaceRadius, + "Set the surface radius on the contact point.") + .def("ode_slip", &sdf::Torsional::ODESlip, + "Get the ODE force dependent slip for torsional friction.") + .def("set_ode_slip", &sdf::Torsional::SetODESlip, + "Set the ODE force dependent slip for torsional friction.") + .def("__copy__", [](const sdf::Torsional &self) { + return sdf::Torsional(self); + }) + .def("__deepcopy__", [](const sdf::Torsional &self, pybind11::dict) { + return sdf::Torsional(self); + }, "memo"_a); +} } // namespace python } // namespace SDF_VERSION_NAMESPACE } // namespace sdf diff --git a/python/src/sdf/pySurface.hh b/python/src/sdf/pySurface.hh index 75e29f691..b45b547a2 100644 --- a/python/src/sdf/pySurface.hh +++ b/python/src/sdf/pySurface.hh @@ -52,6 +52,18 @@ void defineODE(pybind11::object module); * \param[in] module a pybind11 module to add the definition to */ void defineSurface(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::BulletFriction +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineBulletFriction(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::Torsional +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineTorsional(pybind11::object module); } // namespace python } // namespace SDF_VERSION_NAMESPACE } // namespace sdf diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index 2651b1113..ed6875c0d 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -14,7 +14,8 @@ import copy from gz_test_deps.math import Vector3d -from gz_test_deps.sdformat import Surface, Contact, Friction, ODE +from gz_test_deps.sdformat import Surface, Contact, Friction, ODE, \ + BulletFriction, Torsional import unittest @@ -34,8 +35,24 @@ def test_assigment_construction(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) + friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -48,6 +65,17 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -60,6 +88,20 @@ def test_assigment_construction(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) + surface1.set_friction(friction) self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) @@ -74,6 +116,29 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1.1, 2.1, 3.1)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.03) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.7) def test_copy_construction(self): surface1 = Surface() @@ -84,8 +149,21 @@ def test_copy_construction(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -98,6 +176,17 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -110,6 +199,20 @@ def test_copy_construction(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) + surface1.set_friction(friction) self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) @@ -124,6 +227,29 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) def test_deepcopy(self): surface1 = Surface() @@ -134,8 +260,21 @@ def test_deepcopy(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -148,6 +287,17 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -160,7 +310,19 @@ def test_deepcopy(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) surface1.set_friction(friction) + self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) self.assertEqual(surface1.friction().ode().slip1(), 3.1) @@ -174,6 +336,29 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) def test_default_contact_construction(self): contact = Contact() @@ -196,6 +381,21 @@ def test_default_ode_construction(self): self.assertEqual(ode.fdir1(), Vector3d(0, 0, 0)) + def test_default_bullet_friction_construction(self): + bullet = BulletFriction() + self.assertEqual(bullet.friction(), 1.0) + self.assertEqual(bullet.friction2(), 1.0) + self.assertEqual(bullet.fdir1(), + Vector3d(0, 0, 0)) + self.assertEqual(bullet.rolling_friction(), 1.0) + + def test_default_torsional_construction(self): + torsional = Torsional() + self.assertEqual(torsional.coefficient(), 1.0) + self.assertTrue(torsional.use_patch_radius()) + self.assertEqual(torsional.patch_radius(), 0.0) + self.assertEqual(torsional.surface_radius(), 0.0) + self.assertEqual(torsional.ode_slip(), 0.0) if __name__ == '__main__': unittest.main() From 8c0b857cd4bdeac52f890b76e5603f441c1f2f49 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 28 May 2024 17:57:37 +0900 Subject: [PATCH 17/23] Backport voxel_resolution sdf element Signed-off-by: Ian Chen --- include/sdf/Mesh.hh | 12 ++++++++++++ python/src/sdf/pyConvexDecomposition.cc | 7 +++++++ python/test/pyMesh_TEST.py | 6 ++++++ sdf/1.11/mesh_shape.sdf | 3 +++ src/Mesh.cc | 22 ++++++++++++++++++++++ src/Mesh_TEST.cc | 6 ++++++ test/integration/geometry_dom.cc | 1 + test/sdf/shapes.sdf | 1 + 8 files changed, 58 insertions(+) diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 7cc48c955..c77cb7bff 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -69,11 +69,23 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Get the maximum number of convex hulls that can be generated. + /// \return Maximum number of convex hulls. public: unsigned int MaxConvexHulls() const; /// \brief Set the maximum number of convex hulls that can be generated. + /// \param[in] _maxConvexHulls Maximum number of convex hulls. public: void SetMaxConvexHulls(unsigned int _maxConvexHulls); + /// Get the voxel resolution to use for representing the mesh. Applicable + /// only to voxel based convex decomposition methods. + /// \param[in] _voxelResolution Voxel resolution to use. + public: unsigned int VoxelResolution() const; + + /// Set the voxel resolution to use for representing the mesh. Applicable + /// only to voxel based convex decomposition methods. + /// \param[in] _voxelResolution Voxel resolution to use. + public: void SetVoxelResolution(unsigned int _voxelResolution); + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/python/src/sdf/pyConvexDecomposition.cc b/python/src/sdf/pyConvexDecomposition.cc index aacd7ee42..c3cefd5d8 100644 --- a/python/src/sdf/pyConvexDecomposition.cc +++ b/python/src/sdf/pyConvexDecomposition.cc @@ -38,6 +38,13 @@ void defineConvexDecomposition(pybind11::object module) "Get the maximum number of convex hulls that can be generated.") .def("set_max_convex_hulls", &sdf::ConvexDecomposition::SetMaxConvexHulls, "Set the maximum number of convex hulls that can be generated.") + .def("voxel_resolution", &sdf::ConvexDecomposition::VoxelResolution, + "Get the voxel resolution to use. Applicable only to voxel based " + "convex decomposition methods.") + .def("set_voxel_resolution", &sdf::ConvexDecomposition::SetVoxelResolution, + "Set the voxel resolution to use. Applicable only to voxel based " + "convex decomposition methods.") + .def("__copy__", [](const sdf::ConvexDecomposition &self) { return sdf::ConvexDecomposition(self); }) diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index ac9840ea6..0990095d6 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -45,6 +45,7 @@ def test_assigment(self): convexDecomp = ConvexDecomposition() convexDecomp.set_max_convex_hulls(10) + convexDecomp.set_voxel_resolution(100000) mesh.set_convex_decomposition(convexDecomp) mesh2 = mesh @@ -58,6 +59,7 @@ def test_assigment(self): convexDecomp2 = mesh2.convex_decomposition() self.assertEqual(10, convexDecomp2.max_convex_hulls()) + self.assertEqual(100000, convexDecomp2.voxel_resolution()) mesh.set_file_path("/apple") self.assertEqual("/apple", mesh2.file_path()) @@ -86,6 +88,7 @@ def test_deepcopy_construction(self): convexDecomp = ConvexDecomposition() convexDecomp.set_max_convex_hulls(10) + convexDecomp.set_voxel_resolution(100000) mesh.set_convex_decomposition(convexDecomp) mesh2 = copy.deepcopy(mesh) @@ -99,6 +102,7 @@ def test_deepcopy_construction(self): convexDecomp2 = mesh2.convex_decomposition() self.assertEqual(10, convexDecomp2.max_convex_hulls()) + self.assertEqual(100000, convexDecomp2.voxel_resolution()) mesh.set_file_path("/apple") mesh.set_scale(Vector3d(0.3, 0.2, 0.4)) @@ -131,8 +135,10 @@ def test_set(self): convexDecomp = ConvexDecomposition() convexDecomp.set_max_convex_hulls(10) + convexDecomp.set_voxel_resolution(100000) mesh.set_convex_decomposition(convexDecomp) self.assertEqual(10, mesh.convex_decomposition().max_convex_hulls()) + self.assertEqual(100000, mesh.convex_decomposition().voxel_resolution()) self.assertEqual("", mesh.uri()) mesh.set_uri("http://myuri.com") diff --git a/sdf/1.11/mesh_shape.sdf b/sdf/1.11/mesh_shape.sdf index 1ed64ca44..38754e5f5 100644 --- a/sdf/1.11/mesh_shape.sdf +++ b/sdf/1.11/mesh_shape.sdf @@ -12,6 +12,9 @@ Maximum number of convex hulls to decompose into. This sets the maximum number of submeshes that the final decomposed mesh will contain. + + The number of voxels to use for representing the source mesh before decomposition. Applicable only to voxel based convex decomposition methods. + diff --git a/src/Mesh.cc b/src/Mesh.cc index f026e2d63..d2ee4b7a8 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -45,6 +45,10 @@ class sdf::ConvexDecomposition::Implementation /// \brief Maximum number of convex hulls to generate. public: unsigned int maxConvexHulls{16u}; + /// \brief Voxel resolution. Applicable only to voxel based methods for + /// representing the mesh before decomposition + public: unsigned int voxelResolution{200000u}; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf = nullptr; }; @@ -112,6 +116,10 @@ Errors ConvexDecomposition::Load(ElementPtr _sdf) errors, "max_convex_hulls", this->dataPtr->maxConvexHulls).first; + this->dataPtr->voxelResolution = _sdf->Get( + errors, "voxel_resolution", + this->dataPtr->voxelResolution).first; + return errors; } @@ -133,6 +141,18 @@ void ConvexDecomposition::SetMaxConvexHulls(unsigned int _maxConvexHulls) this->dataPtr->maxConvexHulls = _maxConvexHulls; } +///////////////////////////////////////////////// +unsigned int ConvexDecomposition::VoxelResolution() const +{ + return this->dataPtr->voxelResolution; +} + +///////////////////////////////////////////////// +void ConvexDecomposition::SetVoxelResolution(unsigned int _voxelResolution) +{ + this->dataPtr->voxelResolution = _voxelResolution; +} + ///////////////////////////////////////////////// Mesh::Mesh() : dataPtr(gz::utils::MakeImpl()) @@ -403,6 +423,8 @@ sdf::ElementPtr Mesh::ToElement(sdf::Errors &_errors) const _errors); convexDecomp->GetElement("max_convex_hulls")->Set( this->dataPtr->convexDecomposition->MaxConvexHulls()); + convexDecomp->GetElement("voxel_resolution")->Set( + this->dataPtr->convexDecomposition->VoxelResolution()); } // Uri diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 29ef70d85..391283976 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -58,6 +58,7 @@ TEST(DOMMesh, MoveConstructor) sdf::ConvexDecomposition convexDecomp; EXPECT_EQ(nullptr, convexDecomp.Element()); convexDecomp.SetMaxConvexHulls(10u); + convexDecomp.SetVoxelResolution(100000u); mesh.SetConvexDecomposition(convexDecomp); sdf::Mesh mesh2(std::move(mesh)); @@ -72,6 +73,7 @@ TEST(DOMMesh, MoveConstructor) auto convexDecomp2 = mesh2.ConvexDecomposition(); ASSERT_NE(nullptr, convexDecomp2); EXPECT_EQ(10u, convexDecomp2->MaxConvexHulls()); + EXPECT_EQ(100000u, convexDecomp2->VoxelResolution()); } ///////////////////////////////////////////////// @@ -186,9 +188,11 @@ TEST(DOMMesh, Set) sdf::ConvexDecomposition convexDecomp; convexDecomp.SetMaxConvexHulls(10u); + convexDecomp.SetVoxelResolution(200000u); mesh.SetConvexDecomposition(convexDecomp); ASSERT_NE(nullptr, mesh.ConvexDecomposition()); EXPECT_EQ(10u, mesh.ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ(200000u, mesh.ConvexDecomposition()->VoxelResolution()); EXPECT_EQ(std::string(), mesh.Uri()); mesh.SetUri("http://myuri.com"); @@ -365,6 +369,7 @@ TEST(DOMMesh, ToElement) sdf::ConvexDecomposition convexDecomp; convexDecomp.SetMaxConvexHulls(10u); + convexDecomp.SetVoxelResolution(300000u); mesh.SetConvexDecomposition(convexDecomp); sdf::ElementPtr elem = mesh.ToElement(); @@ -381,6 +386,7 @@ TEST(DOMMesh, ToElement) EXPECT_EQ(mesh.CenterSubmesh(), mesh2.CenterSubmesh()); ASSERT_NE(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ(10u, mesh2.ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ(300000u, mesh2.ConvexDecomposition()->VoxelResolution()); } ///////////////////////////////////////////////// diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 297d1f0af..419a04dc9 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -184,6 +184,7 @@ TEST(DOMGeometry, Shapes) meshColGeom->Optimization()); ASSERT_NE(nullptr, meshColGeom->ConvexDecomposition()); EXPECT_EQ(4u, meshColGeom->ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ(400000u, meshColGeom->ConvexDecomposition()->VoxelResolution()); EXPECT_EQ("https://fuel.gazebosim.org/1.0/an_org/models/a_model/mesh/" "mesh.dae", meshColGeom->Uri()); diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 036348ef2..37c56277c 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -138,6 +138,7 @@ 4 + 400000 https://fuel.gazebosim.org/1.0/an_org/models/a_model/mesh/mesh.dae From 128667576be386ba5e538ea343765ff8f5660bc0 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 13 Jun 2024 00:32:53 -0700 Subject: [PATCH 18/23] Prepare for 12.8.0 release (#1430) (#1432) Signed-off-by: Ian Chen --- Changelog.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/Changelog.md b/Changelog.md index ecb491ea1..684128366 100644 --- a/Changelog.md +++ b/Changelog.md @@ -590,6 +590,21 @@ ## libsdformat 12.X +### libsdformat 12.8.0 (2024-06-06) + +1. Add support for no gravity link + * [Pull request #1410](https://github.com/gazebosim/sdformat/pull/1410) + +1. Add bullet and torsional friction DOM + * [Pull request #1351](https://github.com/gazebosim/sdformat/pull/1351) + +1. Fix static builds and optimize test compilation + * [Pull request #1343](https://github.com/gazebosim/sdformat/pull/1343) + * [Pull request #1347](https://github.com/gazebosim/sdformat/pull/1347) + +1. Update github action workflows + * [Pull request #1345](https://github.com/gazebosim/sdformat/pull/1345) + ### libsdformat 12.7.2 (2023-09-01) 1. Fixed 1.9/light.sdf From 43ba646de34db3a607452a37a56ebb533b12fb6e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 14 Jun 2024 12:55:53 -0700 Subject: [PATCH 19/23] Prepare for 14.3.0 release (#1437) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 32 ++++++++++++++++++++++++++++++++ package.xml | 2 +- 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f771a9c65..877198921 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0004 NEW) endif(COMMAND CMAKE_POLICY) -project (sdformat14 VERSION 14.2.0) +project (sdformat14 VERSION 14.3.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software diff --git a/Changelog.md b/Changelog.md index 684128366..0c375bb86 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,37 @@ ## libsdformat 14.X +### libsdformat 14.3.0 (2024-06-14) + +1. Backport voxel_resolution sdf element + * [Pull request #1429](https://github.com/gazebosim/sdformat/pull/1429) + +1. Added Automatic Moment of Inertia Calculations for Basic Shapes Python wrappers + * [Pull request #1424](https://github.com/gazebosim/sdformat/pull/1424) + +1. Add support for no gravity link + * [Pull request #1410](https://github.com/gazebosim/sdformat/pull/1410) + * [Pull request #1419](https://github.com/gazebosim/sdformat/pull/1419) + +1. Update default camera instrinsics skew to 0, which matches spec + * [Pull request #1423](https://github.com/gazebosim/sdformat/pull/1423) + * [Pull request #1425](https://github.com/gazebosim/sdformat/pull/1425) + +1. Allow empty strings in plugin and custom attributes + * [Pull request #1407](https://github.com/gazebosim/sdformat/pull/1407) + +1. (Backport) Enable 24.04 CI, remove distutils dependency + * [Pull request #1413](https://github.com/gazebosim/sdformat/pull/1413) + +1. Fix macOS workflow and backport windows fix + * [Pull request #1409](https://github.com/gazebosim/sdformat/pull/1409) + +1. Fix warning with pybind11 2.12 + * [Pull request #1389](https://github.com/gazebosim/sdformat/pull/1389) + +1. Add bullet and torsional friction DOM + * [Pull request #1351](https://github.com/gazebosim/sdformat/pull/1351) + * [Pull request #1427](https://github.com/gazebosim/sdformat/pull/1427) + ### libsdformat 14.2.0 (2024-04-23) 1. Fix trivial warning on 24.04 for JointAxis_TEST.cc diff --git a/package.xml b/package.xml index 3132f5f8f..b065874cd 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ sdformat14 - 14.2.0 + 14.3.0 SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications From f0dfc5faceca0c0510132aa7f4d221de990bb234 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 18 Jun 2024 21:32:59 -0700 Subject: [PATCH 20/23] Add custom attribute to custom element in test (#1406) Update test to confirm that #54 is fixed. Signed-off-by: Steve Peters --- test/integration/custom_elems_attrs.sdf | 2 +- test/integration/sdf_custom.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/custom_elems_attrs.sdf b/test/integration/custom_elems_attrs.sdf index 54c9f9c9e..e936450e1 100644 --- a/test/integration/custom_elems_attrs.sdf +++ b/test/integration/custom_elems_attrs.sdf @@ -10,7 +10,7 @@ L2 - + transmission_interface/SimpleTransmission EffortJointInterface diff --git a/test/integration/sdf_custom.cc b/test/integration/sdf_custom.cc index cdf1be1ee..ef04256b9 100644 --- a/test/integration/sdf_custom.cc +++ b/test/integration/sdf_custom.cc @@ -139,7 +139,7 @@ TEST(SDFParser, ReloadCustomElements) ASSERT_NE(nullptr, customElem2); const std::string customElemStr = -R"( +R"( transmission_interface/SimpleTransmission EffortJointInterface From c3e5690b79d4c4d7a2635c297a2d0bac88da95ee Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 20 Jun 2024 14:24:32 -0400 Subject: [PATCH 21/23] Add Cone as a primitive parametric shape. (#1415) * Backport: Add cone shape to SDFormat spec (#1418) Signed-off-by: Benjamin Perseghetti Co-authored-by: Steve Peters --- include/sdf/Cone.hh | 108 +++++++++ include/sdf/Geometry.hh | 15 ++ include/sdf/Visual.hh | 1 + python/CMakeLists.txt | 2 + python/src/sdf/_gz_sdformat_pybind11.cc | 2 + python/src/sdf/pyCone.cc | 60 +++++ python/src/sdf/pyCone.hh | 43 ++++ python/src/sdf/pyGeometry.cc | 8 + python/test/pyCollision_TEST.py | 3 +- python/test/pyCone_TEST.py | 114 ++++++++++ python/test/pyGeometry_TEST.py | 20 +- python/test/pyVisual_TEST.py | 1 + sdf/1.11/CMakeLists.txt | 1 + sdf/1.11/cone_shape.sdf | 9 + sdf/1.11/geometry.sdf | 1 + src/Collision_TEST.cc | 1 + src/Cone.cc | 189 ++++++++++++++++ src/Cone_TEST.cc | 288 ++++++++++++++++++++++++ src/Geometry.cc | 30 +++ src/Geometry_TEST.cc | 126 +++++++++++ src/Visual_TEST.cc | 1 + test/integration/geometry_dom.cc | 21 ++ test/sdf/basic_shapes.sdf | 25 +- test/sdf/shapes.sdf | 20 +- test/sdf/shapes_world.sdf | 25 +- 25 files changed, 1109 insertions(+), 5 deletions(-) create mode 100644 include/sdf/Cone.hh create mode 100644 python/src/sdf/pyCone.cc create mode 100644 python/src/sdf/pyCone.hh create mode 100644 python/test/pyCone_TEST.py create mode 100644 sdf/1.11/cone_shape.sdf create mode 100644 src/Cone.cc create mode 100644 src/Cone_TEST.cc diff --git a/include/sdf/Cone.hh b/include/sdf/Cone.hh new file mode 100644 index 000000000..641c5a304 --- /dev/null +++ b/include/sdf/Cone.hh @@ -0,0 +1,108 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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_CONE_HH_ +#define SDF_CONE_HH_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + /// \brief Cone represents a cone shape, and is usually accessed + /// through a Geometry. + class SDFORMAT_VISIBLE Cone + { + /// \brief Constructor + public: Cone(); + + /// \brief Load the cone geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the cone's radius in meters. + /// \return The radius of the cone in meters. + public: double Radius() const; + + /// \brief Set the cone's radius in meters. + /// \param[in] _radius The radius of the cone in meters. + public: void SetRadius(double _radius); + + /// \brief Get the cone's length in meters. + /// \return The length of the cone in meters. + public: double Length() const; + + /// \brief Set the cone's length in meters. + /// \param[in] _length The length of the cone in meters. + public: void SetLength(double _length); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the Gazebo Math representation of this cone. + /// \return A const reference to a gz::math::Sphered object. + public: const gz::math::Coned &Shape() const; + + /// \brief Get a mutable Gazebo Math representation of this cone. + /// \return A reference to a gz::math::Coned object. + public: gz::math::Coned &Shape(); + + /// \brief Calculate and return the Inertial values for the cone. In + /// order to calculate the inertial properties, the function mutates the + /// object by updating its material properties. + /// \param[in] _density Density of the cone in kg/m^3 + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional + CalculateInertial(double _density); + + /// \brief Create and return an SDF element filled with data from this + /// cone. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \return SDF element pointer with updated cone values. + public: sdf::ElementPtr ToElement() const; + + /// \brief Create and return an SDF element filled with data from this + /// cone. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[out] _errors Vector of errors. + /// \return SDF element pointer with updated cone values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } +} +#endif diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 800dfb9a6..5f5443ec0 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -37,6 +37,7 @@ namespace sdf // Forward declare private data class. class Box; class Capsule; + class Cone; class Cylinder; class Ellipsoid; class Heightmap; @@ -79,6 +80,9 @@ namespace sdf /// \brief A polyline geometry. POLYLINE = 9, + + /// \brief A polyline geometry. + CONE = 10, }; /// \brief Geometry provides access to a shape, such as a Box. Use the @@ -140,6 +144,17 @@ namespace sdf /// \param[in] _capsule The capsule shape. public: void SetCapsuleShape(const Capsule &_capsule); + /// \brief Get the cone geometry, or nullptr if the contained + /// geometry is not a cone. + /// \return Pointer to the visual's cone geometry, or nullptr if the + /// geometry is not a cone. + /// \sa GeometryType Type() const + public: const Cone *ConeShape() const; + + /// \brief Set the cone shape. + /// \param[in] _cone The cone shape. + public: void SetConeShape(const Cone &_cone); + /// \brief Get the cylinder geometry, or nullptr if the contained /// geometry is not a cylinder. /// \return Pointer to the visual's cylinder geometry, or nullptr if the diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index 571cfcdd9..0f5808851 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -22,6 +22,7 @@ #include #include #include "sdf/Box.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Element.hh" #include "sdf/Material.hh" diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f9cc0917d..864d38c18 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -52,6 +52,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/sdf/pyCamera.cc src/sdf/pyCapsule.cc src/sdf/pyCollision.cc + src/sdf/pyCone.cc src/sdf/pyConvexDecomposition.cc src/sdf/pyCylinder.cc src/sdf/pyElement.cc @@ -135,6 +136,7 @@ if (BUILD_TESTING AND NOT WIN32) pyCamera_TEST pyCapsule_TEST pyCollision_TEST + pyCone_TEST pyCylinder_TEST pyElement_TEST pyEllipsoid_TEST diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 0a2c9fff4..80892100e 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -26,6 +26,7 @@ #include "pyCamera.hh" #include "pyCapsule.hh" #include "pyCollision.hh" +#include "pyCone.hh" #include "pyConvexDecomposition.hh" #include "pyCylinder.hh" #include "pyElement.hh" @@ -87,6 +88,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineCamera(m); sdf::python::defineCapsule(m); sdf::python::defineCollision(m); + sdf::python::defineCone(m); sdf::python::defineConvexDecomposition(m); sdf::python::defineContact(m); sdf::python::defineCylinder(m); diff --git a/python/src/sdf/pyCone.cc b/python/src/sdf/pyCone.cc new file mode 100644 index 000000000..05cb9cb15 --- /dev/null +++ b/python/src/sdf/pyCone.cc @@ -0,0 +1,60 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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 "pyCone.hh" + +#include + +#include "sdf/Cone.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineCone(pybind11::object module) +{ + pybind11::class_(module, "Cone") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("radius", &sdf::Cone::Radius, + "Get the cone's radius in meters.") + .def("set_radius", &sdf::Cone::SetRadius, + "Set the cone's radius in meters.") + .def("length", &sdf::Cone::Length, + "Get the cone's length in meters.") + .def("set_length", &sdf::Cone::SetLength, + "Set the cone's length in meters.") + .def( + "shape", + pybind11::overload_cast<>(&sdf::Cone::Shape, pybind11::const_), + pybind11::return_value_policy::reference, + "Get a mutable Gazebo Math representation of this Cone.") + .def("__copy__", [](const sdf::Cone &self) { + return sdf::Cone(self); + }) + .def("__deepcopy__", [](const sdf::Cone &self, pybind11::dict) { + return sdf::Cone(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyCone.hh b/python/src/sdf/pyCone.hh new file mode 100644 index 000000000..1f7f3dc85 --- /dev/null +++ b/python/src/sdf/pyCone.hh @@ -0,0 +1,43 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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 SDFORMAT_PYTHON_CONE_HH_ +#define SDFORMAT_PYTHON_CONE_HH_ + +#include + +#include "sdf/Cone.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Cone +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineCone(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_CONE_HH_ diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index ffcbabbe5..a30d3106f 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -23,6 +23,7 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" @@ -64,6 +65,12 @@ void defineGeometry(pybind11::object module) "geometry is not a capsule.") .def("set_capsule_shape", &sdf::Geometry::SetCapsuleShape, "Set the capsule shape.") + .def("cone_shape", &sdf::Geometry::ConeShape, + pybind11::return_value_policy::reference, + "Get the cone geometry, or None if the contained " + "geometry is not a cone.") + .def("set_cone_shape", &sdf::Geometry::SetConeShape, + "Set the cone shape.") .def("cylinder_shape", &sdf::Geometry::CylinderShape, pybind11::return_value_policy::reference, "Get the cylinder geometry, or None if the contained " @@ -104,6 +111,7 @@ void defineGeometry(pybind11::object module) pybind11::enum_(module, "GeometryType") .value("EMPTY", sdf::GeometryType::EMPTY) .value("BOX", sdf::GeometryType::BOX) + .value("CONE", sdf::GeometryType::CONE) .value("CYLINDER", sdf::GeometryType::CYLINDER) .value("PLANE", sdf::GeometryType::PLANE) .value("SPHERE", sdf::GeometryType::SPHERE) diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index 8d8d96735..7068c53d2 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d -from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error, +from gz_test_deps.sdformat import (Box, Collision, Cone, Contact, Cylinder, Error, Geometry, ParserConfig, Plane, Root, Surface, Sphere, SDFErrorsException) import gz_test_deps.sdformat as sdf @@ -61,6 +61,7 @@ def test_default_construction(self): self.assertNotEqual(None, collision.geometry()) self.assertEqual(sdf.GeometryType.EMPTY, collision.geometry().type()) self.assertEqual(None, collision.geometry().box_shape()) + self.assertEqual(None, collision.geometry().cone_shape()) self.assertEqual(None, collision.geometry().cylinder_shape()) self.assertEqual(None, collision.geometry().plane_shape()) self.assertEqual(None, collision.geometry().sphere_shape()) diff --git a/python/test/pyCone_TEST.py b/python/test/pyCone_TEST.py new file mode 100644 index 000000000..e36e65f63 --- /dev/null +++ b/python/test/pyCone_TEST.py @@ -0,0 +1,114 @@ +# Copyright 2024 CogniPilot Foundation +# Copyright 2024 Open Source Robotics Foundation +# Copyright 2024 Rudis Laboratories + +# 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. + +import copy + +import math + +from gz_test_deps.sdformat import Cone + +import unittest + + +class ConeTEST(unittest.TestCase): + + def test_default_construction(self): + cone = Cone() + + self.assertEqual(math.pi * math.pow(0.5, 2) * 1.0 / 3.0, + cone.shape().volume()) + + self.assertEqual(0.5, cone.radius()) + self.assertEqual(1.0, cone.length()) + + cone.set_radius(0.5) + cone.set_length(2.3) + + self.assertEqual(0.5, cone.radius()) + self.assertEqual(2.3, cone.length()) + + def test_assignment(self): + cone = Cone() + cone.set_radius(0.2) + cone.set_length(3.0) + self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0 / 3.0, + cone.shape().volume()) + + cone2 = cone + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0 / 3.0, + cone2.shape().volume()) + self.assertEqual(0.2, cone2.shape().radius()) + self.assertEqual(3.0, cone2.shape().length()) + + cone.set_radius(2.0) + cone.set_length(0.3) + + self.assertEqual(2.0, cone.radius()) + self.assertEqual(0.3, cone.length()) + self.assertEqual(2.0, cone2.radius()) + self.assertEqual(0.3, cone2.length()) + + + def test_copy_construction(self): + cone = Cone(); + cone.set_radius(0.2) + cone.set_length(3.0) + + cone2 = Cone(cone) + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + cone.set_radius(2.) + cone.set_length(0.3) + + self.assertEqual(2, cone.radius()) + self.assertEqual(0.3, cone.length()) + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + def test_deepcopy(self): + cone = Cone(); + cone.set_radius(0.2) + cone.set_length(3.0) + + cone2 = copy.deepcopy(cone); + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + cone.set_radius(2.) + cone.set_length(0.3) + + self.assertEqual(2, cone.radius()) + self.assertEqual(0.3, cone.length()) + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + def test_shape(self): + cone = Cone(); + self.assertEqual(0.5, cone.radius()) + self.assertEqual(1.0, cone.length()) + + cone.shape().set_radius(0.123) + cone.shape().set_length(0.456) + self.assertEqual(0.123, cone.radius()) + self.assertEqual(0.456, cone.length()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index c4cd9c511..e76506051 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cylinder, Ellipsoid, +from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid, Mesh, ParserConfig, Plane, Sphere) from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf @@ -33,6 +33,9 @@ def test_default_construction(self): geom.set_type(sdf.GeometryType.CAPSULE) self.assertEqual(sdf.GeometryType.CAPSULE, geom.type()) + geom.set_type(sdf.GeometryType.CONE) + self.assertEqual(sdf.GeometryType.CONE, geom.type()) + geom.set_type(sdf.GeometryType.CYLINDER) self.assertEqual(sdf.GeometryType.CYLINDER, geom.type()) @@ -123,6 +126,21 @@ def test_capsule(self): self.assertEqual(4.56, geom.capsule_shape().length()) + def test_cone(self): + geom = Geometry() + geom.set_type(sdf.GeometryType.CONE) + + coneShape = Cone() + coneShape.set_radius(0.123) + coneShape.set_length(4.56) + geom.set_cone_shape(coneShape) + + self.assertEqual(sdf.GeometryType.CONE, geom.type()) + self.assertNotEqual(None, geom.cone_shape()) + self.assertEqual(0.123, geom.cone_shape().radius()) + self.assertEqual(4.56, geom.cone_shape().length()) + + def test_cylinder(self): geom = Geometry() geom.set_type(sdf.GeometryType.CYLINDER) diff --git a/python/test/pyVisual_TEST.py b/python/test/pyVisual_TEST.py index bedadb0eb..1e07f1412 100644 --- a/python/test/pyVisual_TEST.py +++ b/python/test/pyVisual_TEST.py @@ -65,6 +65,7 @@ def test_default_construction(self): self.assertNotEqual(None, visual.geometry()) self.assertEqual(sdf.GeometryType.EMPTY, visual.geometry().type()) self.assertEqual(None, visual.geometry().box_shape()) + self.assertEqual(None, visual.geometry().cone_shape()) self.assertEqual(None, visual.geometry().cylinder_shape()) self.assertEqual(None, visual.geometry().plane_shape()) self.assertEqual(None, visual.geometry().sphere_shape()) diff --git a/sdf/1.11/CMakeLists.txt b/sdf/1.11/CMakeLists.txt index 2e56bf0a7..fb7f6a38a 100644 --- a/sdf/1.11/CMakeLists.txt +++ b/sdf/1.11/CMakeLists.txt @@ -11,6 +11,7 @@ set (sdfs camera.sdf capsule_shape.sdf collision.sdf + cone_shape.sdf contact.sdf cylinder_shape.sdf ellipsoid_shape.sdf diff --git a/sdf/1.11/cone_shape.sdf b/sdf/1.11/cone_shape.sdf new file mode 100644 index 000000000..5805ba6bb --- /dev/null +++ b/sdf/1.11/cone_shape.sdf @@ -0,0 +1,9 @@ + + Cone shape + + Radius of the cone + + + Length of the cone along the z axis + + diff --git a/sdf/1.11/geometry.sdf b/sdf/1.11/geometry.sdf index 884902afb..447338dbf 100644 --- a/sdf/1.11/geometry.sdf +++ b/sdf/1.11/geometry.sdf @@ -8,6 +8,7 @@ + diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 1ab3d1b18..aaa0cd2dc 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -84,6 +84,7 @@ TEST(DOMcollision, Construction) ASSERT_NE(nullptr, collision.Geom()); EXPECT_EQ(sdf::GeometryType::EMPTY, collision.Geom()->Type()); EXPECT_EQ(nullptr, collision.Geom()->BoxShape()); + EXPECT_EQ(nullptr, collision.Geom()->ConeShape()); EXPECT_EQ(nullptr, collision.Geom()->CylinderShape()); EXPECT_EQ(nullptr, collision.Geom()->PlaneShape()); EXPECT_EQ(nullptr, collision.Geom()->SphereShape()); diff --git a/src/Cone.cc b/src/Cone.cc new file mode 100644 index 000000000..7e094bdd0 --- /dev/null +++ b/src/Cone.cc @@ -0,0 +1,189 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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/Cone.hh" +#include "sdf/parser.hh" +#include "Utils.hh" + +using namespace sdf; + +// Private data class +class sdf::Cone::Implementation +{ + // A cone with a length of 1 meter and radius if 0.5 meters. + public: gz::math::Coned cone{1.0, 0.5}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Cone::Cone() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Cone::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a cone, but the provided SDF " + "element is null."}); + return errors; + } + + // We need a cone child element + if (_sdf->GetName() != "cone") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a cone geometry, but the provided SDF " + "element is not a ."}); + return errors; + } + + { + std::pair pair = _sdf->Get(errors, "radius", + this->dataPtr->cone.Radius()); + + if (!pair.second) + { + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a radius of " + << this->dataPtr->cone.Radius() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); + } + this->dataPtr->cone.SetRadius(pair.first); + } + + { + std::pair pair = _sdf->Get(errors, "length", + this->dataPtr->cone.Length()); + + if (!pair.second) + { + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a length of " + << this->dataPtr->cone.Length() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); + } + this->dataPtr->cone.SetLength(pair.first); + } + + return errors; +} + +////////////////////////////////////////////////// +double Cone::Radius() const +{ + return this->dataPtr->cone.Radius(); +} + +////////////////////////////////////////////////// +void Cone::SetRadius(double _radius) +{ + this->dataPtr->cone.SetRadius(_radius); +} + +////////////////////////////////////////////////// +double Cone::Length() const +{ + return this->dataPtr->cone.Length(); +} + +////////////////////////////////////////////////// +void Cone::SetLength(double _length) +{ + this->dataPtr->cone.SetLength(_length); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Cone::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +const gz::math::Coned &Cone::Shape() const +{ + return this->dataPtr->cone; +} + +///////////////////////////////////////////////// +gz::math::Coned &Cone::Shape() +{ + return this->dataPtr->cone; +} + +std::optional Cone::CalculateInertial(double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->cone.SetMat(material); + + auto coneMassMatrix = this->dataPtr->cone.MassMatrix(); + + if (!coneMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald coneInertial; + coneInertial.SetMassMatrix(coneMassMatrix.value()); + coneInertial.SetPose({0, 0, -0.25 * this->dataPtr->cone.Length(), 0, 0, 0}); + return std::make_optional(coneInertial); + } +} + +///////////////////////////////////////////////// +sdf::ElementPtr Cone::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Cone::ToElement(sdf::Errors &_errors) const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("cone_shape.sdf", elem); + + sdf::ElementPtr radiusElem = elem->GetElement("radius", _errors); + radiusElem->Set(_errors, this->Radius()); + + sdf::ElementPtr lengthElem = elem->GetElement("length", _errors); + lengthElem->Set(_errors, this->Length()); + + return elem; +} diff --git a/src/Cone_TEST.cc b/src/Cone_TEST.cc new file mode 100644 index 000000000..59c5dbf3e --- /dev/null +++ b/src/Cone_TEST.cc @@ -0,0 +1,288 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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/Cone.hh" +#include "test_utils.hh" +#include +#include +#include +#include + +///////////////////////////////////////////////// +TEST(DOMCone, Construction) +{ + sdf::Cone cone; + EXPECT_EQ(nullptr, cone.Element()); + // A default cone has a length of 1 meter and radius if 0.5 meters. + EXPECT_DOUBLE_EQ(GZ_PI * std::pow(0.5, 2) * 1.0 / 3.0, + cone.Shape().Volume()); + + EXPECT_DOUBLE_EQ(0.5, cone.Radius()); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + + cone.SetRadius(0.5); + cone.SetLength(2.3); + + EXPECT_DOUBLE_EQ(0.5, cone.Radius()); + EXPECT_DOUBLE_EQ(2.3, cone.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, MoveConstructor) +{ + sdf::Cone cone; + cone.SetRadius(0.2); + cone.SetLength(3.0); + + sdf::Cone cone2(std::move(cone)); + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); + + EXPECT_DOUBLE_EQ(GZ_PI * std::pow(0.2, 2) * 3.0 / 3.0, + cone2.Shape().Volume()); + EXPECT_DOUBLE_EQ(0.2, cone2.Shape().Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Shape().Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, CopyConstructor) +{ + sdf::Cone cone; + cone.SetRadius(0.2); + cone.SetLength(3.0); + + sdf::Cone cone2(cone); + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, CopyAssignmentOperator) +{ + sdf::Cone cone; + cone.SetRadius(0.2); + cone.SetLength(3.0); + + sdf::Cone cone2; + cone2 = cone; + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, MoveAssignmentConstructor) +{ + sdf::Cone cone; + cone.SetRadius(0.2); + cone.SetLength(3.0); + + sdf::Cone cone2; + cone2 = std::move(cone); + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, CopyAssignmentAfterMove) +{ + sdf::Cone cone1; + cone1.SetRadius(0.2); + cone1.SetLength(3.0); + + sdf::Cone cone2; + cone2.SetRadius(2); + cone2.SetLength(30.0); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Cone tmp = std::move(cone1); + cone1 = cone2; + cone2 = tmp; + + EXPECT_DOUBLE_EQ(2, cone1.Radius()); + EXPECT_DOUBLE_EQ(30, cone1.Length()); + + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, Load) +{ + sdf::Cone cone; + sdf::Errors errors; + + // Null element name + errors = cone.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, cone.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = cone.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, cone.Element()); + + // Missing and elements + sdf->SetName("cone"); + errors = cone.Load(sdf); + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[1].Code()); + EXPECT_NE(std::string::npos, errors[1].Message().find("Invalid ")) + << errors[1].Message(); + EXPECT_NE(nullptr, cone.Element()); + + // Add a radius element + sdf::ElementPtr radiusDesc(new sdf::Element()); + radiusDesc->SetName("radius"); + radiusDesc->AddValue("double", "1.0", true, "radius"); + sdf->AddElementDescription(radiusDesc); + sdf::ElementPtr radiusElem = sdf->AddElement("radius"); + radiusElem->Set(2.0); + + // Missing element + sdf->SetName("cone"); + errors = cone.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); +} + +///////////////////////////////////////////////// +TEST(DOMCone, Shape) +{ + sdf::Cone cone; + EXPECT_DOUBLE_EQ(0.5, cone.Radius()); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + + cone.Shape().SetRadius(0.123); + cone.Shape().SetLength(0.456); + EXPECT_DOUBLE_EQ(0.123, cone.Radius()); + EXPECT_DOUBLE_EQ(0.456, cone.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, CalculateInertial) +{ + sdf::Cone cone; + + // density of aluminium + const double density = 2170; + + // Invalid dimensions leading to std::nullopt return in + // CalculateInertial() + cone.SetLength(-1); + cone.SetRadius(0); + auto invalidConeInertial = cone.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidConeInertial); + + const double l = 2.0; + const double r = 0.1; + + cone.SetLength(l); + cone.SetRadius(r); + + double expectedMass = cone.Shape().Volume() * density; + double ixxIyy = (3 / 80.0) * expectedMass * (4 * r * r + l * l); + double izz = 3.0 * expectedMass * r * r / 10.0; + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixxIyy, ixxIyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose({0, 0, -l / 4.0, 0, 0, 0}); + + auto coneInertial = cone.CalculateInertial(density); + EXPECT_EQ(cone.Shape().Mat().Density(), density); + ASSERT_NE(std::nullopt, coneInertial); + EXPECT_EQ(expectedInertial, *coneInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + coneInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + coneInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), coneInertial->Pose()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, ToElement) +{ + sdf::Cone cone; + + cone.SetRadius(1.2); + cone.SetLength(0.5); + + sdf::ElementPtr elem = cone.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Cone cone2; + cone2.Load(elem); + + EXPECT_DOUBLE_EQ(cone.Radius(), cone2.Radius()); + EXPECT_DOUBLE_EQ(cone.Length(), cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, ToElementErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::Cone cone; + sdf::Errors errors; + + cone.SetRadius(1.2); + cone.SetLength(0.5); + + sdf::ElementPtr elem = cone.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Cone cone2; + errors = cone2.Load(elem); + EXPECT_TRUE(errors.empty()); + + EXPECT_DOUBLE_EQ(cone.Radius(), cone2.Radius()); + EXPECT_DOUBLE_EQ(cone.Length(), cone2.Length()); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} diff --git a/src/Geometry.cc b/src/Geometry.cc index 75bcca03f..ec87a01da 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -21,6 +21,7 @@ #include "sdf/Geometry.hh" #include "sdf/Box.hh" #include "sdf/Capsule.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Heightmap.hh" @@ -49,6 +50,9 @@ class sdf::Geometry::Implementation /// \brief Optional capsule. public: std::optional capsule; + /// \brief Optional cone. + public: std::optional cone; + /// \brief Optional cylinder. public: std::optional cylinder; @@ -127,6 +131,14 @@ Errors Geometry::Load(ElementPtr _sdf, const ParserConfig &_config) _sdf->GetElement("capsule", errors)); errors.insert(errors.end(), err.begin(), err.end()); } + else if (_sdf->HasElement("cone")) + { + this->dataPtr->type = GeometryType::CONE; + this->dataPtr->cone.emplace(); + Errors err = this->dataPtr->cone->Load( + _sdf->GetElement("cone", errors)); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (_sdf->HasElement("cylinder")) { this->dataPtr->type = GeometryType::CYLINDER; @@ -240,6 +252,18 @@ void Geometry::SetCapsuleShape(const Capsule &_capsule) this->dataPtr->capsule = _capsule; } +///////////////////////////////////////////////// +const Cone *Geometry::ConeShape() const +{ + return optionalToPointer(this->dataPtr->cone); +} + +///////////////////////////////////////////////// +void Geometry::SetConeShape(const Cone &_cone) +{ + this->dataPtr->cone = _cone; +} + ///////////////////////////////////////////////// const Cylinder *Geometry::CylinderShape() const { @@ -327,6 +351,9 @@ std::optional Geometry::CalculateInertial( case GeometryType::CAPSULE: geomInertial = this->dataPtr->capsule->CalculateInertial(_density); break; + case GeometryType::CONE: + geomInertial = this->dataPtr->cone->CalculateInertial(_density); + break; case GeometryType::CYLINDER: geomInertial = this->dataPtr->cylinder->CalculateInertial(_density); break; @@ -384,6 +411,9 @@ sdf::ElementPtr Geometry::ToElement(sdf::Errors &_errors) const case GeometryType::BOX: elem->InsertElement(this->dataPtr->box->ToElement(_errors), true); break; + case GeometryType::CONE: + elem->InsertElement(this->dataPtr->cone->ToElement(_errors), true); + break; case GeometryType::CYLINDER: elem->InsertElement(this->dataPtr->cylinder->ToElement(_errors), true); break; diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 7b12f878a..05f4d7205 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -18,6 +18,7 @@ #include #include "sdf/Box.hh" #include "sdf/Capsule.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" @@ -49,6 +50,9 @@ TEST(DOMGeometry, Construction) geom.SetType(sdf::GeometryType::CAPSULE); EXPECT_EQ(sdf::GeometryType::CAPSULE, geom.Type()); + geom.SetType(sdf::GeometryType::CONE); + EXPECT_EQ(sdf::GeometryType::CONE, geom.Type()); + geom.SetType(sdf::GeometryType::CYLINDER); EXPECT_EQ(sdf::GeometryType::CYLINDER, geom.Type()); @@ -203,6 +207,23 @@ TEST(DOMGeometry, Capsule) EXPECT_DOUBLE_EQ(4.56, geom.CapsuleShape()->Length()); } +///////////////////////////////////////////////// +TEST(DOMGeometry, Cone) +{ + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::CONE); + + sdf::Cone coneShape; + coneShape.SetRadius(0.123); + coneShape.SetLength(4.56); + geom.SetConeShape(coneShape); + + EXPECT_EQ(sdf::GeometryType::CONE, geom.Type()); + EXPECT_NE(nullptr, geom.ConeShape()); + EXPECT_DOUBLE_EQ(0.123, geom.ConeShape()->Radius()); + EXPECT_DOUBLE_EQ(4.56, geom.ConeShape()->Length()); +} + ///////////////////////////////////////////////// TEST(DOMGeometry, Cylinder) { @@ -395,6 +416,37 @@ TEST(DOMGeometry, CalculateInertial) EXPECT_EQ(expectedInertial.Pose(), capsuleInertial->Pose()); } + // Cone + { + sdf::Cone cone; + const double l = 2.0; + const double r = 0.1; + + cone.SetLength(l); + cone.SetRadius(r); + + expectedMass = cone.Shape().Volume() * density; + double ixxIyy = (3 / 80.0) * expectedMass * (4 * r * r + l * l); + double izz = 3.0 * expectedMass * r * r / 10.0; + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixxIyy, ixxIyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d({0, 0, -l / 4.0, 0, 0, 0})); + + geom.SetType(sdf::GeometryType::CONE); + geom.SetConeShape(cone); + auto coneInertial = geom.CalculateInertial(errors, + sdfParserConfig, density, autoInertiaParams); + + ASSERT_NE(std::nullopt, coneInertial); + EXPECT_EQ(expectedInertial, *coneInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), coneInertial->Pose()); + } + // Cylinder { sdf::Cylinder cylinder; @@ -561,6 +613,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_NE(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -587,6 +640,34 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_NE(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + EXPECT_TRUE(geom2.PolylineShape().empty()); + } + + // Cone + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::CONE); + sdf::Cone cone; + geom.SetConeShape(cone); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_NE(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -613,6 +694,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_NE(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -639,6 +721,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_NE(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -665,6 +748,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_NE(nullptr, geom2.SphereShape()); @@ -691,6 +775,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -717,6 +802,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -743,6 +829,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -769,6 +856,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -816,6 +904,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_NE(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -844,6 +933,36 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_NE(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + EXPECT_TRUE(geom2.PolylineShape().empty()); + } + + // Cone + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::CONE); + sdf::Cone cone; + geom.SetConeShape(cone); + + sdf::ElementPtr elem = geom.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + errors = geom2.Load(elem); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_NE(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -872,6 +991,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_NE(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -900,6 +1020,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_NE(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -928,6 +1049,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_NE(nullptr, geom2.SphereShape()); @@ -956,6 +1078,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -988,6 +1111,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -1020,6 +1144,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -1048,6 +1173,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); diff --git a/src/Visual_TEST.cc b/src/Visual_TEST.cc index 935e99ed6..3e7248445 100644 --- a/src/Visual_TEST.cc +++ b/src/Visual_TEST.cc @@ -68,6 +68,7 @@ TEST(DOMVisual, Construction) ASSERT_NE(nullptr, visual.Geom()); EXPECT_EQ(sdf::GeometryType::EMPTY, visual.Geom()->Type()); EXPECT_EQ(nullptr, visual.Geom()->BoxShape()); + EXPECT_EQ(nullptr, visual.Geom()->ConeShape()); EXPECT_EQ(nullptr, visual.Geom()->CylinderShape()); EXPECT_EQ(nullptr, visual.Geom()->PlaneShape()); EXPECT_EQ(nullptr, visual.Geom()->SphereShape()); diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 419a04dc9..927850087 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -21,6 +21,7 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" #include "sdf/Collision.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Element.hh" #include "sdf/Ellipsoid.hh" @@ -94,6 +95,26 @@ TEST(DOMGeometry, Shapes) EXPECT_DOUBLE_EQ(2.1, capsuleVisGeom->Radius()); EXPECT_DOUBLE_EQ(10.2, capsuleVisGeom->Length()); + // Test cone collision + const sdf::Collision *coneCol = link->CollisionByName("cone_col"); + ASSERT_NE(nullptr, coneCol); + ASSERT_NE(nullptr, coneCol->Geom()); + EXPECT_EQ(sdf::GeometryType::CONE, coneCol->Geom()->Type()); + const sdf::Cone *coneColGeom = coneCol->Geom()->ConeShape(); + ASSERT_NE(nullptr, coneColGeom); + EXPECT_DOUBLE_EQ(0.2, coneColGeom->Radius()); + EXPECT_DOUBLE_EQ(0.1, coneColGeom->Length()); + + // Test cone visual + const sdf::Visual *coneVis = link->VisualByName("cone_vis"); + ASSERT_NE(nullptr, coneVis); + ASSERT_NE(nullptr, coneVis->Geom()); + EXPECT_EQ(sdf::GeometryType::CONE, coneVis->Geom()->Type()); + const sdf::Cone *coneVisGeom = coneVis->Geom()->ConeShape(); + ASSERT_NE(nullptr, coneVisGeom); + EXPECT_DOUBLE_EQ(2.1, coneVisGeom->Radius()); + EXPECT_DOUBLE_EQ(10.2, coneVisGeom->Length()); + // Test cylinder collision const sdf::Collision *cylinderCol = link->CollisionByName("cylinder_col"); ASSERT_NE(nullptr, cylinderCol); diff --git a/test/sdf/basic_shapes.sdf b/test/sdf/basic_shapes.sdf index 32e0b416f..93c9fafc1 100644 --- a/test/sdf/basic_shapes.sdf +++ b/test/sdf/basic_shapes.sdf @@ -1,5 +1,5 @@ - + true @@ -155,5 +155,28 @@ + + + 2 0 0 0 0 0 + + + + + 0.2 + 0.1 + + + + + + + + 0.2 + 0.1 + + + + + diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 37c56277c..5a2e72d4f 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -1,5 +1,5 @@ - + @@ -65,6 +65,24 @@ + + + + 0.2 + 0.1 + + + + + + + + 2.1 + 10.2 + + + + diff --git a/test/sdf/shapes_world.sdf b/test/sdf/shapes_world.sdf index 5eb324490..3a7be8b91 100644 --- a/test/sdf/shapes_world.sdf +++ b/test/sdf/shapes_world.sdf @@ -1,5 +1,5 @@ - + true @@ -147,5 +147,28 @@ + + + 2 0 0 0 0 0 + + + + + 0.2 + 0.1 + + + + + + + + 0.2 + 0.1 + + + + + From 2a66550ce4dca10e05307d07f5ae867811bf1933 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Fri, 21 Jun 2024 02:22:36 +0200 Subject: [PATCH 22/23] Prepare release 14.4.0 (#1441) Signed-off-by: Jose Luis Rivero Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 9 +++++++++ package.xml | 2 +- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 877198921..0eea9fb7b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0004 NEW) endif(COMMAND CMAKE_POLICY) -project (sdformat14 VERSION 14.3.0) +project (sdformat14 VERSION 14.4.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software diff --git a/Changelog.md b/Changelog.md index 0c375bb86..d41f497bb 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,14 @@ ## libsdformat 14.X +### libsdformat 14.4.0 (2024-06-20) + +1. Add Cone as a primitive parametric shape. + * [Pull request #1415](https://github.com/gazebosim/sdformat/pull/1415) + * Thanks to Benjamin Perseghetti + +1. Add custom attribute to custom element in test + * [Pull request #1406](https://github.com/gazebosim/sdformat/pull/1406) + ### libsdformat 14.3.0 (2024-06-14) 1. Backport voxel_resolution sdf element diff --git a/package.xml b/package.xml index b065874cd..ed841192a 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ sdformat14 - 14.3.0 + 14.4.0 SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications From e8e45a87e4ccce8d2a98b40f03253e1575120bed Mon Sep 17 00:00:00 2001 From: "aditya.agrawal.a.777@gmail.com" Date: Fri, 16 Aug 2024 10:57:17 +0800 Subject: [PATCH 23/23] Style fixes Signed-off-by: Aditya Agrawal --- format_schema.py | 74 ++++++++++---------- include/sdf/Actor.hh | 2 +- include/sdf/AirPressure.hh | 2 +- include/sdf/AirSpeed.hh | 2 +- include/sdf/Altimeter.hh | 2 +- include/sdf/Atmosphere.hh | 2 +- include/sdf/Camera.hh | 2 +- include/sdf/Collision.hh | 2 +- include/sdf/Error.hh | 3 + include/sdf/ForceTorque.hh | 3 +- include/sdf/Frame.hh | 2 +- include/sdf/Geometry.hh | 2 +- include/sdf/Gui.hh | 2 +- include/sdf/Imu.hh | 2 +- include/sdf/Joint.hh | 2 +- include/sdf/Lidar.hh | 2 +- include/sdf/Light.hh | 2 +- include/sdf/Link.hh | 2 +- include/sdf/Magnetometer.hh | 2 +- include/sdf/Material.hh | 2 +- include/sdf/Model.hh | 2 +- include/sdf/NavSat.hh | 7 +- include/sdf/Noise.hh | 2 +- include/sdf/ParticleEmitter.hh | 2 +- include/sdf/Physics.hh | 2 +- include/sdf/Plugin.hh | 2 +- include/sdf/Projector.hh | 2 +- include/sdf/Root.hh | 2 +- include/sdf/SDFImpl.hh | 94 +++++++++++++++++++++++++ include/sdf/Scene.hh | 2 +- include/sdf/Sensor.hh | 2 +- include/sdf/Surface.hh | 2 +- include/sdf/Visual.hh | 2 +- include/sdf/World.hh | 16 ++++- src/Actor.cc | 4 +- src/AirPressure.cc | 4 +- src/AirSpeed.cc | 4 +- src/Altimeter.cc | 4 +- src/Atmosphere.cc | 4 +- src/Camera.cc | 4 +- src/Collision.cc | 4 +- src/Converter.cc | 10 +-- src/Converter.hh | 4 +- src/Converter_TEST.cc | 21 ++++-- src/ForceTorque.cc | 6 +- src/Frame.cc | 4 +- src/Geometry.cc | 4 +- src/Gui.cc | 4 +- src/Imu.cc | 4 +- src/Joint.cc | 4 +- src/Lidar.cc | 4 +- src/Light.cc | 4 +- src/Link.cc | 4 +- src/Magnetometer.cc | 4 +- src/Material.cc | 4 +- src/Model.cc | 4 +- src/NavSat.cc | 4 +- src/Noise.cc | 4 +- src/ParamPassing.cc | 52 +++++++------- src/ParticleEmitter.cc | 4 +- src/Physics.cc | 4 +- src/Plugin.cc | 4 +- src/Projector.cc | 4 +- src/Root.cc | 4 +- src/SDF.cc | 124 +++++++++++++++++++++++++++++---- src/SDF_TEST.cc | 40 +++++++++++ src/Scene.cc | 4 +- src/Sensor.cc | 4 +- src/Surface.cc | 4 +- src/Visual.cc | 4 +- src/World.cc | 23 ++++-- src/World_TEST.cc | 15 ++++ src/XmlUtils.cc | 30 ++++++-- src/XmlUtils.hh | 18 ++++- src/XmlUtils_TEST.cc | 49 ++++++++++++- 75 files changed, 565 insertions(+), 196 deletions(-) diff --git a/format_schema.py b/format_schema.py index b7ae61978..a4fcaefc0 100644 --- a/format_schema.py +++ b/format_schema.py @@ -6,23 +6,19 @@ for line in lines: # Remove .sdf extension snake_case = line.strip().split('.')[0] - # Convert from snake_case to CamelCase camel_case = ''.join([word.capitalize() for word in snake_case.split('_')]) - addHeader = """ - /// \\brief Get the schema file name accessor - public: static inline std::string_view SchemaFile(); - """ + addHeader = """\n /// \\brief Get the schema file name accessor + public: static inline std::string_view SchemaFile();\n""" addImpl = f""" ///////////////////////////////////////////////// -inline std::string_view {camel_case}::SchemaFile() +inline std::string_view {camel_case}::SchemaFile() {{ static const char kSchemaFile[] = "{line}"; return kSchemaFile; }}\n\n""" - # Debug print statements # print(addHeader) # print(addImpl) @@ -31,31 +27,31 @@ print(line) - # # Edit './include/sdf/{camel_case}.hh' - # # Find the line with 'class SDFFORMAT_VISIBLE {camel_case}` - # try: - # f = open('./include/sdf/' + camel_case + '.hh', 'r') - # read_lines = f.readlines() - # f.close() - # line_number = 0 - # for i, line_raw in enumerate(read_lines): - # if line_raw == " class SDFORMAT_VISIBLE " + camel_case + '\n': - # line_number = i+3 # After the class line there is `\n{\n` and then the constructor - # break - # if line_number == 0: - # print("Error: Could not find class declaration in " + camel_case + ".hh") - # exit(1) - # try: - # with open('./include/sdf/' + camel_case + '.hh', 'w') as file: - # for i, line_raw in enumerate(read_lines): - # file.write(line_raw) - # if i == line_number: - # file.write(addHeader) - # except: - # print("Unexpected error while writing to: " + camel_case + ".hh.") - # except: - # print("Error while writing to: " + camel_case + ".hh." + " Check if file exists.") - + # Edit './include/sdf/{camel_case}.hh' + # Find the line with 'class SDFFORMAT_VISIBLE {camel_case}` + try: + f = open('./include/sdf/' + camel_case + '.hh', 'r') + read_lines = f.readlines() + f.close() + line_number = 0 + for i, line_raw in enumerate(read_lines): + if line_raw == " class SDFORMAT_VISIBLE " + camel_case + '\n': + line_number = i+3 # After the class line there is `\n{\n` and then the constructor + break + if line_number == 0: + print("Error: Could not find class declaration in " + camel_case + ".hh") + exit(1) + try: + with open('./include/sdf/' + camel_case + '.hh', 'w') as file: + for i, line_raw in enumerate(read_lines): + file.write(line_raw) + if i == line_number: + file.write(addHeader) + except: + print("Unexpected error while writing to: " + camel_case + ".hh.") + except: + print("Error while writing to: " + camel_case + ".hh." + " Check if file exists.") + # Replace all instances of `line` with `std::string(this->SchemaFile())` if (os.path.exists('./src/' + camel_case + '.cc')): try: @@ -72,12 +68,12 @@ except: print("Unexpected error while reading from: " + camel_case + ".cc.") - # # Edit './src/{camel_case}.cc' if it exists - # # Add implementation to end of document - # if os.path.exists('./src/' + camel_case + '.cc'): - # with open('./src/' + camel_case + '.cc', 'a') as file: - # file.write(addImpl) - # else: - # print("Error: Could not find " + camel_case + ".cc") + # Edit './src/{camel_case}.cc' if it exists + # Add implementation to end of document + if os.path.exists('./src/' + camel_case + '.cc'): + with open('./src/' + camel_case + '.cc', 'a') as file: + file.write(addImpl) + else: + print("Error: Could not find " + camel_case + ".cc") print("Changes written to source successfully") diff --git a/include/sdf/Actor.hh b/include/sdf/Actor.hh index 57c5c0fc9..7708f8559 100644 --- a/include/sdf/Actor.hh +++ b/include/sdf/Actor.hh @@ -195,7 +195,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the actor based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/AirPressure.hh b/include/sdf/AirPressure.hh index b8fae845e..2b3ae5491 100644 --- a/include/sdf/AirPressure.hh +++ b/include/sdf/AirPressure.hh @@ -38,7 +38,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the airPressure based on an element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. diff --git a/include/sdf/AirSpeed.hh b/include/sdf/AirSpeed.hh index 050c7fac6..2ad60b994 100644 --- a/include/sdf/AirSpeed.hh +++ b/include/sdf/AirSpeed.hh @@ -38,7 +38,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the air speed based on an element pointer. /// This is *not* the usual entry point. Typical usage of the SDF DOM is /// through the Root object. diff --git a/include/sdf/Altimeter.hh b/include/sdf/Altimeter.hh index d6ac7a962..df769fc31 100644 --- a/include/sdf/Altimeter.hh +++ b/include/sdf/Altimeter.hh @@ -37,7 +37,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the altimeter based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Atmosphere.hh b/include/sdf/Atmosphere.hh index 3efb86813..825544df0 100644 --- a/include/sdf/Atmosphere.hh +++ b/include/sdf/Atmosphere.hh @@ -49,7 +49,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the atmosphere based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 6b25af210..8c7ce146a 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -64,7 +64,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Return true if both Camera objects contain the same values. /// \param[_in] _alt Camera value to compare. /// \returen True if 'this' == _alt. diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index f49700f59..ef693d405 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -54,7 +54,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the collision based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index 0aace5ef7..9664cc850 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -183,6 +183,9 @@ namespace sdf /// \brief The joint axis mimic does not refer to a valid joint in the /// current scope. JOINT_AXIS_MIMIC_INVALID, + + /// \brief Error at the XML level. + XML_ERROR, }; class SDFORMAT_VISIBLE Error diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh index c84cd693f..04fb09a86 100644 --- a/include/sdf/ForceTorque.hh +++ b/include/sdf/ForceTorque.hh @@ -66,7 +66,8 @@ namespace sdf /// \brief Default constructor public: ForceTorque(); - public: static inline std::string_view SchemaFile(); + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); /// \brief Load the force torque sensor based on an element pointer. This is /// *not* the usual entry point. Typical usage of the SDF DOM is through the diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index 90e9d0d45..d57988c15 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -46,7 +46,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the frame based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 5f5443ec0..641f4c2eb 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -96,7 +96,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the geometry based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Gui.hh b/include/sdf/Gui.hh index a66ab4d41..00b4b2432 100644 --- a/include/sdf/Gui.hh +++ b/include/sdf/Gui.hh @@ -35,7 +35,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the gui based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index 83ce17a40..7c3cbe26a 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -37,7 +37,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the IMU based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 1147ad4e2..dc908cdb9 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -89,7 +89,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the joint based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Lidar.hh b/include/sdf/Lidar.hh index dac6bb4ab..d72929e8d 100644 --- a/include/sdf/Lidar.hh +++ b/include/sdf/Lidar.hh @@ -109,7 +109,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the lidar based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index 21f31e763..e2bf64725 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -67,7 +67,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the light based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 291fa27b0..5da46e7d6 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -55,7 +55,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the link based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Magnetometer.hh b/include/sdf/Magnetometer.hh index db1d4f697..07dcbe08a 100644 --- a/include/sdf/Magnetometer.hh +++ b/include/sdf/Magnetometer.hh @@ -38,7 +38,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the magnetometer based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index 32115d9b6..5269f9819 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -50,7 +50,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the material based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 163121965..8f39b0e29 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -58,7 +58,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the model based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/NavSat.hh b/include/sdf/NavSat.hh index e85b6d0f6..5a0e23367 100644 --- a/include/sdf/NavSat.hh +++ b/include/sdf/NavSat.hh @@ -76,15 +76,16 @@ namespace sdf { /// \brief Default constructor public: NavSat(); + + /// \brief Get the schema file name accessor + public: static inline std::string_view SchemaFile(); + /// \brief Load the navsat based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. /// \param[in] _sdf The SDF Element pointer /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error. - - public: static inline std::string_view SchemaFile(); - public: Errors Load(ElementPtr _sdf); /// \brief Get a pointer to the SDF element that was used during diff --git a/include/sdf/Noise.hh b/include/sdf/Noise.hh index 202f57efb..aad165615 100644 --- a/include/sdf/Noise.hh +++ b/include/sdf/Noise.hh @@ -51,7 +51,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Return true if both Noise objects contain the same values. /// \param[_in] _noise Noise value to compare. /// \return True if 'this' == _noise. diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh index d20534db1..dc2f4d98d 100644 --- a/include/sdf/ParticleEmitter.hh +++ b/include/sdf/ParticleEmitter.hh @@ -64,7 +64,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the particle emitter based on an element pointer. This is /// *not* the usual entry point. Typical usage of the SDF DOM is through /// the Root object. diff --git a/include/sdf/Physics.hh b/include/sdf/Physics.hh index 6bd63e54b..0a93a8802 100644 --- a/include/sdf/Physics.hh +++ b/include/sdf/Physics.hh @@ -40,7 +40,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the physics based on an element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Plugin.hh b/include/sdf/Plugin.hh index bc49a4f0d..b530bc8a5 100644 --- a/include/sdf/Plugin.hh +++ b/include/sdf/Plugin.hh @@ -49,7 +49,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Default destructor public: ~Plugin(); diff --git a/include/sdf/Projector.hh b/include/sdf/Projector.hh index a4c8bdd81..322489d6e 100644 --- a/include/sdf/Projector.hh +++ b/include/sdf/Projector.hh @@ -47,7 +47,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the projector based on an element pointer. This is /// *not* the usual entry point. Typical usage of the SDF DOM is through /// the Root object. diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index a8c823cf2..9f10c05ae 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -61,7 +61,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Get the name of the world without loading the entire world /// Users shouldn't normally need to use this API. /// This doesn't load the world, it might return the world name even if the diff --git a/include/sdf/SDFImpl.hh b/include/sdf/SDFImpl.hh index 20e1ec19f..277d45188 100644 --- a/include/sdf/SDFImpl.hh +++ b/include/sdf/SDFImpl.hh @@ -80,6 +80,34 @@ namespace sdf bool _searchLocalPath = true, bool _useCallback = false); + /// \brief Find the absolute path of a file. + /// + /// The search order in the function is as follows: + /// 1. Using the global URI path map, search in paths associated with the URI + /// scheme of the input. + /// 2. Seach in the path defined by the macro `SDF_SHARE_PATH`. + /// 3. Search in the the libsdformat install path. The path is formed by + /// has the pattern `SDF_SHARE_PATH/sdformat//` + /// 4. Directly check if the input path exists in the filesystem. + /// 5. Seach in the path defined by the environment variable `SDF_PATH`. + /// 6. If enabled via _searchLocalPath, prepend the input with the current + /// working directory and check if the result path exists. + /// 7. If enabled via _useCallback and the global callback function is set, + /// invoke the function and return its result. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Name of the file to find. + /// \param[in] _searchLocalPath True to search for the file in the current + /// working directory. + /// \param[in] _useCallback True to find a file based on a registered + /// callback if the file is not found via the normal mechanism. + /// \return File's full path. + SDFORMAT_VISIBLE + std::string findFile(sdf::Errors &_errors, + const std::string &_filename, + bool _searchLocalPath = true, + bool _useCallback = false); + /// \brief Find the absolute path of a file. /// /// This overload uses the URI path map and and the callback function @@ -99,6 +127,26 @@ namespace sdf bool _useCallback, const ParserConfig &_config); + /// \brief Find the absolute path of a file. + /// + /// This overload uses the URI path map and and the callback function + /// configured in the input ParserConfig object instead of their global + /// counterparts. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Name of the file to find. + /// \param[in] _searchLocalPath True to search for the file in the current + /// working directory. + /// \param[in] _useCallback True to find a file based on a registered + /// callback if the file is not found via the normal mechanism. + /// \param[in] _config Custom parser configuration. + /// \return File's full path. + SDFORMAT_VISIBLE + std::string findFile(sdf::Errors &_errors, + const std::string &_filename, + bool _searchLocalPath, + bool _useCallback, + const ParserConfig &_config); /// \brief Associate paths to a URI. /// Example paramters: "model://", "/usr/share/models:~/.gazebo/models" @@ -121,22 +169,45 @@ namespace sdf /// \brief Destructor public: ~SDF(); public: void PrintDescription(); + public: void PrintDescription(sdf::Errors &_errors); public: void PrintDoc(); public: void Write(const std::string &_filename); + public: void Write(sdf::Errors &_errors, const std::string &_filename); /// \brief Output SDF's values to stdout. /// \param[in] _config Configuration for printing the values. public: void PrintValues(const PrintConfig &_config = PrintConfig()); + /// \brief Output SDF's values to stdout. + /// \param[out] _errors Vector of errrors. + /// \param[in] _config Configuration for printing the values. + public: void PrintValues(sdf::Errors &_errors, + const PrintConfig &_config = PrintConfig()); + + /// \brief Convert the SDF values to a string representation. + /// \param[in] _config Configuration for printing the values. + /// \return The string representation. + public: std::string ToString( + const PrintConfig &_config = PrintConfig()) const; + /// \brief Convert the SDF values to a string representation. + /// \param[out] _errors Vector of errors. /// \param[in] _config Configuration for printing the values. /// \return The string representation. public: std::string ToString( + sdf::Errors &_errors, const PrintConfig &_config = PrintConfig()) const; /// \brief Set SDF values from a string + /// \param[in] sdfData String with the values to load. public: void SetFromString(const std::string &_sdfData); + /// \brief Set SDF values from a string + /// \param[out] _errors Vector of errors. + /// \param[in] sdfData String with the values to load. + public: void SetFromString(sdf::Errors &_Errors, + const std::string &_sdfData); + /// \brief Clear the data in this object. public: void Clear(); @@ -182,6 +253,13 @@ namespace sdf /// \return a wrapped clone of the SDF element public: static ElementPtr WrapInRoot(const ElementPtr &_sdf); + /// \brief wraps the SDF element into a root element with the version info. + /// \param[out] _errors Vector of errors. + /// \param[in] _sdf the sdf element. Will be cloned by this function. + /// \return a wrapped clone of the SDF element + public: static ElementPtr WrapInRoot(sdf::Errors &_errors, + const ElementPtr &_sdf); + /// \brief Get a string representation of an SDF specification file. /// This function uses a built-in version of a .sdf file located in /// the sdf directory. The parser.cc code uses this function, which avoids @@ -198,6 +276,22 @@ namespace sdf public: static const std::string &EmbeddedSpec( const std::string &_filename, const bool _quiet); + /// \brief Get a string representation of an SDF specification file. + /// This function uses a built-in version of a .sdf file located in + /// the sdf directory. The parser.cc code uses this function, which avoids + /// touching the filesystem. + /// + /// Most people should not use this function. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Base name of the SDF specification file to + /// load. For example "root.sdf" or "world.sdf". + /// \return A string that contains the contents of the specified + /// _filename. An empty string is returned if the _filename could not be + /// found. + public: static const std::string &EmbeddedSpec( + sdf::Errors &_errors, const std::string &_filename); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index 5c33334c3..726d70b7e 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -37,7 +37,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the scene based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 38c3f1fce..36cea8495 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -144,7 +144,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the sensor based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 09590aef6..cf2ff772a 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -308,7 +308,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the surface based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index 0f5808851..d1c860bea 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -53,7 +53,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the visual based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 4d68a3dcc..e1c5123bd 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -62,7 +62,7 @@ namespace sdf /// \brief Get the schema file name accessor public: static inline std::string_view SchemaFile(); - + /// \brief Load the world based on a element pointer. This is *not* the /// usual entry point. Typical usage of the SDF DOM is through the Root /// object. @@ -281,6 +281,20 @@ namespace sdf /// \return True if there exists an actor with the given name. public: bool ActorNameExists(const std::string &_name) const; + /// \brief Get an actor based on a name. + /// \param[in] _name Name of the actor. + /// \return Pointer to the actor. Nullptr if an actor with the given name + /// does not exist. + /// \sa bool ActorNameExists(const std::string &_name) const + public: const Actor *ActorByName(const std::string &_name) const; + + /// \brief Get a mutable actor based on a name. + /// \param[in] _name Name of the actor. + /// \return Pointer to the actor. Nullptr if an actor with the given name + /// does not exist. + /// \sa bool ActorNameExists(const std::string &_name) const + public: Actor *ActorByName(const std::string &_name); + /// \brief Get the number of explicit frames that are immediate (not nested) /// children of this World object. /// \remark FrameByName() can find explicit frames that are not immediate diff --git a/src/Actor.cc b/src/Actor.cc index 0ca834536..5efaf9c98 100644 --- a/src/Actor.cc +++ b/src/Actor.cc @@ -821,9 +821,9 @@ void Actor::AddPlugin(const Plugin &_plugin) } ///////////////////////////////////////////////// -inline std::string_view Actor::SchemaFile() +inline std::string_view Actor::SchemaFile() { - static char kSchemaFile[] = "actor.sdf"; + static const char kSchemaFile[] = "actor.sdf"; return kSchemaFile; } diff --git a/src/AirPressure.cc b/src/AirPressure.cc index 1f576ed59..f35e841a3 100644 --- a/src/AirPressure.cc +++ b/src/AirPressure.cc @@ -141,9 +141,9 @@ sdf::ElementPtr AirPressure::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view AirPressure::SchemaFile() +inline std::string_view AirPressure::SchemaFile() { - static char kSchemaFile[] = "air_pressure.sdf"; + static const char kSchemaFile[] = "air_pressure.sdf"; return kSchemaFile; } diff --git a/src/AirSpeed.cc b/src/AirSpeed.cc index fd0dec211..0a9e77945 100644 --- a/src/AirSpeed.cc +++ b/src/AirSpeed.cc @@ -108,9 +108,9 @@ sdf::ElementPtr AirSpeed::ToElement() const } ///////////////////////////////////////////////// -inline std::string_view AirSpeed::SchemaFile() +inline std::string_view AirSpeed::SchemaFile() { - static char kSchemaFile[] = "air_speed.sdf"; + static const char kSchemaFile[] = "air_speed.sdf"; return kSchemaFile; } diff --git a/src/Altimeter.cc b/src/Altimeter.cc index f7556c485..d4f421fcb 100644 --- a/src/Altimeter.cc +++ b/src/Altimeter.cc @@ -171,9 +171,9 @@ sdf::ElementPtr Altimeter::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Altimeter::SchemaFile() +inline std::string_view Altimeter::SchemaFile() { - static char kSchemaFile[] = "altimeter.sdf"; + static const char kSchemaFile[] = "altimeter.sdf"; return kSchemaFile; } diff --git a/src/Atmosphere.cc b/src/Atmosphere.cc index 1b4459430..286b55d96 100644 --- a/src/Atmosphere.cc +++ b/src/Atmosphere.cc @@ -174,9 +174,9 @@ sdf::ElementPtr Atmosphere::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Atmosphere::SchemaFile() +inline std::string_view Atmosphere::SchemaFile() { - static char kSchemaFile[] = "atmosphere.sdf"; + static const char kSchemaFile[] = "atmosphere.sdf"; return kSchemaFile; } diff --git a/src/Camera.cc b/src/Camera.cc index 884ce36e1..a18b53ea8 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -1339,9 +1339,9 @@ sdf::ElementPtr Camera::ToElement() const } ///////////////////////////////////////////////// -inline std::string_view Camera::SchemaFile() +inline std::string_view Camera::SchemaFile() { - static char kSchemaFile[] = "camera.sdf"; + static const char kSchemaFile[] = "camera.sdf"; return kSchemaFile; } diff --git a/src/Collision.cc b/src/Collision.cc index 5a3fde531..d668a57f0 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -394,9 +394,9 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Collision::SchemaFile() +inline std::string_view Collision::SchemaFile() { - static char kSchemaFile[] = "collision.sdf"; + static const char kSchemaFile[] = "collision.sdf"; return kSchemaFile; } diff --git a/src/Converter.cc b/src/Converter.cc index 8947d3333..41d550df1 100644 --- a/src/Converter.cc +++ b/src/Converter.cc @@ -264,7 +264,7 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, } else if (name == "copy") { - Move(_elem, childElem, true); + Move(_elem, childElem, true, _errors); } else if (name == "map") { @@ -272,7 +272,7 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, } else if (name == "move") { - Move(_elem, childElem, false); + Move(_elem, childElem, false, _errors); } else if (name == "add") { @@ -923,7 +923,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, ///////////////////////////////////////////////// void Converter::Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy) + const bool _copy, + sdf::Errors &_errors) { SDF_ASSERT(_elem != NULL, "SDF element is NULL"); SDF_ASSERT(_moveElem != NULL, "Move element is NULL"); @@ -1024,7 +1025,8 @@ void Converter::Move(tinyxml2::XMLElement *_elem, if (toElemStr && !toAttrStr) { - tinyxml2::XMLNode *cloned = DeepClone(moveFrom->GetDocument(), moveFrom); + tinyxml2::XMLNode *cloned = DeepClone(_errors, moveFrom->GetDocument(), + moveFrom); tinyxml2::XMLElement *moveTo = static_cast(cloned); moveTo->SetValue(toName); diff --git a/src/Converter.hh b/src/Converter.hh index e992b124c..cfba5413f 100644 --- a/src/Converter.hh +++ b/src/Converter.hh @@ -108,9 +108,11 @@ namespace sdf /// \param[in] _moveElem A 'convert' element that describes the move /// operation. /// \param[in] _copy True to copy the element + /// \param[out] _errors Vector of errors. private: static void Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy); + const bool _copy, + sdf::Errors &_errors); /// \brief Add an element or attribute to an element. /// \param[in] _elem The element to receive the value. diff --git a/src/Converter_TEST.cc b/src/Converter_TEST.cc index 443774017..d56cba30a 100644 --- a/src/Converter_TEST.cc +++ b/src/Converter_TEST.cc @@ -3370,7 +3370,8 @@ TEST(Converter, World_17_to_18) ASSERT_TRUE(errors.empty()); // Compare converted xml with expected - std::string convertedXmlStr = ElementToString(xmlDoc.RootElement()); + std::string convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); std::string expectedXmlStr = R"( @@ -3393,7 +3394,9 @@ TEST(Converter, World_17_to_18) tinyxml2::XMLDocument expectedXmlDoc; expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -3509,7 +3512,8 @@ TEST(Converter, World_17_to_18) ASSERT_TRUE(errors.empty()); // Compare converted xml with expected - convertedXmlStr = ElementToString(xmlDoc.RootElement()); + convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); expectedXmlStr = R"( @@ -3577,7 +3581,9 @@ TEST(Converter, World_17_to_18) expectedXmlDoc.Clear(); expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // ------- Another flattened world in 1.7 format @@ -3616,7 +3622,8 @@ TEST(Converter, World_17_to_18) EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Compare converted xml with expected - convertedXmlStr = ElementToString(xmlDoc.RootElement()); + convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); expectedXmlStr = R"( @@ -3655,7 +3662,9 @@ TEST(Converter, World_17_to_18) expectedXmlDoc.Clear(); expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // Check some basic elements convertedElem = xmlDoc.FirstChildElement(); diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index e55f56c06..9b6ba0bc7 100644 --- a/src/ForceTorque.cc +++ b/src/ForceTorque.cc @@ -369,9 +369,9 @@ sdf::ElementPtr ForceTorque::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view ForceTorque::SchemaFile() +inline std::string_view ForceTorque::SchemaFile() { - static char kSchemaFile[] = "forcetorque.sdf"; - return kSchemaFile; + static const char kSchemaFile[] = "forcetorque.sdf"; + return kSchemaFile; } diff --git a/src/Frame.cc b/src/Frame.cc index bf5f4d243..5410f741c 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -252,9 +252,9 @@ sdf::ElementPtr Frame::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Frame::SchemaFile() +inline std::string_view Frame::SchemaFile() { - static char kSchemaFile[] = "frame.sdf"; + static const char kSchemaFile[] = "frame.sdf"; return kSchemaFile; } diff --git a/src/Geometry.cc b/src/Geometry.cc index ec87a01da..b01d128cd 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -453,9 +453,9 @@ sdf::ElementPtr Geometry::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Geometry::SchemaFile() +inline std::string_view Geometry::SchemaFile() { - static char kSchemaFile[] = "geometry.sdf"; + static const char kSchemaFile[] = "geometry.sdf"; return kSchemaFile; } diff --git a/src/Gui.cc b/src/Gui.cc index 6f61d738e..405063eb1 100644 --- a/src/Gui.cc +++ b/src/Gui.cc @@ -149,9 +149,9 @@ sdf::Plugins &Gui::Plugins() ///////////////////////////////////////////////// -inline std::string_view Gui::SchemaFile() +inline std::string_view Gui::SchemaFile() { - static char kSchemaFile[] = "gui.sdf"; + static const char kSchemaFile[] = "gui.sdf"; return kSchemaFile; } diff --git a/src/Imu.cc b/src/Imu.cc index f239bcf1a..033575bec 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -456,9 +456,9 @@ sdf::ElementPtr Imu::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Imu::SchemaFile() +inline std::string_view Imu::SchemaFile() { - static char kSchemaFile[] = "imu.sdf"; + static const char kSchemaFile[] = "imu.sdf"; return kSchemaFile; } diff --git a/src/Joint.cc b/src/Joint.cc index 4532b5b41..2ce4042a0 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -632,9 +632,9 @@ void Joint::ClearSensors() } ///////////////////////////////////////////////// -inline std::string_view Joint::SchemaFile() +inline std::string_view Joint::SchemaFile() { - static char kSchemaFile[] = "joint.sdf"; + static const char kSchemaFile[] = "joint.sdf"; return kSchemaFile; } diff --git a/src/Lidar.cc b/src/Lidar.cc index 98b5caffe..2098d1c52 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -452,9 +452,9 @@ sdf::ElementPtr Lidar::ToElement() const } ///////////////////////////////////////////////// -inline std::string_view Lidar::SchemaFile() +inline std::string_view Lidar::SchemaFile() { - static char kSchemaFile[] = "lidar.sdf"; + static const char kSchemaFile[] = "lidar.sdf"; return kSchemaFile; } diff --git a/src/Light.cc b/src/Light.cc index ca44f3db1..d007ea2dd 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -558,9 +558,9 @@ sdf::ElementPtr Light::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Light::SchemaFile() +inline std::string_view Light::SchemaFile() { - static char kSchemaFile[] = "light.sdf"; + static const char kSchemaFile[] = "light.sdf"; return kSchemaFile; } diff --git a/src/Link.cc b/src/Link.cc index cc6522966..133bad813 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -1062,9 +1062,9 @@ sdf::ElementPtr Link::ToElement() const } ///////////////////////////////////////////////// -inline std::string_view Link::SchemaFile() +inline std::string_view Link::SchemaFile() { - static char kSchemaFile[] = "link.sdf"; + static const char kSchemaFile[] = "link.sdf"; return kSchemaFile; } diff --git a/src/Magnetometer.cc b/src/Magnetometer.cc index 580b41129..7c154d000 100644 --- a/src/Magnetometer.cc +++ b/src/Magnetometer.cc @@ -157,9 +157,9 @@ sdf::ElementPtr Magnetometer::ToElement() const } ///////////////////////////////////////////////// -inline std::string_view Magnetometer::SchemaFile() +inline std::string_view Magnetometer::SchemaFile() { - static char kSchemaFile[] = "magnetometer.sdf"; + static const char kSchemaFile[] = "magnetometer.sdf"; return kSchemaFile; } diff --git a/src/Material.cc b/src/Material.cc index 0d908aa1e..18f993f89 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -530,9 +530,9 @@ sdf::ElementPtr Material::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Material::SchemaFile() +inline std::string_view Material::SchemaFile() { - static char kSchemaFile[] = "material.sdf"; + static const char kSchemaFile[] = "material.sdf"; return kSchemaFile; } diff --git a/src/Model.cc b/src/Model.cc index 0cb50476a..e541ee6fc 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -1416,9 +1416,9 @@ sdf::Frame Model::PrepareForMerge(sdf::Errors &_errors, } ///////////////////////////////////////////////// -inline std::string_view Model::SchemaFile() +inline std::string_view Model::SchemaFile() { - static char kSchemaFile[] = "model.sdf"; + static const char kSchemaFile[] = "model.sdf"; return kSchemaFile; } diff --git a/src/NavSat.cc b/src/NavSat.cc index 7372ff994..35ad1c2b6 100644 --- a/src/NavSat.cc +++ b/src/NavSat.cc @@ -193,9 +193,9 @@ bool NavSat::operator!=(const NavSat &_navsat) const } ///////////////////////////////////////////////// -inline std::string_view NavSat::SchemaFile() +inline std::string_view NavSat::SchemaFile() { - static char kSchemaFile[] = "navsat.sdf"; + static const char kSchemaFile[] = "navsat.sdf"; return kSchemaFile; } diff --git a/src/Noise.cc b/src/Noise.cc index 4f6c8dc11..32ec382c5 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -302,9 +302,9 @@ sdf::ElementPtr Noise::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Noise::SchemaFile() +inline std::string_view Noise::SchemaFile() { - static char kSchemaFile[] = "noise.sdf"; + static const char kSchemaFile[] = "noise.sdf"; return kSchemaFile; } diff --git a/src/ParamPassing.cc b/src/ParamPassing.cc index 1b80d09cf..5bf0c97a9 100644 --- a/src/ParamPassing.cc +++ b/src/ParamPassing.cc @@ -52,7 +52,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_MISSING, "Element identifier requires an element_id attribute, but the " "element_id is not set. Skipping element alteration:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -67,7 +67,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Missing name after double colons in element identifier. " "Skipping element alteration:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -83,7 +83,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Action [" + actionStr + "] is not a valid action. Skipping " - "element alteration:\n" + ElementToString(childElemXml) + "element alteration:\n" + ElementToString(_errors, childElemXml) }); continue; } @@ -102,7 +102,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_MISSING, "Element to be added is missing a 'name' attribute. " "Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -112,7 +112,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "The 'name' attribute can not be empty. Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -129,7 +129,7 @@ void updateParams(const ParserConfig &_config, + " element_id='" + childElemXml->Attribute("element_id") + "'> because element already exists in included model. " + "Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -157,7 +157,8 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ELEMENT_MISSING, "Could not find element <" + std::string(childElemXml->Name()) + " element_id='" + childElemXml->Attribute("element_id") + "'>. " + - "Skipping element modification:\n" + ElementToString(childElemXml) + "Skipping element modification:\n" + + ElementToString(_errors, childElemXml) }); continue; } @@ -194,7 +195,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ELEMENT_INVALID, "Unable to convert XML to SDF. Skipping element replacement:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -355,7 +356,7 @@ ElementPtr initElementDescription(const tinyxml2::XMLElement *_xml, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Element [" + std::string(_xml->Name()) + "] is not a defined " "SDF element. Skipping element alteration\n: " - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); return nullptr; } @@ -384,7 +385,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Missing an action attribute. Skipping child element modification " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -397,7 +398,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "child element modification with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -411,7 +412,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element removal " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); } else @@ -430,7 +431,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element modification " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); } else @@ -455,7 +456,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "child element modification with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -468,7 +469,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Unable to convert XML to SDF. Skipping child element alteration " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -486,7 +487,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element replacement " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -498,7 +499,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Replacement element is missing a 'name' attribute. " "Skipping element replacement <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) - + "'>:\n" + ElementToString(xmlChild) + + "'>:\n" + ElementToString(_errors, xmlChild) }); continue; } @@ -525,7 +526,7 @@ void add(const ParserConfig &_config, const std::string &_source, { _errors.push_back({ErrorCode::ELEMENT_INVALID, "Unable to convert XML to SDF. Skipping element addition:\n" - + ElementToString(_childXml) + + ElementToString(_errors, _childXml) }); } } @@ -557,7 +558,8 @@ void modifyAttributes(tinyxml2::XMLElement *_xml, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Attribute [" + attrName + "] is invalid. " - "Skipping attribute modification in:\n" + ElementToString(_xml) + "Skipping attribute modification in:\n" + + ElementToString(_errors, _xml) }); continue; } @@ -582,7 +584,7 @@ void modifyChildren(tinyxml2::XMLElement *_xml, { _errors.push_back({ErrorCode::ELEMENT_MISSING, "Could not find element [" + elemName + "]. " - "Skipping modification for:\n" + ElementToString(_xml) + "Skipping modification for:\n" + ElementToString(_errors, _xml) }); continue; } @@ -599,7 +601,7 @@ void modifyChildren(tinyxml2::XMLElement *_xml, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Value [" + std::string(xmlChild->GetText()) + "] for element [" + elemName + "] is invalid. Skipping modification for:\n" - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); continue; } @@ -620,9 +622,9 @@ void modifyChildren(tinyxml2::XMLElement *_xml, // sdf has child elements but no children were specified in xml std::stringstream ss; ss << "No modifications for element " - << ElementToString(xmlChild) + << ElementToString(_errors, xmlChild) << " provided, skipping modification for:\n" - << ElementToString(_xml); + << ElementToString(_errors, _xml); Error err(ErrorCode::WARNING, ss.str()); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), err, _errors); @@ -650,7 +652,7 @@ void modify(tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Value [" + std::string(_xml->GetText()) + "] for element [" + std::string(_xml->Name()) + "] is invalid. Skipping modification for:\n" - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); } } @@ -688,7 +690,7 @@ void remove(const tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + std::string(xmlParent->Name()) + " element_id='" + std::string(xmlParent->Attribute("element_id")) + "'> with parent <" + std::string(_xml->Name()) + ">:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc index 86bb64ab7..05809f548 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -575,9 +575,9 @@ sdf::ElementPtr ParticleEmitter::ToElement(sdf::Errors &_errors) const ///////////////////////////////////////////////// -inline std::string_view ParticleEmitter::SchemaFile() +inline std::string_view ParticleEmitter::SchemaFile() { - static char kSchemaFile[] = "particle_emitter.sdf"; + static const char kSchemaFile[] = "particle_emitter.sdf"; return kSchemaFile; } diff --git a/src/Physics.cc b/src/Physics.cc index c1ca03488..50b0d726f 100644 --- a/src/Physics.cc +++ b/src/Physics.cc @@ -234,9 +234,9 @@ sdf::ElementPtr Physics::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Physics::SchemaFile() +inline std::string_view Physics::SchemaFile() { - static char kSchemaFile[] = "physics.sdf"; + static const char kSchemaFile[] = "physics.sdf"; return kSchemaFile; } diff --git a/src/Plugin.cc b/src/Plugin.cc index 637f12334..78e1de990 100644 --- a/src/Plugin.cc +++ b/src/Plugin.cc @@ -313,9 +313,9 @@ bool Plugin::operator!=(const Plugin &_plugin) const } ///////////////////////////////////////////////// -inline std::string_view Plugin::SchemaFile() +inline std::string_view Plugin::SchemaFile() { - static char kSchemaFile[] = "plugin.sdf"; + static const char kSchemaFile[] = "plugin.sdf"; return kSchemaFile; } diff --git a/src/Projector.cc b/src/Projector.cc index 8dbbf6457..555b1cdd7 100644 --- a/src/Projector.cc +++ b/src/Projector.cc @@ -333,9 +333,9 @@ sdf::ElementPtr Projector::ToElement() const } ///////////////////////////////////////////////// -inline std::string_view Projector::SchemaFile() +inline std::string_view Projector::SchemaFile() { - static char kSchemaFile[] = "projector.sdf"; + static const char kSchemaFile[] = "projector.sdf"; return kSchemaFile; } diff --git a/src/Root.cc b/src/Root.cc index 2449b5a49..1831e91fa 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -646,9 +646,9 @@ sdf::ElementPtr Root::ToElement(const OutputConfig &_config) const } ///////////////////////////////////////////////// -inline std::string_view Root::SchemaFile() +inline std::string_view Root::SchemaFile() { - static char kSchemaFile[] = "root.sdf"; + static const char kSchemaFile[] = "root.sdf"; return kSchemaFile; } diff --git a/src/SDF.cc b/src/SDF.cc index 7a43d0f9a..e033aac2b 100644 --- a/src/SDF.cc +++ b/src/SDF.cc @@ -27,11 +27,13 @@ #include "sdf/parser.hh" #include "sdf/Assert.hh" #include "sdf/Console.hh" +#include "sdf/Error.hh" #include "sdf/Filesystem.hh" #include "sdf/SDFImpl.hh" #include "SDFImplPrivate.hh" #include "sdf/sdf_config.h" #include "EmbeddedSdf.hh" +#include "Utils.hh" #include @@ -61,14 +63,40 @@ void setFindCallback(std::function _cb) ///////////////////////////////////////////////// std::string findFile( const std::string &_filename, bool _searchLocalPath, bool _useCallback) +{ + sdf::Errors errors; + std::string result = findFile(errors, _filename, _searchLocalPath, + _useCallback, ParserConfig::GlobalConfig()); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +std::string findFile( + sdf::Errors &_errors, const std::string &_filename, bool _searchLocalPath, + bool _useCallback) { return findFile( - _filename, _searchLocalPath, _useCallback, ParserConfig::GlobalConfig()); + _errors, _filename, _searchLocalPath, + _useCallback, ParserConfig::GlobalConfig()); } ///////////////////////////////////////////////// std::string findFile(const std::string &_filename, bool _searchLocalPath, bool _useCallback, const ParserConfig &_config) +{ + sdf::Errors errors; + std::string result = findFile(errors, _filename, _searchLocalPath, + _useCallback, _config); + sdf::throwOrPrintErrors(errors); + return result; +} + + +///////////////////////////////////////////////// +std::string findFile(sdf::Errors &_errors, const std::string &_filename, + bool _searchLocalPath, bool _useCallback, + const ParserConfig &_config) { // Check to see if _filename is URI. If so, resolve the URI path. for (const auto &[uriScheme, paths] : _config.URIPathMap()) @@ -162,8 +190,9 @@ std::string findFile(const std::string &_filename, bool _searchLocalPath, { if (!_config.FindFileCallback()) { - sdferr << "Tried to use callback in sdf::findFile(), but the callback " - "is empty. Did you call sdf::setFindCallback()?\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Tried to use callback in sdf::findFile(), but the callback " + "is empty. Did you call sdf::setFindCallback()?"}); return std::string(); } else @@ -195,13 +224,29 @@ SDF::~SDF() ///////////////////////////////////////////////// void SDF::PrintDescription() { - this->Root()->PrintDescription(""); + sdf::Errors errors; + this->PrintDescription(errors); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::PrintDescription(sdf::Errors &_errors) +{ + this->Root()->PrintDescription(_errors, ""); } ///////////////////////////////////////////////// void SDF::PrintValues(const PrintConfig &_config) { - this->Root()->PrintValues("", _config); + sdf::Errors errors; + this->PrintValues(errors, _config); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::PrintValues(sdf::Errors &_errors, const PrintConfig &_config) +{ + this->Root()->PrintValues(_errors, "", _config); } ///////////////////////////////////////////////// @@ -319,13 +364,22 @@ void SDF::PrintDoc() ///////////////////////////////////////////////// void SDF::Write(const std::string &_filename) { - std::string string = this->Root()->ToString(""); + sdf::Errors errors; + this->Write(errors, _filename); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::Write(sdf::Errors &_errors, const std::string &_filename) +{ + std::string string = this->Root()->ToString(_errors, ""); std::ofstream out(_filename.c_str(), std::ios::out); if (!out) { - sdferr << "Unable to open file[" << _filename << "] for writing\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Unable to open file[" + _filename + "] for writing."}); return; } out << string; @@ -334,6 +388,16 @@ void SDF::Write(const std::string &_filename) ///////////////////////////////////////////////// std::string SDF::ToString(const PrintConfig &_config) const +{ + sdf::Errors errors; + std::string result = this->ToString(errors, _config); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +std::string SDF::ToString(sdf::Errors &_errors, + const PrintConfig &_config) const { std::ostringstream stream; @@ -343,7 +407,7 @@ std::string SDF::ToString(const PrintConfig &_config) const stream << "\n"; } - stream << this->Root()->ToString("", _config); + stream << this->Root()->ToString(_errors, "", _config); if (this->Root()->GetName() != "sdf") { @@ -355,11 +419,21 @@ std::string SDF::ToString(const PrintConfig &_config) const ///////////////////////////////////////////////// void SDF::SetFromString(const std::string &_sdfData) +{ + sdf::Errors errors; + this->SetFromString(errors, _sdfData); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::SetFromString(sdf::Errors &_errors, + const std::string &_sdfData) { sdf::initFile("root.sdf", this->Root()); - if (!sdf::readString(_sdfData, this->Root())) + if (!sdf::readString(_sdfData, this->Root(), _errors)) { - sdferr << "Unable to parse sdf string[" << _sdfData << "]\n"; + _errors.push_back({sdf::ErrorCode::PARSING_ERROR, + "Unable to parse sdf string[" + _sdfData + "]"}); } } @@ -429,12 +503,21 @@ void SDF::Version(const std::string &_version) ///////////////////////////////////////////////// ElementPtr SDF::WrapInRoot(const ElementPtr &_sdf) +{ + sdf::Errors errors; + ElementPtr result = SDF::WrapInRoot(errors, _sdf); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +ElementPtr SDF::WrapInRoot(sdf::Errors &_errors, const ElementPtr &_sdf) { ElementPtr root(new Element); root->SetName("sdf"); std::stringstream v; v << Version(); - root->AddAttribute("version", "string", v.str(), true, "version"); + root->AddAttribute("version", "string", v.str(), true, _errors, "version"); root->InsertElement(_sdf->Clone()); return root; } @@ -442,6 +525,19 @@ ElementPtr SDF::WrapInRoot(const ElementPtr &_sdf) ///////////////////////////////////////////////// const std::string &SDF::EmbeddedSpec( const std::string &_filename, const bool _quiet) +{ + sdf::Errors errors; + const std::string &result = EmbeddedSpec(errors, _filename); + if (!_quiet) + { + sdf::throwOrPrintErrors(errors); + } + return result; +} + +///////////////////////////////////////////////// +const std::string &SDF::EmbeddedSpec( + sdf::Errors &_errors, const std::string &_filename) { try { @@ -450,9 +546,9 @@ const std::string &SDF::EmbeddedSpec( } catch(const std::out_of_range &) { - if (!_quiet) - sdferr << "Unable to find SDF filename[" << _filename << "] with " - << "version " << SDF::Version() << "\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Unable to find SDF filename[" + _filename + "] with " + + "version " + SDF::Version()}); } // An empty SDF string is returned if a query into the embeddedSdf map fails. diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc index 44bb9f284..8c8289a9d 100644 --- a/src/SDF_TEST.cc +++ b/src/SDF_TEST.cc @@ -742,6 +742,46 @@ bool create_new_temp_dir(std::string &_new_temp_path) return true; } +///////////////////////////////////////////////// +TEST(SDF, ErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; + + // Test findFile + EXPECT_EQ(sdf::findFile(errors, "adfjialkas31", false, true), ""); + EXPECT_EQ(errors.size(), 1) << errors; + EXPECT_NE(std::string::npos, + errors[0].Message().find("Tried to use callback in sdf::findFile(), " + "but the callback is empty. Did you call " + "sdf::setFindCallback()?")) + << errors[0].Message(); + errors.clear(); + + sdf::SDF sdf; + sdf.SetFromString(errors, "banana"); + EXPECT_EQ(errors.size(), 2) << errors; + EXPECT_NE(std::string::npos, + errors[0].Message().find("Error parsing XML from string")) + << errors[0].Message(); + EXPECT_NE(std::string::npos, + errors[1].Message().find("Unable to parse sdf string[banana]")) + << errors[1].Message(); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} + ///////////////////////////////////////////////// bool g_findFileCbCalled = false; std::string findFileCb(const std::string &) diff --git a/src/Scene.cc b/src/Scene.cc index cf4fcfcd9..c1b7ef2e6 100644 --- a/src/Scene.cc +++ b/src/Scene.cc @@ -214,9 +214,9 @@ sdf::ElementPtr Scene::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Scene::SchemaFile() +inline std::string_view Scene::SchemaFile() { - static char kSchemaFile[] = "scene.sdf"; + static const char kSchemaFile[] = "scene.sdf"; return kSchemaFile; } diff --git a/src/Sensor.cc b/src/Sensor.cc index 4a2ad4864..fcca80fbe 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -850,9 +850,9 @@ void Sensor::AddPlugin(const Plugin &_plugin) } ///////////////////////////////////////////////// -inline std::string_view Sensor::SchemaFile() +inline std::string_view Sensor::SchemaFile() { - static char kSchemaFile[] = "sensor.sdf"; + static const char kSchemaFile[] = "sensor.sdf"; return kSchemaFile; } diff --git a/src/Surface.cc b/src/Surface.cc index 7772583e6..c8b20e775 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -772,9 +772,9 @@ sdf::ElementPtr Surface::ToElement(sdf::Errors &_errors) const } ///////////////////////////////////////////////// -inline std::string_view Surface::SchemaFile() +inline std::string_view Surface::SchemaFile() { - static char kSchemaFile[] = "surface.sdf"; + static const char kSchemaFile[] = "surface.sdf"; return kSchemaFile; } diff --git a/src/Visual.cc b/src/Visual.cc index 9682439ec..eba6d018e 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -380,9 +380,9 @@ void Visual::AddPlugin(const Plugin &_plugin) } ///////////////////////////////////////////////// -inline std::string_view Visual::SchemaFile() +inline std::string_view Visual::SchemaFile() { - static char kSchemaFile[] = "visual.sdf"; + static const char kSchemaFile[] = "visual.sdf"; return kSchemaFile; } diff --git a/src/World.cc b/src/World.cc index 4a884f6fb..275b55e59 100644 --- a/src/World.cc +++ b/src/World.cc @@ -773,14 +773,27 @@ Actor *World::ActorByIndex(uint64_t _index) ///////////////////////////////////////////////// bool World::ActorNameExists(const std::string &_name) const { - for (auto const &a : this->dataPtr->actors) + return nullptr != this->ActorByName(_name); +} + +///////////////////////////////////////////////// +const Actor *World::ActorByName(const std::string &_name) const +{ + for (const Actor &a : this->dataPtr->actors) { if (a.Name() == _name) { - return true; + return &a; } } - return false; + return nullptr; +} + +///////////////////////////////////////////////// +Actor *World::ActorByName(const std::string &_name) +{ + return const_cast( + static_cast(this)->ActorByName(_name)); } ////////////////////////////////////////////////// @@ -1273,9 +1286,9 @@ void World::AddPlugin(const Plugin &_plugin) } ///////////////////////////////////////////////// -inline std::string_view World::SchemaFile() +inline std::string_view World::SchemaFile() { - static char kSchemaFile[] = "world.sdf"; + static const char kSchemaFile[] = "world.sdf"; return kSchemaFile; } diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 3100ce166..ad1e0d2d5 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -64,6 +64,9 @@ TEST(DOMWorld, Construction) EXPECT_EQ(nullptr, world.ModelByName("a::b::c")); EXPECT_EQ(nullptr, world.ModelByName("::::")); + EXPECT_EQ(nullptr, world.ActorByName("")); + EXPECT_EQ(nullptr, world.ActorByName("default")); + EXPECT_EQ(0u, world.FrameCount()); EXPECT_EQ(nullptr, world.FrameByIndex(0)); EXPECT_EQ(nullptr, world.FrameByIndex(1)); @@ -458,15 +461,27 @@ TEST(DOMWorld, AddActor) EXPECT_EQ(1u, world.ActorCount()); EXPECT_FALSE(world.AddActor(actor)); EXPECT_EQ(1u, world.ActorCount()); + EXPECT_NE(nullptr, world.ActorByName("actor1")); world.ClearActors(); EXPECT_EQ(0u, world.ActorCount()); + EXPECT_EQ(nullptr, world.ActorByName("actor1")); EXPECT_TRUE(world.AddActor(actor)); EXPECT_EQ(1u, world.ActorCount()); const sdf::Actor *actorFromWorld = world.ActorByIndex(0); ASSERT_NE(nullptr, actorFromWorld); EXPECT_EQ(actorFromWorld->Name(), actor.Name()); + + const sdf::Actor *actorFromWorldByName = world.ActorByName("actor1"); + ASSERT_NE(nullptr, actorFromWorldByName); + EXPECT_EQ(actorFromWorldByName->Name(), actor.Name()); + + sdf::Actor *mutableActorFromWorldByName = world.ActorByName("actor1"); + ASSERT_NE(nullptr, mutableActorFromWorldByName); + EXPECT_EQ(mutableActorFromWorldByName->Name(), actor.Name()); + mutableActorFromWorldByName->SetName("new_name"); + EXPECT_NE(mutableActorFromWorldByName->Name(), actor.Name()); } ///////////////////////////////////////////////// diff --git a/src/XmlUtils.cc b/src/XmlUtils.cc index 4dbceab71..5d3709707 100644 --- a/src/XmlUtils.cc +++ b/src/XmlUtils.cc @@ -15,7 +15,9 @@ * */ #include +#include +#include "Utils.hh" #include "XmlUtils.hh" #include "sdf/Console.hh" @@ -27,27 +29,41 @@ inline namespace SDF_VERSION_NAMESPACE { ///////////////////////////////////////////////// tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, const tinyxml2::XMLNode *_src) +{ + sdf::Errors errors; + tinyxml2::XMLNode *result = DeepClone(errors, _doc, _src); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +tinyxml2::XMLNode *DeepClone(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + const tinyxml2::XMLNode *_src) { if (_src == nullptr) { - sdferr << "Pointer to XML node _src is NULL\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Pointer to XML node _src is NULL"}); return nullptr; } tinyxml2::XMLNode *copy = _src->ShallowClone(_doc); if (copy == nullptr) { - sdferr << "Failed to clone node " << _src->Value() << "\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Failed to clone node " + std::string(_src->Value())}); return nullptr; } for (const tinyxml2::XMLNode *node = _src->FirstChild(); node != nullptr; node = node->NextSibling()) { - auto *childCopy = DeepClone(_doc, node); + auto *childCopy = DeepClone(_errors, _doc, node); if (childCopy == nullptr) { - sdferr << "Failed to clone child " << node->Value() << "\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Failed to clone child " + std::string(node->Value())}); return nullptr; } copy->InsertEndChild(childCopy); @@ -57,11 +73,13 @@ tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, } ///////////////////////////////////////////////// -std::string ElementToString(const tinyxml2::XMLElement *_elem) +std::string ElementToString(sdf::Errors &_errors, + const tinyxml2::XMLElement *_elem) { if (_elem == nullptr) { - sdferr << "Pointer to XML Element _elem is nullptr\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Pointer to XML Element _elem is nullptr"}); return ""; } diff --git a/src/XmlUtils.hh b/src/XmlUtils.hh index 7ad0c30d4..522458aad 100644 --- a/src/XmlUtils.hh +++ b/src/XmlUtils.hh @@ -41,10 +41,26 @@ namespace sdf tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, const tinyxml2::XMLNode *_src); + /// \brief Perform a deep copy of an XML Node + /// + /// This copies an XMLNode _src and all of its decendants + /// into a specified XMLDocument. + /// + /// \param[out] _errors Vector of errors + /// \param[in] _doc Document in which to place the copied node + /// \param[in] _src The node to deep copy + /// \returns The newly copied node upon success OR + /// nullptr if an error occurs. + tinyxml2::XMLNode *DeepClone(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + const tinyxml2::XMLNode *_src); + /// \brief Converts the XML Element to a string + /// \param[out] _errors Vector of errors /// \param[in] _elem Element to be converted /// \return The string representation - std::string ElementToString(const tinyxml2::XMLElement *_elem); + std::string ElementToString(sdf::Errors &_errors, + const tinyxml2::XMLElement *_elem); } } #endif diff --git a/src/XmlUtils_TEST.cc b/src/XmlUtils_TEST.cc index aef79262d..4190b058b 100644 --- a/src/XmlUtils_TEST.cc +++ b/src/XmlUtils_TEST.cc @@ -37,7 +37,9 @@ TEST(XMLUtils, DeepClone) ASSERT_EQ(tinyxml2::XML_SUCCESS, ret); auto root = oldDoc.FirstChild(); - auto newRoot = sdf::DeepClone(&newDoc, root); + sdf::Errors errors; + auto newRoot = sdf::DeepClone(errors, &newDoc, root); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_STREQ("document", newRoot->ToElement()->Name()); @@ -53,3 +55,48 @@ TEST(XMLUtils, DeepClone) auto childB_text = newChildB->ToElement()->GetText(); EXPECT_STREQ("Hello World", childB_text); } + +///////////////////////////////////////////////// +TEST(XMLUtils, InvalidDeepClone) +{ + sdf::Errors errors; + auto newRoot = sdf::DeepClone(errors, nullptr, nullptr); + EXPECT_EQ(nullptr, newRoot); + EXPECT_EQ(1u, errors.size()) << errors; + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::XML_ERROR); +} + +///////////////////////////////////////////////// +TEST(XMLUtils, ElementToString) +{ + tinyxml2::XMLDocument doc; + + std::string docXml = +R"( + + Hello World + + +)"; + + auto ret = doc.Parse(docXml.c_str()); + ASSERT_EQ(tinyxml2::XML_SUCCESS, ret); + + auto root = doc.FirstChild(); + sdf::Errors errors; + std::string docString = sdf::ElementToString(errors, root->ToElement()); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(docXml, docString); +} + +///////////////////////////////////////////////// +TEST(XMLUtils, InvalidElementToString) +{ + sdf::Errors errors; + std::string docString = sdf::ElementToString(errors, nullptr); + EXPECT_TRUE(docString.empty()); + EXPECT_EQ(1u, errors.size()) << errors; + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::XML_ERROR); +}