From 3e76d92ee4853ed95eaff30b2a3c6352e9a44ff0 Mon Sep 17 00:00:00 2001 From: Stefan Profanter Date: Thu, 4 Feb 2021 23:29:40 +0100 Subject: [PATCH 01/32] Ensure relocatable config files (#419) * fix: Use PACKAGE_PREFIX_DIR to ensure relocatable CMake install for conan Signed-off-by: Stefan Profanter * Compute relative path for pkgconfig folder. This accounts for arch-dependent lib folders. Signed-off-by: Steve Peters Co-authored-by: Steve Peters Co-authored-by: Jose Luis Rivero --- CMakeLists.txt | 8 +++++++- cmake/sdf_config.cmake.in | 8 ++++---- cmake/sdformat_pc.in | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f4a2e5a2f..546f1166f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -304,10 +304,16 @@ else (build_errors) ######################################## # Make the package config file + set(PC_CONFIG_INSTALL_DIR ${LIB_INSTALL_DIR}/pkgconfig) + file(RELATIVE_PATH + PC_CONFIG_RELATIVE_PATH_TO_PREFIX + "${CMAKE_INSTALL_PREFIX}/${PC_CONFIG_INSTALL_DIR}" + "${CMAKE_INSTALL_PREFIX}" + ) configure_file(${CMAKE_SOURCE_DIR}/cmake/sdformat_pc.in ${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_NAME_LOWER}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_NAME_LOWER}.pc DESTINATION - ${LIB_INSTALL_DIR}/pkgconfig COMPONENT pkgconfig) + ${PC_CONFIG_INSTALL_DIR} COMPONENT pkgconfig) ######################################## # Configure documentation uploader diff --git a/cmake/sdf_config.cmake.in b/cmake/sdf_config.cmake.in index f4c511fa9..e51abeefd 100644 --- a/cmake/sdf_config.cmake.in +++ b/cmake/sdf_config.cmake.in @@ -17,14 +17,14 @@ if(NOT TARGET @sdf_import_target_name@) include("${CMAKE_CURRENT_LIST_DIR}/@sdf_target_output_filename@") endif() -list(APPEND @PKG_NAME@_INCLUDE_DIRS "@CMAKE_INSTALL_PREFIX@/include/sdformat-@SDF_VERSION@") +list(APPEND @PKG_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include/sdformat-@SDF_VERSION@") -list(APPEND @PKG_NAME@_CFLAGS "-I@CMAKE_INSTALL_PREFIX@/include/sdformat-@SDF_VERSION@") +list(APPEND @PKG_NAME@_CFLAGS "-I${PACKAGE_PREFIX_DIR}/include/sdformat-@SDF_VERSION@") if (NOT WIN32) list(APPEND @PKG_NAME@_CXX_FLAGS "${@PKG_NAME@_CFLAGS} -std=c++17") endif() -list(APPEND @PKG_NAME@_LIBRARY_DIRS "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@") +list(APPEND @PKG_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/@CMAKE_INSTALL_LIBDIR@") set(@PKG_NAME@_LIBRARIES @sdf_import_target_name@) @@ -37,4 +37,4 @@ find_package(ignition-math@IGN_MATH_VER@) list(APPEND @PKG_NAME@_INCLUDE_DIRS ${IGNITION-MATH_INCLUDE_DIRS}) list(APPEND @PKG_NAME@_LIBRARY_DIRS ${IGNITION-MATH_LIBRARY_DIRS}) -list(APPEND @PKG_NAME@_LDFLAGS "-L@CMAKE_INSTALL_PREFIX@/@LIB_INSTALL_DIR@") +list(APPEND @PKG_NAME@_LDFLAGS "-L${PACKAGE_PREFIX_DIR}/@LIB_INSTALL_DIR@") diff --git a/cmake/sdformat_pc.in b/cmake/sdformat_pc.in index c95a781e2..9ccfa4fbc 100644 --- a/cmake/sdformat_pc.in +++ b/cmake/sdformat_pc.in @@ -1,4 +1,4 @@ -prefix="@CMAKE_INSTALL_PREFIX@" +prefix=${pcfiledir}/@PC_CONFIG_RELATIVE_PATH_TO_PREFIX@ libdir=${prefix}/@LIB_INSTALL_DIR@ includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ From fb48f0245d4ee8d3dfd666f7a38ef81cfd360094 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 10 Aug 2022 20:15:45 -0500 Subject: [PATCH 02/32] Add option to skip python bindings (#1106) Signed-off-by: Michael Carroll --- CMakeLists.txt | 37 +++++++++++++++++++++++-------------- 1 file changed, 23 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 584b7c89b..fe46c5c06 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,14 +22,19 @@ find_package(gz-cmake3 REQUIRED) set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR}) ######################################## +option(SKIP_PYBIND11 + "Skip generating Python bindings via pybind11" + OFF) + # Python interfaces vars -option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION +include(CMakeDependentOption) +cmake_dependent_option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION "Install python modules in standard system paths in the system" - OFF) + OFF "NOT SKIP_PYBIND11" OFF) -option(USE_DIST_PACKAGES_FOR_PYTHON +cmake_dependent_option(USE_DIST_PACKAGES_FOR_PYTHON "Use dist-packages instead of site-package to install python modules" - OFF) + OFF "NOT SKIP_PYBIND11" OFF) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -133,16 +138,20 @@ if (BUILD_SDF) else() message (STATUS "Searching for Python - found version ${Python3_VERSION}.") - set(PYBIND11_PYTHON_VERSION 3) - find_package(pybind11 2.4 QUIET) + if (SKIP_PYBIND11) + message(STATUS "SKIP_PYBIND11 set - disabling python bindings") + else() + set(PYBIND11_PYTHON_VERSION 3) + find_package(pybind11 2.4 QUIET) - if (${pybind11_FOUND}) - find_package(Python3 ${GZ_PYTHON_VERSION} REQUIRED COMPONENTS Development) - message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") - else() - GZ_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") - message (STATUS "Searching for pybind11 - not found.") - endif() + if (${pybind11_FOUND}) + find_package(Python3 ${GZ_PYTHON_VERSION} REQUIRED COMPONENTS Development) + message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + else() + GZ_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") + message (STATUS "Searching for pybind11 - not found.") + endif() + endif() endif() gz_configure_build(HIDE_SYMBOLS_BY_DEFAULT QUIT_IF_BUILD_ERRORS) @@ -152,7 +161,7 @@ if (BUILD_SDF) add_subdirectory(sdf) add_subdirectory(conf) add_subdirectory(doc) - if (${pybind11_FOUND}) + if (pybind11_FOUND AND NOT SKIP_PYBIND11) add_subdirectory(python) endif() endif(BUILD_SDF) From 12e9c1f8d291a732754f04a7bb8fe34f6f590b9c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 11 Aug 2022 17:32:36 -0700 Subject: [PATCH 03/32] Remove unused macros from config.hh (#1108) HAVE_URDFDOM and USE_INTERNAL_URDF aren't used in C++ code and should be removed. Signed-off-by: Steve Peters --- include/sdf/config.hh.in | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/sdf/config.hh.in b/include/sdf/config.hh.in index 4d5612030..d1b3db532 100644 --- a/include/sdf/config.hh.in +++ b/include/sdf/config.hh.in @@ -45,8 +45,6 @@ #define SDF_VERSION_HEADER "Simulation Description Format (SDF), version ${SDF_PROTOCOL_VERSION}\nCopyright (C) 2014 Open Source Robotics Foundation.\nReleased under the Apache 2 License.\nhttp://gazebosim.org/sdf\n\n" -#cmakedefine HAVE_URDFDOM 1 -#cmakedefine USE_INTERNAL_URDF 1 #cmakedefine SDFORMAT_DISABLE_CONSOLE_LOGFILE 1 #define SDF_SHARE_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" From 1af61c906dab1a52b39fee7dfeafdccba68dae7c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 17 Aug 2022 14:17:05 -0700 Subject: [PATCH 04/32] 1.10/joint.sdf: better default limit values (#1112) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Use `±inf` instead of `±1e16` for position limits * Use `inf` instead of `-1` for effort, velocity limits Signed-off-by: Steve Peters --- Migration.md | 6 +++ python/test/pyJointAxis_TEST.py | 4 +- sdf/1.10/joint.sdf | 16 ++++---- src/JointAxis.cc | 60 ++++++++++++++---------------- src/JointAxis_TEST.cc | 5 ++- test/integration/joint_axis_dom.cc | 4 +- 6 files changed, 48 insertions(+), 47 deletions(-) diff --git a/Migration.md b/Migration.md index 7a98ba21c..0d297b7ca 100644 --- a/Migration.md +++ b/Migration.md @@ -538,6 +538,12 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. ### Modifications +1. **joint.sdf**: axis limits default values have changed + + `//limit/lower`: `-inf` (formerly `-1e16`) + + `//limit/upper`: `inf` (formerly `1e16`) + + `//limit/velocity`: `inf` (formerly `-1`) + + `//limit/effort`: `inf` (formerly `-1`) + 1. **plugin.sdf**: name attribute is now optional with empty default value. ## SDFormat specification 1.8 to 1.9 diff --git a/python/test/pyJointAxis_TEST.py b/python/test/pyJointAxis_TEST.py index d9ac24aab..b5f68f34f 100644 --- a/python/test/pyJointAxis_TEST.py +++ b/python/test/pyJointAxis_TEST.py @@ -29,8 +29,8 @@ def test_default_construction(self): self.assertAlmostEqual(0.0, axis.friction()) self.assertAlmostEqual(0.0, axis.spring_reference()) self.assertAlmostEqual(0.0, axis.spring_stiffness()) - self.assertAlmostEqual(-1e16, axis.lower()) - self.assertAlmostEqual(1e16, axis.upper()) + self.assertAlmostEqual(-math.inf, axis.lower()) + self.assertAlmostEqual(math.inf, axis.upper()) self.assertAlmostEqual(math.inf, axis.effort()) self.assertAlmostEqual(math.inf, axis.max_velocity()) self.assertAlmostEqual(1e8, axis.stiffness()) diff --git a/sdf/1.10/joint.sdf b/sdf/1.10/joint.sdf index d472feefa..04d5f52e1 100644 --- a/sdf/1.10/joint.sdf +++ b/sdf/1.10/joint.sdf @@ -74,16 +74,16 @@ specifies the limits of this joint - + Specifies the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + Specifies the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + A value for enforcing the maximum joint effort applied. Limit is not enforced if value is negative. - + A value for enforcing the maximum joint velocity. @@ -132,16 +132,16 @@ - + An attribute specifying the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + An attribute specifying the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + An attribute for enforcing the maximum joint effort applied by Joint::SetForce. Limit is not enforced if value is negative. - + (not implemented) An attribute for enforcing the maximum joint velocity. diff --git a/src/JointAxis.cc b/src/JointAxis.cc index de3ef479d..735254ea1 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include @@ -41,10 +42,6 @@ class sdf::JointAxis::Implementation /// \brief Frame in which xyz is expressed in. public: std::string xyzExpressedIn = ""; - /// \brief Flag to interpret the axis xyz element in the parent model - /// frame instead of joint frame. - public: bool useParentModelFrame = false; - /// \brief The physical velocity dependent viscous damping coefficient /// of the joint. public: double damping = 0.0; @@ -60,18 +57,18 @@ class sdf::JointAxis::Implementation /// \brief Specifies the lower joint limit (radians for revolute joints, /// meters for prismatic joints). Omit if joint is continuous. - public: double lower = -1e16; + public: double lower = -std::numeric_limits::infinity(); /// \brief Specifies the upper joint limit (radians for revolute joints, /// meters for prismatic joints). Omit if joint is continuous. - public: double upper = 1e16; + public: double upper = std::numeric_limits::infinity(); /// \brief A value for enforcing the maximum joint effort applied. /// Limit is not enforced if value is negative. - public: double effort = -1; + public: double effort = std::numeric_limits::infinity(); /// \brief A value for enforcing the maximum joint velocity. - public: double maxVelocity = -1; + public: double maxVelocity = std::numeric_limits::infinity(); /// \brief Joint stop stiffness. public: double stiffness = 1e8; @@ -106,7 +103,8 @@ Errors JointAxis::Load(ElementPtr _sdf) if (_sdf->HasElement("xyz")) { using gz::math::Vector3d; - auto errs = this->SetXyz(_sdf->Get("xyz", Vector3d::UnitZ).first); + auto errs = this->SetXyz(_sdf->Get("xyz", + this->dataPtr->xyz).first); std::copy(errs.begin(), errs.end(), std::back_inserter(errors)); auto e = _sdf->GetElement("xyz"); if (e->HasAttribute("expressed_in")) @@ -120,21 +118,19 @@ Errors JointAxis::Load(ElementPtr _sdf) "The xyz element in joint axis is required"}); } - // Get whether to use the parent model frame. - this->dataPtr->useParentModelFrame = _sdf->Get( - "use_parent_model_frame", false).first; - // Load dynamic values, if present if (_sdf->HasElement("dynamics")) { sdf::ElementPtr dynElement = _sdf->GetElement("dynamics"); - this->dataPtr->damping = dynElement->Get("damping", 0.0).first; - this->dataPtr->friction = dynElement->Get("friction", 0.0).first; - this->dataPtr->springReference = - dynElement->Get("spring_reference", 0.0).first; - this->dataPtr->springStiffness = - dynElement->Get("spring_stiffness", 0.0).first; + this->dataPtr->damping = dynElement->Get("damping", + this->dataPtr->damping).first; + this->dataPtr->friction = dynElement->Get("friction", + this->dataPtr->friction).first; + this->dataPtr->springReference = dynElement->Get("spring_reference", + this->dataPtr->springReference).first; + this->dataPtr->springStiffness = dynElement->Get("spring_stiffness", + this->dataPtr->springStiffness).first; } // Load limit values @@ -142,15 +138,18 @@ Errors JointAxis::Load(ElementPtr _sdf) { sdf::ElementPtr limitElement = _sdf->GetElement("limit"); - this->dataPtr->lower = limitElement->Get("lower", -1e16).first; - this->dataPtr->upper = limitElement->Get("upper", 1e16).first; - this->dataPtr->effort = limitElement->Get("effort", -1).first; - this->dataPtr->maxVelocity = limitElement->Get( - "velocity", -1).first; - this->dataPtr->stiffness = limitElement->Get( - "stiffness", 1e8).first; - this->dataPtr->dissipation = limitElement->Get( - "dissipation", 1.0).first; + this->dataPtr->lower = limitElement->Get("lower", + this->dataPtr->lower).first; + this->dataPtr->upper = limitElement->Get("upper", + this->dataPtr->upper).first; + this->dataPtr->effort = limitElement->Get("effort", + this->dataPtr->effort).first; + this->dataPtr->maxVelocity = limitElement->Get("velocity", + this->dataPtr->maxVelocity).first; + this->dataPtr->stiffness = limitElement->Get("stiffness", + this->dataPtr->stiffness).first; + this->dataPtr->dissipation = limitElement->Get("dissipation", + this->dataPtr->dissipation).first; } else { @@ -392,11 +391,6 @@ sdf::ElementPtr JointAxis::ToElement(unsigned int _index) const xyzElem->GetAttribute("expressed_in")->Set( this->XyzExpressedIn()); } - else if (this->dataPtr->useParentModelFrame) - { - xyzElem->GetAttribute("expressed_in")->Set( - "__model__"); - } sdf::ElementPtr dynElem = axisElem->GetElement("dynamics"); dynElem->GetElement("damping")->Set(this->Damping()); dynElem->GetElement("friction")->Set(this->Friction()); diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index 546dae334..031c989a6 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include "sdf/JointAxis.hh" ///////////////////////////////////////////////// @@ -29,8 +30,8 @@ TEST(DOMJointAxis, Construction) EXPECT_DOUBLE_EQ(0.0, axis.Friction()); EXPECT_DOUBLE_EQ(0.0, axis.SpringReference()); EXPECT_DOUBLE_EQ(0.0, axis.SpringStiffness()); - EXPECT_DOUBLE_EQ(-1e16, axis.Lower()); - EXPECT_DOUBLE_EQ(1e16, axis.Upper()); + EXPECT_DOUBLE_EQ(-std::numeric_limits::infinity(), axis.Lower()); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.Upper()); EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.Effort()); EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.MaxVelocity()); EXPECT_DOUBLE_EQ(1e8, axis.Stiffness()); diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc index 3813dd4dd..b1c102e28 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -244,8 +244,8 @@ TEST(DOMJointAxis, InfiniteLimits) ASSERT_NE(nullptr, joint); auto axis = joint->Axis(0); ASSERT_NE(nullptr, axis); - EXPECT_DOUBLE_EQ(-1e16, axis->Lower()); - EXPECT_DOUBLE_EQ(1e16, axis->Upper()); + EXPECT_DOUBLE_EQ(-kInf, axis->Lower()); + EXPECT_DOUBLE_EQ(kInf, axis->Upper()); EXPECT_DOUBLE_EQ(kInf, axis->Effort()); EXPECT_DOUBLE_EQ(kInf, axis->MaxVelocity()); } From 1bf1aa4e511d3efe0f97a6c14048a35dbcde26ce Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 18 Aug 2022 14:11:50 -0700 Subject: [PATCH 05/32] sdf/1.10: remove unused spec files (#1113) The urdf.sdf and collision_engine.sdf files are not included by any other spec file, so they can be removed. Fixes #635. Signed-off-by: Steve Peters --- Migration.md | 6 ++++++ sdf/1.10/collision_engine.sdf | 17 ----------------- sdf/1.10/urdf.sdf | 19 ------------------- 3 files changed, 6 insertions(+), 36 deletions(-) delete mode 100644 sdf/1.10/collision_engine.sdf delete mode 100644 sdf/1.10/urdf.sdf diff --git a/Migration.md b/Migration.md index 0d297b7ca..92e8f0eff 100644 --- a/Migration.md +++ b/Migration.md @@ -546,6 +546,12 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. 1. **plugin.sdf**: name attribute is now optional with empty default value. +### Removals + +1. **collision_engine.sdf**: unused specification file is removed. + +1. **urdf.sdf**: unused specification file is removed. + ## SDFormat specification 1.8 to 1.9 ### Additions diff --git a/sdf/1.10/collision_engine.sdf b/sdf/1.10/collision_engine.sdf deleted file mode 100644 index 67dbc7034..000000000 --- a/sdf/1.10/collision_engine.sdf +++ /dev/null @@ -1,17 +0,0 @@ - - - The collision_engine tag specifies the type and properties of the collision detection engine. - - - - The type of the collision detection engine. Current default in ODE is OPCODE. - - - - - - The type of the collision detection engine. - - - - diff --git a/sdf/1.10/urdf.sdf b/sdf/1.10/urdf.sdf deleted file mode 100644 index 6068d5cb6..000000000 --- a/sdf/1.10/urdf.sdf +++ /dev/null @@ -1,19 +0,0 @@ - - - The robot element defines a complete robot or any other physical object using URDF. - - - A unique name for the model. This name must not match another model in the world. - - - - A position and orientation in the global coordinate frame for the model. Position(x,y,z) and rotation (roll, pitch yaw) in the global coordinate frame. - - - - - - - - - From 2e891c21910769119b36f71b595fbfa21de03fc1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 24 Aug 2022 15:07:50 -0700 Subject: [PATCH 06/32] urdf: fix sensor/light pose for links lumped by fixed joints (#1114) When converting a fixed joint in a URDF file to SDFormat, by default the contents of the child link are merged into the parent link, but poses in gazebo extensions such as //gazebo/sensor/pose are not properly transformed (#378). This issue has already been fixed in newer branches by #525, so the test and that fix have been backported and refactored to reduce code duplication (about 30 lines removed from parser_urdf.cc) and also fix the treatment of //gazebo/light/pose. Fixes #378. Signed-off-by: Steve Peters --- Migration.md | 9 + src/parser_urdf.cc | 148 +++++-------- test/integration/urdf_gazebo_extensions.cc | 166 +++++++++++++++ test/integration/urdf_gazebo_extensions.urdf | 213 +++++++++++++++++++ 4 files changed, 446 insertions(+), 90 deletions(-) diff --git a/Migration.md b/Migration.md index b9b0b967c..ae3bc26e1 100644 --- a/Migration.md +++ b/Migration.md @@ -12,6 +12,15 @@ forward programmatically. This document aims to contain similar information to those files but with improved human-readability.. +## libsdformat 9.8.0 to 9.8.1 + +### Modifications + +1. URDF parser now properly transforms poses for lights, projectors and sensors + from gazebo extension tags that are moved to a new link during fixed joint + reduction. + + [pull request 1114](https://github.com/osrf/sdformat/pull/1114) + ## libsdformat 9.4 to 9.5 ### Additions diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 2f9501bd3..4e6772b72 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -87,17 +87,12 @@ void InsertSDFExtensionJoint(TiXmlElement *_elem, /// option is set bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt); -/// reduced fixed joints: apply transform reduction for ray sensors +/// reduced fixed joints: apply transform reduction for named elements /// in extensions when doing fixed joint reduction -void ReduceSDFExtensionSensorTransformReduction( +void ReduceSDFExtensionElementTransformReduction( std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform); - -/// reduced fixed joints: apply transform reduction for projectors in -/// extensions when doing fixed joint reduction -void ReduceSDFExtensionProjectorTransformReduction( - std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform); + const ignition::math::Pose3d &_reductionTransform, + const std::string &_elementName); /// reduced fixed joints: apply transform reduction to extensions @@ -400,8 +395,8 @@ void ReduceVisualToParent(urdf::LinkSharedPtr _parentLink, // collision elements of the child link into the parent link void ReduceFixedJoints(TiXmlElement *_root, urdf::LinkSharedPtr _link) { - // if child is attached to self by fixed _link first go up the tree, - // check it's children recursively + // if child is attached to self by fixed joint first go up the tree, + // check its children recursively for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i) { if (FixedJointShouldBeReduced(_link->child_links[i]->parent_joint)) @@ -2453,8 +2448,7 @@ void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link) for (std::vector::iterator ge = ext->second.begin(); ge != ext->second.end(); ++ge) { - (*ge)->reductionTransform = TransformToParentFrame( - (*ge)->reductionTransform, + (*ge)->reductionTransform = CopyPose( _link->parent_joint->parent_to_joint_origin_transform); // for sensor and projector blocks only ReduceSDFExtensionsTransform((*ge)); @@ -2545,11 +2539,12 @@ void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge) for (std::vector::iterator blobIt = _ge->blobs.begin(); blobIt != _ge->blobs.end(); ++blobIt) { - /// @todo make sure we are not missing any additional transform reductions - ReduceSDFExtensionSensorTransformReduction(blobIt, - _ge->reductionTransform); - ReduceSDFExtensionProjectorTransformReduction(blobIt, - _ge->reductionTransform); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "light"); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "projector"); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "sensor"); } } @@ -3282,12 +3277,13 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt) } //////////////////////////////////////////////////////////////////////////////// -void ReduceSDFExtensionSensorTransformReduction( +void ReduceSDFExtensionElementTransformReduction( std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform) + const ignition::math::Pose3d &_reductionTransform, + const std::string &_elementName) { - // overwrite and if they exist - if ((*_blobIt)->ValueStr() == "sensor") + auto element = *_blobIt; + if (element->ValueStr() == _elementName) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform @@ -3298,82 +3294,54 @@ void ReduceSDFExtensionSensorTransformReduction( // { // std::ostringstream streamIn; // streamIn << *elIt; - // sdfdbg << " " << streamIn << "\n"; + // sdfdbg << " " << streamIn.str() << "\n"; // } + auto pose {ignition::math::Pose3d::Zero}; { - TiXmlNode* oldPoseKey = (*_blobIt)->FirstChild("pose"); - /// @todo: FIXME: we should read xyz, rpy and aggregate it to - /// reductionTransform instead of just throwing the info away. + std::string poseText = "0 0 0 0 0 0"; + + TiXmlNode* oldPoseKey = element->FirstChild("pose"); if (oldPoseKey) { - (*_blobIt)->RemoveChild(oldPoseKey); - } - } - - // convert reductionTransform to values - urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), - _reductionTransform.Pos().Y(), - _reductionTransform.Pos().Z()); - urdf::Rotation reductionQ(_reductionTransform.Rot().X(), - _reductionTransform.Rot().Y(), - _reductionTransform.Rot().Z(), - _reductionTransform.Rot().W()); - - urdf::Vector3 reductionRpy; - reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z); - - // output updated pose to text - std::ostringstream poseStream; - poseStream << reductionXyz.x << " " << reductionXyz.y - << " " << reductionXyz.z << " " << reductionRpy.x - << " " << reductionRpy.y << " " << reductionRpy.z; - TiXmlText* poseTxt = new TiXmlText(poseStream.str()); - - TiXmlElement* poseKey = new TiXmlElement("pose"); - poseKey->LinkEndChild(poseTxt); - - (*_blobIt)->LinkEndChild(poseKey); - } -} + const auto& poseElemXml = oldPoseKey->ToElement(); + if (poseElemXml->Attribute("relative_to")) + { + return; + } -//////////////////////////////////////////////////////////////////////////////// -void ReduceSDFExtensionProjectorTransformReduction( - std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform) -{ - // overwrite (xyz/rpy) if it exists - if ((*_blobIt)->ValueStr() == "projector") - { - // parse it and add/replace the reduction transform - // find first instance of xyz and rpy, replace with reduction transform - // - // for (TiXmlNode* elIt = (*_blobIt)->FirstChild(); - // elIt; elIt = elIt->NextSibling()) - // { - // std::ostringstream streamIn; - // streamIn << *elIt; - // sdfdbg << " " << streamIn << "\n"; - // } + if (poseElemXml->GetText()) + { + poseText = poseElemXml->GetText(); + } - // should read ... and agregate reductionTransform - TiXmlNode* poseKey = (*_blobIt)->FirstChild("pose"); - // read pose and save it + // delete the tag, we'll add a new one at the end + element->RemoveChild(oldPoseKey); + } - // remove the tag for now - if (poseKey) - { - (*_blobIt)->RemoveChild(poseKey); + // parse the 6-tuple text into math::Pose3d + std::stringstream ss; + ss.imbue(std::locale::classic()); + ss << poseText; + ss >> pose; + if (ss.fail()) + { + sdferr << "Could not parse <" << _elementName << ">: [" + << poseText << "]\n"; + return; + } } + pose = _reductionTransform * pose; + // convert reductionTransform to values - urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), - _reductionTransform.Pos().Y(), - _reductionTransform.Pos().Z()); - urdf::Rotation reductionQ(_reductionTransform.Rot().X(), - _reductionTransform.Rot().Y(), - _reductionTransform.Rot().Z(), - _reductionTransform.Rot().W()); + urdf::Vector3 reductionXyz(pose.Pos().X(), + pose.Pos().Y(), + pose.Pos().Z()); + urdf::Rotation reductionQ(pose.Rot().X(), + pose.Rot().Y(), + pose.Rot().Z(), + pose.Rot().W()); urdf::Vector3 reductionRpy; reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z); @@ -3383,12 +3351,12 @@ void ReduceSDFExtensionProjectorTransformReduction( poseStream << reductionXyz.x << " " << reductionXyz.y << " " << reductionXyz.z << " " << reductionRpy.x << " " << reductionRpy.y << " " << reductionRpy.z; - TiXmlText* poseTxt = new TiXmlText(poseStream.str()); + TiXmlText* poseTxt = new TiXmlText(poseStream.str().c_str()); - poseKey = new TiXmlElement("pose"); + TiXmlElement* poseKey = new TiXmlElement("pose"); poseKey->LinkEndChild(poseTxt); - (*_blobIt)->LinkEndChild(poseKey); + element->LinkEndChild(poseKey); } } diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 36d0d8603..6b9fbcf37 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -135,4 +135,170 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_FALSE(link->HasElement("velocity_decay")); } } + + sdf::ElementPtr link0; + for (sdf::ElementPtr link = model->GetElement("link"); link; + link = link->GetNextElement("link")) + { + const auto& linkName = link->Get("name"); + if (linkName == "link0") + { + link0 = link; + break; + } + } + ASSERT_TRUE(link0); + + auto checkElementPoses = [&](const std::string &_elementName) -> void + { + bool foundElementNoPose {false}; + bool foundElementPose {false}; + bool foundElementPoseRelative {false}; + bool foundElementPoseTwoLevel {false}; + bool foundIssue378Element {false}; + bool foundIssue67Element {false}; + + for (sdf::ElementPtr element = link0->GetElement(_elementName); element; + element = element->GetNextElement(_elementName)) + { + const auto& elementName = element->Get("name"); + if (elementName == _elementName + "NoPose") + { + foundElementNoPose = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 0.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2, 1e-5); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == _elementName + "Pose") + { + foundElementPose = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 111.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == _elementName + "PoseRelative") + { + foundElementPoseRelative = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 111.0); + EXPECT_DOUBLE_EQ(pose.Y(), 0.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), -1, 1e-5); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == _elementName + "PoseTwoLevel") + { + foundElementPoseTwoLevel = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 111.0); + EXPECT_DOUBLE_EQ(pose.Z(), 222.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == "issue378_" + _elementName) + { + foundIssue378Element = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_DOUBLE_EQ(pose.X(), 1); + EXPECT_DOUBLE_EQ(pose.Y(), 2); + EXPECT_DOUBLE_EQ(pose.Z(), 3); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.1); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.2); + EXPECT_DOUBLE_EQ(pose.Yaw(), 0.3); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == "issue67_" + _elementName) + { + foundIssue67Element = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_GT(std::abs(pose.X() - (-0.20115)), 0.1); + EXPECT_GT(std::abs(pose.Y() - 0.42488), 0.1); + EXPECT_GT(std::abs(pose.Z() - 0.30943), 0.1); + EXPECT_GT(std::abs(pose.Roll() - 1.5708), 0.1); + EXPECT_GT(std::abs(pose.Pitch() - (-0.89012)), 0.1); + EXPECT_GT(std::abs(pose.Yaw() - 1.5708), 0.1); + + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + } + + EXPECT_TRUE(foundElementNoPose) << _elementName; + EXPECT_TRUE(foundElementPose) << _elementName; + EXPECT_TRUE(foundElementPoseRelative) << _elementName; + EXPECT_TRUE(foundElementPoseTwoLevel) << _elementName; + EXPECT_TRUE(foundIssue378Element) << _elementName; + EXPECT_TRUE(foundIssue67Element) << _elementName; + }; + + checkElementPoses("light"); + checkElementPoses("projector"); + checkElementPoses("sensor"); } diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index fc7216a16..a0a92c6c7 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -150,4 +150,217 @@ + + + + + + + + + + + + + + + + + + + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + + + + 111 0 0 0 0 -1 + + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + 1.0 2.0 3.0 0.1 0.2 0.3 + + + 1.0 2.0 3.0 0.1 0.2 0.3 + + + 1 + 400 + 1.0 2.0 3.0 0.1 0.2 0.3 + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 1 1 1.570796 1.570796 1.570796 + + + 1 1 1 1.570796 1.570796 1.570796 + + + true + 1 1 1 1.570796 1.570796 1.570796 + + + + + From 2bdad1b4d2db71bdd9cabee1213b2bfac18f015a Mon Sep 17 00:00:00 2001 From: Marq Rasmussen Date: Tue, 30 Aug 2022 07:05:18 -0600 Subject: [PATCH 07/32] Add camera optical_frame_id element (#1109) Signed-off-by: marqrazz --- include/sdf/Camera.hh | 14 ++++++++++++++ sdf/1.7/camera.sdf | 4 ++++ sdf/1.8/camera.sdf | 4 ++++ sdf/1.9/camera.sdf | 4 ++++ src/Camera.cc | 28 +++++++++++++++++++++++++++- src/Camera_TEST.cc | 6 ++++++ 6 files changed, 59 insertions(+), 1 deletion(-) diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 44c5874e6..9650b1f06 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -348,6 +348,20 @@ namespace sdf /// \param[in] _frame The name of the pose relative-to frame. public: void SetPoseRelativeTo(const std::string &_frame); + /// \brief Get the name of the coordinate frame relative to which this + /// object's camera_info message header is expressed. + /// Note: while Gazebo interprets the camera frame to be looking towards +X, + /// other tools, such as ROS interprets this frame as looking towards +Z. + /// The Camera sensor assumes that the color and depth images are captured + /// at the same frame_id. + /// \return The name of the frame this camera uses in its camera_info topic. + public: const std::string OpticalFrameId() const; + + /// \brief Set the name of the coordinate frame relative to which this + /// object's camera_info is expressed. + /// \param[in] _frame The frame this camera uses in its camera_info topic. + public: void SetOpticalFrameId(const std::string &_frame); + /// \brief Get the lens type. This is the type of the lens mapping. /// Supported values are gnomonical, stereographic, equidistant, /// equisolid_angle, orthographic, custom. For gnomonical (perspective) diff --git a/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf index ef637048d..9974aa544 100644 --- a/sdf/1.7/camera.sdf +++ b/sdf/1.7/camera.sdf @@ -161,5 +161,9 @@ + + An optional frame id name to be used in the camera_info message header. + + diff --git a/sdf/1.8/camera.sdf b/sdf/1.8/camera.sdf index ef637048d..9974aa544 100644 --- a/sdf/1.8/camera.sdf +++ b/sdf/1.8/camera.sdf @@ -161,5 +161,9 @@ + + An optional frame id name to be used in the camera_info message header. + + diff --git a/sdf/1.9/camera.sdf b/sdf/1.9/camera.sdf index 092aba97c..cfd09f890 100644 --- a/sdf/1.9/camera.sdf +++ b/sdf/1.9/camera.sdf @@ -196,5 +196,9 @@ + + An optional frame id name to be used in the camera_info message header. + + diff --git a/src/Camera.cc b/src/Camera.cc index 6700b9144..770ead070 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -142,6 +142,9 @@ class sdf::Camera::Implementation /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; + /// \brief Frame ID the camera_info message header is expressed. + public: std::string opticalFrameId{""}; + /// \brief Lens type. public: std::string lensType{"stereographic"}; @@ -348,6 +351,13 @@ Errors Camera::Load(ElementPtr _sdf) // Load the pose. Ignore the return value since the pose is optional. loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); + // Load the optional optical_frame_id value. + if (_sdf->HasElement("optical_frame_id")) + { + this->dataPtr->opticalFrameId = _sdf->Get("optical_frame_id", + this->dataPtr->opticalFrameId).first; + } + // Load the lens values. if (_sdf->HasElement("lens")) { @@ -692,7 +702,8 @@ bool Camera::operator==(const Camera &_cam) const this->SaveFrames() == _cam.SaveFrames() && this->SaveFramesPath() == _cam.SaveFramesPath() && this->ImageNoise() == _cam.ImageNoise() && - this->VisibilityMask() == _cam.VisibilityMask(); + this->VisibilityMask() == _cam.VisibilityMask() && + this->OpticalFrameId() == _cam.OpticalFrameId(); } ////////////////////////////////////////////////// @@ -809,6 +820,18 @@ void Camera::SetPoseRelativeTo(const std::string &_frame) this->dataPtr->poseRelativeTo = _frame; } +///////////////////////////////////////////////// +const std::string Camera::OpticalFrameId() const +{ + return this->dataPtr->opticalFrameId; +} + +///////////////////////////////////////////////// +void Camera::SetOpticalFrameId(const std::string &_frame) +{ + this->dataPtr->opticalFrameId = _frame; +} + ///////////////////////////////////////////////// std::string Camera::LensType() const { @@ -1149,5 +1172,8 @@ sdf::ElementPtr Camera::ToElement() const this->SegmentationType()); } + elem->GetElement("optical_frame_id")->Set( + this->OpticalFrameId()); + return elem; } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index b76651a23..2fb7cd01e 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -130,6 +130,10 @@ TEST(DOMCamera, Construction) cam.SetPoseRelativeTo("/frame"); EXPECT_EQ("/frame", cam.PoseRelativeTo()); + EXPECT_TRUE(cam.OpticalFrameId().empty()); + cam.SetOpticalFrameId("/optical_frame"); + EXPECT_EQ("/optical_frame", cam.OpticalFrameId()); + EXPECT_EQ("stereographic", cam.LensType()); cam.SetLensType("custom"); EXPECT_EQ("custom", cam.LensType()); @@ -240,6 +244,7 @@ TEST(DOMCamera, ToElement) cam.SetPoseRelativeTo("/frame"); cam.SetSaveFrames(true); cam.SetSaveFramesPath("/tmp"); + cam.SetOpticalFrameId("/optical_frame"); sdf::ElementPtr camElem = cam.ToElement(); EXPECT_NE(nullptr, camElem); @@ -259,6 +264,7 @@ TEST(DOMCamera, ToElement) EXPECT_EQ("/frame", cam2.PoseRelativeTo()); EXPECT_TRUE(cam2.SaveFrames()); EXPECT_EQ("/tmp", cam2.SaveFramesPath()); + EXPECT_EQ("/optical_frame", cam2.OpticalFrameId()); // make changes to DOM and verify ToElement produces updated values cam2.SetNearClip(0.33); From 089aa034c42a2d98d815eba5c9d9a85312ac06b4 Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 31 Aug 2022 13:07:55 -0700 Subject: [PATCH 08/32] sdf/camera.sdf: fields for projection matrix (#1088) Signed-off-by: Brian Chen Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- Migration.md | 7 ++- include/sdf/Camera.hh | 80 ++++++++++++++++++++------ sdf/1.7/camera.sdf | 22 +++++++ src/Camera.cc | 107 +++++++++++++++++++++++++++++++++++ src/Camera_TEST.cc | 24 ++++++++ test/integration/link_dom.cc | 6 ++ test/sdf/sensors.sdf | 8 +++ 7 files changed, 237 insertions(+), 17 deletions(-) diff --git a/Migration.md b/Migration.md index ae3bc26e1..0dcbc4d4c 100644 --- a/Migration.md +++ b/Migration.md @@ -12,7 +12,12 @@ forward programmatically. This document aims to contain similar information to those files but with improved human-readability.. -## libsdformat 9.8.0 to 9.8.1 +## libsdformat 9.8.0 to 9.9.0 + +### Additions + +1. **sdf/Camera.hh** + + Get/Set functions for Camera projection matrix parameters. ### Modifications diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index d3a855dd0..4a3274ff8 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -438,38 +438,86 @@ namespace sdf /// \param[in] _size The lens environment texture size. public: void SetLensEnvironmentTextureSize(int _size); - /// \brief Get the lens X focal length in pixels. - /// \return The lens X focal length in pixels. + /// \brief Get the lens intrinsic matrix X focal length in pixels. + /// \return The lens X focal length in pixels for the intrinsic matrix. public: double LensIntrinsicsFx() const; - /// \brief Set the lens X focal length in pixels. - /// \param[in] _fx The lens X focal length in pixels. + /// \brief Set the lens intrinsic matrix X focal length in pixels. + /// \param[in] _fx The intrinsic matrix lens X focal length in pixels. public: void SetLensIntrinsicsFx(double _fx); - /// \brief Get the lens Y focal length in pixels. - /// \return The lens Y focal length in pixels. + /// \brief Get the lens intrinsic matrix Y focal length in pixels. + /// \return The lens intrinsic matrix Y focal length in pixels. public: double LensIntrinsicsFy() const; - /// \brief Set the lens Y focal length in pixels. - /// \param[in] _fy The lens Y focal length in pixels. + /// \brief Set the lens intrinsic matrix Y focal length in pixels. + /// \param[in] _fy The lens intrinsic matrix Y focal length in pixels. public: void SetLensIntrinsicsFy(double _fy); - /// \brief Get the lens X principal point in pixels. - /// \return The lens X principal point in pixels. + /// \brief Get the lens intrinsic matrix X principal point in pixels. + /// \return The lens intrinsic matrix X principal point in pixels. public: double LensIntrinsicsCx() const; - /// \brief Set the lens X principal point in pixels. - /// \param[in] _cx The lens X principal point in pixels. + /// \brief Set the lens intrinsic matrix X principal point in pixels. + /// \param[in] _cx The lens intrinsic matrix X principal point in pixels. public: void SetLensIntrinsicsCx(double _cx); - /// \brief Get the lens Y principal point in pixels. - /// \return The lens Y principal point in pixels. + /// \brief Get the lens intrinsic matrix Y principal point in pixels. + /// \return The lens intrinsic matrix Y principal point in pixels. public: double LensIntrinsicsCy() const; - /// \brief Set the lens Y principal point in pixels. - /// \param[in] _cy The lens Y principal point in pixels. + /// \brief Set the lens intrinsic matrix Y principal point in pixels. + /// \param[in] _cy The lens intrinsic matrix Y principal point in pixels. public: void SetLensIntrinsicsCy(double _cy); + /// \brief Get the lens projection matrix X focal length in pixels. + /// \return The lens projection matrix X focal length in pixels. + public: double LensProjectionFx() const; + + /// \brief Set the lens projection matrix X focal length in pixels. + /// \param[in] _fx_p The lens projection matrix X focal length in pixels. + public: void SetLensProjectionFx(double _fx_p); + + /// \brief Get the lens projection matrix Y focal length in pixels. + /// \return The lens projection matrix Y focal length in pixels. + public: double LensProjectionFy() const; + + /// \brief Set the lens projection matrix Y focal length in pixels. + /// \param[in] _fy_p The lens projection matrix Y focal length in pixels. + public: void SetLensProjectionFy(double _fy_p); + + /// \brief Get the lens projection matrix X principal point in pixels. + /// \return The lens projection matrix X principal point in pixels. + public: double LensProjectionCx() const; + + /// \brief Set the lens projection matrix X principal point in pixels. + /// \param[in] _cx_p The lens projection matrix X principal point in pixels. + public: void SetLensProjectionCx(double _cx_p); + + /// \brief Get the lens projection matrix Y principal point in pixels. + /// \return The lens projection matrix Y principal point in pixels. + public: double LensProjectionCy() const; + + /// \brief Set the lens projection matrix Y principal point in pixels. + /// \param[in] _cy_p The lens projection matrix Y principal point in pixels. + public: void SetLensProjectionCy(double _cy_p); + + /// \brief Get the lens projection matrix X translation in pixels. + /// \return The lens projection matrix X translation in pixels. + public: double LensProjectionTx() const; + + /// \brief Set the lens projection matrix X translation in pixels. + /// \param[in] _tx The lens projection matrix X translation in pixels. + public: void SetLensProjectionTx(double _tx); + + /// \brief Get the lens projection matrix Y translation in pixels. + /// \return The lens projection matrix Y translation in pixels. + public: double LensProjectionTy() const; + + /// \brief Set the lens projection matrix Y translation in pixels. + /// \param[in] _ty The lens projection matrix Y translation in pixels. + public: void SetLensProjectionTy(double _ty); + /// \brief Get the lens XY axis skew. /// \return The lens XY axis skew. public: double LensIntrinsicsSkew() const; diff --git a/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf index ef637048d..de99908c2 100644 --- a/sdf/1.7/camera.sdf +++ b/sdf/1.7/camera.sdf @@ -155,6 +155,28 @@ XY axis skew + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + diff --git a/src/Camera.cc b/src/Camera.cc index 7976f6a3c..c089810d4 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -162,6 +162,24 @@ class sdf::CameraPrivate /// \brief lens instrinsics cy. public: double lensIntrinsicsCy{120.0}; + /// \brief lens projection fx + public: double lensProjectionFx{277.0}; + + /// \brief lens projection fy + public: double lensProjectionFy{277.0}; + + /// \brief lens projection cx + public: double lensProjectionCx{160.0}; + + /// \brief lens projection cy + public: double lensProjectionCy{120.0}; + + /// \brief lens projection tx + public: double lensProjectionTx{0.0}; + + /// \brief lens projection ty + public: double lensProjectionTy{0.0}; + /// \brief lens instrinsics s. public: double lensIntrinsicsS{1.0}; @@ -385,6 +403,23 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->lensIntrinsicsS = intrinsics->Get("s", this->dataPtr->lensIntrinsicsS).first; } + + if (elem->HasElement("projection")) { + sdf::ElementPtr projection = elem->GetElement("projection"); + this->dataPtr->lensProjectionFx = projection->Get("p_fx", + this->dataPtr->lensProjectionFx).first; + this->dataPtr->lensProjectionFy = projection->Get("p_fy", + this->dataPtr->lensProjectionFy).first; + this->dataPtr->lensProjectionCx = projection->Get("p_cx", + this->dataPtr->lensProjectionCx).first; + this->dataPtr->lensProjectionCy = projection->Get("p_cy", + this->dataPtr->lensProjectionCy).first; + this->dataPtr->lensProjectionTx = projection->Get("tx", + this->dataPtr->lensProjectionTx).first; + this->dataPtr->lensProjectionTy = projection->Get("ty", + this->dataPtr->lensProjectionTy).first; + + } } if (_sdf->HasElement("visibility_mask")) @@ -907,6 +942,78 @@ void Camera::SetLensIntrinsicsCy(double _cy) this->dataPtr->lensIntrinsicsCy = _cy; } +///////////////////////////////////////////////// +double Camera::LensProjectionFx() const +{ + return this->dataPtr->lensProjectionFx; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionFx(double _fx_p) +{ + this->dataPtr->lensProjectionFx = _fx_p; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionFy() const +{ + return this->dataPtr->lensProjectionFy; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionFy(double _fy_p) +{ + this->dataPtr->lensProjectionFy = _fy_p; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionCx() const +{ + return this->dataPtr->lensProjectionCx; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionCx(double _cx_p) +{ + this->dataPtr->lensProjectionCx = _cx_p; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionCy() const +{ + return this->dataPtr->lensProjectionCy; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionCy(double _cy_p) +{ + this->dataPtr->lensProjectionCy = _cy_p; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionTx() const +{ + return this->dataPtr->lensProjectionTx; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionTx(double _tx) +{ + this->dataPtr->lensProjectionTx = _tx; +} + +///////////////////////////////////////////////// +double Camera::LensProjectionTy() const +{ + return this->dataPtr->lensProjectionTy; +} + +///////////////////////////////////////////////// +void Camera::SetLensProjectionTy(double _ty) +{ + this->dataPtr->lensProjectionTy = _ty; +} + ///////////////////////////////////////////////// double Camera::LensIntrinsicsSkew() const { diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 08e125f63..56711e087 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -158,6 +158,30 @@ TEST(DOMCamera, Construction) cam.SetLensIntrinsicsCy(123); EXPECT_DOUBLE_EQ(123, cam.LensIntrinsicsCy()); + EXPECT_DOUBLE_EQ(277, cam.LensProjectionFx()); + cam.SetLensProjectionFx(132); + EXPECT_DOUBLE_EQ(132, cam.LensProjectionFx()); + + EXPECT_DOUBLE_EQ(277, cam.LensProjectionFy()); + cam.SetLensProjectionFy(456); + EXPECT_DOUBLE_EQ(456, cam.LensProjectionFy()); + + EXPECT_DOUBLE_EQ(160, cam.LensProjectionCx()); + cam.SetLensProjectionCx(254); + EXPECT_DOUBLE_EQ(254, cam.LensProjectionCx()); + + EXPECT_DOUBLE_EQ(120, cam.LensProjectionCy()); + cam.SetLensProjectionCy(123); + EXPECT_DOUBLE_EQ(123, cam.LensProjectionCy()); + + EXPECT_DOUBLE_EQ(0, cam.LensProjectionTx()); + cam.SetLensProjectionTx(1); + EXPECT_DOUBLE_EQ(1, cam.LensProjectionTx()); + + EXPECT_DOUBLE_EQ(0, cam.LensProjectionTy()); + cam.SetLensProjectionTy(2); + EXPECT_DOUBLE_EQ(2, cam.LensProjectionTy()); + EXPECT_DOUBLE_EQ(1.0, cam.LensIntrinsicsSkew()); cam.SetLensIntrinsicsSkew(2.3); EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index aeb507449..7806cbe62 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -302,6 +302,12 @@ TEST(DOMLink, Sensors) EXPECT_DOUBLE_EQ(162, camSensor->LensIntrinsicsCx()); EXPECT_DOUBLE_EQ(124, camSensor->LensIntrinsicsCy()); EXPECT_DOUBLE_EQ(1.2, camSensor->LensIntrinsicsSkew()); + EXPECT_DOUBLE_EQ(280, camSensor->LensProjectionFx()); + EXPECT_DOUBLE_EQ(281, camSensor->LensProjectionFy()); + EXPECT_DOUBLE_EQ(162, camSensor->LensProjectionCx()); + EXPECT_DOUBLE_EQ(124, camSensor->LensProjectionCy()); + EXPECT_DOUBLE_EQ(0, camSensor->LensProjectionTx()); + EXPECT_DOUBLE_EQ(0, camSensor->LensProjectionTy()); ignition::math::Pose3d pose; diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index 087dee6bd..8ea6ef043 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -73,6 +73,14 @@ 124 1.2 + + 280 + 281 + 162 + 124 + 0 + 0 + From 02a3de2cdbd76333dd3d850c1d482a58033f11a4 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 7 Sep 2022 10:26:39 -0700 Subject: [PATCH 09/32] Add pybind11 module as MODULE (#1127) This fixes an issue with linking to python libraries on macOS. Signed-off-by: Steve Peters --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b84023c92..9edd1b291 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,7 +36,7 @@ function(configure_build_install_location _library_name) ) endfunction() -pybind11_add_module(sdformat SHARED +pybind11_add_module(sdformat MODULE src/sdf/_gz_sdformat_pybind11.cc src/sdf/pyAirPressure.cc src/sdf/pyAltimeter.cc From 6a3320162bd2c7e86103236d77e132326dc5c243 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 7 Sep 2022 11:50:51 -0700 Subject: [PATCH 10/32] sdf/1.8, sdf/1.9 updates (#1133) * camera.sdf: fields for projection matrix Signed-off-by: Steve Peters --- sdf/1.8/camera.sdf | 22 ++++++++++++++++++++++ sdf/1.9/camera.sdf | 22 ++++++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/sdf/1.8/camera.sdf b/sdf/1.8/camera.sdf index 9974aa544..ec0bcafaa 100644 --- a/sdf/1.8/camera.sdf +++ b/sdf/1.8/camera.sdf @@ -155,6 +155,28 @@ XY axis skew + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + diff --git a/sdf/1.9/camera.sdf b/sdf/1.9/camera.sdf index cfd09f890..708e1d6eb 100644 --- a/sdf/1.9/camera.sdf +++ b/sdf/1.9/camera.sdf @@ -190,6 +190,28 @@ XY axis skew + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + From 8a27994a232fcf70ac2236e948fb4cc070cc5c5f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 7 Sep 2022 14:32:11 -0700 Subject: [PATCH 11/32] Prepare for 12.6.0~pre1 release (#1132) Update Changelog and Migration guide. Signed-off-by: Steve Peters --- CMakeLists.txt | 4 ++-- Changelog.md | 44 ++++++++++++++++++++++++++++++++++++++++++++ Migration.md | 5 +++++ 3 files changed, 51 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ba9009de..fe4952dbc 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 (sdformat12 VERSION 12.5.0) +project (sdformat12 VERSION 12.6.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software @@ -25,7 +25,7 @@ if (BUILD_SDF) ign_configure_project( NO_IGNITION_PREFIX REPLACE_IGNITION_INCLUDE_PATH sdf - VERSION_SUFFIX) + VERSION_SUFFIX pre1) ################################################# # Find tinyxml2. diff --git a/Changelog.md b/Changelog.md index d1510ab3e..aac36db8b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,49 @@ ## libsdformat 12.X +### libsdformat 12.6.0 (2022-09-07) + +1. Add camera `optical_frame_id` element + * [Pull request #1109](https://github.com/gazebosim/sdformat/pull/1109) + +1. urdf: fix sensor/light pose for links lumped by fixed joints + * [Pull request #1114](https://github.com/gazebosim/sdformat/pull/1114) + +1. Removed USD component from SDFormat and move to gz-usd + * [Pull request #1094](https://github.com/gazebosim/sdformat/pull/1094) + +1. Fix URDF fixed joint reduction of SDF joints + * [Pull request #1089](https://github.com/gazebosim/sdformat/pull/1089) + +1. Test model name as `placement_frame` + * [Pull request #1079](https://github.com/gazebosim/sdformat/pull/1079) + +1. Test using `__model__`, `world` in `@attached_to`, `@relative_to` + * [Pull request #1066](https://github.com/gazebosim/sdformat/pull/1066) + +1. Remove unused sdf.hh.in template + * [Pull request #1081](https://github.com/gazebosim/sdformat/pull/1081) + +1. Readme: Ignition -> Gazebo + * [Pull request #1080](https://github.com/gazebosim/sdformat/pull/1080) + +1. Document major and minor SDFormat version numbers + * [Pull request #1065](https://github.com/gazebosim/sdformat/pull/1065) + +1. Add skybox URI + * [Pull request #1037](https://github.com/gazebosim/sdformat/pull/1037) + +1. Bash completion for flags + * [Pull request #1042](https://github.com/gazebosim/sdformat/pull/1042) + +1. Fix bug with resolving poses for joint sensors. + * [Pull request #1033](https://github.com/gazebosim/sdformat/pull/1033) + +1. sdf::Joint: Mutable versions of SensorByName and SensorByIndex + * [Pull request #1031](https://github.com/gazebosim/sdformat/pull/1031) + +1. Add Link::ResolveInertial API + * [Pull request #1012](https://github.com/gazebosim/sdformat/pull/1012) + ### libsdformat 12.5.0 (2022-05-12) 1. Add visibility mask to Lidar / Ray sensor diff --git a/Migration.md b/Migration.md index 0d356ec3f..9914f6bc8 100644 --- a/Migration.md +++ b/Migration.md @@ -19,6 +19,11 @@ but with improved human-readability.. 1. USD component now is living in https://github.com/gazebosim/gz-usd as an independent package. +1. URDF parser now properly transforms poses for lights, projectors and sensors + from gazebo extension tags that are moved to a new link during fixed joint + reduction. + + [pull request 1114](https://github.com/osrf/sdformat/pull/1114) + ## libsdformat 11.x to 12.0 An error is now emitted instead of a warning for a file containing more than From 3b82d6a208f60b88ca6d36c7cd97692a3c087813 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 8 Sep 2022 08:51:46 -0700 Subject: [PATCH 12/32] Prepare for 9.9.0~pre1 prerelease (#1134) Signed-off-by: Steve Peters --- CMakeLists.txt | 4 ++-- Changelog.md | 27 ++++++++++++++++++++++++++- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 546f1166f..adfc18c18 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,11 +27,11 @@ project (sdformat9) set (SDF_PROTOCOL_VERSION 1.7) set (SDF_MAJOR_VERSION 9) -set (SDF_MINOR_VERSION 8) +set (SDF_MINOR_VERSION 9) set (SDF_PATCH_VERSION 0) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) -set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}) +set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}~pre1) string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) string(REGEX REPLACE "[0-9]+" "" PROJECT_NAME_NO_VERSION ${PROJECT_NAME}) diff --git a/Changelog.md b/Changelog.md index 1818d56b7..79440b9d6 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,31 @@ ## libsdformat 9.X -### libsdformat 9.X.X (202X-XX-XX) +### libsdformat 9.9.0 (2022-09-07) + +1. sdf/camera.sdf: fields for projection matrix + * [Pull request #1088](https://github.com/gazebosim/sdformat/pull/1088) + +1. urdf: fix sensor/light pose for links lumped by fixed joints + * [Pull request #1114](https://github.com/gazebosim/sdformat/pull/1114) + +1. Ensure relocatable config files + * [Pull request #419](https://github.com/gazebosim/sdformat/pull/419) + * [Pull request #1093](https://github.com/gazebosim/sdformat/pull/1093) + +1. Test using `__model__`, `world` in @attached_to, @relative_to + * [Pull request #1066](https://github.com/gazebosim/sdformat/pull/1066) + +1. Readme: Ignition -> Gazebo + * [Pull request #1080](https://github.com/gazebosim/sdformat/pull/1080) + +1. Document major and minor SDFormat version numbers + * [Pull request #1065](https://github.com/gazebosim/sdformat/pull/1065) + +1. Bash completion for flags + * [Pull request #1042](https://github.com/gazebosim/sdformat/pull/1042) + +1. Add Link::ResolveInertial API + * [Pull request #1012](https://github.com/gazebosim/sdformat/pull/1012) ### libsdformat 9.8.0 (2022-04-26) From 6c79a184f7528731790366fde84cd84505e4aee4 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 13 Sep 2022 14:26:47 -0700 Subject: [PATCH 13/32] Update Changelog for 13.0.0 (#1139) Signed-off-by: Steve Peters --- Changelog.md | 181 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 181 insertions(+) diff --git a/Changelog.md b/Changelog.md index 7133ac891..33cff8d6d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,187 @@ ### libsdformat 13.0.0 (202X-XX-XX) +1. Add camera `optical_frame_id` element + * [Pull request #1109](https://github.com/gazebosim/sdformat/pull/1109) + +1. sdf/1.10: remove unused spec files + * [Pull request #1113](https://github.com/gazebosim/sdformat/pull/1113) + +1. 1.10/joint.sdf: better default limit values + * [Pull request #1112](https://github.com/gazebosim/sdformat/pull/1112) + +1. Remove unused macros from config.hh + * [Pull request #1108](https://github.com/gazebosim/sdformat/pull/1108) + +1. Make //plugin/@name optional + * [Pull request #1101](https://github.com/gazebosim/sdformat/pull/1101) + +1. Add Error enums and update Migration guide + * [Pull request #1099](https://github.com/gazebosim/sdformat/pull/1099) + +1. Warn by default on unrecognized elements + * [Pull request #1096](https://github.com/gazebosim/sdformat/pull/1096) + +1. InterfaceElements: remove deprecated data members + * [Pull request #1097](https://github.com/gazebosim/sdformat/pull/1097) + +1. Add fluid added mass to inertial + * [Pull request #1077](https://github.com/gazebosim/sdformat/pull/1077) + +1. Change Root to non-unique impl pointer + * [Pull request #844](https://github.com/gazebosim/sdformat/pull/844) + +1. Add python interfaces + * [Pull request #932](https://github.com/gazebosim/sdformat/pull/932) + * [Pull request #933](https://github.com/gazebosim/sdformat/pull/933) + * [Pull request #934](https://github.com/gazebosim/sdformat/pull/934) + * [Pull request #937](https://github.com/gazebosim/sdformat/pull/937) + * [Pull request #938](https://github.com/gazebosim/sdformat/pull/938) + * [Pull request #940](https://github.com/gazebosim/sdformat/pull/940) + * [Pull request #941](https://github.com/gazebosim/sdformat/pull/941) + * [Pull request #942](https://github.com/gazebosim/sdformat/pull/942) + * [Pull request #944](https://github.com/gazebosim/sdformat/pull/944) + * [Pull request #945](https://github.com/gazebosim/sdformat/pull/945) + * [Pull request #946](https://github.com/gazebosim/sdformat/pull/946) + * [Pull request #947](https://github.com/gazebosim/sdformat/pull/947) + * [Pull request #948](https://github.com/gazebosim/sdformat/pull/948) + * [Pull request #949](https://github.com/gazebosim/sdformat/pull/949) + * [Pull request #950](https://github.com/gazebosim/sdformat/pull/950) + * [Pull request #951](https://github.com/gazebosim/sdformat/pull/951) + * [Pull request #952](https://github.com/gazebosim/sdformat/pull/952) + * [Pull request #953](https://github.com/gazebosim/sdformat/pull/953) + * [Pull request #957](https://github.com/gazebosim/sdformat/pull/957) + * [Pull request #960](https://github.com/gazebosim/sdformat/pull/960) + * [Pull request #961](https://github.com/gazebosim/sdformat/pull/961) + * [Pull request #962](https://github.com/gazebosim/sdformat/pull/962) + * [Pull request #963](https://github.com/gazebosim/sdformat/pull/963) + * [Pull request #964](https://github.com/gazebosim/sdformat/pull/964) + * [Pull request #967](https://github.com/gazebosim/sdformat/pull/967) + * [Pull request #968](https://github.com/gazebosim/sdformat/pull/968) + * [Pull request #969](https://github.com/gazebosim/sdformat/pull/969) + * [Pull request #970](https://github.com/gazebosim/sdformat/pull/970) + * [Pull request #971](https://github.com/gazebosim/sdformat/pull/971) + * [Pull request #972](https://github.com/gazebosim/sdformat/pull/972) + * [Pull request #973](https://github.com/gazebosim/sdformat/pull/973) + * [Pull request #981](https://github.com/gazebosim/sdformat/pull/981) + * [Pull request #982](https://github.com/gazebosim/sdformat/pull/982) + * [Pull request #983](https://github.com/gazebosim/sdformat/pull/983) + * [Pull request #984](https://github.com/gazebosim/sdformat/pull/984) + * [Pull request #988](https://github.com/gazebosim/sdformat/pull/988) + * [Pull request #989](https://github.com/gazebosim/sdformat/pull/989) + * [Pull request #992](https://github.com/gazebosim/sdformat/pull/992) + * [Pull request #993](https://github.com/gazebosim/sdformat/pull/993) + * [Pull request #994](https://github.com/gazebosim/sdformat/pull/994) + * [Pull request #995](https://github.com/gazebosim/sdformat/pull/995) + * [Pull request #996](https://github.com/gazebosim/sdformat/pull/996) + * [Pull request #997](https://github.com/gazebosim/sdformat/pull/997) + * [Pull request #998](https://github.com/gazebosim/sdformat/pull/998) + * [Pull request #999](https://github.com/gazebosim/sdformat/pull/999) + * [Pull request #1001](https://github.com/gazebosim/sdformat/pull/1001) + * [Pull request #1020](https://github.com/gazebosim/sdformat/pull/1020) + * [Pull request #1028](https://github.com/gazebosim/sdformat/pull/1028) + * [Pull request #1029](https://github.com/gazebosim/sdformat/pull/1029) + * [Pull request #1036](https://github.com/gazebosim/sdformat/pull/1036) + * [Pull request #1060](https://github.com/gazebosim/sdformat/pull/1060) + * [Pull request #1061](https://github.com/gazebosim/sdformat/pull/1061) + * [Pull request #1063](https://github.com/gazebosim/sdformat/pull/1063) + * [Pull request #1078](https://github.com/gazebosim/sdformat/pull/1078) + * [Pull request #1083](https://github.com/gazebosim/sdformat/pull/1083) + * [Pull request #1084](https://github.com/gazebosim/sdformat/pull/1084) + * [Pull request #1085](https://github.com/gazebosim/sdformat/pull/1085) + * [Pull request #1106](https://github.com/gazebosim/sdformat/pull/1106) + * [Pull request #1127](https://github.com/gazebosim/sdformat/pull/1127) + +1. Copy skybox uri field to sdf/1.10/scene.sdf + * [Pull request #1082](https://github.com/gazebosim/sdformat/pull/1082) + +1. Accept moon and custom surfaces in world spherical coordinates + * [Pull request #1050](https://github.com/gazebosim/sdformat/pull/1050) + +1. Migrate ign -> gz + * [Pull request #1008](https://github.com/gazebosim/sdformat/pull/1008) + * [Pull request #1022](https://github.com/gazebosim/sdformat/pull/1022) + * [Pull request #1040](https://github.com/gazebosim/sdformat/pull/1040) + * [Pull request #1045](https://github.com/gazebosim/sdformat/pull/1045) + * [Pull request #1047](https://github.com/gazebosim/sdformat/pull/1047) + * [Pull request #1048](https://github.com/gazebosim/sdformat/pull/1048) + * [Pull request #1052](https://github.com/gazebosim/sdformat/pull/1052) + * [Pull request #1057](https://github.com/gazebosim/sdformat/pull/1057) + * [Pull request #1058](https://github.com/gazebosim/sdformat/pull/1058) + * [Pull request #1067](https://github.com/gazebosim/sdformat/pull/1067) + * [Pull request #1074](https://github.com/gazebosim/sdformat/pull/1074) + * [Pull request #1078](https://github.com/gazebosim/sdformat/pull/1078) + * [Pull request #1092](https://github.com/gazebosim/sdformat/pull/1092) + +1. Copy 1.9 spec to 1.10 + * [Pull request #1073](https://github.com/gazebosim/sdformat/pull/1073) + * [Pull request #1076](https://github.com/gazebosim/sdformat/pull/1076) + +1. Root: get the world name + * [Pull request #1027](https://github.com/gazebosim/sdformat/pull/1027) + +1. Add SDF::SetRoot and deprecate non-const SDF::Root + * [Pull request #1070](https://github.com/gazebosim/sdformat/pull/1070) + +1. Update GoogleTest to latest version + * [Pull request #1059](https://github.com/gazebosim/sdformat/pull/1059) + * [Pull request #1072](https://github.com/gazebosim/sdformat/pull/1072) + +1. Update return types for Plugin's Name and Filename + * [Pull request #1055](https://github.com/gazebosim/sdformat/pull/1055) + +1. Surface::ToElement: add //friction/ode/mu + * [Pull request #1049](https://github.com/gazebosim/sdformat/pull/1049) + +1. Joint: rename parent/child `*LinkName` APIs + * [Pull request #1053](https://github.com/gazebosim/sdformat/pull/1053) + * [Pull request #1103](https://github.com/gazebosim/sdformat/pull/1103) + +1. Deprecate sdf::Inertia class + * [Pull request #1019](https://github.com/gazebosim/sdformat/pull/1019) + +1. Don't include the gz/math.hh header from library code + * [Pull request #1043](https://github.com/gazebosim/sdformat/pull/1043) + +1. Use pose multiplication instead of subtraction + * [Pull request #1039](https://github.com/gazebosim/sdformat/pull/1039) + +1. Remove deprecation warnings + * [Pull request #1014](https://github.com/gazebosim/sdformat/pull/1014) + +1. Added light methods to Link, Root and World + * [Pull request #1013](https://github.com/gazebosim/sdformat/pull/1013) + +1. Add sdf::Error logging in sdf::Param + * [Pull request #939](https://github.com/gazebosim/sdformat/pull/939) + +1. Changes for replacing PythonInterp with Python3 + * [Pull request #907](https://github.com/gazebosim/sdformat/pull/907) + +1. Combine find_package(ignition-utils) calls + * [Pull request #966](https://github.com/gazebosim/sdformat/pull/966) + +1. Change default floating point precision to max + * [Pull request #872](https://github.com/gazebosim/sdformat/pull/872) + +1. Clean up compiler warnings + * [Pull request #882](https://github.com/gazebosim/sdformat/pull/882) + +1. Switch to utils version of env functions + * [Pull request #854](https://github.com/gazebosim/sdformat/pull/854) + +1. Updated tests for ign-math's ostream fix + * [Pull request #847](https://github.com/gazebosim/sdformat/pull/847) + +1. Infrastructure + * [Pull request #803](https://github.com/gazebosim/sdformat/pull/803) + * [Pull request #805](https://github.com/gazebosim/sdformat/pull/805) + * [Pull request #878](https://github.com/gazebosim/sdformat/pull/878) + * [Pull request #980](https://github.com/gazebosim/sdformat/pull/980) + * [Pull request #974](https://github.com/gazebosim/sdformat/pull/974) + +1. Remove completely unused define + * [Pull request #758](https://github.com/gazebosim/sdformat/pull/758) ## libsdformat 12.X From 5cbe319b66f702bdf6206e988e7a5dfc72fbc45a Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Fri, 16 Sep 2022 01:34:22 +0200 Subject: [PATCH 14/32] Use gz.math7 (#1142) Adapting the code to the change in gazebosim/gz-math#503 Signed-off-by: Jose Luis Rivero --- python/test/pyAirPressure_TEST.py | 2 +- python/test/pyAltimeter_TEST.py | 2 +- python/test/pyAtmosphere_TEST.py | 2 +- python/test/pyBox_TEST.py | 2 +- python/test/pyCamera_TEST.py | 2 +- python/test/pyCapsule_TEST.py | 2 +- python/test/pyCollision_TEST.py | 2 +- python/test/pyCylinder_TEST.py | 2 +- python/test/pyEllipsoid_TEST.py | 2 +- python/test/pyFrame_TEST.py | 2 +- python/test/pyGeometry_TEST.py | 2 +- python/test/pyHeightmap_TEST.py | 2 +- python/test/pyIMU_TEST.py | 2 +- python/test/pyJointAxis_TEST.py | 2 +- python/test/pyJoint_TEST.py | 2 +- python/test/pyLidar_TEST.py | 2 +- python/test/pyLight_TEST.py | 2 +- python/test/pyLink_TEST.py | 2 +- python/test/pyMagnetometer_TEST.py | 2 +- python/test/pyMaterial_TEST.py | 2 +- python/test/pyMesh_TEST.py | 2 +- python/test/pyModel_TEST.py | 2 +- python/test/pyParticleEmitter_TEST.py | 2 +- python/test/pyPlane_TEST.py | 2 +- python/test/pyPolyline_TEST.py | 2 +- python/test/pyRoot_TEST.py | 2 +- python/test/pyScene_TEST.py | 2 +- python/test/pySemanticPose_TEST.py | 2 +- python/test/pySensor_TEST.py | 2 +- python/test/pySky_TEST.py | 2 +- python/test/pySphere_TEST.py | 2 +- python/test/pySurface_TEST.py | 2 +- python/test/pyVisual_TEST.py | 2 +- python/test/pyWorld_TEST.py | 2 +- 34 files changed, 34 insertions(+), 34 deletions(-) diff --git a/python/test/pyAirPressure_TEST.py b/python/test/pyAirPressure_TEST.py index 0b294ade5..f4ce64274 100644 --- a/python/test/pyAirPressure_TEST.py +++ b/python/test/pyAirPressure_TEST.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math import Vector3d +from gz.math7 import Vector3d from sdformat import AirPressure, Noise import sdformat as sdf import unittest diff --git a/python/test/pyAltimeter_TEST.py b/python/test/pyAltimeter_TEST.py index ff9b9498c..9189303c0 100644 --- a/python/test/pyAltimeter_TEST.py +++ b/python/test/pyAltimeter_TEST.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math import Vector3d +from gz.math7 import Vector3d from sdformat import Altimeter, Noise import sdformat as sdf import unittest diff --git a/python/test/pyAtmosphere_TEST.py b/python/test/pyAtmosphere_TEST.py index e5162159b..9d08a2a44 100644 --- a/python/test/pyAtmosphere_TEST.py +++ b/python/test/pyAtmosphere_TEST.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math import Pose3d, Temperature +from gz.math7 import Pose3d, Temperature from sdformat import Atmosphere import sdformat as sdf import unittest diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index 4eac4f2b7..df609011b 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.math import Vector3d +from gz.math7 import Vector3d from sdformat import Box import unittest diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index e3ec95261..ec0f26499 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Angle, Pose3d, Vector2d +from gz.math7 import Angle, Pose3d, Vector2d import math from sdformat import Camera import sdformat as sdf diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 1e375dd6e..f2f694c17 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -14,7 +14,7 @@ import copy -from gz.math import Vector3d, Capsuled +from gz.math7 import Vector3d, Capsuled import math diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index fbb6d5d82..a17f72f45 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d +from gz.math7 import Pose3d from sdformat import (Box, Collision, Contact, Cylinder, Error, Geometry, Plane, Surface, Sphere, SDFErrorsException) import sdformat as sdf diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index 7294abb54..cdd818b25 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -14,7 +14,7 @@ import copy -from gz.math import Vector3d +from gz.math7 import Vector3d import math diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index c77105f7d..f80bc4889 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.math import Vector3d, Ellipsoidd +from gz.math7 import Vector3d, Ellipsoidd import math from sdformat import Ellipsoid import unittest diff --git a/python/test/pyFrame_TEST.py b/python/test/pyFrame_TEST.py index bf0dcb9e7..aea698bc0 100644 --- a/python/test/pyFrame_TEST.py +++ b/python/test/pyFrame_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d +from gz.math7 import Pose3d from sdformat import Frame, Error, SDFErrorsException, ErrorCode import unittest import math diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index dcee570fb..e1220ef4b 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -14,7 +14,7 @@ import copy from sdformat import Geometry, Box, Capsule, Cylinder, Ellipsoid, Mesh, Plane, Sphere -from gz.math import Vector3d, Vector2d +from gz.math7 import Vector3d, Vector2d import sdformat as sdf import unittest diff --git a/python/test/pyHeightmap_TEST.py b/python/test/pyHeightmap_TEST.py index d58bfa7ea..ed6186eb4 100644 --- a/python/test/pyHeightmap_TEST.py +++ b/python/test/pyHeightmap_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Vector3d +from gz.math7 import Vector3d from sdformat import Heightmap, HeightmapBlend, HeightmapTexture import unittest diff --git a/python/test/pyIMU_TEST.py b/python/test/pyIMU_TEST.py index 8eda014ec..42885ab7d 100644 --- a/python/test/pyIMU_TEST.py +++ b/python/test/pyIMU_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Vector3d +from gz.math7 import Vector3d from sdformat import IMU, Noise import sdformat as sdf import unittest diff --git a/python/test/pyJointAxis_TEST.py b/python/test/pyJointAxis_TEST.py index b5f68f34f..c158a6f23 100644 --- a/python/test/pyJointAxis_TEST.py +++ b/python/test/pyJointAxis_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d, Vector3d +from gz.math7 import Pose3d, Vector3d from sdformat import JointAxis, Error, SDFErrorsException import math import unittest diff --git a/python/test/pyJoint_TEST.py b/python/test/pyJoint_TEST.py index f0ad371a9..ccbea9487 100644 --- a/python/test/pyJoint_TEST.py +++ b/python/test/pyJoint_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d, Vector3d +from gz.math7 import Pose3d, Vector3d from sdformat import (Joint, JointAxis, Error, SemanticPose, Sensor, SDFErrorsException) import sdformat as sdf diff --git a/python/test/pyLidar_TEST.py b/python/test/pyLidar_TEST.py index b9ea1b7bb..871f7c2e6 100644 --- a/python/test/pyLidar_TEST.py +++ b/python/test/pyLidar_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Angle, Pose3d, Vector3d, Helpers +from gz.math7 import Angle, Pose3d, Vector3d, Helpers from sdformat import Lidar, Error, Noise import math import unittest diff --git a/python/test/pyLight_TEST.py b/python/test/pyLight_TEST.py index 167757e4c..d7d9657f6 100644 --- a/python/test/pyLight_TEST.py +++ b/python/test/pyLight_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Angle, Color, Pose3d, Vector3d +from gz.math7 import Angle, Color, Pose3d, Vector3d from sdformat import Light, SDFErrorsException import sdformat as sdf import math diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index 4bb10665c..e4f8f2141 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d, Inertiald, MassMatrix3d, Vector3d +from gz.math7 import Pose3d, Inertiald, MassMatrix3d, Vector3d from sdformat import (Collision, Light, Link, Sensor, Visual, SDFErrorsException) import unittest diff --git a/python/test/pyMagnetometer_TEST.py b/python/test/pyMagnetometer_TEST.py index 6a7597346..bb0ece24c 100644 --- a/python/test/pyMagnetometer_TEST.py +++ b/python/test/pyMagnetometer_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d +from gz.math7 import Pose3d from sdformat import Magnetometer, Noise import sdformat as sdf import unittest diff --git a/python/test/pyMaterial_TEST.py b/python/test/pyMaterial_TEST.py index ec180df6d..467682c91 100644 --- a/python/test/pyMaterial_TEST.py +++ b/python/test/pyMaterial_TEST.py @@ -14,7 +14,7 @@ import copy from sdformat import Material, Pbr, PbrWorkflow -from gz.math import Color +from gz.math7 import Color import sdformat as sdf import unittest diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index a854d5c5f..4f8a0d145 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -14,7 +14,7 @@ import copy from sdformat import Mesh -from gz.math import Vector3d +from gz.math7 import Vector3d import unittest diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index efb0aa303..8abbac38f 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d, Vector3d +from gz.math7 import Pose3d, Vector3d from sdformat import (Plugin, Model, Joint, Link, Error, Frame, SemanticPose, SDFErrorsException) import sdformat as sdf diff --git a/python/test/pyParticleEmitter_TEST.py b/python/test/pyParticleEmitter_TEST.py index 2464bb1b2..03cadde60 100644 --- a/python/test/pyParticleEmitter_TEST.py +++ b/python/test/pyParticleEmitter_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Color, Pose3d, Vector3d, Helpers +from gz.math7 import Color, Pose3d, Vector3d, Helpers from sdformat import ParticleEmitter import unittest diff --git a/python/test/pyPlane_TEST.py b/python/test/pyPlane_TEST.py index bd0729f02..baa52b616 100644 --- a/python/test/pyPlane_TEST.py +++ b/python/test/pyPlane_TEST.py @@ -14,7 +14,7 @@ import copy from sdformat import Plane -from gz.math import Vector3d, Vector2d, Planed +from gz.math7 import Vector3d, Vector2d, Planed import unittest diff --git a/python/test/pyPolyline_TEST.py b/python/test/pyPolyline_TEST.py index 2b544e190..3a4f57a89 100644 --- a/python/test/pyPolyline_TEST.py +++ b/python/test/pyPolyline_TEST.py @@ -14,7 +14,7 @@ import copy from sdformat import Polyline -from gz.math import Vector2d +from gz.math7 import Vector2d import unittest diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index 3bd90e9e1..c0909a449 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Vector3d, Pose3d +from gz.math7 import Vector3d, Pose3d from sdformat import (Error, Model, Light, Root, SDF_VERSION, SDFErrorsException, SDF_PROTOCOL_VERSION, World) import sdformat as sdf diff --git a/python/test/pyScene_TEST.py b/python/test/pyScene_TEST.py index 294149075..d0d572133 100644 --- a/python/test/pyScene_TEST.py +++ b/python/test/pyScene_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Color +from gz.math7 import Color from sdformat import Scene, Sky import unittest diff --git a/python/test/pySemanticPose_TEST.py b/python/test/pySemanticPose_TEST.py index 2a30d1993..06f3b5f6a 100644 --- a/python/test/pySemanticPose_TEST.py +++ b/python/test/pySemanticPose_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d +from gz.math7 import Pose3d from sdformat import Link, SemanticPose import unittest diff --git a/python/test/pySensor_TEST.py b/python/test/pySensor_TEST.py index d389e0efe..69bd50e87 100644 --- a/python/test/pySensor_TEST.py +++ b/python/test/pySensor_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d +from gz.math7 import Pose3d from sdformat import (AirPressure, Altimeter, Camera, IMU, ForceTorque, Lidar, Magnetometer, NavSat, Noise, Plugin, SemanticPose, Sensor, SDFErrorsException) diff --git a/python/test/pySky_TEST.py b/python/test/pySky_TEST.py index 7f58561dc..16c1dda18 100644 --- a/python/test/pySky_TEST.py +++ b/python/test/pySky_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Angle, Color +from gz.math7 import Angle, Color from sdformat import Sky import unittest diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index 370d574d5..d72b6536c 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Sphered +from gz.math7 import Sphered import math from sdformat import Sphere import unittest diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index 27936dea4..86c4404f8 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Vector3d +from gz.math7 import Vector3d from sdformat import Surface, Contact, Friction, ODE import unittest diff --git a/python/test/pyVisual_TEST.py b/python/test/pyVisual_TEST.py index 1733da573..65020546c 100644 --- a/python/test/pyVisual_TEST.py +++ b/python/test/pyVisual_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Pose3d, Color +from gz.math7 import Pose3d, Color from sdformat import Geometry, Material, Visual, Plugin, SDFErrorsException import sdformat as sdf import unittest diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 88d929ccd..583b1aa5f 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math import Color, Pose3d, Vector3d, SphericalCoordinates +from gz.math7 import Color, Pose3d, Vector3d, SphericalCoordinates from sdformat import Atmosphere, Gui, Physics, Plugin, Error, Frame, Light, Model, Scene, World import sdformat as sdf import unittest From 7508a1ad98149973fca2345c24acd86c3bd6f050 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Fri, 16 Sep 2022 12:34:44 +0200 Subject: [PATCH 15/32] Use versioned python module name (#1143) * Change to sdformat13 module name for python Signed-off-by: Jose Luis Rivero --- python/CMakeLists.txt | 17 ++++++++++++++--- python/src/sdf/_gz_sdformat_pybind11.cc | 2 +- python/test/pyAirPressure_TEST.py | 4 ++-- python/test/pyAltimeter_TEST.py | 4 ++-- python/test/pyAtmosphere_TEST.py | 4 ++-- python/test/pyBox_TEST.py | 2 +- python/test/pyCamera_TEST.py | 4 ++-- python/test/pyCapsule_TEST.py | 2 +- python/test/pyCollision_TEST.py | 4 ++-- python/test/pyCylinder_TEST.py | 2 +- python/test/pyEllipsoid_TEST.py | 2 +- python/test/pyError_TEST.py | 4 ++-- python/test/pyForceTorque_TEST.py | 4 ++-- python/test/pyFrame_TEST.py | 2 +- python/test/pyGeometry_TEST.py | 4 ++-- python/test/pyGui_TEST.py | 2 +- python/test/pyHeightmap_TEST.py | 2 +- python/test/pyIMU_TEST.py | 4 ++-- python/test/pyJointAxis_TEST.py | 2 +- python/test/pyJoint_TEST.py | 4 ++-- python/test/pyLidar_TEST.py | 2 +- python/test/pyLight_TEST.py | 4 ++-- python/test/pyLink_TEST.py | 2 +- python/test/pyMagnetometer_TEST.py | 4 ++-- python/test/pyMaterial_TEST.py | 4 ++-- python/test/pyMesh_TEST.py | 2 +- python/test/pyModel_TEST.py | 4 ++-- python/test/pyNavSat_TEST.py | 2 +- python/test/pyNoise_TEST.py | 4 ++-- python/test/pyParserConfig_TEST.py | 2 +- python/test/pyParticleEmitter_TEST.py | 2 +- python/test/pyPbr_TEST.py | 4 ++-- python/test/pyPhysics_TEST.py | 2 +- python/test/pyPlane_TEST.py | 2 +- python/test/pyPlugin_TEST.py | 2 +- python/test/pyPolyline_TEST.py | 2 +- python/test/pyRoot_TEST.py | 4 ++-- python/test/pyScene_TEST.py | 2 +- python/test/pySemanticPose_TEST.py | 2 +- python/test/pySensor_TEST.py | 4 ++-- python/test/pySky_TEST.py | 2 +- python/test/pySphere_TEST.py | 2 +- python/test/pySurface_TEST.py | 2 +- python/test/pyVisual_TEST.py | 4 ++-- python/test/pyWorld_TEST.py | 4 ++-- 45 files changed, 78 insertions(+), 67 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9edd1b291..e536c749f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -36,7 +36,10 @@ function(configure_build_install_location _library_name) ) endfunction() -pybind11_add_module(sdformat MODULE +# sdformatX target already exists, use pysdformatX + OUTPUT_NAME to get +# sdformatX file name generated and map BINDINGS_MODULE_NAME to sdformatX +set(BINDINGS_MODULE_NAME "pysdformat${PROJECT_VERSION_MAJOR}") +pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/sdf/_gz_sdformat_pybind11.cc src/sdf/pyAirPressure.cc src/sdf/pyAltimeter.cc @@ -84,11 +87,19 @@ pybind11_add_module(sdformat MODULE src/sdf/pybind11_helpers.cc ) -target_link_libraries(sdformat PRIVATE +target_link_libraries(${BINDINGS_MODULE_NAME} PRIVATE ${PROJECT_LIBRARY_TARGET_NAME} ) -configure_build_install_location(sdformat) +# different from the target name since the target name was not able to use +# sdformatX since it conflicts with the project name +target_compile_definitions(${BINDINGS_MODULE_NAME} PRIVATE + BINDINGS_MODULE_NAME=sdformat${PROJECT_VERSION_MAJOR}) + +set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES + OUTPUT_NAME "sdformat${PROJECT_VERSION_MAJOR}") + +configure_build_install_location(${BINDINGS_MODULE_NAME}) if (BUILD_TESTING) pybind11_add_module(sdformattest SHARED diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 294819cde..681bf6990 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -62,7 +62,7 @@ #include "pyVisual.hh" #include "pyWorld.hh" -PYBIND11_MODULE(sdformat, m) { +PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { m.doc() = "sdformat Python Library."; sdf::python::defineAirPressure(m); diff --git a/python/test/pyAirPressure_TEST.py b/python/test/pyAirPressure_TEST.py index f4ce64274..4abf3bccc 100644 --- a/python/test/pyAirPressure_TEST.py +++ b/python/test/pyAirPressure_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. from gz.math7 import Vector3d -from sdformat import AirPressure, Noise -import sdformat as sdf +from sdformat13 import AirPressure, Noise +import sdformat13 as sdf import unittest class AtmosphereTEST(unittest.TestCase): diff --git a/python/test/pyAltimeter_TEST.py b/python/test/pyAltimeter_TEST.py index 9189303c0..96253e5da 100644 --- a/python/test/pyAltimeter_TEST.py +++ b/python/test/pyAltimeter_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. from gz.math7 import Vector3d -from sdformat import Altimeter, Noise -import sdformat as sdf +from sdformat13 import Altimeter, Noise +import sdformat13 as sdf import unittest diff --git a/python/test/pyAtmosphere_TEST.py b/python/test/pyAtmosphere_TEST.py index 9d08a2a44..d2363c1af 100644 --- a/python/test/pyAtmosphere_TEST.py +++ b/python/test/pyAtmosphere_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. from gz.math7 import Pose3d, Temperature -from sdformat import Atmosphere -import sdformat as sdf +from sdformat13 import Atmosphere +import sdformat13 as sdf import unittest class AtmosphereTEST(unittest.TestCase): diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index df609011b..13e9a99ed 100644 --- a/python/test/pyBox_TEST.py +++ b/python/test/pyBox_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. from gz.math7 import Vector3d -from sdformat import Box +from sdformat13 import Box import unittest class BoxTEST(unittest.TestCase): diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index ec0f26499..67471694b 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -15,8 +15,8 @@ import copy from gz.math7 import Angle, Pose3d, Vector2d import math -from sdformat import Camera -import sdformat as sdf +from sdformat13 import Camera +import sdformat13 as sdf import unittest class CameraTEST(unittest.TestCase): diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index f2f694c17..8924c9ba2 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -18,7 +18,7 @@ import math -from sdformat import Capsule +from sdformat13 import Capsule import unittest diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index a17f72f45..a0efbecdd 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -14,9 +14,9 @@ import copy from gz.math7 import Pose3d -from sdformat import (Box, Collision, Contact, Cylinder, Error, Geometry, +from sdformat13 import (Box, Collision, Contact, Cylinder, Error, Geometry, Plane, Surface, Sphere, SDFErrorsException) -import sdformat as sdf +import sdformat13 as sdf import unittest import math diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index cdd818b25..55b5ecf9a 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -18,7 +18,7 @@ import math -from sdformat import Cylinder +from sdformat13 import Cylinder import unittest diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index f80bc4889..e66953bb9 100644 --- a/python/test/pyEllipsoid_TEST.py +++ b/python/test/pyEllipsoid_TEST.py @@ -15,7 +15,7 @@ import copy from gz.math7 import Vector3d, Ellipsoidd import math -from sdformat import Ellipsoid +from sdformat13 import Ellipsoid import unittest diff --git a/python/test/pyError_TEST.py b/python/test/pyError_TEST.py index b8c82e595..84aa5e32d 100644 --- a/python/test/pyError_TEST.py +++ b/python/test/pyError_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Error -import sdformat as sdf +from sdformat13 import Error +import sdformat13 as sdf import unittest class ErrorColor(unittest.TestCase): diff --git a/python/test/pyForceTorque_TEST.py b/python/test/pyForceTorque_TEST.py index 7a72bff90..8b3ae782b 100644 --- a/python/test/pyForceTorque_TEST.py +++ b/python/test/pyForceTorque_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import ForceTorque, Noise -import sdformat as sdf +from sdformat13 import ForceTorque, Noise +import sdformat13 as sdf import unittest class ForceTorqueTEST(unittest.TestCase): diff --git a/python/test/pyFrame_TEST.py b/python/test/pyFrame_TEST.py index aea698bc0..89b9bbd99 100644 --- a/python/test/pyFrame_TEST.py +++ b/python/test/pyFrame_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Pose3d -from sdformat import Frame, Error, SDFErrorsException, ErrorCode +from sdformat13 import Frame, Error, SDFErrorsException, ErrorCode import unittest import math diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index e1220ef4b..60811c558 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from sdformat import Geometry, Box, Capsule, Cylinder, Ellipsoid, Mesh, Plane, Sphere +from sdformat13 import Geometry, Box, Capsule, Cylinder, Ellipsoid, Mesh, Plane, Sphere from gz.math7 import Vector3d, Vector2d -import sdformat as sdf +import sdformat13 as sdf import unittest diff --git a/python/test/pyGui_TEST.py b/python/test/pyGui_TEST.py index 7c66ca4c4..7454ba590 100644 --- a/python/test/pyGui_TEST.py +++ b/python/test/pyGui_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Gui, Plugin +from sdformat13 import Gui, Plugin import unittest diff --git a/python/test/pyHeightmap_TEST.py b/python/test/pyHeightmap_TEST.py index ed6186eb4..6d2016e06 100644 --- a/python/test/pyHeightmap_TEST.py +++ b/python/test/pyHeightmap_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Vector3d -from sdformat import Heightmap, HeightmapBlend, HeightmapTexture +from sdformat13 import Heightmap, HeightmapBlend, HeightmapTexture import unittest diff --git a/python/test/pyIMU_TEST.py b/python/test/pyIMU_TEST.py index 42885ab7d..fb7b5ab27 100644 --- a/python/test/pyIMU_TEST.py +++ b/python/test/pyIMU_TEST.py @@ -14,8 +14,8 @@ import copy from gz.math7 import Vector3d -from sdformat import IMU, Noise -import sdformat as sdf +from sdformat13 import IMU, Noise +import sdformat13 as sdf import unittest class IMUTest(unittest.TestCase): diff --git a/python/test/pyJointAxis_TEST.py b/python/test/pyJointAxis_TEST.py index c158a6f23..4d742de54 100644 --- a/python/test/pyJointAxis_TEST.py +++ b/python/test/pyJointAxis_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Pose3d, Vector3d -from sdformat import JointAxis, Error, SDFErrorsException +from sdformat13 import JointAxis, Error, SDFErrorsException import math import unittest diff --git a/python/test/pyJoint_TEST.py b/python/test/pyJoint_TEST.py index ccbea9487..a526b8f99 100644 --- a/python/test/pyJoint_TEST.py +++ b/python/test/pyJoint_TEST.py @@ -14,9 +14,9 @@ import copy from gz.math7 import Pose3d, Vector3d -from sdformat import (Joint, JointAxis, Error, SemanticPose, Sensor, +from sdformat13 import (Joint, JointAxis, Error, SemanticPose, Sensor, SDFErrorsException) -import sdformat as sdf +import sdformat13 as sdf import math import unittest diff --git a/python/test/pyLidar_TEST.py b/python/test/pyLidar_TEST.py index 871f7c2e6..3b81fa255 100644 --- a/python/test/pyLidar_TEST.py +++ b/python/test/pyLidar_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Angle, Pose3d, Vector3d, Helpers -from sdformat import Lidar, Error, Noise +from sdformat13 import Lidar, Error, Noise import math import unittest diff --git a/python/test/pyLight_TEST.py b/python/test/pyLight_TEST.py index d7d9657f6..f24717dee 100644 --- a/python/test/pyLight_TEST.py +++ b/python/test/pyLight_TEST.py @@ -14,8 +14,8 @@ import copy from gz.math7 import Angle, Color, Pose3d, Vector3d -from sdformat import Light, SDFErrorsException -import sdformat as sdf +from sdformat13 import Light, SDFErrorsException +import sdformat13 as sdf import math import unittest diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index e4f8f2141..9f52d84a0 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Pose3d, Inertiald, MassMatrix3d, Vector3d -from sdformat import (Collision, Light, Link, Sensor, Visual, +from sdformat13 import (Collision, Light, Link, Sensor, Visual, SDFErrorsException) import unittest import math diff --git a/python/test/pyMagnetometer_TEST.py b/python/test/pyMagnetometer_TEST.py index bb0ece24c..9366c0fe3 100644 --- a/python/test/pyMagnetometer_TEST.py +++ b/python/test/pyMagnetometer_TEST.py @@ -14,8 +14,8 @@ import copy from gz.math7 import Pose3d -from sdformat import Magnetometer, Noise -import sdformat as sdf +from sdformat13 import Magnetometer, Noise +import sdformat13 as sdf import unittest diff --git a/python/test/pyMaterial_TEST.py b/python/test/pyMaterial_TEST.py index 467682c91..98a5f8c03 100644 --- a/python/test/pyMaterial_TEST.py +++ b/python/test/pyMaterial_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from sdformat import Material, Pbr, PbrWorkflow +from sdformat13 import Material, Pbr, PbrWorkflow from gz.math7 import Color -import sdformat as sdf +import sdformat13 as sdf import unittest diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index 4f8a0d145..0bc65d2e0 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Mesh +from sdformat13 import Mesh from gz.math7 import Vector3d import unittest diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index 8abbac38f..7d756ceda 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -14,9 +14,9 @@ import copy from gz.math7 import Pose3d, Vector3d -from sdformat import (Plugin, Model, Joint, Link, Error, Frame, SemanticPose, +from sdformat13 import (Plugin, Model, Joint, Link, Error, Frame, SemanticPose, SDFErrorsException) -import sdformat as sdf +import sdformat13 as sdf import math import unittest diff --git a/python/test/pyNavSat_TEST.py b/python/test/pyNavSat_TEST.py index e6383f06a..49d8c4aca 100644 --- a/python/test/pyNavSat_TEST.py +++ b/python/test/pyNavSat_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import NavSat, Noise +from sdformat13 import NavSat, Noise import unittest class NavSatColor(unittest.TestCase): diff --git a/python/test/pyNoise_TEST.py b/python/test/pyNoise_TEST.py index f8e47e61e..07d60b2b1 100644 --- a/python/test/pyNoise_TEST.py +++ b/python/test/pyNoise_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Noise -import sdformat as sdf +from sdformat13 import Noise +import sdformat13 as sdf import math import unittest diff --git a/python/test/pyParserConfig_TEST.py b/python/test/pyParserConfig_TEST.py index d2516d98c..1c5558d43 100644 --- a/python/test/pyParserConfig_TEST.py +++ b/python/test/pyParserConfig_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import ParserConfig +from sdformat13 import ParserConfig from sdformattest import source_file, test_file import unittest diff --git a/python/test/pyParticleEmitter_TEST.py b/python/test/pyParticleEmitter_TEST.py index 03cadde60..eb119b37b 100644 --- a/python/test/pyParticleEmitter_TEST.py +++ b/python/test/pyParticleEmitter_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Color, Pose3d, Vector3d, Helpers -from sdformat import ParticleEmitter +from sdformat13 import ParticleEmitter import unittest diff --git a/python/test/pyPbr_TEST.py b/python/test/pyPbr_TEST.py index 2faf52164..d3a6585d5 100644 --- a/python/test/pyPbr_TEST.py +++ b/python/test/pyPbr_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat import Pbr, PbrWorkflow -import sdformat as sdf +from sdformat13 import Pbr, PbrWorkflow +import sdformat13 as sdf import unittest diff --git a/python/test/pyPhysics_TEST.py b/python/test/pyPhysics_TEST.py index d053a9d33..d8255799d 100644 --- a/python/test/pyPhysics_TEST.py +++ b/python/test/pyPhysics_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Physics +from sdformat13 import Physics import unittest diff --git a/python/test/pyPlane_TEST.py b/python/test/pyPlane_TEST.py index baa52b616..118304b01 100644 --- a/python/test/pyPlane_TEST.py +++ b/python/test/pyPlane_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Plane +from sdformat13 import Plane from gz.math7 import Vector3d, Vector2d, Planed import unittest diff --git a/python/test/pyPlugin_TEST.py b/python/test/pyPlugin_TEST.py index 058a7af87..c494c9985 100644 --- a/python/test/pyPlugin_TEST.py +++ b/python/test/pyPlugin_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Plugin +from sdformat13 import Plugin import unittest diff --git a/python/test/pyPolyline_TEST.py b/python/test/pyPolyline_TEST.py index 3a4f57a89..4f4b9ccc4 100644 --- a/python/test/pyPolyline_TEST.py +++ b/python/test/pyPolyline_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat import Polyline +from sdformat13 import Polyline from gz.math7 import Vector2d import unittest diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index c0909a449..d2fb58ef0 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -14,9 +14,9 @@ import copy from gz.math7 import Vector3d, Pose3d -from sdformat import (Error, Model, Light, Root, SDF_VERSION, +from sdformat13 import (Error, Model, Light, Root, SDF_VERSION, SDFErrorsException, SDF_PROTOCOL_VERSION, World) -import sdformat as sdf +import sdformat13 as sdf import unittest diff --git a/python/test/pyScene_TEST.py b/python/test/pyScene_TEST.py index d0d572133..b5398465b 100644 --- a/python/test/pyScene_TEST.py +++ b/python/test/pyScene_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Color -from sdformat import Scene, Sky +from sdformat13 import Scene, Sky import unittest diff --git a/python/test/pySemanticPose_TEST.py b/python/test/pySemanticPose_TEST.py index 06f3b5f6a..4218be6e3 100644 --- a/python/test/pySemanticPose_TEST.py +++ b/python/test/pySemanticPose_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Pose3d -from sdformat import Link, SemanticPose +from sdformat13 import Link, SemanticPose import unittest class semantic_poseTEST(unittest.TestCase): diff --git a/python/test/pySensor_TEST.py b/python/test/pySensor_TEST.py index 69bd50e87..6215941e1 100644 --- a/python/test/pySensor_TEST.py +++ b/python/test/pySensor_TEST.py @@ -14,10 +14,10 @@ import copy from gz.math7 import Pose3d -from sdformat import (AirPressure, Altimeter, Camera, IMU, ForceTorque, Lidar, +from sdformat13 import (AirPressure, Altimeter, Camera, IMU, ForceTorque, Lidar, Magnetometer, NavSat, Noise, Plugin, SemanticPose, Sensor, SDFErrorsException) -import sdformat as sdf +import sdformat13 as sdf import unittest diff --git a/python/test/pySky_TEST.py b/python/test/pySky_TEST.py index 16c1dda18..1967d548a 100644 --- a/python/test/pySky_TEST.py +++ b/python/test/pySky_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Angle, Color -from sdformat import Sky +from sdformat13 import Sky import unittest diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index d72b6536c..792719f27 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -15,7 +15,7 @@ import copy from gz.math7 import Sphered import math -from sdformat import Sphere +from sdformat13 import Sphere import unittest class SphereTEST(unittest.TestCase): diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index 86c4404f8..b40130464 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -14,7 +14,7 @@ import copy from gz.math7 import Vector3d -from sdformat import Surface, Contact, Friction, ODE +from sdformat13 import Surface, Contact, Friction, ODE import unittest diff --git a/python/test/pyVisual_TEST.py b/python/test/pyVisual_TEST.py index 65020546c..91a0f1c4b 100644 --- a/python/test/pyVisual_TEST.py +++ b/python/test/pyVisual_TEST.py @@ -14,8 +14,8 @@ import copy from gz.math7 import Pose3d, Color -from sdformat import Geometry, Material, Visual, Plugin, SDFErrorsException -import sdformat as sdf +from sdformat13 import Geometry, Material, Visual, Plugin, SDFErrorsException +import sdformat13 as sdf import unittest import math diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 583b1aa5f..6c053bdd4 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -14,8 +14,8 @@ import copy from gz.math7 import Color, Pose3d, Vector3d, SphericalCoordinates -from sdformat import Atmosphere, Gui, Physics, Plugin, Error, Frame, Light, Model, Scene, World -import sdformat as sdf +from sdformat13 import Atmosphere, Gui, Physics, Plugin, Error, Frame, Light, Model, Scene, World +import sdformat13 as sdf import unittest import math From 26e380fac94e167e7ccf20722f14525098206917 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Fri, 16 Sep 2022 16:27:22 +0200 Subject: [PATCH 16/32] Update for 13.0.0~pre2 (#1146) * Update to 13.0.0~pre2 Signed-off-by: Jose Luis Rivero * Update Changelog Signed-off-by: Jose Luis Rivero Signed-off-by: Jose Luis Rivero --- CMakeLists.txt | 2 +- Changelog.md | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fe46c5c06..8530f159e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,7 @@ if (BUILD_SDF) gz_configure_project( NO_PROJECT_PREFIX REPLACE_INCLUDE_PATH sdf - VERSION_SUFFIX pre1) + VERSION_SUFFIX pre2) ################################################# # Find tinyxml2. diff --git a/Changelog.md b/Changelog.md index 33cff8d6d..fcd033925 100644 --- a/Changelog.md +++ b/Changelog.md @@ -92,6 +92,7 @@ * [Pull request #1085](https://github.com/gazebosim/sdformat/pull/1085) * [Pull request #1106](https://github.com/gazebosim/sdformat/pull/1106) * [Pull request #1127](https://github.com/gazebosim/sdformat/pull/1127) + * [Pull request #1143](https://github.com/gazebosim/sdformat/pull/1143) 1. Copy skybox uri field to sdf/1.10/scene.sdf * [Pull request #1082](https://github.com/gazebosim/sdformat/pull/1082) From b7f0cba731101eed7e3fe84d2ea4330031f89037 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 21 Sep 2022 14:21:34 -0700 Subject: [PATCH 17/32] urdf: fix test and clean up internals (#1126) The urdf_gazebo_extensions.urdf file had incorrect kinematics with link3 being the child of two different joints. This adds a link4 and replaces joint13 with joint14 connecting link1 and link4. This also fixes some expectations about the implicit_spring_damper tags. * parser_urdf: clean up internal functions by removing unused parameters and making other parameters const. Signed-off-by: Steve Peters --- src/parser_urdf.cc | 30 +++++++------------- test/integration/urdf_gazebo_extensions.cc | 17 +++++++---- test/integration/urdf_gazebo_extensions.urdf | 30 ++++++++++++++------ 3 files changed, 42 insertions(+), 35 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 4e6772b72..dd5ad0e21 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -123,7 +123,7 @@ void CreateVisual(TiXmlElement *_elem, urdf::LinkConstSharedPtr _link, /// create SDF Joint block based on URDF void CreateJoint(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - ignition::math::Pose3d &_currentTransform); + const ignition::math::Pose3d &_currentTransform); /// insert extensions into links void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName); @@ -142,12 +142,11 @@ void AddTransform(TiXmlElement *_elem, const ignition::math::Pose3d &_transform); /// create SDF from URDF link -void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - const ignition::math::Pose3d &_transform); +void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link); /// create SDF Link block based on URDF void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - ignition::math::Pose3d &_currentTransform); + const ignition::math::Pose3d &_currentTransform); /// reduced fixed joints: apply appropriate frame updates in joint /// inside urdf extensions when doing fixed joint reduction @@ -1025,12 +1024,10 @@ void ReduceJointsToParent(urdf::LinkSharedPtr _link) { // go down the tree until we hit a parent joint that is not fixed urdf::LinkSharedPtr newParentLink = _link; - ignition::math::Pose3d jointAnchorTransform; while (newParentLink->parent_joint && newParentLink->getParent()->name != "world" && FixedJointShouldBeReduced(newParentLink->parent_joint) ) { - jointAnchorTransform = jointAnchorTransform * jointAnchorTransform; parentJoint->parent_to_joint_origin_transform = TransformToParentFrame( parentJoint->parent_to_joint_origin_transform, @@ -2606,11 +2603,8 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference) //////////////////////////////////////////////////////////////////////////////// void CreateSDF(TiXmlElement *_root, - urdf::LinkConstSharedPtr _link, - const ignition::math::Pose3d &_transform) + urdf::LinkConstSharedPtr _link) { - ignition::math::Pose3d _currentTransform = _transform; - // must have an block and cannot have zero mass. // allow det(I) == zero, in the case of point mass geoms. // @todo: keyword "world" should be a constant defined somewhere else @@ -2652,13 +2646,13 @@ void CreateSDF(TiXmlElement *_root, (!_link->parent_joint || !FixedJointShouldBeReduced(_link->parent_joint))) { - CreateLink(_root, _link, _currentTransform); + CreateLink(_root, _link, ignition::math::Pose3d::Zero); } // recurse into children for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i) { - CreateSDF(_root, _link->child_links[i], _currentTransform); + CreateSDF(_root, _link->child_links[i]); } } @@ -2693,7 +2687,7 @@ urdf::Pose CopyPose(ignition::math::Pose3d _pose) //////////////////////////////////////////////////////////////////////////////// void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - ignition::math::Pose3d &_currentTransform) + const ignition::math::Pose3d &_currentTransform) { // create new body TiXmlElement *elem = new TiXmlElement("link"); @@ -2870,7 +2864,7 @@ void CreateInertial(TiXmlElement *_elem, //////////////////////////////////////////////////////////////////////////////// void CreateJoint(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - ignition::math::Pose3d &/*_currentTransform*/) + const ignition::math::Pose3d &/*_currentTransform*/) { // compute the joint tag std::string jtype; @@ -3150,10 +3144,6 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr, return sdfXmlOut; } - // initialize transform for the model, urdf is recursive, - // while sdf defines all links relative to model frame - ignition::math::Pose3d transform; - // parse sdf extension TiXmlDocument urdfXml; urdfXml.Parse(_urdfStr.c_str()); @@ -3196,13 +3186,13 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr, child = rootLink->child_links.begin(); child != rootLink->child_links.end(); ++child) { - CreateSDF(robot, (*child), transform); + CreateSDF(robot, (*child)); } } else { // convert, starting from root link - CreateSDF(robot, rootLink, transform); + CreateSDF(robot, rootLink); } // insert the extensions without reference into root level diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 6b9fbcf37..e9d0a8d5b 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -86,17 +86,22 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) else if (jointName == "joint12") { // cfmDamping not provided - ASSERT_TRUE(joint->HasElement("physics")); + EXPECT_TRUE(joint->HasElement("physics")); sdf::ElementPtr physics = joint->GetElement("physics"); - EXPECT_FALSE(physics->HasElement("implicit_spring_damper")); + EXPECT_TRUE(physics->HasElement("ode")); + sdf::ElementPtr ode = physics->GetElement("ode"); + EXPECT_TRUE(ode->HasElement("implicit_spring_damper")); + EXPECT_FALSE(ode->Get("implicit_spring_damper")); } - else if (jointName == "joint13") + else if (jointName == "joint14") { // implicitSpringDamper = 1 - ASSERT_TRUE(joint->HasElement("physics")); + EXPECT_TRUE(joint->HasElement("physics")); sdf::ElementPtr physics = joint->GetElement("physics"); - ASSERT_TRUE(physics->HasElement("implicit_spring_damper")); - EXPECT_TRUE(physics->Get("implicit_spring_damper")); + EXPECT_TRUE(physics->HasElement("ode")); + sdf::ElementPtr ode = physics->GetElement("ode"); + EXPECT_TRUE(ode->HasElement("implicit_spring_damper")); + EXPECT_TRUE(ode->Get("implicit_spring_damper")); } } diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index a0a92c6c7..bfbde44e2 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -95,14 +95,6 @@ 0.1 - - - - - - - - @@ -125,7 +117,27 @@ - + + + + + + + + + + + + + + + + + + + + + true From b5291c479ffc5b41ce04a00b577fafa14de85975 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 23 Sep 2022 09:35:31 -0700 Subject: [PATCH 18/32] 13.0.0 release (#1169) Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8530f159e..ccdf63b6b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,7 @@ if (BUILD_SDF) gz_configure_project( NO_PROJECT_PREFIX REPLACE_INCLUDE_PATH sdf - VERSION_SUFFIX pre2) + VERSION_SUFFIX) ################################################# # Find tinyxml2. From f3cdd0778957800d6ad1d4a270316a944cc81dca Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 27 Sep 2022 05:22:21 -0700 Subject: [PATCH 19/32] Reduce the number of include dirs for sdformat (#1170) These directories are added via our cmake macros and don't need to be here. Additionally, one of the patterns was incorrect and adding a relative path to the include dirs. Signed-off-by: Michael Carroll --- src/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 68992a2da..a16582d63 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -121,9 +121,6 @@ if (WIN32) endif() target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} - PUBLIC - $ - $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ) From 06912cb73c189469bd4f812062489453923cf8c5 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 27 Sep 2022 15:09:48 -0700 Subject: [PATCH 20/32] Fix arm tests (#1173) * Just disable the tests Signed-off-by: Nate Koenig --- src/gz_TEST.cc | 4 ++++ test/integration/error_output.cc | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 8522cef21..1a80c22c6 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -1854,6 +1854,8 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); } +// Disable on arm +#if !defined __ARM_ARCH ///////////////////////////////////////////////// TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { @@ -1936,6 +1938,8 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) EXPECT_EQ(expectedOutput, output); } } +// #if !defined __ARM_ARCH +#endif ////////////////////////////////////////////////// /// \brief Check help message and bash completion script for consistent flags diff --git a/test/integration/error_output.cc b/test/integration/error_output.cc index d730774fd..a3fa97914 100644 --- a/test/integration/error_output.cc +++ b/test/integration/error_output.cc @@ -114,11 +114,13 @@ TEST(Error, ErrorOutput) "Undefined attribute //pose[@rotation_format='invalid_format'], " "only 'euler_rpy' and 'quat_xyzw' is supported.")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::PARAMETER_ERROR); +#if !defined __ARM_ARCH EXPECT_NE(std::string::npos, errors[1].Message().find( "Failed to set value '1 2 3 0.40000000000000002 0.5 " "0.59999999999999987' to key [] for new parent element of name ''," " reverting to previous value '1 2 3 0.40000000000000002 0.5 " "0.59999999999999987'.")); +#endif errors.clear(); sdf::Param param4("key", "bool", "15", false, "a", "b", errors, From e64595147e85af326e3126944ff332a76e095f30 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Sep 2022 15:56:14 -0700 Subject: [PATCH 21/32] Prepare for 13.0.1 release (#1175) Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ccdf63b6b..6d753d4ab 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 (sdformat13 VERSION 13.0.0) +project (sdformat13 VERSION 13.0.1) # 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 fcd033925..a935715e6 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,11 @@ ## libsdformat 13.X -### libsdformat 13.0.0 (202X-XX-XX) +### libsdformat 13.0.1 (2022-09-27) + +1. Fix arm tests + * [Pull request #1173](https://github.com/gazebosim/sdformat/pull/1173) + +### libsdformat 13.0.0 (2022-09-23) 1. Add camera `optical_frame_id` element * [Pull request #1109](https://github.com/gazebosim/sdformat/pull/1109) From 1ee5f0717a09c3951a9aeea9929b5ab50d1706c6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 27 Sep 2022 16:37:41 -0700 Subject: [PATCH 22/32] 12.0.0 release (#1174) Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- Changelog.md | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fe4952dbc..dec312403 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,7 +25,7 @@ if (BUILD_SDF) ign_configure_project( NO_IGNITION_PREFIX REPLACE_IGNITION_INCLUDE_PATH sdf - VERSION_SUFFIX pre1) + VERSION_SUFFIX) ################################################# # Find tinyxml2. diff --git a/Changelog.md b/Changelog.md index 74c80d9ac..8def8bffc 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,9 @@ ### libsdformat 12.6.0 (2022-09-07) +1. Reduce the number of include dirs for sdformat. + * [Pull request #1170](https://github.com/gazebosim/sdformat/pull/1170) + 1. Add camera `optical_frame_id` element * [Pull request #1109](https://github.com/gazebosim/sdformat/pull/1109) From 91a6ffc5577e31b79988aebca2f2b5ac37255662 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 28 Sep 2022 11:21:41 -0700 Subject: [PATCH 23/32] Param::Set: fix truncation of floating-point values (#1137) * Add test showing truncation by Param::Set * Param: use ParamStreamer to preserve digits in Set and GetDefault * World_TEST: increase expected precision * add precision test for #103 Signed-off-by: Steve Peters Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- include/sdf/Param.hh | 5 +- src/Param_TEST.cc | 83 +++++++++++++++++++++++++++++++++ src/World_TEST.cc | 4 +- test/integration/CMakeLists.txt | 1 + test/integration/precision.cc | 63 +++++++++++++++++++++++++ 5 files changed, 152 insertions(+), 4 deletions(-) create mode 100644 test/integration/precision.cc diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index cc456c22f..6c9ef452b 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -682,7 +682,7 @@ namespace sdf try { std::stringstream ss; - ss << _value; + ss << ParamStreamer{_value, std::numeric_limits::max()}; return this->SetFromString(ss.str(), true, _errors); } catch(...) @@ -777,7 +777,8 @@ namespace sdf try { - ss << ParamStreamer{this->dataPtr->defaultValue}; + ss << ParamStreamer{this->dataPtr->defaultValue, + std::numeric_limits::max()}; ss >> _value; } catch(...) diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index fa727c59b..56f30b4e0 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -98,6 +98,89 @@ TEST(Param, Bool) EXPECT_TRUE(value); } +//////////////////////////////////////////////////// +/// Test getting and setting a floating point value +TEST(Param, Double) +{ + sdf::Param doubleParam("key", "double", "0.0", false, "description"); + double value = -1.0; + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(0.0, value); + + doubleParam.Set(1.0); + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(1.0, value); + + const double sixteenDigits = 12345678.87654321; + doubleParam.Set(sixteenDigits); + EXPECT_TRUE(doubleParam.Get(value)); + EXPECT_DOUBLE_EQ(sixteenDigits, value); +} + +//////////////////////////////////////////////////// +/// Test getting and setting a Vector3d +TEST(Param, Vector3d) +{ + sdf::Param vectorParam("key", "vector3", "0 0 0", false, "description"); + gz::math::Vector3d value{1.0, 2.0, 3.0}; + EXPECT_TRUE(vectorParam.Get(value)); + EXPECT_DOUBLE_EQ(0.0, value.X()); + EXPECT_DOUBLE_EQ(0.0, value.Y()); + EXPECT_DOUBLE_EQ(0.0, value.Z()); + + vectorParam.Set(gz::math::Vector3d::One); + EXPECT_TRUE(vectorParam.Get(value)); + EXPECT_DOUBLE_EQ(1.0, value.X()); + EXPECT_DOUBLE_EQ(1.0, value.Y()); + EXPECT_DOUBLE_EQ(1.0, value.Z()); + + const double fifteenDigits = 0.123456789012345; + const gz::math::Vector3d manyDigits{ + fifteenDigits, 1.0 + fifteenDigits, 2.0 + fifteenDigits}; + vectorParam.Set(manyDigits); + EXPECT_TRUE(vectorParam.Get(value)); + EXPECT_DOUBLE_EQ(0.0 + fifteenDigits, value.X()); + EXPECT_DOUBLE_EQ(1.0 + fifteenDigits, value.Y()); + EXPECT_DOUBLE_EQ(2.0 + fifteenDigits, value.Z()); +} + +//////////////////////////////////////////////////// +/// Test getting and setting a Pose3d +TEST(Param, Pose3d) +{ + sdf::Param poseParam("key", "pose", "0 0 0 0 0 0", false, "description"); + gz::math::Pose3d value{1.0, 2.0, 3.0, 0.1, 0.2, 0.3}; + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_DOUBLE_EQ(0.0, value.Pos().X()); + EXPECT_DOUBLE_EQ(0.0, value.Pos().Y()); + EXPECT_DOUBLE_EQ(0.0, value.Pos().Z()); + EXPECT_DOUBLE_EQ(0.0, value.Rot().Euler().X()); + EXPECT_DOUBLE_EQ(0.0, value.Rot().Euler().Y()); + EXPECT_DOUBLE_EQ(0.0, value.Rot().Euler().Z()); + + poseParam.Set(gz::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_DOUBLE_EQ(1.0, value.Pos().X()); + EXPECT_DOUBLE_EQ(2.0, value.Pos().Y()); + EXPECT_DOUBLE_EQ(3.0, value.Pos().Z()); + EXPECT_DOUBLE_EQ(0.1, value.Rot().Euler().X()); + EXPECT_DOUBLE_EQ(0.2, value.Rot().Euler().Y()); + EXPECT_DOUBLE_EQ(0.3, value.Rot().Euler().Z()); + + const double fifteenDigits = 0.123456789012345; + const gz::math::Pose3d manyDigits{ + fifteenDigits, 1.0 + fifteenDigits, 2.0 + fifteenDigits, + 0.5 * fifteenDigits, fifteenDigits, 2.0 * fifteenDigits}; + poseParam.Set(manyDigits); + EXPECT_TRUE(poseParam.Get(value)); + EXPECT_DOUBLE_EQ(0.0 + fifteenDigits, value.Pos().X()); + EXPECT_DOUBLE_EQ(1.0 + fifteenDigits, value.Pos().Y()); + EXPECT_DOUBLE_EQ(2.0 + fifteenDigits, value.Pos().Z()); + EXPECT_DOUBLE_EQ(0.5 * fifteenDigits, value.Rot().Euler().X()); + EXPECT_DOUBLE_EQ(1.0 * fifteenDigits, value.Rot().Euler().Y()); + EXPECT_DOUBLE_EQ(2.0 * fifteenDigits, value.Rot().Euler().Z()); +} + //////////////////////////////////////////////////// /// Test decimal number TEST(SetFromString, Decimals) diff --git a/src/World_TEST.cc b/src/World_TEST.cc index e850edccb..d8c15faa7 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -596,8 +596,8 @@ TEST(DOMWorld, ToElement) " 0\n" " 0\n" " ENU\n" - " 6378140\n" - " 6356750\n" + " 6378137\n" + " 6356752.3142449996\n" "\n"; EXPECT_EQ(sphericalCoordsSdf, diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index dcb8f891f..056f63afc 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -42,6 +42,7 @@ set(tests plugin_bool.cc plugin_include.cc pose_1_9_sdf.cc + precision.cc print_config.cc provide_feedback.cc root_dom.cc diff --git a/test/integration/precision.cc b/test/integration/precision.cc new file mode 100644 index 000000000..b2503ecd3 --- /dev/null +++ b/test/integration/precision.cc @@ -0,0 +1,63 @@ +/* + * Copyright 2015 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include "sdf/sdf.hh" + +std::string get_sdf_string() +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " 182536.87" + << " " + << " " + << " " + << "" + << ""; + return stream.str(); +} + +//////////////////////////////////////// +// make sure that XML errors get caught +TEST(Precision, StringToDouble) +{ + sdf::SDFPtr model(new sdf::SDF()); + sdf::init(model); + ASSERT_TRUE(sdf::readString(get_sdf_string(), model)); + + sdf::ElementPtr modelElem = model->Root()->GetElement("model"); + EXPECT_TRUE(modelElem->HasElement("link")); + sdf::ElementPtr linkElem = modelElem->GetElement("link"); + EXPECT_TRUE(linkElem->HasElement("visual")); + sdf::ElementPtr visualElem = linkElem->GetElement("visual"); + EXPECT_TRUE(visualElem->HasElement("geometry")); + sdf::ElementPtr geomElem = visualElem->GetElement("geometry"); + EXPECT_TRUE(geomElem->HasElement("sphere")); + sdf::ElementPtr sphereElem = geomElem->GetElement("sphere"); + EXPECT_TRUE(sphereElem->HasElement("radius")); + sdf::ElementPtr radiusElem = sphereElem->GetElement("radius"); + + std::cout << std::setprecision(10) << radiusElem->Get() << std::endl; + + EXPECT_DOUBLE_EQ(radiusElem->Get(), 182536.87); +} From 26569a57cc2a5ca19abb79c30f7610aa8d286bfe Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 28 Sep 2022 18:56:50 -0700 Subject: [PATCH 24/32] parser_urdf: add //frame for reduced links/joints (#1148) Currently the location of some links and joints is lost when fixed joints are reduced and links lumped together. This adds //model/frame tags with the same name and pose of the reduced links and joints. Signed-off-by: Steve Peters --- src/parser_urdf.cc | 36 +++++++ test/integration/fixed_joint_example.urdf | 63 +++++++++++ test/integration/fixed_joint_simple.urdf | 34 ++++++ test/integration/urdf_gazebo_extensions.cc | 115 ++++++++++++++++++++- 4 files changed, 244 insertions(+), 4 deletions(-) create mode 100644 test/integration/fixed_joint_example.urdf create mode 100644 test/integration/fixed_joint_simple.urdf diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index dd5ad0e21..e6f07def2 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -412,6 +412,42 @@ void ReduceFixedJoints(TiXmlElement *_root, urdf::LinkSharedPtr _link) sdfdbg << "Fixed Joint Reduction: extension lumping from [" << _link->name << "] to [" << _link->getParent()->name << "]\n"; + // Add //model/frame tag to memorialize reduced joint + std::stringstream ssj; + ssj << "\n"; + ssj << " " + << CopyPose(_link->parent_joint->parent_to_joint_origin_transform) + << "\n"; + ssj << "\n"; + + // Add //model/frame tag to memorialize reduced link + std::stringstream ssl; + ssl << "\n"; + + // Serialize sdf::Frame objects to xml and add to SDFExtension + SDFExtensionPtr sdfExt = std::make_shared(); + auto stringToExtension = [&sdfExt](const std::string &_frame) + { + TiXmlDocument xmlNewDoc; + xmlNewDoc.Parse(_frame.c_str()); + if (xmlNewDoc.Error()) + { + sdferr << "Error while parsing serialized frames: " + << xmlNewDoc.ErrorDesc() << '\n'; + } + + TiXmlElementPtr blob = + std::make_shared(*xmlNewDoc.FirstChildElement()); + sdfExt->blobs.push_back(blob); + }; + stringToExtension(ssj.str()); + stringToExtension(ssl.str()); + + // Add //frame tags to model extension vector + g_extensions[""].push_back(sdfExt); + // lump sdf extensions to parent, (give them new reference _link names) ReduceSDFExtensionToParent(_link); diff --git a/test/integration/fixed_joint_example.urdf b/test/integration/fixed_joint_example.urdf new file mode 100644 index 000000000..9f09ab285 --- /dev/null +++ b/test/integration/fixed_joint_example.urdf @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/integration/fixed_joint_simple.urdf b/test/integration/fixed_joint_simple.urdf new file mode 100644 index 000000000..3443f962f --- /dev/null +++ b/test/integration/fixed_joint_simple.urdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index e9d0a8d5b..b75ea6f2c 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -29,11 +29,17 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) const std::string urdfTestFile = sdf::testing::TestFile("integration", "urdf_gazebo_extensions.urdf"); - sdf::SDFPtr robot(new sdf::SDF()); - sdf::init(robot); - ASSERT_TRUE(sdf::readFile(urdfTestFile, robot)); + sdf::Root root; + auto errors = root.Load(urdfTestFile); + EXPECT_TRUE(errors.empty()); + for (auto e : errors) + { + std::cerr << e << std::endl; + } - sdf::ElementPtr model = robot->Root()->GetElement("model"); + auto modelDom = root.ModelByIndex(0); + ASSERT_NE(nullptr, modelDom); + sdf::ElementPtr model = modelDom->Element(); for (sdf::ElementPtr joint = model->GetElement("joint"); joint; joint = joint->GetNextElement("joint")) { @@ -306,4 +312,105 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) checkElementPoses("light"); checkElementPoses("projector"); checkElementPoses("sensor"); + + // Check that //model/frame elements are added for reduced joints + EXPECT_EQ(14u, modelDom->FrameCount()); + + EXPECT_TRUE(modelDom->FrameNameExists("issue378_link_joint")); + EXPECT_TRUE(modelDom->FrameNameExists("jCamera")); + EXPECT_TRUE(modelDom->FrameNameExists("jointSensorNoPose")); + EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPose")); + EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseRelative")); + EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseTwoLevel")); + EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseTwoLevel2")); + + EXPECT_TRUE(modelDom->FrameNameExists("issue378_link")); + EXPECT_TRUE(modelDom->FrameNameExists("Camera")); + EXPECT_TRUE(modelDom->FrameNameExists("linkSensorNoPose")); + EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPose")); + EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseRelative")); + EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseTwoLevel")); + EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseTwoLevel2")); +} + +///////////////////////////////////////////////// +TEST(SDFParser, FixedJointExample) +{ + const std::string urdfTestFile = + sdf::testing::TestFile("integration", "fixed_joint_example.urdf"); + + sdf::Root root; + auto errors = root.Load(urdfTestFile); + EXPECT_TRUE(errors.empty()); + for (auto e : errors) + { + std::cerr << e << std::endl; + } + + auto model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("fixed_joint_example", model->Name()); + + EXPECT_EQ(2u, model->LinkCount()); + EXPECT_TRUE(model->LinkNameExists("base")); + EXPECT_TRUE(model->LinkNameExists("rotary_link")); + + // Expect MassMatrix3 values to match for links + auto link1 = model->LinkByName("base"); + auto link2 = model->LinkByName("rotary_link"); + ASSERT_NE(nullptr, link1); + ASSERT_NE(nullptr, link2); + auto massMatrix1 = link1->Inertial().MassMatrix(); + auto massMatrix2 = link2->Inertial().MassMatrix(); + EXPECT_DOUBLE_EQ(massMatrix1.Mass(), massMatrix2.Mass()); + EXPECT_EQ(massMatrix1.Moi(), massMatrix2.Moi()); + + EXPECT_EQ(1u, model->JointCount()); + EXPECT_TRUE(model->JointNameExists("rotary_joint")); + + EXPECT_EQ(2u, model->FrameCount()); + ASSERT_TRUE(model->FrameNameExists("intermediate_joint")); + ASSERT_TRUE(model->FrameNameExists("intermediate_link")); + + const std::string j = "intermediate_joint"; + const std::string l = "intermediate_link"; + std::string body; + EXPECT_TRUE(model->FrameByName(j)->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("base", body); + EXPECT_TRUE(model->FrameByName(l)->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("base", body); +} + +///////////////////////////////////////////////// +TEST(SDFParser, FixedJointSimple) +{ + const std::string urdfTestFile = + sdf::testing::TestFile("integration", "fixed_joint_simple.urdf"); + + sdf::Root root; + auto errors = root.Load(urdfTestFile); + EXPECT_TRUE(errors.empty()); + for (auto e : errors) + { + std::cerr << e << std::endl; + } + + auto model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("fixed_joint_simple", model->Name()); + + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_TRUE(model->LinkNameExists("base")); + + auto link = model->LinkByName("base"); + ASSERT_NE(nullptr, link); + auto massMatrix = link->Inertial().MassMatrix(); + EXPECT_DOUBLE_EQ(2.0, massMatrix.Mass()); + EXPECT_EQ(2.0 * ignition::math::Matrix3d::Identity, massMatrix.Moi()); + + EXPECT_EQ(0u, model->JointCount()); + + EXPECT_EQ(2u, model->FrameCount()); + ASSERT_TRUE(model->FrameNameExists("fixed_joint")); + ASSERT_TRUE(model->FrameNameExists("child_link")); } From 9a67dfdbe047554be73f83309ab725ef748b28c7 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 28 Sep 2022 20:20:45 -0700 Subject: [PATCH 25/32] sdf/1.10/camera.sdf: update from 1.9 (#1177) Signed-off-by: Steve Peters --- sdf/1.10/camera.sdf | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/sdf/1.10/camera.sdf b/sdf/1.10/camera.sdf index 092aba97c..708e1d6eb 100644 --- a/sdf/1.10/camera.sdf +++ b/sdf/1.10/camera.sdf @@ -190,11 +190,37 @@ XY axis skew + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + + + An optional frame id name to be used in the camera_info message header. + + From 8bde4a9df848199e69f4017d77d22726abe4cbd1 Mon Sep 17 00:00:00 2001 From: Will Stott Date: Fri, 30 Sep 2022 03:58:04 +0100 Subject: [PATCH 26/32] python: Import gz.math at startup to fix #1129 (#1130) * python: Don't import more than is used in the tests - should cause a failure due to gz.math not always being imported. * python: Import gz.math at startup to fix #1129 Signed-off-by: Will Stott Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- python/src/sdf/_gz_sdformat_pybind11.cc | 7 +++++++ python/test/pyAirPressure_TEST.py | 1 - python/test/pyAltimeter_TEST.py | 1 - python/test/pyAtmosphere_TEST.py | 2 +- python/test/pyCapsule_TEST.py | 2 -- python/test/pyCylinder_TEST.py | 2 -- python/test/pyEllipsoid_TEST.py | 2 +- python/test/pyJointAxis_TEST.py | 2 +- python/test/pyLidar_TEST.py | 2 +- python/test/pyMagnetometer_TEST.py | 1 - python/test/pyModel_TEST.py | 2 +- python/test/pyPlane_TEST.py | 2 +- python/test/pyRoot_TEST.py | 2 +- python/test/pySphere_TEST.py | 1 - python/test/pyWorld_TEST.py | 2 +- 15 files changed, 15 insertions(+), 16 deletions(-) diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 681bf6990..0e64d1e8a 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -16,6 +16,7 @@ #include #include +#include #include "pyAirPressure.hh" #include "pyAltimeter.hh" @@ -65,6 +66,12 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { m.doc() = "sdformat Python Library."; + // Import the gz.math library to automatically add the type conversions + // this module requires to pass mathematical types to python code. + std::string gzMathModule = + std::string("gz.math") + std::to_string(GZ_MATH_MAJOR_VERSION); + pybind11::module::import(gzMathModule.c_str()); + sdf::python::defineAirPressure(m); sdf::python::defineAltimeter(m); sdf::python::defineAtmosphere(m); diff --git a/python/test/pyAirPressure_TEST.py b/python/test/pyAirPressure_TEST.py index 4abf3bccc..3848f6b16 100644 --- a/python/test/pyAirPressure_TEST.py +++ b/python/test/pyAirPressure_TEST.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math7 import Vector3d from sdformat13 import AirPressure, Noise import sdformat13 as sdf import unittest diff --git a/python/test/pyAltimeter_TEST.py b/python/test/pyAltimeter_TEST.py index 96253e5da..27e862a74 100644 --- a/python/test/pyAltimeter_TEST.py +++ b/python/test/pyAltimeter_TEST.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math7 import Vector3d from sdformat13 import Altimeter, Noise import sdformat13 as sdf import unittest diff --git a/python/test/pyAtmosphere_TEST.py b/python/test/pyAtmosphere_TEST.py index d2363c1af..e0ac4d3d6 100644 --- a/python/test/pyAtmosphere_TEST.py +++ b/python/test/pyAtmosphere_TEST.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math7 import Pose3d, Temperature +from gz.math7 import Temperature from sdformat13 import Atmosphere import sdformat13 as sdf import unittest diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 8924c9ba2..b91ba6eb6 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -14,8 +14,6 @@ import copy -from gz.math7 import Vector3d, Capsuled - import math from sdformat13 import Capsule diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index 55b5ecf9a..50b121991 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -14,8 +14,6 @@ import copy -from gz.math7 import Vector3d - import math from sdformat13 import Cylinder diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index e66953bb9..d3dd7291a 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.math7 import Vector3d, Ellipsoidd +from gz.math7 import Vector3d import math from sdformat13 import Ellipsoid import unittest diff --git a/python/test/pyJointAxis_TEST.py b/python/test/pyJointAxis_TEST.py index 4d742de54..96f139f15 100644 --- a/python/test/pyJointAxis_TEST.py +++ b/python/test/pyJointAxis_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math7 import Pose3d, Vector3d +from gz.math7 import Vector3d from sdformat13 import JointAxis, Error, SDFErrorsException import math import unittest diff --git a/python/test/pyLidar_TEST.py b/python/test/pyLidar_TEST.py index 3b81fa255..585c48259 100644 --- a/python/test/pyLidar_TEST.py +++ b/python/test/pyLidar_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math7 import Angle, Pose3d, Vector3d, Helpers +from gz.math7 import Angle, Helpers from sdformat13 import Lidar, Error, Noise import math import unittest diff --git a/python/test/pyMagnetometer_TEST.py b/python/test/pyMagnetometer_TEST.py index 9366c0fe3..636a5f3aa 100644 --- a/python/test/pyMagnetometer_TEST.py +++ b/python/test/pyMagnetometer_TEST.py @@ -13,7 +13,6 @@ # limitations under the License. import copy -from gz.math7 import Pose3d from sdformat13 import Magnetometer, Noise import sdformat13 as sdf import unittest diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index 7d756ceda..ae2e575a0 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math7 import Pose3d, Vector3d +from gz.math7 import Pose3d from sdformat13 import (Plugin, Model, Joint, Link, Error, Frame, SemanticPose, SDFErrorsException) import sdformat13 as sdf diff --git a/python/test/pyPlane_TEST.py b/python/test/pyPlane_TEST.py index 118304b01..66ee406ac 100644 --- a/python/test/pyPlane_TEST.py +++ b/python/test/pyPlane_TEST.py @@ -14,7 +14,7 @@ import copy from sdformat13 import Plane -from gz.math7 import Vector3d, Vector2d, Planed +from gz.math7 import Vector3d, Vector2d import unittest diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index d2fb58ef0..6fc956c9d 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math7 import Vector3d, Pose3d +from gz.math7 import Pose3d from sdformat13 import (Error, Model, Light, Root, SDF_VERSION, SDFErrorsException, SDF_PROTOCOL_VERSION, World) import sdformat13 as sdf diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index 792719f27..ed92a72f3 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -13,7 +13,6 @@ # limitations under the License. import copy -from gz.math7 import Sphered import math from sdformat13 import Sphere import unittest diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 6c053bdd4..05696766a 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz.math7 import Color, Pose3d, Vector3d, SphericalCoordinates +from gz.math7 import Color, Vector3d, SphericalCoordinates from sdformat13 import Atmosphere, Gui, Physics, Plugin, Error, Frame, Light, Model, Scene, World import sdformat13 as sdf import unittest From f20edfd284662dba3256a0eca29f15ff761707eb Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 30 Sep 2022 11:28:16 -0700 Subject: [PATCH 27/32] urdf: use sdf::Frame::ToElement internally (#1182) Follow-up for #1148 with a cleaner implementation for the sdf13 branch that constructs sdf::Frame objects and serializes them to a string rather than using a stringstream to construct XML. Signed-off-by: Steve Peters --- src/parser_urdf.cc | 28 +++++++++++----------- test/integration/urdf_gazebo_extensions.cc | 20 ++++------------ 2 files changed, 18 insertions(+), 30 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 6bb0cbea2..c2660ae9b 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -425,25 +425,25 @@ void ReduceFixedJoints(tinyxml2::XMLElement *_root, urdf::LinkSharedPtr _link) << _link->name << "] to [" << _link->getParent()->name << "]\n"; // Add //model/frame tag to memorialize reduced joint - std::stringstream ssj; - ssj << "\n"; - ssj << " " - << CopyPose(_link->parent_joint->parent_to_joint_origin_transform) - << "\n"; - ssj << "\n"; + sdf::Frame jointFrame; + jointFrame.SetName(_link->parent_joint->name); + jointFrame.SetAttachedTo(_link->getParent()->name); + jointFrame.SetRawPose( + CopyPose(_link->parent_joint->parent_to_joint_origin_transform)); // Add //model/frame tag to memorialize reduced link - std::stringstream ssl; - ssl << "\n"; + sdf::Frame linkFrame; + linkFrame.SetName(_link->name); + linkFrame.SetAttachedTo(_link->parent_joint->name); // Serialize sdf::Frame objects to xml and add to SDFExtension SDFExtensionPtr sdfExt = std::make_shared(); - auto stringToExtension = [&sdfExt](const std::string &_frame) + auto sdfFrameToExtension = [&sdfExt](const sdf::Frame &_frame) { XMLDocumentPtr xmlNewDoc = std::make_shared(); - xmlNewDoc->Parse(_frame.c_str()); + sdf::PrintConfig config; + config.SetOutPrecision(16); + xmlNewDoc->Parse(_frame.ToElement()->ToString("", config).c_str()); if (xmlNewDoc->Error()) { sdferr << "Error while parsing serialized frames: " @@ -451,8 +451,8 @@ void ReduceFixedJoints(tinyxml2::XMLElement *_root, urdf::LinkSharedPtr _link) } sdfExt->blobs.push_back(xmlNewDoc); }; - stringToExtension(ssj.str()); - stringToExtension(ssl.str()); + sdfFrameToExtension(jointFrame); + sdfFrameToExtension(linkFrame); // Add //frame tags to model extension vector g_extensions[""].push_back(sdfExt); diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 9201e8218..541ac5217 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -31,11 +31,7 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) sdf::Root root; auto errors = root.Load(urdfTestFile); - EXPECT_TRUE(errors.empty()); - for (auto e : errors) - { - std::cerr << e << std::endl; - } + EXPECT_TRUE(errors.empty()) << errors; auto modelDom = root.Model(); ASSERT_NE(nullptr, modelDom); @@ -334,11 +330,7 @@ TEST(SDFParser, FixedJointExample) sdf::Root root; auto errors = root.Load(urdfTestFile); - EXPECT_TRUE(errors.empty()); - for (auto e : errors) - { - std::cerr << e << std::endl; - } + EXPECT_TRUE(errors.empty()) << errors; auto model = root.Model(); ASSERT_NE(nullptr, model); @@ -382,11 +374,7 @@ TEST(SDFParser, FixedJointSimple) sdf::Root root; auto errors = root.Load(urdfTestFile); - EXPECT_TRUE(errors.empty()); - for (auto e : errors) - { - std::cerr << e << std::endl; - } + EXPECT_TRUE(errors.empty()) << errors; auto model = root.Model(); ASSERT_NE(nullptr, model); @@ -399,7 +387,7 @@ TEST(SDFParser, FixedJointSimple) ASSERT_NE(nullptr, link); auto massMatrix = link->Inertial().MassMatrix(); EXPECT_DOUBLE_EQ(2.0, massMatrix.Mass()); - EXPECT_EQ(2.0 * ignition::math::Matrix3d::Identity, massMatrix.Moi()); + EXPECT_EQ(2.0 * gz::math::Matrix3d::Identity, massMatrix.Moi()); EXPECT_EQ(0u, model->JointCount()); From 3173930c3dc19affa5f5fb82e6b69a9558520711 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Tue, 16 Aug 2022 00:06:34 +0800 Subject: [PATCH 28/32] Element: add sdf::Errors output to API methods (#1095) Signed-off-by: Marco A. Gutierrez Co-authored-by: Steve Peters Co-authored-by: Addisu Z. Taddese --- include/sdf/Element.hh | 91 +++++++++++++- include/sdf/Error.hh | 4 + include/sdf/Param.hh | 4 +- src/Element.cc | 210 +++++++++++++++++++++++++------ src/Param.cc | 2 +- src/Utils.cc | 17 +++ src/Utils.hh | 5 + src/gz_TEST.cc | 8 +- src/parser_TEST.cc | 2 +- test/integration/error_output.cc | 75 ++++++++++- 10 files changed, 373 insertions(+), 45 deletions(-) diff --git a/include/sdf/Element.hh b/include/sdf/Element.hh index 52ce5f7cb..1afc35799 100644 --- a/include/sdf/Element.hh +++ b/include/sdf/Element.hh @@ -83,10 +83,20 @@ namespace sdf /// \return A copy of this Element. public: ElementPtr Clone() const; + /// \brief Create a copy of this Element. + /// \param[out] _errors Vector of errors. + /// \return A copy of this Element, NULL if there was an error. + public: ElementPtr Clone(sdf::Errors &_errors) const; + /// \brief Copy values from an Element. /// \param[in] _elem Element to copy value from. public: void Copy(const ElementPtr _elem); + /// \brief Copy values from an Element. + /// \param[in] _elem Element to copy value from. + /// \param[out] _errors Vector of errors. + public: void Copy(const ElementPtr _elem, sdf::Errors &_errors); + /// \brief Get a pointer to this Element's parent. /// \return Pointer to this Element's parent, NULL if there is no /// parent. @@ -220,6 +230,21 @@ namespace sdf bool _required, const std::string &_description = ""); + /// \brief Add an attribute value. + /// \param[in] _key Key value. + /// \param[in] _type Type of data the attribute will hold. + /// \param[in] _defaultValue Default value for the attribute. + /// \param[in] _required Requirement string. \as Element::SetRequired. + /// \param[out] _errors Vector of errors. + /// \param[in] _description A text description of the attribute. + /// \throws sdf::AssertionInternalError if an invalid type is given. + public: void AddAttribute(const std::string &_key, + const std::string &_type, + const std::string &_defaultvalue, + bool _required, + sdf::Errors &_errors, + const std::string &_description = ""); + /// \brief Add a value to this Element. /// \param[in] _type Type of data the parameter will hold. /// \param[in] _defaultValue Default value for the parameter. @@ -230,6 +255,18 @@ namespace sdf const std::string &_defaultValue, bool _required, const std::string &_description = ""); + /// \brief Add a value to this Element. + /// \param[in] _type Type of data the parameter will hold. + /// \param[in] _defaultValue Default value for the parameter. + /// \param[in] _required Requirement string. \as Element::SetRequired. + /// \param[out] _errors Vector of errors. + /// \param[in] _description A text description of the parameter. + /// \throws sdf::AssertionInternalError if an invalid type is given. + public: void AddValue(const std::string &_type, + const std::string &_defaultValue, bool _required, + sdf::Errors &_errors, + const std::string &_description = ""); + /// \brief Add a value to this Element. This override allows passing min and /// max values of the parameter. /// \param[in] _type Type of data the parameter will hold. @@ -245,6 +282,23 @@ namespace sdf const std::string &_maxValue, const std::string &_description = ""); + /// \brief Add a value to this Element. This override allows passing min and + /// max values of the parameter. + /// \param[in] _type Type of data the parameter will hold. + /// \param[in] _defaultValue Default value for the parameter. + /// \param[in] _required Requirement string. \as Element::SetRequired. + /// \param[in] _minValue Minimum allowed value for the parameter. + /// \param[in] _maxValue Maximum allowed value for the parameter. + /// \param[out] _errors Vector of errors. + /// \param[in] _description A text description of the parameter. + /// \throws sdf::AssertionInternalError if an invalid type is given. + public: void AddValue(const std::string &_type, + const std::string &_defaultValue, bool _required, + const std::string &_minValue, + const std::string &_maxValue, + sdf::Errors &_errors, + const std::string &_description = ""); + /// \brief Get the param of an attribute. /// \param[in] _key the name of the attribute. /// \return The parameter attribute value. NULL if the key is invalid. @@ -309,6 +363,14 @@ namespace sdf /// \return The element as a std::any. public: std::any GetAny(const std::string &_key = "") const; + /// \brief Get the element value/attribute as a std::any. + /// \param[in] _key The key of the attribute. If empty, get the value of + /// the element. Defaults to empty. + /// \param[out] _errors Vector of errors. + /// \return The element as a std::any. + public: std::any GetAny(sdf::Errors &_errors, + const std::string &_key = "") const; + /// \brief Get the value of a key. This function assumes the _key /// exists. /// \param[in] _key the name of a child attribute or element. @@ -431,6 +493,20 @@ namespace sdf /// element if an existing child element did not exist. public: ElementPtr GetElement(const std::string &_name); + /// \brief Return a pointer to the child element with the provided name. + /// + /// A new child element, with the provided name, is added to this element + /// if there is no existing child element. If this is not desired see \ref + /// FindElement + /// \remarks If there are multiple elements with the given tag, it returns + /// the first one. + /// \param[in] _name Name of the child element to retreive. + /// \param[out] _errors Vector of errors. + /// \return Pointer to the existing child element, or a new child + /// element if an existing child element did not exist. + public: ElementPtr GetElement(const std::string &_name, + sdf::Errors &_errors); + /// \brief Return a pointer to the child element with the provided name. /// /// Unlike \ref GetElement, this does not create a new child element if it @@ -458,6 +534,13 @@ namespace sdf /// \return A pointer to the newly created Element object. public: ElementPtr AddElement(const std::string &_name); + /// \brief Add a named element. + /// \param[in] _name the name of the element to add. + /// \param[out] _errors Vector of errors. + /// \return A pointer to the newly created Element object. + public: ElementPtr AddElement(const std::string &_name, + sdf::Errors &_errors); + /// \brief Add an element object. /// \param[in] _elem the element object to add. public: void InsertElement(ElementPtr _elem); @@ -476,6 +559,11 @@ namespace sdf /// \param[in] _child Pointer to the child to remove. public: void RemoveChild(ElementPtr _child); + /// \brief Remove a child element. + /// \param[in] _child Pointer to the child to remove. + /// \param[out] _errors Vector of errors. + public: void RemoveChild(ElementPtr _child, sdf::Errors &_errors); + /// \brief Remove all child elements. public: void ClearElements(); @@ -602,15 +690,16 @@ namespace sdf /// int,...). /// \param[in] _defaultValue Default value. /// \param[in] _required True if the parameter is required to be set. + /// \param[out] _errors Vector of errors. /// \param[in] _description Description of the parameter. /// \return A pointer to the new Param object. private: ParamPtr CreateParam(const std::string &_key, const std::string &_type, const std::string &_defaultValue, bool _required, + sdf::Errors &_errors, const std::string &_description = ""); - /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index ff424e98f..14fc60675 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -161,6 +161,10 @@ namespace sdf /// \brief The specified parameter (values of SDFormat elements /// or attributes) type is unknown. UNKNOWN_PARAMETER_TYPE, + + /// \brief Generic error to be thrown with SDF_ASSERT by the caller. + /// This has been created to help preserve behavior. + FATAL_ERROR, }; class SDFORMAT_VISIBLE Error diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index 6c9ef452b..29c5b90e5 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -307,14 +307,14 @@ namespace sdf /// \brief Set the parent Element of this Param. /// \param[in] _parentElement Pointer to new parent Element. A nullptr can - /// provided to remove the current parent Element. + /// be provided to remove the current parent Element. /// \return True if the parent Element was set and the value was reparsed /// successfully. public: bool SetParentElement(ElementPtr _parentElement); /// \brief Set the parent Element of this Param. /// \param[in] _parentElement Pointer to new parent Element. A nullptr can - /// provided to remove the current parent Element. + /// be provided to remove the current parent Element. /// \param[out] _errors Vector of errors. /// \return True if the parent Element was set and the value was reparsed /// successfully. diff --git a/src/Element.cc b/src/Element.cc index 68f65e1c5..5dfc243c6 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -22,6 +22,7 @@ #include "sdf/Assert.hh" #include "sdf/Element.hh" #include "sdf/Filesystem.hh" +#include "Utils.hh" using namespace sdf; @@ -137,8 +138,35 @@ void Element::AddValue(const std::string &_type, bool _required, const std::string &_description) { + sdf::Errors errors; this->dataPtr->value = this->CreateParam(this->dataPtr->name, - _type, _defaultValue, _required, _description); + _type, _defaultValue, _required, errors, _description); + throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void Element::AddValue(const std::string &_type, + const std::string &_defaultValue, + bool _required, + sdf::Errors &_errors, + const std::string &_description) +{ + this->dataPtr->value = this->CreateParam(this->dataPtr->name, + _type, _defaultValue, _required, _errors, _description); +} + +///////////////////////////////////////////////// +void Element::AddValue(const std::string &_type, + const std::string &_defaultValue, + bool _required, + const std::string &_minValue, + const std::string &_maxValue, + const std::string &_description) +{ + sdf::Errors errors; + this->AddValue(_type, _defaultValue, _required, _minValue, _maxValue, + errors, _description); + throwOrPrintErrors(errors); } ///////////////////////////////////////////////// @@ -147,13 +175,18 @@ void Element::AddValue(const std::string &_type, bool _required, const std::string &_minValue, const std::string &_maxValue, + sdf::Errors &_errors, const std::string &_description) { this->dataPtr->value = std::make_shared(this->dataPtr->name, _type, _defaultValue, - _required, _minValue, _maxValue, _description); - SDF_ASSERT(this->dataPtr->value->SetParentElement(shared_from_this()), - "Cannot set parent Element of value to itself."); + _required, _minValue, _maxValue, _errors, + _description); + if (!this->dataPtr->value->SetParentElement(shared_from_this(), _errors)) + { + _errors.push_back({ErrorCode::FATAL_ERROR, + "Cannot set parent Element of value to itself."}); + } } ///////////////////////////////////////////////// @@ -161,12 +194,17 @@ ParamPtr Element::CreateParam(const std::string &_key, const std::string &_type, const std::string &_defaultValue, bool _required, + sdf::Errors &_errors, const std::string &_description) { ParamPtr param = std::make_shared( - _key, _type, _defaultValue, _required, _description); - SDF_ASSERT(param->SetParentElement(shared_from_this()), - "Cannot set parent Element of created Param to itself."); + _key, _type, _defaultValue, _required, _errors, _description); + + if(!param->SetParentElement(shared_from_this(), _errors)) + { + _errors.push_back({ErrorCode::FATAL_ERROR, + "Cannot set parent Element of created Param to itself."}); + } return param; } @@ -176,13 +214,37 @@ void Element::AddAttribute(const std::string &_key, const std::string &_defaultValue, bool _required, const std::string &_description) +{ + sdf::Errors errors; + this->AddAttribute(_key, _type, _defaultValue, _required, + errors, _description); + throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void Element::AddAttribute(const std::string &_key, + const std::string &_type, + const std::string &_defaultValue, + bool _required, + sdf::Errors &_errors, + const std::string &_description) { this->dataPtr->attributes.push_back( - this->CreateParam(_key, _type, _defaultValue, _required, _description)); + this->CreateParam(_key, _type, _defaultValue, + _required, _errors, _description)); } ///////////////////////////////////////////////// ElementPtr Element::Clone() const +{ + sdf::Errors errors; + ElementPtr elem = this->Clone(errors); + throwOrPrintErrors(errors); + return elem; +} + +///////////////////////////////////////////////// +ElementPtr Element::Clone(sdf::Errors &_errors) const { ElementPtr clone(new Element); clone->dataPtr->description = this->dataPtr->description; @@ -201,9 +263,13 @@ ElementPtr Element::Clone() const aiter != this->dataPtr->attributes.end(); ++aiter) { auto clonedAttribute = (*aiter)->Clone(); - SDF_ASSERT(clonedAttribute->SetParentElement(clone), - "Cannot set parent Element of cloned attribute Param to cloned " - "Element."); + if (!clonedAttribute->SetParentElement(clone, _errors)) + { + _errors.push_back({ErrorCode::FATAL_ERROR, + "Cannot set parent Element of cloned attribute Param to cloned " + "Element."}); + return nullptr; + } clone->dataPtr->attributes.push_back(clonedAttribute); } @@ -224,13 +290,20 @@ ElementPtr Element::Clone() const if (this->dataPtr->value) { clone->dataPtr->value = this->dataPtr->value->Clone(); - SDF_ASSERT(clone->dataPtr->value->SetParentElement(clone), - "Cannot set parent Element of cloned value Param to cloned Element."); + + if (!clone->dataPtr->value->SetParentElement(clone, _errors)) + { + _errors.push_back({ErrorCode::FATAL_ERROR, + "Cannot set parent Element of cloned value Param to cloned " + "Element."}); + return nullptr; + } } if (this->dataPtr->includeElement) { - clone->dataPtr->includeElement = this->dataPtr->includeElement->Clone(); + clone->dataPtr->includeElement = + this->dataPtr->includeElement->Clone(_errors); } return clone; @@ -239,6 +312,15 @@ ElementPtr Element::Clone() const ///////////////////////////////////////////////// void Element::Copy(const ElementPtr _elem) { + sdf::Errors errors; + this->Copy(_elem, errors); + throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void Element::Copy(const ElementPtr _elem, sdf::Errors &_errors) +{ + this->dataPtr->name = _elem->GetName(); this->dataPtr->description = _elem->GetDescription(); this->dataPtr->required = _elem->GetRequired(); @@ -259,8 +341,13 @@ void Element::Copy(const ElementPtr _elem) } ParamPtr param = this->GetAttribute((*iter)->GetKey()); (*param) = (**iter); - SDF_ASSERT(param->SetParentElement(shared_from_this()), - "Cannot set parent Element of copied attribute Param to itself."); + + if (!param->SetParentElement(shared_from_this(), _errors)) + { + _errors.push_back({ErrorCode::FATAL_ERROR, + "Cannot set parent Element of copied attribute Param to itself."}); + return; + } } if (_elem->GetValue()) @@ -273,8 +360,12 @@ void Element::Copy(const ElementPtr _elem) { *(this->dataPtr->value) = *(_elem->GetValue()); } - SDF_ASSERT(this->dataPtr->value->SetParentElement(shared_from_this()), - "Cannot set parent Element of copied value Param to itself."); + if (!this->dataPtr->value->SetParentElement(shared_from_this(), _errors)) + { + _errors.push_back({ErrorCode::FATAL_ERROR, + "Cannot set parent Element of copied attribute Param to itself."}); + return; + } } this->dataPtr->elementDescriptions.clear(); @@ -282,15 +373,16 @@ void Element::Copy(const ElementPtr _elem) _elem->dataPtr->elementDescriptions.begin(); iter != _elem->dataPtr->elementDescriptions.end(); ++iter) { - this->dataPtr->elementDescriptions.push_back((*iter)->Clone()); + this->dataPtr->elementDescriptions.push_back((*iter)->Clone(_errors)); } this->dataPtr->elements.clear(); for (ElementPtr_V::iterator iter = _elem->dataPtr->elements.begin(); iter != _elem->dataPtr->elements.end(); ++iter) { - ElementPtr elem = (*iter)->Clone(); - elem->Copy(*iter); + ElementPtr elem; + elem = (*iter)->Clone(_errors); + elem->Copy(*iter, _errors); elem->SetParent(shared_from_this()); this->dataPtr->elements.push_back(elem); } @@ -299,11 +391,13 @@ void Element::Copy(const ElementPtr _elem) { if (!this->dataPtr->includeElement) { - this->dataPtr->includeElement = _elem->dataPtr->includeElement->Clone(); + this->dataPtr->includeElement = + _elem->dataPtr->includeElement->Clone(_errors); } else { - this->dataPtr->includeElement->Copy(_elem->dataPtr->includeElement); + this->dataPtr->includeElement->Copy(_elem->dataPtr->includeElement, + _errors); } } } @@ -931,11 +1025,20 @@ std::map Element::CountNamedElements( ///////////////////////////////////////////////// ElementPtr Element::GetElement(const std::string &_name) +{ + sdf::Errors errors; + ElementPtr result = this->GetElement(_name, errors); + throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +ElementPtr Element::GetElement(const std::string &_name, sdf::Errors &_errors) { ElementPtr result = this->GetElementImpl(_name); if (result == ElementPtr()) { - result = this->AddElement(_name); + result = this->AddElement(_name, _errors); } return result; @@ -975,6 +1078,15 @@ bool Element::HasElementDescription(const std::string &_name) const ///////////////////////////////////////////////// ElementPtr Element::AddElement(const std::string &_name) +{ + sdf::Errors errors; + ElementPtr elem = this->AddElement(_name, errors); + throwOrPrintErrors(errors); + return elem; +} + +///////////////////////////////////////////////// +ElementPtr Element::AddElement(const std::string &_name, sdf::Errors &_errors) { // if this element is a reference sdf and does not have any element // descriptions then get them from its parent @@ -986,7 +1098,7 @@ ElementPtr Element::AddElement(const std::string &_name) for (unsigned int i = 0; i < parent->GetElementDescriptionCount(); ++i) { this->dataPtr->elementDescriptions.push_back( - parent->GetElementDescription(i)->Clone()); + parent->GetElementDescription(i)->Clone(_errors)); } } @@ -996,7 +1108,7 @@ ElementPtr Element::AddElement(const std::string &_name) { if ((*iter)->dataPtr->name == _name) { - ElementPtr elem = (*iter)->Clone(); + ElementPtr elem = (*iter)->Clone(_errors); elem->SetParent(shared_from_this()); this->dataPtr->elements.push_back(elem); @@ -1007,15 +1119,15 @@ ElementPtr Element::AddElement(const std::string &_name) // Add only required child element if ((*iter2)->GetRequired() == "1") { - elem->AddElement((*iter2)->dataPtr->name); + elem->AddElement((*iter2)->dataPtr->name, _errors); } } - return this->dataPtr->elements.back(); } } - sdferr << "Missing element description for [" << _name << "]\n"; + _errors.push_back({ErrorCode::ELEMENT_ERROR, + "Missing element description for [" + _name + "]\n"}); return ElementPtr(); } @@ -1191,7 +1303,20 @@ void Element::RemoveFromParent() ///////////////////////////////////////////////// void Element::RemoveChild(ElementPtr _child) { - SDF_ASSERT(_child, "Cannot remove a nullptr child pointer"); + sdf::Errors errors; + RemoveChild(_child, errors); + throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void Element::RemoveChild(ElementPtr _child, sdf::Errors &_errors) +{ + if (!_child) + { + _errors.push_back({ErrorCode::FATAL_ERROR, + "Cannot remove a nullptr child pointer"}); + return; + } ElementPtr_V::iterator iter; iter = std::find(this->dataPtr->elements.begin(), @@ -1206,14 +1331,23 @@ void Element::RemoveChild(ElementPtr _child) ///////////////////////////////////////////////// std::any Element::GetAny(const std::string &_key) const +{ + sdf::Errors errors; + std::any result = this->GetAny(errors, _key); + throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +std::any Element::GetAny(sdf::Errors &_errors, const std::string &_key) const { std::any result; if (_key.empty() && this->dataPtr->value) { if (!this->dataPtr->value->GetAny(result)) { - sdferr << "Couldn't get element [" << this->GetName() - << "] as std::any\n"; + _errors.push_back({ErrorCode::ELEMENT_ERROR, + "Couldn't get element [" + this->GetName() + "] as std::any\n"}); } } else if (!_key.empty()) @@ -1221,9 +1355,10 @@ std::any Element::GetAny(const std::string &_key) const ParamPtr param = this->GetAttribute(_key); if (param) { - if (!this->GetAttribute(_key)->GetAny(result)) + if (!this->GetAttribute(_key)->GetAny(result, _errors)) { - sdferr << "Couldn't get attribute [" << _key << "] as std::any\n"; + _errors.push_back({ErrorCode::ELEMENT_ERROR, + "Couldn't get attribute [" + _key + "] as std::any\n"}); } } else @@ -1231,18 +1366,19 @@ std::any Element::GetAny(const std::string &_key) const ElementPtr tmp = this->GetElementImpl(_key); if (tmp != ElementPtr()) { - result = tmp->GetAny(); + result = tmp->GetAny(_errors); } else { tmp = this->GetElementDescription(_key); if (tmp != ElementPtr()) { - result = tmp->GetAny(); + result = tmp->GetAny(_errors); } else { - sdferr << "Unable to find value for key [" << _key << "]\n"; + _errors.push_back({ErrorCode::ELEMENT_ERROR, + "Unable to find value for key [" + _key + "]\n"}); } } } diff --git a/src/Param.cc b/src/Param.cc index cee4f1a2f..8d0ce5a30 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -987,7 +987,7 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, { _errors.push_back({ErrorCode::PARAMETER_ERROR, "Invalid argument. Unable to set value [" - + _valueStr + " ] for key[" + + _valueStr + "] for key[" + this->key + "]."}); return false; } diff --git a/src/Utils.cc b/src/Utils.cc index dbce93006..1908baeb7 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -17,6 +17,7 @@ #include #include #include +#include "sdf/Assert.hh" #include "sdf/SDFImpl.hh" #include "Utils.hh" @@ -165,6 +166,22 @@ void enforceConfigurablePolicyCondition( } } +///////////////////////////////////////////////// +void throwOrPrintErrors(const sdf::Errors& _errors) +{ + for(auto& error : _errors) + { + if (error.Code() == sdf::ErrorCode::FATAL_ERROR) + { + SDF_ASSERT(false, error.Message()); + } + else + { + sdferr << error.Message(); + } + } +} + ///////////////////////////////////////////////// /// \brief Compute the absolute name of an entity by walking up the element /// tree. diff --git a/src/Utils.hh b/src/Utils.hh index ab872f424..c2689706b 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -85,6 +85,11 @@ namespace sdf const sdf::Error &_error, sdf::Errors &_errors); + /// \brief It will print the errors to sdferr or throw them using + /// SDF_ASSERT depending on their ErrorCode. + /// \param[in] _errors The vector of errors. + void throwOrPrintErrors(const sdf::Errors& _errors); + /// \brief Load all objects of a specific sdf element type. No error /// is returned if an element is not present. This function assumes that /// an element has a "name" attribute that must be unique. diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 1a80c22c6..8d1e4f585 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -23,6 +23,7 @@ #include +#include "sdf/Error.hh" #include "sdf/Filesystem.hh" #include "sdf/parser.hh" #include "sdf/SDFImpl.hh" @@ -1862,7 +1863,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) std::string pathBase = PROJECT_SOURCE_PATH; pathBase += "/test/sdf"; - auto expectedOutput = + std::string expectedOutput = "Inertial statistics for model: test_model\n" "---\n" "Total mass of the model: 24\n" @@ -1900,7 +1901,10 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } expectedOutput = - "Error Code 19: Msg: A link named link has invalid inertia.\n\n" + "Error Code " + + std::to_string(static_cast( + sdf::ErrorCode::LINK_INERTIA_INVALID)) + + ": Msg: A link named link has invalid inertia.\n\n" "Inertial statistics for model: model\n" "---\n" "Total mass of the model: 0\n" diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index 48e822aeb..3e06c945a 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -319,7 +319,7 @@ TEST(Parser, SyntaxErrorInValues) sdf::readFile(path, sdf); EXPECT_PRED2(contains, buffer.str(), - "Unable to set value [bad ] for key[linear]"); + "Unable to set value [bad] for key[linear]"); EXPECT_PRED2(contains, buffer.str(), "bad_syntax_double.sdf:L7"); EXPECT_PRED2(contains, buffer.str(), "/sdf/world[@name=\"default\"]/model[@name=\"robot1\"]/" diff --git a/test/integration/error_output.cc b/test/integration/error_output.cc index a3fa97914..d47948a31 100644 --- a/test/integration/error_output.cc +++ b/test/integration/error_output.cc @@ -28,7 +28,7 @@ //////////////////////////////////////// // Test Param class for sdf::Errors outputs -TEST(Error, ErrorOutput) +TEST(ErrorOutput, ParamErrorOutput) { std::stringstream buffer; sdf::testing::RedirectConsoleStream redir( @@ -148,3 +148,76 @@ TEST(Error, ErrorOutput) // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } + +//////////////////////////////////////// +// Test Element class for sdf::Errors outputs +TEST(ErrorOutput, ElementErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + sdf::Errors errors; + sdf::ElementPtr elem = std::make_shared(); + elem->SetName("testElement"); + + elem->GetAny(errors, "test"); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::ELEMENT_ERROR); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Unable to find value for key [test]")); + + errors.clear(); + elem->GetElement("missingElement", errors); + ASSERT_EQ(errors.size(), 1u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Missing element description for [missingElement]")); + + errors.clear(); + elem->AddAttribute( + "invalidAttribute", "int", "invalidFormat", false, errors); + ASSERT_EQ(errors.size(), 2u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Invalid argument. Unable to set value [invalidFormat]" + " for key[invalidAttribute]")); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "Invalid parameter")); + + errors.clear(); + elem->AddValue("type", "value", true, "a", "b", errors); + ASSERT_EQ(errors.size(), 9u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Unknown parameter type[type]")); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "Invalid parameter")); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "Unknown parameter type[type]")); + EXPECT_NE(std::string::npos, errors[3].Message().find( + "Invalid [min] parameter in SDFormat description of [testElement]")); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "Unknown parameter type[type]")); + EXPECT_NE(std::string::npos, errors[5].Message().find( + "Invalid [max] parameter in SDFormat description of [testElement]")); + EXPECT_NE(std::string::npos, errors[6].Message().find( + "Unknown parameter type[type]")); + EXPECT_NE(std::string::npos, errors[7].Message().find( + "Failed to set value '0' to key [testElement] for new parent element" + " of name 'testElement', reverting to previous value '0'.")); + EXPECT_NE(std::string::npos, errors[8].Message().find( + "Cannot set parent Element of value to itself.")); + errors.clear(); + + elem->GetElement("nonExistentElement", errors); + ASSERT_EQ(errors.size(), 1u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Missing element description for [nonExistentElement]")); + errors.clear(); + + elem->RemoveChild(sdf::ElementPtr(), errors); + ASSERT_EQ(errors.size(), 1u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Cannot remove a nullptr child pointer")); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} From 8a8d4464eeb3cb0315d24c0eba061b5dfe2887b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Mon, 29 Aug 2022 18:32:53 +0800 Subject: [PATCH 29/32] PrintConfig: add sdf::Errors output to API methods (#1098) Signed-off-by: Marco A. Gutierrez Co-authored-by: Steve Peters --- include/sdf/PrintConfig.hh | 14 +++++ src/PrintConfig.cc | 26 ++++++-- test/integration/error_output.cc | 102 +++++++++++++++++++------------ 3 files changed, 99 insertions(+), 43 deletions(-) diff --git a/include/sdf/PrintConfig.hh b/include/sdf/PrintConfig.hh index f3957f56d..826dc128e 100644 --- a/include/sdf/PrintConfig.hh +++ b/include/sdf/PrintConfig.hh @@ -22,6 +22,7 @@ #include "sdf/sdf_config.h" #include "sdf/system_util.hh" +#include "sdf/Types.hh" namespace sdf { @@ -54,6 +55,19 @@ namespace sdf public: bool SetRotationSnapToDegrees(unsigned int _interval, double _tolerance); + /// \brief Sets the option for printing pose rotation in degrees as well as + /// snapping the rotation to the desired interval, with the provided + /// tolerance. + /// \param[in] _interval Degrees interval to snap to, this value must be + /// larger than 0, and less than or equal to 360. + /// \param[in] _tolerance Tolerance which snapping occurs, this value must + /// be larger than 0, less than 360, and less than the provided interval. + /// \param[out] _errors Vector of Errors. + /// \return True, unless any of the provided values are not valid. + public: bool SetRotationSnapToDegrees(unsigned int _interval, + double _tolerance, + sdf::Errors &_errors); + /// \brief Returns the current degree value that pose rotations will snap to /// when printed. /// \return The assigned degrees interval value to snap to. If it has not diff --git a/src/PrintConfig.cc b/src/PrintConfig.cc index 01f74a195..1a7fbf59e 100644 --- a/src/PrintConfig.cc +++ b/src/PrintConfig.cc @@ -18,6 +18,7 @@ #include "sdf/PrintConfig.hh" #include "sdf/Console.hh" +#include "Utils.hh" using namespace sdf; @@ -75,19 +76,36 @@ bool PrintConfig::PreserveIncludes() const ///////////////////////////////////////////////// bool PrintConfig::SetRotationSnapToDegrees(unsigned int _interval, double _tolerance) +{ + sdf::Errors errors; + bool result = this->SetRotationSnapToDegrees(_interval, + _tolerance, + errors); + throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +bool PrintConfig::SetRotationSnapToDegrees(unsigned int _interval, + double _tolerance, + sdf::Errors &_errors) { if (_interval == 0 || _interval > 360) { - sdferr << "Interval value to snap to must be larger than 0, and less than " - << "or equal to 360.\n"; + std::stringstream ss; + ss << "Interval value to snap to must be larger than 0, and less than " + << "or equal to 360."; + _errors.push_back({ErrorCode::ROTATION_SNAP_CONFIG_ERROR, ss.str()}); return false; } if (_tolerance <= 0 || _tolerance > 360 || _tolerance >= static_cast(_interval)) { - sdferr << "Tolerance must be larger than 0, less than or equal to " - << "360, and less than the provided interval.\n"; + std::stringstream ss; + ss << "Tolerance must be larger than 0, less than or equal to " + << "360, and less than the provided interval."; + _errors.push_back({ErrorCode::ROTATION_SNAP_CONFIG_ERROR, ss.str()}); return false; } diff --git a/test/integration/error_output.cc b/test/integration/error_output.cc index d47948a31..6df407556 100644 --- a/test/integration/error_output.cc +++ b/test/integration/error_output.cc @@ -32,16 +32,16 @@ TEST(ErrorOutput, ParamErrorOutput) { std::stringstream buffer; sdf::testing::RedirectConsoleStream redir( - sdf::Console::Instance()->GetMsgStream(), &buffer); + sdf::Console::Instance()->GetMsgStream(), &buffer); sdf::Errors errors; ASSERT_NO_THROW(sdf::Param param1("key", "not_valid_type", "true", false, - errors, "description")); + errors, "description")); ASSERT_GE(2u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::UNKNOWN_PARAMETER_TYPE); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Unknown parameter type[not_valid_type]")); + "Unknown parameter type[not_valid_type]")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[1].Message().find("Invalid parameter")); @@ -52,10 +52,10 @@ TEST(ErrorOutput, ParamErrorOutput) ASSERT_GE(2u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::UNKNOWN_PARAMETER_TYPE); EXPECT_NE(std::string::npos, - errors[0].Message().find("Unknown parameter type[not_valid_type]")); + errors[0].Message().find("Unknown parameter type[not_valid_type]")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, - errors[1].Message().find("Invalid parameter")); + errors[1].Message().find("Invalid parameter")); errors.clear(); sdf::Param param3("key", "bool", "true", false, errors, "description"); @@ -73,15 +73,15 @@ TEST(ErrorOutput, ParamErrorOutput) ASSERT_GE(errors.size(), 1u); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[0].Message().find( - "The value for //pose[@rotation_format='euler_rpy'] must have 6 " - "values, but 1 were found instead in '1'.")); + "The value for //pose[@rotation_format='euler_rpy'] must have 6 " + "values, but 1 were found instead in '1'.")); errors.clear(); param3.Update(errors); ASSERT_GE(errors.size(), 1u); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[0].Message().find( - "[updateFunc] is not set.")); + "[updateFunc] is not set.")); errors.clear(); sdf::Param requiredParam("key", "int", "1", true, "2", "4", errors, @@ -92,14 +92,13 @@ TEST(ErrorOutput, ParamErrorOutput) ASSERT_GE(errors.size(), 1u); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Empty string used when setting a required parameter. Key[key]")); + "Empty string used when setting a required parameter. Key[key]")); EXPECT_FALSE(requiredParam.ValidateValue(errors)); ASSERT_GE(errors.size(), 2u); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[1].Message().find( - "The value [1] is less than the minimum allowed value of [2] for " - "key [key]")); - + "The value [1] is less than the minimum allowed value of [2] for " + "key [key]")); errors.clear(); // Adding a parent with @rotation_format to something invalid @@ -111,8 +110,8 @@ TEST(ErrorOutput, ParamErrorOutput) ASSERT_EQ(errors.size(), 2u); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Undefined attribute //pose[@rotation_format='invalid_format'], " - "only 'euler_rpy' and 'quat_xyzw' is supported.")); + "Undefined attribute //pose[@rotation_format='invalid_format'], " + "only 'euler_rpy' and 'quat_xyzw' is supported.")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::PARAMETER_ERROR); #if !defined __ARM_ARCH EXPECT_NE(std::string::npos, errors[1].Message().find( @@ -128,22 +127,22 @@ TEST(ErrorOutput, ParamErrorOutput) ASSERT_EQ(errors.size(), 6u); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Invalid boolean value")); + "Invalid boolean value")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[1].Message().find( - "Invalid parameter")); + "Invalid parameter")); EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[2].Message().find( - "Invalid boolean value")); + "Invalid boolean value")); EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[3].Message().find( - "Invalid [min] parameter in SDFormat description of [key]")); + "Invalid [min] parameter in SDFormat description of [key]")); EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[4].Message().find( - "Invalid boolean value")); + "Invalid boolean value")); EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::PARAMETER_ERROR); EXPECT_NE(std::string::npos, errors[5].Message().find( - "Invalid [max] parameter in SDFormat description of [key]")); + "Invalid [max] parameter in SDFormat description of [key]")); // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); @@ -155,7 +154,7 @@ TEST(ErrorOutput, ElementErrorOutput) { std::stringstream buffer; sdf::testing::RedirectConsoleStream redir( - sdf::Console::Instance()->GetMsgStream(), &buffer); + sdf::Console::Instance()->GetMsgStream(), &buffer); sdf::Errors errors; sdf::ElementPtr elem = std::make_shared(); @@ -165,59 +164,84 @@ TEST(ErrorOutput, ElementErrorOutput) ASSERT_EQ(errors.size(), 1u); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::ELEMENT_ERROR); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Unable to find value for key [test]")); + "Unable to find value for key [test]")); errors.clear(); elem->GetElement("missingElement", errors); ASSERT_EQ(errors.size(), 1u); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Missing element description for [missingElement]")); + "Missing element description for [missingElement]")); errors.clear(); elem->AddAttribute( - "invalidAttribute", "int", "invalidFormat", false, errors); + "invalidAttribute", "int", "invalidFormat", false, errors); ASSERT_EQ(errors.size(), 2u); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Invalid argument. Unable to set value [invalidFormat]" - " for key[invalidAttribute]")); + "Invalid argument. Unable to set value [invalidFormat]" + " for key[invalidAttribute]")); EXPECT_NE(std::string::npos, errors[1].Message().find( - "Invalid parameter")); + "Invalid parameter")); errors.clear(); elem->AddValue("type", "value", true, "a", "b", errors); ASSERT_EQ(errors.size(), 9u); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Unknown parameter type[type]")); + "Unknown parameter type[type]")); EXPECT_NE(std::string::npos, errors[1].Message().find( - "Invalid parameter")); + "Invalid parameter")); EXPECT_NE(std::string::npos, errors[2].Message().find( - "Unknown parameter type[type]")); + "Unknown parameter type[type]")); EXPECT_NE(std::string::npos, errors[3].Message().find( - "Invalid [min] parameter in SDFormat description of [testElement]")); + "Invalid [min] parameter in SDFormat description of [testElement]")); EXPECT_NE(std::string::npos, errors[4].Message().find( - "Unknown parameter type[type]")); + "Unknown parameter type[type]")); EXPECT_NE(std::string::npos, errors[5].Message().find( - "Invalid [max] parameter in SDFormat description of [testElement]")); + "Invalid [max] parameter in SDFormat description of [testElement]")); EXPECT_NE(std::string::npos, errors[6].Message().find( - "Unknown parameter type[type]")); + "Unknown parameter type[type]")); EXPECT_NE(std::string::npos, errors[7].Message().find( - "Failed to set value '0' to key [testElement] for new parent element" - " of name 'testElement', reverting to previous value '0'.")); + "Failed to set value '0' to key [testElement] for new parent element" + " of name 'testElement', reverting to previous value '0'.")); EXPECT_NE(std::string::npos, errors[8].Message().find( - "Cannot set parent Element of value to itself.")); + "Cannot set parent Element of value to itself.")); errors.clear(); elem->GetElement("nonExistentElement", errors); ASSERT_EQ(errors.size(), 1u); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Missing element description for [nonExistentElement]")); + "Missing element description for [nonExistentElement]")); errors.clear(); elem->RemoveChild(sdf::ElementPtr(), errors); ASSERT_EQ(errors.size(), 1u); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Cannot remove a nullptr child pointer")); + "Cannot remove a nullptr child pointer")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} +//////////////////////////////////////// +// Test PrintConfig class for sdf::Errors outputs +TEST(ErrorOutput, PrintConfigErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + sdf::Errors errors; + sdf::PrintConfig printConfig; + ASSERT_FALSE(printConfig.SetRotationSnapToDegrees(361, 300, errors)); + ASSERT_EQ(errors.size(), 1u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Interval value to snap to must be larger than 0," + " and less than or equal to 360.")); + errors.clear(); + + ASSERT_FALSE(printConfig.SetRotationSnapToDegrees(300, 300, errors)); + ASSERT_EQ(errors.size(), 1u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Tolerance must be larger than 0, less than or equal to 360, " + "and less than the provided interval.")); // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } From a5be6ab100dc8a8b97c661cc12f31d311e637068 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Fri, 9 Sep 2022 01:53:16 +0800 Subject: [PATCH 30/32] World: sdfwarns to sdf::Errors when warnings policy set to sdf::EnforcementPolicy::ERR (#1131) Signed-off-by: Marco A. Gutierrez --- include/sdf/Error.hh | 3 ++ src/World.cc | 33 ++++++++++----------- test/integration/error_output.cc | 51 ++++++++++++++++++++++++++++++++ 3 files changed, 69 insertions(+), 18 deletions(-) diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index 14fc60675..b3c58f2a1 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -165,6 +165,9 @@ namespace sdf /// \brief Generic error to be thrown with SDF_ASSERT by the caller. /// This has been created to help preserve behavior. FATAL_ERROR, + + /// \brief Generic warning saved as error due to WarningsPolicy config + WARNING, }; class SDFORMAT_VISIBLE Error diff --git a/src/World.cc b/src/World.cc index c5d6c821b..2ed7c15e6 100644 --- a/src/World.cc +++ b/src/World.cc @@ -206,20 +206,13 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { if (size > 1) { - sdfwarn << "Non-unique name[" << name << "] detected " << size - << " times in XML children of world with name[" << this->Name() - << "].\n"; - } - } - - for (const auto &[name, size] : - _sdf->CountNamedElements("", Element::NameUniquenessExceptions())) - { - if (size > 1) - { - sdfwarn << "Non-unique name[" << name << "] detected " << size - << " times in XML children of world with name[" << this->Name() - << "].\n"; + std::stringstream ss; + ss << "Non-unique name[" << name << "] detected " << size + << " times in XML children of world with name[" << this->Name() + << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); } } @@ -288,10 +281,14 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { frameName = frame.Name() + "_frame" + std::to_string(i++); } - sdfwarn << "Frame with name [" << frame.Name() << "] " - << "in world with name [" << this->Name() << "] " - << "has a name collision, changing frame name to [" - << frameName << "].\n"; + std::stringstream ss; + ss << "Frame with name [" << frame.Name() << "] " + << "in world with name [" << this->Name() << "] " + << "has a name collision, changing frame name to [" + << frameName << "].\n"; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); frame.SetName(frameName); } frameNames.insert(frameName); diff --git a/test/integration/error_output.cc b/test/integration/error_output.cc index 6df407556..b205d57c2 100644 --- a/test/integration/error_output.cc +++ b/test/integration/error_output.cc @@ -24,7 +24,9 @@ #include "sdf/Error.hh" #include "sdf/Param.hh" #include "sdf/Types.hh" +#include "sdf/World.hh" #include "test_utils.hh" +#include "test_config.hh" //////////////////////////////////////// // Test Param class for sdf::Errors outputs @@ -245,3 +247,52 @@ TEST(ErrorOutput, PrintConfigErrorOutput) // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } + +//////////////////////////////////////// +// Test World class for sdf::Errors outputs +TEST(ErrorOutput, WorldErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + sdf::Errors errors; + + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::readString(stream.str(), parserConfig, sdfParsed, errors); + EXPECT_TRUE(errors.empty()); + + sdf::World world; + errors = world.Load(sdfParsed->Root()->GetElement("world"), parserConfig); + ASSERT_EQ(errors.size(), 3u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Non-unique name[common_name] detected 3 times in XML children of world" + " with name[test_world].")); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "model with name[common_name] already exists.")); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "Frame with name [common_name] in world with name [test_world] has a name" + " collision, changing frame name to [common_name_frame].")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} From 707cb303830ac30b32d979d40b6df5753c03b2dd Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Oct 2022 14:18:15 -0700 Subject: [PATCH 31/32] Add a configuration option to resolve URIs (#1147) This adds an option for the SDF parser to attempt to resolve any URIs in the document via a user-provided callback. This works similarly to how include/uri is resolved, but works for all uri elements. A ParserConfig parameter is added to opt in to this behavior. In the case that the user-provided callback cannot resolve the URI (returns an empty string), the original contents of the uri tag will be preserved. Signed-off-by: Michael Carroll --- include/sdf/Collision.hh | 10 + include/sdf/Geometry.hh | 10 + include/sdf/Heightmap.hh | 23 ++ include/sdf/Link.hh | 10 + include/sdf/Material.hh | 10 + include/sdf/Mesh.hh | 12 + include/sdf/ParserConfig.hh | 18 ++ include/sdf/Scene.hh | 9 + include/sdf/Sky.hh | 13 + include/sdf/Visual.hh | 10 + src/Collision.cc | 9 +- src/Geometry.cc | 11 +- src/Heightmap.cc | 28 ++- src/Link.cc | 10 +- src/Material.cc | 9 +- src/Mesh.cc | 11 +- src/Model.cc | 2 +- src/ParserConfig.cc | 20 ++ src/ParserConfig_TEST.cc | 7 + src/Scene.cc | 8 +- src/Sky.cc | 15 +- src/Utils.cc | 19 ++ src/Utils.hh | 19 +- src/Visual.cc | 12 +- src/World.cc | 2 +- test/integration/CMakeLists.txt | 1 + test/integration/resolve_uris.cc | 420 +++++++++++++++++++++++++++++++ test/sdf/resolve_uris.sdf | 68 +++++ 28 files changed, 769 insertions(+), 27 deletions(-) create mode 100644 test/integration/resolve_uris.cc create mode 100644 test/sdf/resolve_uris.sdf diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 76ee5ce23..26b5dd843 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -34,6 +34,7 @@ namespace sdf // // Forward declaration. class Geometry; + class ParserConfig; class Surface; struct PoseRelativeToGraph; template class ScopedGraph; @@ -55,6 +56,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \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. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \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(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the name of the collision. /// The name of the collision must be unique within the scope of a Link. /// \return Name of the collision. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 55d893e14..5ff016f97 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -37,6 +37,7 @@ namespace sdf class Ellipsoid; class Heightmap; class Mesh; + class ParserConfig; class Plane; class Polyline; class Sphere; @@ -93,6 +94,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \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. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \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(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the type of geometry. /// \return The geometry type. public: GeometryType Type() const; diff --git a/include/sdf/Heightmap.hh b/include/sdf/Heightmap.hh index 2071a057d..abba9e443 100644 --- a/include/sdf/Heightmap.hh +++ b/include/sdf/Heightmap.hh @@ -28,6 +28,11 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { + // + + // Forward declarations. + class ParserConfig; + /// \brief Texture to be used on heightmaps. class SDFORMAT_VISIBLE HeightmapTexture { @@ -42,6 +47,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the heightmap texture geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \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(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the heightmap texture's size. /// \return The size of the heightmap texture in meters. public: double Size() const; @@ -129,6 +143,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the heightmap geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \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, const ParserConfig &_config); + /// \brief Get the heightmap's URI. /// \return The URI of the heightmap data. public: std::string Uri() const; diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index f92824d7c..a5b3c4de7 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -37,6 +37,7 @@ namespace sdf // Forward declarations. class Collision; class Light; + class ParserConfig; class ParticleEmitter; class Sensor; class Visual; @@ -56,6 +57,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \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. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \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, const ParserConfig &_config); + /// \brief Get the name of the link. /// The name of a link must be unique within the scope of a Model. /// \return Name of the link. diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index 5d076f0cc..ae3e6339e 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -31,6 +31,7 @@ namespace sdf // // Forward declarations. + class ParserConfig; class Pbr; enum class ShaderType : int @@ -55,6 +56,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \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. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \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(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the ambient color. The ambient color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 1752e370d..c04e0f748 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -30,6 +30,9 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // + // Forward declarations. + class ParserConfig; + /// \brief Mesh represents a mesh shape, and is usually accessed through a /// Geometry. class SDFORMAT_VISIBLE Mesh @@ -45,6 +48,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the mesh 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 + /// \param[in] _config Parser configuration + /// \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(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the mesh's URI. /// \return The URI of the mesh data. public: std::string Uri() const; diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 69c5cdf42..af17a8e76 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -169,11 +169,29 @@ class SDFORMAT_VISIBLE ParserConfig public: const std::vector &CustomModelParsers() const; /// \brief Set the preserveFixedJoint flag. + /// \param[in] _preserveFixedJoint flag value to set public: void URDFSetPreserveFixedJoint(bool _preserveFixedJoint); /// \brief Get the preserveFixedJoint flag value. + /// \return Current flag value public: bool URDFPreserveFixedJoint() const; + /// \brief Set the storeResolvedURIs flag value. + /// \param[in] _resolveURI True to make the parser attempt to resolve any + /// URIs found and store them. False to preserve original URIs + /// + /// The Parser will use the FindFileCallback provided to attempt to resolve + /// URIs in the Mesh, Material, Heightmap, and Skybox DOM objects + /// If the FindFileCallback provides a non-empty string, the URI will be + /// stored in the DOM object, and the original (unresolved) URI will be + /// stored in the underlying Element. + public: void SetStoreResovledURIs(bool _resolveURI); + + /// \brief Get the storeResolvedURIs flag value. + /// \return True if the parser will attempt to resolve any URIs found and + /// store them. False to preserve original URIs + public: bool StoreResolvedURIs() const; + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index 922073063..f192adff1 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -43,6 +43,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \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. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \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(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the ambient color of the scene /// \return Scene ambient color public: gz::math::Color Ambient() const; diff --git a/include/sdf/Sky.hh b/include/sdf/Sky.hh index bf26a3498..f677a3c73 100644 --- a/include/sdf/Sky.hh +++ b/include/sdf/Sky.hh @@ -32,6 +32,11 @@ namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { + // + + // Forward declarations. + class ParserConfig; + class SDFORMAT_VISIBLE Sky { /// \brief Default constructor @@ -117,6 +122,14 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Load the sky 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, const ParserConfig &_config); + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index bc1b8898c..ff9da957f 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -41,6 +41,7 @@ namespace sdf // Forward declarations. class Geometry; + class ParserConfig; struct PoseRelativeToGraph; template class ScopedGraph; @@ -57,6 +58,15 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \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. + /// \param[in] _sdf The SDF Element pointer + /// \param[in] _config Parser configuration + /// \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, const ParserConfig &_config); + /// \brief Get the name of the visual. /// The name of the visual must be unique within the scope of a Link. /// \return Name of the visual. diff --git a/src/Collision.cc b/src/Collision.cc index b0b33aa24..d45b47877 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -64,6 +64,12 @@ Collision::Collision() ///////////////////////////////////////////////// Errors Collision::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -98,7 +104,8 @@ Errors Collision::Load(ElementPtr _sdf) loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo); // Load the geometry - Errors geomErr = this->dataPtr->geom.Load(_sdf->GetElement("geometry")); + Errors geomErr = this->dataPtr->geom.Load( + _sdf->GetElement("geometry"), _config); errors.insert(errors.end(), geomErr.begin(), geomErr.end()); // Load the surface parameters if they are given diff --git a/src/Geometry.cc b/src/Geometry.cc index ceae9cd72..e31a5e69b 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -78,6 +78,12 @@ Geometry::Geometry() ///////////////////////////////////////////////// Errors Geometry::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Geometry::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -148,14 +154,15 @@ Errors Geometry::Load(ElementPtr _sdf) { this->dataPtr->type = GeometryType::MESH; this->dataPtr->mesh.emplace(); - Errors err = this->dataPtr->mesh->Load(_sdf->GetElement("mesh")); + Errors err = this->dataPtr->mesh->Load(_sdf->GetElement("mesh"), _config); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("heightmap")) { this->dataPtr->type = GeometryType::HEIGHTMAP; this->dataPtr->heightmap.emplace(); - Errors err = this->dataPtr->heightmap->Load(_sdf->GetElement("heightmap")); + Errors err = this->dataPtr->heightmap->Load( + _sdf->GetElement("heightmap"), _config); errors.insert(errors.end(), err.begin(), err.end()); } else if (_sdf->HasElement("polyline")) diff --git a/src/Heightmap.cc b/src/Heightmap.cc index 2a57a67cf..b9a5619e5 100644 --- a/src/Heightmap.cc +++ b/src/Heightmap.cc @@ -91,6 +91,12 @@ HeightmapTexture::HeightmapTexture() ///////////////////////////////////////////////// Errors HeightmapTexture::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors HeightmapTexture::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -126,8 +132,9 @@ Errors HeightmapTexture::Load(ElementPtr _sdf) if (_sdf->HasElement("diffuse")) { - this->dataPtr->diffuse = _sdf->Get("diffuse", - this->dataPtr->diffuse).first; + this->dataPtr->diffuse = resolveURI( + _sdf->Get("diffuse", this->dataPtr->diffuse).first, + _config, errors); } else { @@ -137,8 +144,9 @@ Errors HeightmapTexture::Load(ElementPtr _sdf) if (_sdf->HasElement("normal")) { - this->dataPtr->normal = _sdf->Get("normal", - this->dataPtr->normal).first; + this->dataPtr->normal = resolveURI( + _sdf->Get("normal", this->dataPtr->normal).first, + _config, errors); } else { @@ -285,6 +293,12 @@ Heightmap::Heightmap() ///////////////////////////////////////////////// Errors Heightmap::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Heightmap::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -311,7 +325,9 @@ Errors Heightmap::Load(ElementPtr _sdf) if (_sdf->HasElement("uri")) { - this->dataPtr->uri = _sdf->Get("uri", "").first; + this->dataPtr->uri = resolveURI( + _sdf->Get("uri", "").first, + _config, errors); } else { @@ -332,7 +348,7 @@ Errors Heightmap::Load(ElementPtr _sdf) this->dataPtr->sampling).first; Errors textureLoadErrors = loadRepeated(_sdf, - "texture", this->dataPtr->textures); + "texture", this->dataPtr->textures, _config); errors.insert(errors.end(), textureLoadErrors.begin(), textureLoadErrors.end()); diff --git a/src/Link.cc b/src/Link.cc index 3ac2413e2..859848c4e 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -86,6 +86,12 @@ Link::Link() ///////////////////////////////////////////////// Errors Link::Load(ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { Errors errors; @@ -121,12 +127,12 @@ Errors Link::Load(ElementPtr _sdf) // Load all the visuals. Errors visLoadErrors = loadUniqueRepeated(_sdf, "visual", - this->dataPtr->visuals); + this->dataPtr->visuals, _config); errors.insert(errors.end(), visLoadErrors.begin(), visLoadErrors.end()); // Load all the collisions. Errors collLoadErrors = loadUniqueRepeated(_sdf, "collision", - this->dataPtr->collisions); + this->dataPtr->collisions, _config); errors.insert(errors.end(), collLoadErrors.begin(), collLoadErrors.end()); // Load all the lights. diff --git a/src/Material.cc b/src/Material.cc index 9f1cbfcc0..c5c41c394 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -83,6 +83,12 @@ Material::Material() ///////////////////////////////////////////////// Errors Material::Load(sdf::ElementPtr _sdf) +{ + return this->Load(_sdf, ParserConfig::GlobalConfig()); +} + +///////////////////////////////////////////////// +Errors Material::Load(sdf::ElementPtr _sdf, const sdf::ParserConfig &_config) { Errors errors; @@ -114,7 +120,8 @@ Errors Material::Load(sdf::ElementPtr _sdf) "A + + + + + + + shapes.sdf + + + + From 3fbdaf758bbc5c5bac7d281c27a618fb0ddb4599 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 11 Oct 2022 18:16:58 -0500 Subject: [PATCH 32/32] Add test helper python package for encapsulating versioned python packages (#1180) Signed-off-by: Addisu Z. Taddese --- python/test/gz_test_deps/README | 8 ++++++++ python/test/gz_test_deps/__init__.py | 0 python/test/gz_test_deps/math.py | 1 + python/test/gz_test_deps/sdformat.py | 1 + python/test/pyAirPressure_TEST.py | 4 ++-- python/test/pyAltimeter_TEST.py | 4 ++-- python/test/pyAtmosphere_TEST.py | 6 +++--- python/test/pyBox_TEST.py | 4 ++-- python/test/pyCamera_TEST.py | 6 +++--- python/test/pyCapsule_TEST.py | 2 +- python/test/pyCollision_TEST.py | 9 +++++---- python/test/pyCylinder_TEST.py | 2 +- python/test/pyEllipsoid_TEST.py | 4 ++-- python/test/pyError_TEST.py | 4 ++-- python/test/pyForceTorque_TEST.py | 4 ++-- python/test/pyFrame_TEST.py | 5 ++--- python/test/pyGeometry_TEST.py | 7 ++++--- python/test/pyGui_TEST.py | 2 +- python/test/pyHeightmap_TEST.py | 4 ++-- python/test/pyIMU_TEST.py | 6 +++--- python/test/pyJointAxis_TEST.py | 4 ++-- python/test/pyJoint_TEST.py | 8 ++++---- python/test/pyLidar_TEST.py | 4 ++-- python/test/pyLight_TEST.py | 6 +++--- python/test/pyLink_TEST.py | 6 +++--- python/test/pyMagnetometer_TEST.py | 4 ++-- python/test/pyMaterial_TEST.py | 6 +++--- python/test/pyMesh_TEST.py | 4 ++-- python/test/pyModel_TEST.py | 8 ++++---- python/test/pyNavSat_TEST.py | 2 +- python/test/pyNoise_TEST.py | 4 ++-- python/test/pyParserConfig_TEST.py | 2 +- python/test/pyParticleEmitter_TEST.py | 4 ++-- python/test/pyPbr_TEST.py | 4 ++-- python/test/pyPhysics_TEST.py | 2 +- python/test/pyPlane_TEST.py | 4 ++-- python/test/pyPlugin_TEST.py | 2 +- python/test/pyPolyline_TEST.py | 4 ++-- python/test/pyRoot_TEST.py | 9 +++++---- python/test/pyScene_TEST.py | 4 ++-- python/test/pySemanticPose_TEST.py | 4 ++-- python/test/pySensor_TEST.py | 11 ++++++----- python/test/pySky_TEST.py | 4 ++-- python/test/pySphere_TEST.py | 2 +- python/test/pySurface_TEST.py | 4 ++-- python/test/pyVisual_TEST.py | 7 ++++--- python/test/pyWorld_TEST.py | 7 ++++--- 47 files changed, 114 insertions(+), 99 deletions(-) create mode 100644 python/test/gz_test_deps/README create mode 100644 python/test/gz_test_deps/__init__.py create mode 100644 python/test/gz_test_deps/math.py create mode 100644 python/test/gz_test_deps/sdformat.py diff --git a/python/test/gz_test_deps/README b/python/test/gz_test_deps/README new file mode 100644 index 000000000..29f5c57e1 --- /dev/null +++ b/python/test/gz_test_deps/README @@ -0,0 +1,8 @@ +This python package encapsulates versioned python packages of gz library +bindings such that the version number only has to be changed in this package. +Here's an example of how to use this in a python test: + +```python +from gz_test_deps import math # instead of from gz import math7 +from gz_test_deps.math import Vector3d # instead of from gz.math7 import Vector3d +``` diff --git a/python/test/gz_test_deps/__init__.py b/python/test/gz_test_deps/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/test/gz_test_deps/math.py b/python/test/gz_test_deps/math.py new file mode 100644 index 000000000..cb2860c79 --- /dev/null +++ b/python/test/gz_test_deps/math.py @@ -0,0 +1 @@ +from gz.math7 import * diff --git a/python/test/gz_test_deps/sdformat.py b/python/test/gz_test_deps/sdformat.py new file mode 100644 index 000000000..241e91374 --- /dev/null +++ b/python/test/gz_test_deps/sdformat.py @@ -0,0 +1 @@ +from sdformat13 import * diff --git a/python/test/pyAirPressure_TEST.py b/python/test/pyAirPressure_TEST.py index 3848f6b16..254646786 100644 --- a/python/test/pyAirPressure_TEST.py +++ b/python/test/pyAirPressure_TEST.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from sdformat13 import AirPressure, Noise -import sdformat13 as sdf +from gz_test_deps.sdformat import AirPressure, Noise +import gz_test_deps.sdformat as sdf import unittest class AtmosphereTEST(unittest.TestCase): diff --git a/python/test/pyAltimeter_TEST.py b/python/test/pyAltimeter_TEST.py index 27e862a74..8a5fbad85 100644 --- a/python/test/pyAltimeter_TEST.py +++ b/python/test/pyAltimeter_TEST.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from sdformat13 import Altimeter, Noise -import sdformat13 as sdf +from gz_test_deps.sdformat import Altimeter, Noise +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyAtmosphere_TEST.py b/python/test/pyAtmosphere_TEST.py index e0ac4d3d6..cffc26ee6 100644 --- a/python/test/pyAtmosphere_TEST.py +++ b/python/test/pyAtmosphere_TEST.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math7 import Temperature -from sdformat13 import Atmosphere -import sdformat13 as sdf +from gz_test_deps.math import Temperature +from gz_test_deps.sdformat import Atmosphere +import gz_test_deps.sdformat as sdf import unittest class AtmosphereTEST(unittest.TestCase): diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index 13e9a99ed..d98440de5 100644 --- a/python/test/pyBox_TEST.py +++ b/python/test/pyBox_TEST.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz.math7 import Vector3d -from sdformat13 import Box +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import Box import unittest class BoxTEST(unittest.TestCase): diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index 67471694b..40000b7f0 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -13,10 +13,10 @@ # limitations under the License. import copy -from gz.math7 import Angle, Pose3d, Vector2d +from gz_test_deps.math import Angle, Pose3d, Vector2d import math -from sdformat13 import Camera -import sdformat13 as sdf +from gz_test_deps.sdformat import Camera +import gz_test_deps.sdformat as sdf import unittest class CameraTEST(unittest.TestCase): diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index b91ba6eb6..52472ed91 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -16,7 +16,7 @@ import math -from sdformat13 import Capsule +from gz_test_deps.sdformat import Capsule import unittest diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index a0efbecdd..9e92f521b 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -13,10 +13,11 @@ # limitations under the License. import copy -from gz.math7 import Pose3d -from sdformat13 import (Box, Collision, Contact, Cylinder, Error, Geometry, - Plane, Surface, Sphere, SDFErrorsException) -import sdformat13 as sdf +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error, + Geometry, Plane, Surface, Sphere, + SDFErrorsException) +import gz_test_deps.sdformat as sdf import unittest import math diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index 50b121991..99bcf7823 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -16,7 +16,7 @@ import math -from sdformat13 import Cylinder +from gz_test_deps.sdformat import Cylinder import unittest diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index d3dd7291a..a102a42ad 100644 --- a/python/test/pyEllipsoid_TEST.py +++ b/python/test/pyEllipsoid_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz.math7 import Vector3d +from gz_test_deps.math import Vector3d import math -from sdformat13 import Ellipsoid +from gz_test_deps.sdformat import Ellipsoid import unittest diff --git a/python/test/pyError_TEST.py b/python/test/pyError_TEST.py index 84aa5e32d..f9b075ac1 100644 --- a/python/test/pyError_TEST.py +++ b/python/test/pyError_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat13 import Error -import sdformat13 as sdf +from gz_test_deps.sdformat import Error +import gz_test_deps.sdformat as sdf import unittest class ErrorColor(unittest.TestCase): diff --git a/python/test/pyForceTorque_TEST.py b/python/test/pyForceTorque_TEST.py index 8b3ae782b..3ca3a8d8f 100644 --- a/python/test/pyForceTorque_TEST.py +++ b/python/test/pyForceTorque_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat13 import ForceTorque, Noise -import sdformat13 as sdf +from gz_test_deps.sdformat import ForceTorque, Noise +import gz_test_deps.sdformat as sdf import unittest class ForceTorqueTEST(unittest.TestCase): diff --git a/python/test/pyFrame_TEST.py b/python/test/pyFrame_TEST.py index 89b9bbd99..fb93e4d3f 100644 --- a/python/test/pyFrame_TEST.py +++ b/python/test/pyFrame_TEST.py @@ -12,9 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import copy -from gz.math7 import Pose3d -from sdformat13 import Frame, Error, SDFErrorsException, ErrorCode +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import Frame, Error, SDFErrorsException, ErrorCode import unittest import math diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index 60811c558..a1f4ec65b 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,9 +13,10 @@ # limitations under the License. import copy -from sdformat13 import Geometry, Box, Capsule, Cylinder, Ellipsoid, Mesh, Plane, Sphere -from gz.math7 import Vector3d, Vector2d -import sdformat13 as sdf +from gz_test_deps.sdformat import (Geometry, Box, Capsule, Cylinder, Ellipsoid, + Mesh, Plane, Sphere) +from gz_test_deps.math import Vector3d, Vector2d +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyGui_TEST.py b/python/test/pyGui_TEST.py index 7454ba590..f7581b749 100644 --- a/python/test/pyGui_TEST.py +++ b/python/test/pyGui_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat13 import Gui, Plugin +from gz_test_deps.sdformat import Gui, Plugin import unittest diff --git a/python/test/pyHeightmap_TEST.py b/python/test/pyHeightmap_TEST.py index 6d2016e06..13848ad5f 100644 --- a/python/test/pyHeightmap_TEST.py +++ b/python/test/pyHeightmap_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math7 import Vector3d -from sdformat13 import Heightmap, HeightmapBlend, HeightmapTexture +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import Heightmap, HeightmapBlend, HeightmapTexture import unittest diff --git a/python/test/pyIMU_TEST.py b/python/test/pyIMU_TEST.py index fb7b5ab27..d1bf66ebd 100644 --- a/python/test/pyIMU_TEST.py +++ b/python/test/pyIMU_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz.math7 import Vector3d -from sdformat13 import IMU, Noise -import sdformat13 as sdf +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import IMU, Noise +import gz_test_deps.sdformat as sdf import unittest class IMUTest(unittest.TestCase): diff --git a/python/test/pyJointAxis_TEST.py b/python/test/pyJointAxis_TEST.py index 96f139f15..4a1dffafa 100644 --- a/python/test/pyJointAxis_TEST.py +++ b/python/test/pyJointAxis_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math7 import Vector3d -from sdformat13 import JointAxis, Error, SDFErrorsException +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import JointAxis, Error, SDFErrorsException import math import unittest diff --git a/python/test/pyJoint_TEST.py b/python/test/pyJoint_TEST.py index a526b8f99..f8577b93c 100644 --- a/python/test/pyJoint_TEST.py +++ b/python/test/pyJoint_TEST.py @@ -13,10 +13,10 @@ # limitations under the License. import copy -from gz.math7 import Pose3d, Vector3d -from sdformat13 import (Joint, JointAxis, Error, SemanticPose, Sensor, - SDFErrorsException) -import sdformat13 as sdf +from gz_test_deps.math import Pose3d, Vector3d +from gz_test_deps.sdformat import (Joint, JointAxis, Error, SemanticPose, + Sensor, SDFErrorsException) +import gz_test_deps.sdformat as sdf import math import unittest diff --git a/python/test/pyLidar_TEST.py b/python/test/pyLidar_TEST.py index 585c48259..2f781948e 100644 --- a/python/test/pyLidar_TEST.py +++ b/python/test/pyLidar_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math7 import Angle, Helpers -from sdformat13 import Lidar, Error, Noise +from gz_test_deps.math import Angle, Helpers +from gz_test_deps.sdformat import Lidar, Error, Noise import math import unittest diff --git a/python/test/pyLight_TEST.py b/python/test/pyLight_TEST.py index f24717dee..233804270 100644 --- a/python/test/pyLight_TEST.py +++ b/python/test/pyLight_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz.math7 import Angle, Color, Pose3d, Vector3d -from sdformat13 import Light, SDFErrorsException -import sdformat13 as sdf +from gz_test_deps.math import Angle, Color, Pose3d, Vector3d +from gz_test_deps.sdformat import Light, SDFErrorsException +import gz_test_deps.sdformat as sdf import math import unittest diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index 9f52d84a0..7bf0a3494 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz.math7 import Pose3d, Inertiald, MassMatrix3d, Vector3d -from sdformat13 import (Collision, Light, Link, Sensor, Visual, - SDFErrorsException) +from gz_test_deps.math import Pose3d, Inertiald, MassMatrix3d, Vector3d +from gz_test_deps.sdformat import (Collision, Light, Link, Sensor, Visual, + SDFErrorsException) import unittest import math diff --git a/python/test/pyMagnetometer_TEST.py b/python/test/pyMagnetometer_TEST.py index 636a5f3aa..a875628dc 100644 --- a/python/test/pyMagnetometer_TEST.py +++ b/python/test/pyMagnetometer_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat13 import Magnetometer, Noise -import sdformat13 as sdf +from gz_test_deps.sdformat import Magnetometer, Noise +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyMaterial_TEST.py b/python/test/pyMaterial_TEST.py index 98a5f8c03..01198aab0 100644 --- a/python/test/pyMaterial_TEST.py +++ b/python/test/pyMaterial_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from sdformat13 import Material, Pbr, PbrWorkflow -from gz.math7 import Color -import sdformat13 as sdf +from gz_test_deps.sdformat import Material, Pbr, PbrWorkflow +from gz_test_deps.math import Color +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index 0bc65d2e0..64d89a57a 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat13 import Mesh -from gz.math7 import Vector3d +from gz_test_deps.sdformat import Mesh +from gz_test_deps.math import Vector3d import unittest diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index ae2e575a0..ce8d7e5b4 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -13,10 +13,10 @@ # limitations under the License. import copy -from gz.math7 import Pose3d -from sdformat13 import (Plugin, Model, Joint, Link, Error, Frame, SemanticPose, - SDFErrorsException) -import sdformat13 as sdf +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import (Plugin, Model, Joint, Link, Error, Frame, + SemanticPose, SDFErrorsException) +import gz_test_deps.sdformat as sdf import math import unittest diff --git a/python/test/pyNavSat_TEST.py b/python/test/pyNavSat_TEST.py index 49d8c4aca..783828ab9 100644 --- a/python/test/pyNavSat_TEST.py +++ b/python/test/pyNavSat_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat13 import NavSat, Noise +from gz_test_deps.sdformat import NavSat, Noise import unittest class NavSatColor(unittest.TestCase): diff --git a/python/test/pyNoise_TEST.py b/python/test/pyNoise_TEST.py index 07d60b2b1..0e444cde3 100644 --- a/python/test/pyNoise_TEST.py +++ b/python/test/pyNoise_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat13 import Noise -import sdformat13 as sdf +from gz_test_deps.sdformat import Noise +import gz_test_deps.sdformat as sdf import math import unittest diff --git a/python/test/pyParserConfig_TEST.py b/python/test/pyParserConfig_TEST.py index 1c5558d43..a56bfc405 100644 --- a/python/test/pyParserConfig_TEST.py +++ b/python/test/pyParserConfig_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat13 import ParserConfig +from gz_test_deps.sdformat import ParserConfig from sdformattest import source_file, test_file import unittest diff --git a/python/test/pyParticleEmitter_TEST.py b/python/test/pyParticleEmitter_TEST.py index eb119b37b..9995eb66d 100644 --- a/python/test/pyParticleEmitter_TEST.py +++ b/python/test/pyParticleEmitter_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math7 import Color, Pose3d, Vector3d, Helpers -from sdformat13 import ParticleEmitter +from gz_test_deps.math import Color, Pose3d, Vector3d, Helpers +from gz_test_deps.sdformat import ParticleEmitter import unittest diff --git a/python/test/pyPbr_TEST.py b/python/test/pyPbr_TEST.py index d3a6585d5..3ab554e5f 100644 --- a/python/test/pyPbr_TEST.py +++ b/python/test/pyPbr_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat13 import Pbr, PbrWorkflow -import sdformat13 as sdf +from gz_test_deps.sdformat import Pbr, PbrWorkflow +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyPhysics_TEST.py b/python/test/pyPhysics_TEST.py index d8255799d..f2425ff52 100644 --- a/python/test/pyPhysics_TEST.py +++ b/python/test/pyPhysics_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat13 import Physics +from gz_test_deps.sdformat import Physics import unittest diff --git a/python/test/pyPlane_TEST.py b/python/test/pyPlane_TEST.py index 66ee406ac..ad2536b5d 100644 --- a/python/test/pyPlane_TEST.py +++ b/python/test/pyPlane_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat13 import Plane -from gz.math7 import Vector3d, Vector2d +from gz_test_deps.sdformat import Plane +from gz_test_deps.math import Vector3d, Vector2d import unittest diff --git a/python/test/pyPlugin_TEST.py b/python/test/pyPlugin_TEST.py index c494c9985..2db8ceedd 100644 --- a/python/test/pyPlugin_TEST.py +++ b/python/test/pyPlugin_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from sdformat13 import Plugin +from gz_test_deps.sdformat import Plugin import unittest diff --git a/python/test/pyPolyline_TEST.py b/python/test/pyPolyline_TEST.py index 4f4b9ccc4..ea93acecf 100644 --- a/python/test/pyPolyline_TEST.py +++ b/python/test/pyPolyline_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from sdformat13 import Polyline -from gz.math7 import Vector2d +from gz_test_deps.sdformat import Polyline +from gz_test_deps.math import Vector2d import unittest diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index 6fc956c9d..0df1d3023 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -13,10 +13,11 @@ # limitations under the License. import copy -from gz.math7 import Pose3d -from sdformat13 import (Error, Model, Light, Root, SDF_VERSION, - SDFErrorsException, SDF_PROTOCOL_VERSION, World) -import sdformat13 as sdf +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import (Error, Model, Light, Root, SDF_VERSION, + SDFErrorsException, SDF_PROTOCOL_VERSION, + World) +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pyScene_TEST.py b/python/test/pyScene_TEST.py index b5398465b..6f3448c58 100644 --- a/python/test/pyScene_TEST.py +++ b/python/test/pyScene_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math7 import Color -from sdformat13 import Scene, Sky +from gz_test_deps.math import Color +from gz_test_deps.sdformat import Scene, Sky import unittest diff --git a/python/test/pySemanticPose_TEST.py b/python/test/pySemanticPose_TEST.py index 4218be6e3..6b514f605 100644 --- a/python/test/pySemanticPose_TEST.py +++ b/python/test/pySemanticPose_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math7 import Pose3d -from sdformat13 import Link, SemanticPose +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import Link, SemanticPose import unittest class semantic_poseTEST(unittest.TestCase): diff --git a/python/test/pySensor_TEST.py b/python/test/pySensor_TEST.py index 6215941e1..4a7f28415 100644 --- a/python/test/pySensor_TEST.py +++ b/python/test/pySensor_TEST.py @@ -13,11 +13,12 @@ # limitations under the License. import copy -from gz.math7 import Pose3d -from sdformat13 import (AirPressure, Altimeter, Camera, IMU, ForceTorque, Lidar, - Magnetometer, NavSat, Noise, Plugin, SemanticPose, - Sensor, SDFErrorsException) -import sdformat13 as sdf +from gz_test_deps.math import Pose3d +from gz_test_deps.sdformat import (AirPressure, Altimeter, Camera, IMU, + ForceTorque, Lidar, Magnetometer, NavSat, + Noise, Plugin, SemanticPose, Sensor, + SDFErrorsException) +import gz_test_deps.sdformat as sdf import unittest diff --git a/python/test/pySky_TEST.py b/python/test/pySky_TEST.py index 1967d548a..922da298d 100644 --- a/python/test/pySky_TEST.py +++ b/python/test/pySky_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math7 import Angle, Color -from sdformat13 import Sky +from gz_test_deps.math import Angle, Color +from gz_test_deps.sdformat import Sky import unittest diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index ed92a72f3..0636d602b 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -14,7 +14,7 @@ import copy import math -from sdformat13 import Sphere +from gz_test_deps.sdformat import Sphere import unittest class SphereTEST(unittest.TestCase): diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index b40130464..2651b1113 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -13,8 +13,8 @@ # limitations under the License. import copy -from gz.math7 import Vector3d -from sdformat13 import Surface, Contact, Friction, ODE +from gz_test_deps.math import Vector3d +from gz_test_deps.sdformat import Surface, Contact, Friction, ODE import unittest diff --git a/python/test/pyVisual_TEST.py b/python/test/pyVisual_TEST.py index 91a0f1c4b..bedadb0eb 100644 --- a/python/test/pyVisual_TEST.py +++ b/python/test/pyVisual_TEST.py @@ -13,9 +13,10 @@ # limitations under the License. import copy -from gz.math7 import Pose3d, Color -from sdformat13 import Geometry, Material, Visual, Plugin, SDFErrorsException -import sdformat13 as sdf +from gz_test_deps.math import Pose3d, Color +from gz_test_deps.sdformat import (Geometry, Material, Visual, Plugin, + SDFErrorsException) +import gz_test_deps.sdformat as sdf import unittest import math diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 05696766a..4048a349d 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -13,9 +13,10 @@ # limitations under the License. import copy -from gz.math7 import Color, Vector3d, SphericalCoordinates -from sdformat13 import Atmosphere, Gui, Physics, Plugin, Error, Frame, Light, Model, Scene, World -import sdformat13 as sdf +from gz_test_deps.math import Color, Vector3d, SphericalCoordinates +from gz_test_deps.sdformat import (Atmosphere, Gui, Physics, Plugin, Error, + Frame, Light, Model, Scene, World) +import gz_test_deps.sdformat as sdf import unittest import math