From b19e21705aa1ea55953e1f4dd94c82fdaf90aaea Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 29 Sep 2022 08:46:03 -0700 Subject: [PATCH 01/31] Prepare for 9.9.0 release (#1178) Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index adfc18c18..80ed65a9a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,7 @@ 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}~pre1) +set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}) 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 79440b9d6..a6b42f213 100644 --- a/Changelog.md +++ b/Changelog.md @@ -5,9 +5,15 @@ 1. sdf/camera.sdf: fields for projection matrix * [Pull request #1088](https://github.com/gazebosim/sdformat/pull/1088) +1. urdf: add //frame for reduced links/joints + * [Pull request #1148](https://github.com/gazebosim/sdformat/pull/1148) + 1. urdf: fix sensor/light pose for links lumped by fixed joints * [Pull request #1114](https://github.com/gazebosim/sdformat/pull/1114) +1. urdf: fix test and clean up internals + * [Pull request #1126](https://github.com/gazebosim/sdformat/pull/1126) + 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) From 4ba0b584400b9fbcd2626ae5848e9e061c930c78 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 14 Oct 2022 04:36:35 +0800 Subject: [PATCH 02/31] Model: add sdf::Errors output to API methods (#1122) Signed-off-by: Marco A. Gutierrez Co-authored-by: Steve Peters Co-authored-by: Addisu Z. Taddese --- src/Model.cc | 72 ++++++--- test/integration/error_output.cc | 244 ++++++++++++++++++++++++++++++- test/integration/joint_dom.cc | 10 +- 3 files changed, 293 insertions(+), 33 deletions(-) diff --git a/src/Model.cc b/src/Model.cc index ae7840973..4f4f7b0f3 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -203,9 +203,13 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { if (size > 1) { - sdfwarn << "Non-unique name[" << name << "] detected " << size - << " times in XML children of model with name[" << this->Name() - << "].\n"; + std::stringstream ss; + ss << "Non-unique name[" << name << "] detected " << size + << " times in XML children of model with name[" << this->Name() + << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); } } @@ -298,17 +302,23 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { linkName = link.Name() + "_link" + std::to_string(i++); } - sdfwarn << "Link with name [" << link.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing link name to [" - << linkName << "].\n"; + std::stringstream ss; + ss << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing link name to [" + << linkName << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); link.SetName(linkName); } else { - sdferr << "Link with name [" << link.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this link.\n"; + std::stringstream ss; + ss << "Link with name [" << link.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this link."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); } } frameNames.insert(linkName); @@ -346,17 +356,24 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { jointName = joint.Name() + "_joint" + std::to_string(i++); } - sdfwarn << "Joint with name [" << joint.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing joint name to [" - << jointName << "].\n"; + std::stringstream ss; + ss << "Joint with name [" << joint.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing joint name to [" + << jointName << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); + joint.SetName(jointName); } else { - sdferr << "Joint with name [" << joint.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this joint.\n"; + std::stringstream ss; + ss << "Joint with name [" << joint.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this joint."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); } } frameNames.insert(jointName); @@ -383,17 +400,24 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) { frameName = frame.Name() + "_frame" + std::to_string(i++); } - sdfwarn << "Frame with name [" << frame.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision, changing frame name to [" - << frameName << "].\n"; + std::stringstream ss; + ss << "Frame with name [" << frame.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision, changing frame name to [" + << frameName << "]."; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); + frame.SetName(frameName); } else { - sdferr << "Frame with name [" << frame.Name() << "] " - << "in model with name [" << this->Name() << "] " - << "has a name collision. Please rename this frame.\n"; + std::stringstream ss; + ss << "Frame with name [" << frame.Name() << "] " + << "in model with name [" << this->Name() << "] " + << "has a name collision. Please rename this frame."; + errors.push_back({ErrorCode::DUPLICATE_NAME, ss.str()}); } } frameNames.insert(frameName); diff --git a/test/integration/error_output.cc b/test/integration/error_output.cc index b205d57c2..a4d53e2d2 100644 --- a/test/integration/error_output.cc +++ b/test/integration/error_output.cc @@ -22,7 +22,9 @@ #include "sdf/Element.hh" #include "sdf/Error.hh" +#include "sdf/Model.hh" #include "sdf/Param.hh" +#include "sdf/parser.hh" #include "sdf/Types.hh" #include "sdf/World.hh" #include "test_utils.hh" @@ -36,6 +38,15 @@ TEST(ErrorOutput, ParamErrorOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; ASSERT_NO_THROW(sdf::Param param1("key", "not_valid_type", "true", false, errors, "description")); @@ -117,10 +128,10 @@ TEST(ErrorOutput, ParamErrorOutput) 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'.")); + "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(); @@ -158,6 +169,15 @@ TEST(ErrorOutput, ElementErrorOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; sdf::ElementPtr elem = std::make_shared(); elem->SetName("testElement"); @@ -230,6 +250,15 @@ TEST(ErrorOutput, PrintConfigErrorOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; sdf::PrintConfig printConfig; ASSERT_FALSE(printConfig.SetRotationSnapToDegrees(361, 300, errors)); @@ -256,6 +285,15 @@ TEST(ErrorOutput, WorldErrorOutput) sdf::testing::RedirectConsoleStream redir( sdf::Console::Instance()->GetMsgStream(), &buffer); + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + sdf::Errors errors; std::ostringstream stream; @@ -296,3 +334,201 @@ TEST(ErrorOutput, WorldErrorOutput) // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } + +//////////////////////////////////////// +// Test Model class for sdf::Errors outputs +TEST(ErrorOutput, ModelErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::Errors errors; + + std::function findFileCb = [](const std::string &_uri) + { + return sdf::testing::TestFile("integration", "model", _uri); + }; + + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " test_model" + << " common_name" + << " " + << " " + << " " + << " " + << " world" + << " child" + << " " + << " " + << " world" + << " child" + << " " + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + parserConfig.SetFindCallback(findFileCb); + sdf::readString(stream.str(), parserConfig, sdfParsed, errors); + EXPECT_TRUE(errors.empty()); + + { + sdf::Model model; + errors = model.Load(sdfParsed->Root()->GetElement("model"), parserConfig); + ASSERT_EQ(errors.size(), 7u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Non-unique name[common_name] detected 7 times in XML children of model" + " with name[test_model].")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "Link with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this link.")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[3].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "Joint with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this joint.")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[5].Message().find( + "frame with name[common_name] already exists.")); + EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[6].Message().find( + "Frame with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this frame.")); + errors.clear(); + } + + { + sdf::Model model; + // Check warnings are still printed when the policy is not set to error. + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::WARN); + errors = model.Load(sdfParsed->Root()->GetElement("model"), parserConfig); + ASSERT_EQ(errors.size(), 6u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "Link with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this link.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[3].Message().find( + "Joint with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this joint.")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "frame with name[common_name] already exists.")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[5].Message().find( + "Frame with name [common_name] in model with name [test_model] has a " + "name collision. Please rename this frame.")); + // Check printed warnings + EXPECT_NE(std::string::npos, buffer.str().find( + "Non-unique name[common_name] detected 7 times in XML children of model" + " with name[test_model].")) + << buffer.str(); + buffer.str(""); + buffer.clear(); + errors.clear(); + } + + { + sdf::Model model; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + // Set SDF version to someting lower than 1.7 for extra errors + sdfParsed->Root()->GetElement("model")->SetOriginalVersion("1.6"); + errors = model.Load(sdfParsed->Root()->GetElement("model"), parserConfig); + + ASSERT_EQ(errors.size(), 7u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Non-unique name[common_name] detected 7 times in XML children of model" + " with name[test_model].")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "Link with name [common_name] in model with name [test_model] has a " + "name collision, changing link name to [common_name_link].")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[3].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[4].Message().find( + "Joint with name [common_name] in model with name [test_model] has a " + "name collision, changing joint name to [common_name_joint].")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[5].Message().find( + "frame with name[common_name] already exists.")); + EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[6].Message().find( + "Frame with name [common_name] in model with name [test_model] has a " + "name collision, changing frame name to [common_name_frame].")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + errors.clear(); + } + + { + sdf::Model model; + // Check warnings are still printed when the policy is not set to error. + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::WARN); + errors = model.Load(sdfParsed->Root()->GetElement("model"), parserConfig); + ASSERT_EQ(errors.size(), 3u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "link with name[common_name] already exists.")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "joint with name[common_name] already exists.")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_NE(std::string::npos, errors[2].Message().find( + "frame with name[common_name] already exists.")); + // Check printed warnings + EXPECT_NE(std::string::npos, buffer.str().find( + "Non-unique name[common_name] detected 7 times in XML children of model" + " with name[test_model].")) + << buffer.str(); + EXPECT_NE(std::string::npos, buffer.str().find( + "Link with name [common_name] in model with name [test_model] has a " + "name collision, changing link name to [common_name_link].")) + << buffer.str(); + EXPECT_NE(std::string::npos, buffer.str().find( + "Joint with name [common_name] in model with name [test_model] has a " + "name collision, changing joint name to [common_name_joint].")) + << buffer.str(); + EXPECT_NE(std::string::npos, buffer.str().find( + "Frame with name [common_name] in model with name [test_model] has a " + "name collision, changing frame name to [common_name_frame].")) + << buffer.str(); + } +} diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 2dffa07be..9a1711f65 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -650,15 +650,15 @@ TEST(DOMJoint, LoadLinkJointSameName17Invalid) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(9u, errors.size()); - EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_EQ(10u, errors.size()); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, - errors[0].Message().find( + errors[1].Message().find( "Joint with non-unique name [attachment] detected in model with name " "[link_joint_same_name].")); - EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::DUPLICATE_NAME); EXPECT_NE(std::string::npos, - errors[3].Message().find( + errors[4].Message().find( "Joint with non-unique name [attachment] detected in model with name " "[link_joint_same_name].")); } From bcc2a92a06f671a2cbccc821e8cc90faa1e62da9 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 15 Oct 2022 00:19:48 -0700 Subject: [PATCH 03/31] sdf/1.10: support //world/joint specification (#1117) * Add world_joint_* test/sdf files * Support //world/joint in DOM API * Add //world/joint to graphs, add test * ParserConfig: add missing doc-strings Signed-off-by: Steve Peters * Added python bindings for joint related methods Signed-off-by: Liam Han Co-authored-by: Liam Han --- Migration.md | 4 + include/sdf/Joint.hh | 3 + include/sdf/ParserConfig.hh | 7 +- include/sdf/World.hh | 53 ++++++- python/src/sdf/pyWorld.cc | 18 +++ python/test/pyWorld_TEST.py | 54 +++++-- sdf/1.10/joint.sdf | 2 +- sdf/1.10/world.sdf | 1 + src/FrameSemantics.cc | 27 +++- src/World.cc | 98 +++++++++++++ src/World_TEST.cc | 91 +++++++++++- src/gz_TEST.cc | 3 + test/integration/joint_dom.cc | 139 ++++++++++++++++++- test/integration/nested_model.cc | 11 ++ test/sdf/world_joint_child_frame.sdf | 32 +++++ test/sdf/world_joint_invalid_child_world.sdf | 28 ++++ test/sdf/world_joint_parent_frame.sdf | 32 +++++ 17 files changed, 576 insertions(+), 27 deletions(-) create mode 100644 test/sdf/world_joint_child_frame.sdf create mode 100644 test/sdf/world_joint_invalid_child_world.sdf create mode 100644 test/sdf/world_joint_parent_frame.sdf diff --git a/Migration.md b/Migration.md index 513746002..3bda44b1c 100644 --- a/Migration.md +++ b/Migration.md @@ -555,6 +555,10 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. ## SDFormat specification 1.9 to 1.10 +### Additions + +1. **world.sdf**: A joint can be specified directly in a world. + ### Modifications 1. **joint.sdf**: axis limits default values have changed diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 73764b7e7..3826e709b 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -291,6 +291,9 @@ namespace sdf /// \brief Allow Model::Load to call SetPoseRelativeToGraph. friend class Model; + /// \brief Allow World::Load to call SetPoseRelativeToGraph. + friend class World; + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index af17a8e76..ad8195fe5 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -166,14 +166,17 @@ class SDFORMAT_VISIBLE ParserConfig public: void RegisterCustomModelParser(CustomModelParser _modelParser); /// \brief Get the registered custom model parsers + /// \return Vector of registered model parser callbacks. public: const std::vector &CustomModelParsers() const; /// \brief Set the preserveFixedJoint flag. - /// \param[in] _preserveFixedJoint flag value to set + /// \param[in] _preserveFixedJoint True to preserve fixed joints, false to + /// reduce the fixed joints and merge the child link into the parent. public: void URDFSetPreserveFixedJoint(bool _preserveFixedJoint); /// \brief Get the preserveFixedJoint flag value. - /// \return Current flag value + /// \return True to preserve fixed joints, false to reduce the fixed joints + /// and merge the child link into the parent. public: bool URDFPreserveFixedJoint() const; /// \brief Set the storeResolvedURIs flag value. diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 8d1116d33..d99dc4992 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -45,6 +45,7 @@ namespace sdf class Actor; class Frame; class InterfaceModel; + class Joint; class Light; class Model; class ParserConfig; @@ -212,6 +213,12 @@ namespace sdf /// exists. public: bool AddActor(const Actor &_actor); + /// \brief Add a joint to the world. + /// \param[in] _joint Joint to add. + /// \return True if successful, false if a joint with the name already + /// exists. + public: bool AddJoint(const Joint &_joint); + /// \brief Add a light to the world. /// \param[in] _light Light to add. /// \return True if successful, false if a light with the name already @@ -233,10 +240,13 @@ namespace sdf /// \brief Remove all models. public: void ClearModels(); - /// \brief Remove all models. + /// \brief Remove all actors. public: void ClearActors(); - /// \brief Remove all models. + /// \brief Remove all joints. + public: void ClearJoints(); + + /// \brief Remove all lights. public: void ClearLights(); /// \brief Remove all physics. @@ -316,6 +326,45 @@ namespace sdf /// \return True if there exists an explicit frame with the given name. public: bool FrameNameExists(const std::string &_name) const; + /// \brief Get the number of joints. + /// \return Number of joints contained in this World object. + public: uint64_t JointCount() const; + + /// \brief Get a joint based on an index. + /// \param[in] _index Index of the joint. The index should be in the + /// range [0..JointCount()). + /// \return Pointer to the joint. Nullptr if the index does not exist. + /// \sa uint64_t JointCount() const + public: const Joint *JointByIndex(uint64_t _index) const; + + /// \brief Get a mutable joint based on an index. + /// \param[in] _index Index of the joint. The index should be in the + /// range [0..JointCount()). + /// \return Pointer to the joint. Nullptr if the index does not exist. + /// \sa uint64_t JointCount() const + public: Joint *JointByIndex(uint64_t _index); + + /// \brief Get an joint based on a name. + /// \param[in] _name Name of the joint. + /// To get a joint in a nested model, prefix the joint name with the + /// sequence of nested models containing this joint, delimited by "::". + /// \return Pointer to the joint. Nullptr if the name does not + /// exist. + public: const Joint *JointByName(const std::string &_name) const; + + /// \brief Get a mutable joint based on a name. + /// \param[in] _name Name of the joint. + /// To get a joint in a nested model, prefix the joint name with the + /// sequence of nested models containing this joint, delimited by "::". + /// \return Pointer to the joint. Nullptr if the name does not + /// exist. + public: Joint *JointByName(const std::string &_name); + + /// \brief Get whether a joint name exists. + /// \param[in] _name Name of the joint to check. + /// \return True if there exists a joint with the given name. + public: bool JointNameExists(const std::string &_name) const; + /// \brief Get the number of lights. /// \return Number of lights contained in this World object. public: uint64_t LightCount() const; diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index c82fd2dc4..bc2619877 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -117,6 +117,8 @@ void defineWorld(pybind11::object module) "Add a physics object to the world.") .def("add_frame", &sdf::World::AddFrame, "Add a frame object to the world.") + .def("add_joint", &sdf::World::AddJoint, + "Add a joint to the world.") .def("clear_models", &sdf::World::ClearModels, "Remove all models.") // .def("clear_actors", &sdf::World::ClearActors, @@ -127,6 +129,8 @@ void defineWorld(pybind11::object module) "Remove all physics objects.") .def("clear_frames", &sdf::World::ClearFrames, "Remove all frames.") + .def("clear_joints", &sdf::World::ClearJoints, + "Remove all joints.") .def("actor_count", &sdf::World::ActorCount, "Get the number of actors.") .def("frame_count", &sdf::World::FrameCount, @@ -145,6 +149,20 @@ void defineWorld(pybind11::object module) "index.") .def("frame_name_exists", &sdf::World::FrameNameExists, "Get whether a frame name exists.") + .def("joint_count", &sdf::World::JointCount, + "Get the number of joints.") + .def("joint_by_index", + pybind11::overload_cast( + &sdf::World::JointByIndex), + pybind11::return_value_policy::reference_internal, + "Get a mutable joint based on an index.") + .def("joint_by_name", + pybind11::overload_cast( + &sdf::World::JointByName), + pybind11::return_value_policy::reference_internal, + "Get a mutable joint based on name.") + .def("joint_name_exists", &sdf::World::JointNameExists, + "Get whether a joint name exists.") .def("light_count", &sdf::World::LightCount, "Get the number of lights.") .def("light_by_index", diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 4048a349d..63fe26a01 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -15,7 +15,7 @@ import copy 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) + Frame, Joint, Light, Model, Scene, World) import gz_test_deps.sdformat as sdf import unittest import math @@ -52,13 +52,28 @@ def test_default_construction(self): self.assertEqual(None, world.frame_by_index(0)) self.assertEqual(None, world.frame_by_index(1)) self.assertFalse(world.frame_name_exists("")) - self.assertFalse(world.frame_name_exists("default")) - - self.assertEqual(0, world.frame_count()) - self.assertEqual(None, world.frame_by_index(0)) - self.assertEqual(None, world.frame_by_index(1)) - self.assertFalse(world.frame_name_exists("")) - self.assertFalse(world.frame_name_exists("default")) + self.assertFalse(world.frame_name_exists("a::b")) + self.assertFalse(world.frame_name_exists("a::b::c")) + self.assertFalse(world.frame_name_exists("::::")) + self.assertEqual(None, world.frame_by_name("")) + self.assertEqual(None, world.frame_by_name("default")) + self.assertEqual(None, world.frame_by_name("a::b")) + self.assertEqual(None, world.frame_by_name("a::b::c")) + self.assertEqual(None, world.frame_by_name("::::")) + + self.assertEqual(0, world.joint_count()) + self.assertEqual(None, world.joint_by_index(0)) + self.assertEqual(None, world.joint_by_index(1)) + self.assertFalse(world.joint_name_exists("")) + self.assertFalse(world.joint_name_exists("default")) + self.assertFalse(world.joint_name_exists("a::b")) + self.assertFalse(world.joint_name_exists("a::b::c")) + self.assertFalse(world.joint_name_exists("::::")) + self.assertEqual(None, world.joint_by_name("")) + self.assertEqual(None, world.joint_by_name("default")) + self.assertEqual(None, world.joint_by_name("a::b")) + self.assertEqual(None, world.joint_by_name("a::b::c")) + self.assertEqual(None, world.joint_by_name("::::")) self.assertEqual(1, world.physics_count()) @@ -318,6 +333,10 @@ def test_mutable_by_index(self): frame.set_name("frame1") self.assertTrue(world.add_frame(frame)) + joint = Joint() + joint.set_name("joint1") + self.assertTrue(world.add_joint(joint)) + # Modify the model m = world.model_by_index(0) self.assertNotEqual(None, m) @@ -353,6 +372,13 @@ def test_mutable_by_index(self): f.set_name("frame2") self.assertEqual("frame2", world.frame_by_index(0).name()) + # Modify the joint + j = world.joint_by_index(0) + self.assertNotEqual(None, j) + self.assertEqual("joint1", j.name()) + j.set_name("joint2") + self.assertEqual("joint2", world.joint_by_index(0).name()) + def test_mutable_by_name(self): world = World() @@ -364,6 +390,10 @@ def test_mutable_by_name(self): frame.set_name("frame1") self.assertTrue(world.add_frame(frame)) + joint = Joint() + joint.set_name("joint1") + self.assertTrue(world.add_joint(joint)) + # Modify the model m = world.model_by_name("model1") self.assertNotEqual(None, m) @@ -380,6 +410,14 @@ def test_mutable_by_name(self): self.assertFalse(world.frame_by_name("frame1")) self.assertTrue(world.frame_by_name("frame2")) + # Modify the joint + j = world.joint_by_name("joint1") + self.assertNotEqual(None, j) + self.assertEqual("joint1", j.name()) + j.set_name("joint2") + self.assertFalse(world.joint_by_name("joint1")) + self.assertTrue(world.joint_by_name("joint2")) + def test_plugins(self): world = World() self.assertEqual(0, len(world.plugins())) diff --git a/sdf/1.10/joint.sdf b/sdf/1.10/joint.sdf index 04d5f52e1..e60090c9d 100644 --- a/sdf/1.10/joint.sdf +++ b/sdf/1.10/joint.sdf @@ -3,7 +3,7 @@ A joint connects two links with kinematic and dynamic properties. By default, the pose of a joint is expressed in the child link frame. - A unique name for the joint within the scope of the model. + A unique name for the joint within its scope. diff --git a/sdf/1.10/world.sdf b/sdf/1.10/world.sdf index 0f3efe585..07dafc08e 100644 --- a/sdf/1.10/world.sdf +++ b/sdf/1.10/world.sdf @@ -60,6 +60,7 @@ + diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 0f22e111b..e4d0aedad 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -599,6 +599,10 @@ struct WorldWrapper : public WrapperBase { this->frames.emplace_back(*_world.FrameByIndex(i)); } + for (uint64_t i = 0; i < _world.JointCount(); ++i) + { + this->joints.emplace_back(*_world.JointByIndex(i)); + } for (uint64_t i = 0; i < _world.ModelCount(); ++i) { this->models.emplace_back(*_world.ModelByIndex(i)); @@ -612,6 +616,8 @@ struct WorldWrapper : public WrapperBase /// \brief Children frames. std::vector frames; + /// \brief Children joints. + std::vector joints; /// \brief Children models and interface models. std::vector models; }; @@ -755,11 +761,11 @@ void addEdgesToGraph(ScopedGraph &_out, /// \param[in,out] _out The FrameAttachedTo graph to which edges will be added. /// \param[in] _joints List of joints for which edges will be created in the /// graph. -/// \param[in] _model Parent model of the items in `_items`. +/// \param[in] _parent Parent of the joints in `_joints`. /// \param[out] _errors Errors encountered while adding edges. void addEdgesToGraph(ScopedGraph &_out, const std::vector &_joints, - const ModelWrapper &_model, Errors &_errors) + const WrapperBase &_parent, Errors &_errors) { for (const auto &joint : _joints) { @@ -771,7 +777,8 @@ void addEdgesToGraph(ScopedGraph &_out, {ErrorCode::JOINT_CHILD_LINK_INVALID, "Child frame with name[" + joint.childName + "] specified by " + lowercase(joint.elementType) + " with name[" + joint.name + - "] not found in model with name[" + _model.name + "]."}); + "] not found in " + lowercase(_parent.elementType) + + " with name[" + _parent.name + "]."}); continue; } auto childFrameId = _out.VertexIdByName(joint.childName); @@ -784,7 +791,7 @@ void addEdgesToGraph(ScopedGraph &_out, /// \param[in,out] _out The FrameAttachedTo graph to which edges will be added. /// \param[in] _frames List of frames for which edges will be created in the /// graph. -/// \param[in] _model Parent model of the items in `_items`. +/// \param[in] _parent Parent of the frames in `_frames`. /// \param[out] _errors Errors encountered while adding edges. void addEdgesToGraph(ScopedGraph &_out, const std::vector &_frames, @@ -991,9 +998,15 @@ Errors buildFrameAttachedToGraph( // add model vertices addVerticesToGraph(_out, _world.models, _world, errors); + // add joint vertices + addVerticesToGraph(_out, _world.joints, _world, errors); + // add frame vertices addVerticesToGraph(_out, _world.frames, _world, errors); + // add edges from joint to child frames + addEdgesToGraph(_out, _world.joints, _world, errors); + // add frame edges addEdgesToGraph(_out, _world.frames, _world, errors); @@ -1161,6 +1174,9 @@ Errors wrapperBuildPoseRelativeToGraph( // add model vertices addVerticesToGraph(_out, _world.models, _world, errors); + // add joint vertices + addVerticesToGraph(_out, _world.joints, _world, errors); + // add frame vertices addVerticesToGraph(_out, _world.frames, _world, errors); @@ -1169,6 +1185,9 @@ Errors wrapperBuildPoseRelativeToGraph( // add model edges addEdgesToGraph(_out, _world.models, _world, errors); + // add joint edges + addEdgesToGraph(_out, _world.joints, _world, errors); + // add frame edges addEdgesToGraph(_out, _world.frames, _world, errors); diff --git a/src/World.cc b/src/World.cc index 53504d950..05dccde19 100644 --- a/src/World.cc +++ b/src/World.cc @@ -25,6 +25,7 @@ #include "sdf/InterfaceElements.hh" #include "sdf/InterfaceModel.hh" #include "sdf/InterfaceModelPoseGraph.hh" +#include "sdf/Joint.hh" #include "sdf/Light.hh" #include "sdf/Model.hh" #include "sdf/ParserConfig.hh" @@ -65,6 +66,9 @@ class sdf::World::Implementation /// \brief The frames specified in this world. public: std::vector frames; + /// \brief The joints specified in this world. + public: std::vector joints; + /// \brief The lights specified in this world. public: std::vector lights; @@ -259,6 +263,11 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->actors); errors.insert(errors.end(), actorLoadErrors.begin(), actorLoadErrors.end()); + // Load all the joints. + Errors jointLoadErrors = loadUniqueRepeated(_sdf, "joint", + this->dataPtr->joints); + errors.insert(errors.end(), jointLoadErrors.begin(), jointLoadErrors.end()); + // Load all the lights. Errors lightLoadErrors = loadUniqueRepeated(_sdf, "light", this->dataPtr->lights); @@ -566,6 +575,67 @@ Frame *World::FrameByName(const std::string &_name) static_cast(this)->FrameByName(_name)); } +///////////////////////////////////////////////// +uint64_t World::JointCount() const +{ + return this->dataPtr->joints.size(); +} + +///////////////////////////////////////////////// +const Joint *World::JointByIndex(const uint64_t _index) const +{ + if (_index < this->dataPtr->joints.size()) + return &this->dataPtr->joints[_index]; + return nullptr; +} + +///////////////////////////////////////////////// +Joint *World::JointByIndex(uint64_t _index) +{ + return const_cast( + static_cast(this)->JointByIndex(_index)); +} + +///////////////////////////////////////////////// +const Joint *World::JointByName(const std::string &_name) const +{ + auto index = _name.rfind("::"); + if (index != std::string::npos) + { + const Model *model = this->ModelByName(_name.substr(0, index)); + if (nullptr != model) + { + return model->JointByName(_name.substr(index + 2)); + } + + // The nested model name preceding the last "::" could not be found. + // Since "::" are reserved and not allowed in names, return a nullptr. + return nullptr; + } + + for (auto const &f : this->dataPtr->joints) + { + if (f.Name() == _name) + { + return &f; + } + } + return nullptr; +} + +///////////////////////////////////////////////// +Joint *World::JointByName(const std::string &_name) +{ + return const_cast( + static_cast(this)->JointByName(_name)); +} + +///////////////////////////////////////////////// +bool World::JointNameExists(const std::string &_name) const +{ + return nullptr != this->JointByName(_name); +} + ///////////////////////////////////////////////// uint64_t World::LightCount() const { @@ -728,6 +798,10 @@ void World::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); } + for (auto &joint : this->dataPtr->joints) + { + joint.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } for (auto &light : this->dataPtr->lights) { light.SetXmlParentName("world"); @@ -745,6 +819,10 @@ void World::SetFrameAttachedToGraph( { frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); } + for (auto &joint : this->dataPtr->joints) + { + joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } for (auto &model : this->dataPtr->models) { model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); @@ -920,6 +998,10 @@ sdf::ElementPtr World::ToElement(const OutputConfig &_config) const for (const sdf::Actor &actor : this->dataPtr->actors) elem->InsertElement(actor.ToElement(), true); + // Joints + for (const sdf::Joint &joint : this->dataPtr->joints) + elem->InsertElement(joint.ToElement(), true); + // Lights for (const sdf::Light &light : this->dataPtr->lights) elem->InsertElement(light.ToElement(), true); @@ -985,6 +1067,12 @@ void World::ClearActors() this->dataPtr->actors.clear(); } +///////////////////////////////////////////////// +void World::ClearJoints() +{ + this->dataPtr->joints.clear(); +} + ///////////////////////////////////////////////// void World::ClearLights() { @@ -1022,6 +1110,16 @@ bool World::AddActor(const Actor &_actor) return true; } +///////////////////////////////////////////////// +bool World::AddJoint(const Joint &_joint) +{ + if (this->JointNameExists(_joint.Name())) + return false; + this->dataPtr->joints.push_back(_joint); + + return true; +} + ///////////////////////////////////////////////// bool World::AddLight(const Light &_light) { diff --git a/src/World_TEST.cc b/src/World_TEST.cc index d8c15faa7..88679e4e5 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -58,12 +58,28 @@ TEST(DOMWorld, Construction) EXPECT_EQ(nullptr, world.FrameByIndex(1)); EXPECT_FALSE(world.FrameNameExists("")); EXPECT_FALSE(world.FrameNameExists("default")); - - EXPECT_EQ(0u, world.FrameCount()); - EXPECT_EQ(nullptr, world.FrameByIndex(0)); - EXPECT_EQ(nullptr, world.FrameByIndex(1)); - EXPECT_FALSE(world.FrameNameExists("")); - EXPECT_FALSE(world.FrameNameExists("default")); + EXPECT_FALSE(world.FrameNameExists("a::b")); + EXPECT_FALSE(world.FrameNameExists("a::b::c")); + EXPECT_FALSE(world.FrameNameExists("::::")); + EXPECT_EQ(nullptr, world.FrameByName("")); + EXPECT_EQ(nullptr, world.FrameByName("default")); + EXPECT_EQ(nullptr, world.FrameByName("a::b")); + EXPECT_EQ(nullptr, world.FrameByName("a::b::c")); + EXPECT_EQ(nullptr, world.FrameByName("::::")); + + EXPECT_EQ(0u, world.JointCount()); + EXPECT_EQ(nullptr, world.JointByIndex(0)); + EXPECT_EQ(nullptr, world.JointByIndex(1)); + EXPECT_FALSE(world.JointNameExists("")); + EXPECT_FALSE(world.JointNameExists("default")); + EXPECT_FALSE(world.JointNameExists("a::b")); + EXPECT_FALSE(world.JointNameExists("a::b::c")); + EXPECT_FALSE(world.JointNameExists("::::")); + EXPECT_EQ(nullptr, world.JointByName("")); + EXPECT_EQ(nullptr, world.JointByName("default")); + EXPECT_EQ(nullptr, world.JointByName("a::b")); + EXPECT_EQ(nullptr, world.JointByName("a::b::c")); + EXPECT_EQ(nullptr, world.JointByName("::::")); EXPECT_EQ(1u, world.PhysicsCount()); @@ -435,6 +451,29 @@ TEST(DOMWorld, AddActor) EXPECT_EQ(actorFromWorld->Name(), actor.Name()); } +///////////////////////////////////////////////// +TEST(DOMWorld, AddJoint) +{ + sdf::World world; + EXPECT_EQ(0u, world.JointCount()); + + sdf::Joint joint; + joint.SetName("joint1"); + EXPECT_TRUE(world.AddJoint(joint)); + EXPECT_EQ(1u, world.JointCount()); + EXPECT_FALSE(world.AddJoint(joint)); + EXPECT_EQ(1u, world.JointCount()); + + world.ClearJoints(); + EXPECT_EQ(0u, world.JointCount()); + + EXPECT_TRUE(world.AddJoint(joint)); + EXPECT_EQ(1u, world.JointCount()); + const sdf::Joint *jointFromWorld = world.JointByIndex(0); + ASSERT_NE(nullptr, jointFromWorld); + EXPECT_EQ(jointFromWorld->Name(), joint.Name()); +} + ///////////////////////////////////////////////// TEST(DOMWorld, AddLight) { @@ -515,6 +554,19 @@ TEST(DOMWorld, ToElement) world.ClearActors(); } + for (int j = 0; j <= 1; ++j) + { + for (int i = 0; i < 4; ++i) + { + sdf::Joint joint; + joint.SetName("joint" + std::to_string(i)); + EXPECT_TRUE(world.AddJoint(joint)); + EXPECT_FALSE(world.AddJoint(joint)); + } + if (j == 0) + world.ClearJoints(); + } + for (int j = 0; j <= 1; ++j) { for (int i = 0; i < 4; ++i) @@ -572,6 +624,10 @@ TEST(DOMWorld, ToElement) for (uint64_t i = 0; i < world2.ModelCount(); ++i) EXPECT_NE(nullptr, world2.ModelByIndex(i)); + EXPECT_EQ(world.JointCount(), world2.JointCount()); + for (uint64_t i = 0; i < world2.JointCount(); ++i) + EXPECT_NE(nullptr, world2.JointByIndex(i)); + EXPECT_EQ(world.LightCount(), world2.LightCount()); for (uint64_t i = 0; i < world2.LightCount(); ++i) EXPECT_NE(nullptr, world2.LightByIndex(i)); @@ -628,6 +684,10 @@ TEST(DOMWorld, MutableByIndex) actor.SetName("actor1"); EXPECT_TRUE(world.AddActor(actor)); + sdf::Joint joint; + joint.SetName("joint1"); + EXPECT_TRUE(world.AddJoint(joint)); + sdf::Light light; light.SetName("light1"); EXPECT_TRUE(world.AddLight(light)); @@ -654,6 +714,13 @@ TEST(DOMWorld, MutableByIndex) a->SetName("actor2"); EXPECT_EQ("actor2", world.ActorByIndex(0)->Name()); + // Modify the joint + sdf::Joint *j = world.JointByIndex(0); + ASSERT_NE(nullptr, j); + EXPECT_EQ("joint1", j->Name()); + j->SetName("joint2"); + EXPECT_EQ("joint2", world.JointByIndex(0)->Name()); + // Modify the light sdf::Light *l = world.LightByIndex(0); ASSERT_NE(nullptr, l); @@ -689,6 +756,10 @@ TEST(DOMWorld, MutableByName) frame.SetName("frame1"); EXPECT_TRUE(world.AddFrame(frame)); + sdf::Joint joint; + joint.SetName("joint1"); + EXPECT_TRUE(world.AddJoint(joint)); + // Modify the model sdf::Model *m = world.ModelByName("model1"); ASSERT_NE(nullptr, m); @@ -704,6 +775,14 @@ TEST(DOMWorld, MutableByName) f->SetName("frame2"); EXPECT_FALSE(world.FrameByName("frame1")); EXPECT_TRUE(world.FrameByName("frame2")); + + // Modify the joint + sdf::Joint *j = world.JointByName("joint1"); + ASSERT_NE(nullptr, j); + EXPECT_EQ("joint1", j->Name()); + j->SetName("joint2"); + EXPECT_FALSE(world.JointByName("joint1")); + EXPECT_TRUE(world.JointByName("joint2")); } ///////////////////////////////////////////////// diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 8d1e4f585..9d4bde234 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -355,6 +355,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) EXPECT_NE(output.find("Joint with name[joint] specified invalid " "child link [world]."), std::string::npos) << output; + EXPECT_NE(output.find("Child frame with name[world] specified by joint " + "with name[joint] not found in model"), + std::string::npos) << output; } // Check an SDF file with the world specified as a parent link. diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 9a1711f65..0587457b6 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -30,6 +30,7 @@ #include "sdf/SDFImpl.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" +#include "sdf/World.hh" #include "sdf/parser.hh" #include "test_config.hh" @@ -356,7 +357,7 @@ TEST(DOMJoint, LoadJointParentFrame) EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), model->FrameByName("parent_frame")->RawPose()); - // Test ResolveFrame to get each link, joint and frame pose in model frame. + // Test Resolve to get each link, joint and frame pose in model frame. Pose pose; EXPECT_TRUE( model->LinkByName("parent_link")-> @@ -448,7 +449,7 @@ TEST(DOMJoint, LoadJointChildFrame) EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), model->FrameByName("child_frame")->RawPose()); - // Test ResolveFrame to get each link, joint and frame pose in model frame. + // Test Resolve to get each link, joint and frame pose in model frame. Pose pose; EXPECT_TRUE( model->LinkByName("parent_link")-> @@ -478,6 +479,136 @@ TEST(DOMJoint, LoadJointChildFrame) EXPECT_EQ(Pose(1, 1, 9, 0, 0, 0), pose); } +///////////////////////////////////////////////// +TEST(DOMJoint, LoadWorldJointChildFrame) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_joint_child_frame.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = gz::math::Pose3d; + + // Get the world + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ("world_joint_child_frame", world->Name()); + EXPECT_EQ(2u, world->ModelCount()); + EXPECT_NE(nullptr, world->ModelByIndex(0)); + EXPECT_NE(nullptr, world->ModelByIndex(1)); + EXPECT_EQ(nullptr, world->ModelByIndex(2)); + + ASSERT_TRUE(world->ModelNameExists("parent_model")); + ASSERT_TRUE(world->ModelNameExists("child_model")); + EXPECT_TRUE(world->ModelByName("parent_model")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->ModelByName("child_model")->PoseRelativeTo().empty()); + + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), + world->ModelByName("parent_model")->RawPose()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), + world->ModelByName("child_model")->RawPose()); + + EXPECT_EQ(2u, world->JointCount()); + EXPECT_NE(nullptr, world->JointByIndex(0)); + EXPECT_NE(nullptr, world->JointByIndex(1)); + EXPECT_EQ(nullptr, world->JointByIndex(2)); + ASSERT_TRUE(world->JointNameExists("J1")); + ASSERT_TRUE(world->JointNameExists("J2")); + EXPECT_EQ("child_frame", world->JointByName("J1")->ChildName()); + EXPECT_EQ("parent_model::L", world->JointByName("J1")->ParentName()); + EXPECT_EQ("child_model::L", world->JointByName("J2")->ChildName()); + EXPECT_EQ("parent_model::L", world->JointByName("J2")->ParentName()); + + std::string resolvedLinkName; + EXPECT_TRUE( + world->JointByName("J1")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("child_model::L", resolvedLinkName); + EXPECT_TRUE( + world->JointByName("J1")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("parent_model::L", resolvedLinkName); + EXPECT_TRUE( + world->JointByName("J2")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("child_model::L", resolvedLinkName); + EXPECT_TRUE( + world->JointByName("J2")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("parent_model::L", resolvedLinkName); + + EXPECT_TRUE(world->JointByName("J1")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->JointByName("J2")->PoseRelativeTo().empty()); + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), world->JointByName("J1")->RawPose()); + EXPECT_EQ(Pose(10, 0, 0, 0, 0, 0), world->JointByName("J2")->RawPose()); + + EXPECT_EQ(1u, world->FrameCount()); + EXPECT_NE(nullptr, world->FrameByIndex(0)); + EXPECT_EQ(nullptr, world->FrameByIndex(1)); + + ASSERT_TRUE(world->FrameNameExists("child_frame")); + + EXPECT_EQ(Pose(0, 1, 0, 0, 0, 0), + world->FrameByName("child_frame")->RawPose()); + + // Test Resolve to get each model, joint and frame pose in world frame. + Pose pose; + EXPECT_TRUE( + world->ModelByName("parent_model")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), pose); + EXPECT_TRUE( + world->ModelByName("child_model")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J1")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(1, 1, 10, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J2")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(10, 0, 10, 0, 0, 0), pose); + EXPECT_TRUE( + world->FrameByName("child_frame")-> + SemanticPose().Resolve(pose, "world").empty()); + EXPECT_EQ(Pose(0, 1, 10, 0, 0, 0), pose); + + // joint frame relative to parent and child models + EXPECT_TRUE( + world->JointByName("J1")-> + SemanticPose().Resolve(pose, "child_frame").empty()); + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J1")-> + SemanticPose().Resolve(pose, "parent_model::L").empty()); + EXPECT_EQ(Pose(1, 1, 9, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J2")-> + SemanticPose().Resolve(pose, "child_model::L").empty()); + EXPECT_EQ(Pose(10, 0, 0, 0, 0, 0), pose); + EXPECT_TRUE( + world->JointByName("J2")-> + SemanticPose().Resolve(pose, "parent_model::L").empty()); + EXPECT_EQ(Pose(10, 0, 9, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(DOMJoint, WorldJointInvalidChildWorld) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "world_joint_invalid_child_world.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "Joint with name[J2] specified invalid child link [world]")); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadJointPoseRelativeTo) { @@ -531,7 +662,7 @@ TEST(DOMJoint, LoadJointPoseRelativeTo) EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), model->JointByName("J1")->RawPose()); EXPECT_EQ(Pose(0, 0, 2, 0, 0, 0), model->JointByName("J2")->RawPose()); - // Test ResolveFrame to get each link and joint pose in the model frame. + // Test Resolve to get each link and joint pose in the model frame. Pose pose; EXPECT_TRUE( model->LinkByName("P1")-> @@ -710,7 +841,7 @@ TEST(DOMJoint, LoadLinkJointSameName16Valid) EXPECT_EQ(Pose(0, 0, 3, 0, 0, 0), model->JointByName("attachment_joint")->RawPose()); - // Test ResolveFrame to get each link and joint pose in the model frame. + // Test Resolve to get each link and joint pose in the model frame. Pose pose; EXPECT_TRUE( model->LinkByName("base")-> diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 4182c136e..ec9181c4e 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -375,6 +375,17 @@ TEST(NestedModel, NestedInclude) ASSERT_NE(nullptr, upperJoint1); ASSERT_NE(nullptr, upperJoint2); ASSERT_NE(nullptr, upperJoint3); + // test World::JointByName + EXPECT_EQ(lowerJoint1, world->JointByName(name + "::lower_joint")); + EXPECT_EQ(lowerJoint2, world->JointByName( + "include_with_rotation::" + name + "::lower_joint")); + EXPECT_EQ(lowerJoint3, world->JointByName( + "include_with_rotation_1.4::" + name + "_14::lower_joint")); + EXPECT_EQ(upperJoint1, world->JointByName(name + "::upper_joint")); + EXPECT_EQ(upperJoint2, world->JointByName( + "include_with_rotation::" + name + "::upper_joint")); + EXPECT_EQ(upperJoint3, world->JointByName( + "include_with_rotation_1.4::" + name + "_14::upper_joint")); const sdf::JointAxis *lowerAxis1 = lowerJoint1->Axis(0); const sdf::JointAxis *lowerAxis2 = lowerJoint2->Axis(0); diff --git a/test/sdf/world_joint_child_frame.sdf b/test/sdf/world_joint_child_frame.sdf new file mode 100644 index 000000000..9d0bc95b9 --- /dev/null +++ b/test/sdf/world_joint_child_frame.sdf @@ -0,0 +1,32 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 0 10 0 0 0 + + + + 0 1 0 0 0 0 + + + 1 0 0 0 0 0 + parent_model::L + child_frame + + + 10 0 0 0 0 0 + parent_model::L + child_model::L + + + diff --git a/test/sdf/world_joint_invalid_child_world.sdf b/test/sdf/world_joint_invalid_child_world.sdf new file mode 100644 index 000000000..6502eabc2 --- /dev/null +++ b/test/sdf/world_joint_invalid_child_world.sdf @@ -0,0 +1,28 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 1 0 0 0 0 + + + 1 0 0 0 0 0 + parent_model::L + child_frame + + + 10 0 0 0 0 0 + parent_model::L + world + + + diff --git a/test/sdf/world_joint_parent_frame.sdf b/test/sdf/world_joint_parent_frame.sdf new file mode 100644 index 000000000..6418bd8cc --- /dev/null +++ b/test/sdf/world_joint_parent_frame.sdf @@ -0,0 +1,32 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 0 10 0 0 0 + + + + 0 1 0 0 0 0 + + + 1 0 0 0 0 0 + parent_frame + child_model::L + + + 10 0 0 0 0 0 + parent_model::L + child_model::L + + + From 1c2c2afd969cfb991d07fdddbca09ac363032c76 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 18 Oct 2022 13:40:37 -0700 Subject: [PATCH 04/31] sdf/1.10/joint.sdf: add screw_thread_pitch (#1168) This clarifies the definition (including units and sign convention) of the //joint/thread_pitch parameter based on the interpretation of this parameter in gazebo-classic, fixing #1125. This also adds a new //joint/screw_thread_pitch element with units of meters / revolution that is positive for right-handed threads, and deprecates //joint/thread_pitch in SDFormat 1.10. C++ and python APIs for getting and setting the thread pitch using the new units / sign convention are added as well, while preserving the functionality of the existing API. Signed-off-by: Steve Peters --- Migration.md | 2 ++ include/sdf/Joint.hh | 23 +++++++++--- python/src/sdf/pyJoint.cc | 12 +++++-- python/test/pyJoint_TEST.py | 8 ++++- sdf/1.10/joint.sdf | 20 +++++++++-- sdf/1.4/joint.sdf | 7 +++- sdf/1.5/joint.sdf | 7 +++- sdf/1.6/joint.sdf | 7 +++- sdf/1.7/joint.sdf | 7 +++- sdf/1.8/joint.sdf | 7 +++- sdf/1.9/joint.sdf | 7 +++- src/Joint.cc | 32 ++++++++++++++--- src/Joint_TEST.cc | 10 +++++- test/integration/joint_dom.cc | 43 ++++++++++++++++++++++ test/sdf/joint_screw_thread_pitch.sdf | 52 +++++++++++++++++++++++++++ 15 files changed, 224 insertions(+), 20 deletions(-) create mode 100644 test/sdf/joint_screw_thread_pitch.sdf diff --git a/Migration.md b/Migration.md index 3bda44b1c..5fbe44b91 100644 --- a/Migration.md +++ b/Migration.md @@ -567,6 +567,8 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. + `//limit/velocity`: `inf` (formerly `-1`) + `//limit/effort`: `inf` (formerly `-1`) +1. **joint.sdf**: thread_pitch is deprecated in favor of screw_thread_pitch. + 1. **plugin.sdf**: name attribute is now optional with empty default value. ### Removals diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 3826e709b..26c4b226f 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -203,12 +203,27 @@ namespace sdf /// \param[in] _frame The name of the pose relative-to frame. public: void SetPoseRelativeTo(const std::string &_frame); - /// \brief Get the thread pitch (only valid for screw joints) - /// \return The thread pitch + /// \brief Get the displacement along the joint axis for each complete + /// revolution around the joint axis (only valid for screw joints). + /// \return The thread pitch with units of meters per revolution with a + /// positive value corresponding to a right-handed thread. + public: double ScrewThreadPitch() const; + + /// \brief Set the thread pitch (only valid for screw joints). + /// \param[in] _threadPitch The thread pitch with units of meters per + /// revolution with a positive value corresponding to a right-handed thread. + public: void SetScrewThreadPitch(double _threadPitch); + + /// \brief Get the thread pitch in gazebo-classic format (only valid for + /// screw joints). This will be deprecated in a future version. + /// \return The thread pitch with units of radians / meters and a positive + /// value coresponding to a left-handed thread. public: double ThreadPitch() const; - /// \brief Set the thread pitch (only valid for screw joints) - /// \param[in] _threadPitch The thread pitch of the joint + /// \brief Set the thread pitch in gazebo-classic format (only valid for + /// screw joints). This will be deprecated in a future version. + /// \param[in] _threadPitch The thread pitch with units of radians / meters + /// and a positive value coresponding to a left-handed thread. public: void SetThreadPitch(double _threadPitch); /// \brief Get a pointer to the SDF element that was used during diff --git a/python/src/sdf/pyJoint.cc b/python/src/sdf/pyJoint.cc index 3ea9b48e6..4d9199b93 100644 --- a/python/src/sdf/pyJoint.cc +++ b/python/src/sdf/pyJoint.cc @@ -92,10 +92,18 @@ void defineJoint(pybind11::object module) "Set the name of the coordinate frame relative to which this " "object's pose is expressed. An empty value indicates that the frame " "is relative to the child link frame.") + .def("screw_thread_pitch", &sdf::Joint::ScrewThreadPitch, + "Get the thread pitch in meters per revolution with a positive value " + "for right-handed threads (only valid for screw joints)") + .def("set_screw_thread_pitch", &sdf::Joint::SetScrewThreadPitch, + "Set the thread pitch in meters per revolution with a positive value " + "for right-handed threads (only valid for screw joints)") .def("thread_pitch", &sdf::Joint::ThreadPitch, - "Get the thread pitch (only valid for screw joints)") + "Get the thread pitch in gazebo-classic format (only valid for screw " + "joints)") .def("set_thread_pitch", &sdf::Joint::SetThreadPitch, - "Set the thread pitch (only valid for screw joints)") + "Get the thread pitch in gazebo-classic format (only valid for screw " + "joints)") .def("semantic_pose", &sdf::Joint::SemanticPose, "Get SemanticPose object of this object to aid in resolving " "poses.") diff --git a/python/test/pyJoint_TEST.py b/python/test/pyJoint_TEST.py index f8577b93c..578fc4145 100644 --- a/python/test/pyJoint_TEST.py +++ b/python/test/pyJoint_TEST.py @@ -98,10 +98,16 @@ def test_default_construction(self): self.assertEqual(axis.xyz(), joint.axis(0).xyz()) self.assertEqual(axis1.xyz(), joint.axis(1).xyz()) - self.assertAlmostEqual(1.0, joint.thread_pitch()) + # Default thread pitch + self.assertAlmostEqual(1.0, joint.screw_thread_pitch()) + self.assertAlmostEqual(-2*math.pi, joint.thread_pitch()) threadPitch = 0.1 + joint.set_screw_thread_pitch(threadPitch) + self.assertAlmostEqual(threadPitch, joint.screw_thread_pitch()) + self.assertAlmostEqual(-2*math.pi / threadPitch, joint.thread_pitch()) joint.set_thread_pitch(threadPitch) self.assertAlmostEqual(threadPitch, joint.thread_pitch()) + self.assertAlmostEqual(-2*math.pi / threadPitch, joint.screw_thread_pitch()) self.assertEqual(0, joint.sensor_count()) self.assertEqual(None, joint.sensor_by_index(0)) diff --git a/sdf/1.10/joint.sdf b/sdf/1.10/joint.sdf index e60090c9d..b757614e9 100644 --- a/sdf/1.10/joint.sdf +++ b/sdf/1.10/joint.sdf @@ -36,8 +36,24 @@ Parameter for gearbox joints. Gearbox ratio is enforced over two joint angles. First joint angle (theta_1) is the angle from the gearbox_reference_body to the parent link in the direction of the axis element and the second joint angle (theta_2) is the angle from the gearbox_reference_body to the child link in the direction of the axis2 element. - - Parameter for screw joints. + + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + The parameter is now deprecated in favor of `screw_thread_pitch`. + + + + + + A parameter for screw joint kinematics, representing the + axial distance traveled for each revolution of the joint, + with units of meters / revolution with a positive value corresponding + to a right-handed thread. + This parameter supersedes `thread_pitch`. + diff --git a/sdf/1.4/joint.sdf b/sdf/1.4/joint.sdf index 4ff4e1137..b919223ad 100644 --- a/sdf/1.4/joint.sdf +++ b/sdf/1.4/joint.sdf @@ -31,7 +31,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.5/joint.sdf b/sdf/1.5/joint.sdf index d6719b4a7..c8c9cee50 100644 --- a/sdf/1.5/joint.sdf +++ b/sdf/1.5/joint.sdf @@ -36,7 +36,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.6/joint.sdf b/sdf/1.6/joint.sdf index 21bfb7d97..33538429e 100644 --- a/sdf/1.6/joint.sdf +++ b/sdf/1.6/joint.sdf @@ -37,7 +37,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.7/joint.sdf b/sdf/1.7/joint.sdf index b4a94b0f6..9d704af27 100644 --- a/sdf/1.7/joint.sdf +++ b/sdf/1.7/joint.sdf @@ -37,7 +37,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.8/joint.sdf b/sdf/1.8/joint.sdf index efea1d1ec..d64981657 100644 --- a/sdf/1.8/joint.sdf +++ b/sdf/1.8/joint.sdf @@ -37,7 +37,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/sdf/1.9/joint.sdf b/sdf/1.9/joint.sdf index d472feefa..2ebea2c71 100644 --- a/sdf/1.9/joint.sdf +++ b/sdf/1.9/joint.sdf @@ -37,7 +37,12 @@ - Parameter for screw joints. + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + diff --git a/src/Joint.cc b/src/Joint.cc index d4574a308..f27405c86 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -52,7 +52,8 @@ class sdf::Joint::Implementation /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; - /// \brief Thread pitch for screw joints. + /// \brief Thread pitch for screw joints in meters / revolution with a + /// positive value for right-handed threads. public: double threadPitch = 1.0; /// \brief Joint axis @@ -179,7 +180,18 @@ Errors Joint::Load(ElementPtr _sdf) errors.insert(errors.end(), axisErrors.begin(), axisErrors.end()); } - this->dataPtr->threadPitch = _sdf->Get("thread_pitch", 1.0).first; + if (_sdf->HasElement("screw_thread_pitch")) + { + // Prefer the screw_thread_pitch parameter if available. + this->dataPtr->threadPitch = _sdf->Get("screw_thread_pitch"); + } + else if (_sdf->HasElement("thread_pitch")) + { + // If thread_pitch is available, convert to meters / revolution + // and fix sign. + this->dataPtr->threadPitch = -2*GZ_PI / _sdf->Get("thread_pitch"); + } + // Otherwise the default value of threadPitch will be used // Read the type std::pair typePair = _sdf->Get("type", ""); @@ -426,17 +438,29 @@ sdf::SemanticPose Joint::SemanticPose() const } ///////////////////////////////////////////////// -double Joint::ThreadPitch() const +double Joint::ScrewThreadPitch() const { return this->dataPtr->threadPitch; } ///////////////////////////////////////////////// -void Joint::SetThreadPitch(double _threadPitch) +void Joint::SetScrewThreadPitch(double _threadPitch) { this->dataPtr->threadPitch = _threadPitch; } +///////////////////////////////////////////////// +double Joint::ThreadPitch() const +{ + return -2*GZ_PI / this->dataPtr->threadPitch; +} + +///////////////////////////////////////////////// +void Joint::SetThreadPitch(double _threadPitch) +{ + this->dataPtr->threadPitch = -2*GZ_PI / _threadPitch; +} + ///////////////////////////////////////////////// sdf::ElementPtr Joint::Element() const { diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index 50d4682e6..f4c23abf9 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -119,10 +119,18 @@ TEST(DOMJoint, Construction) EXPECT_EQ(axis.Xyz(), joint.Axis(0)->Xyz()); EXPECT_EQ(axis1.Xyz(), joint.Axis(1)->Xyz()); - EXPECT_DOUBLE_EQ(1.0, joint.ThreadPitch()); + // Default thread pitch + EXPECT_DOUBLE_EQ(1.0, joint.ScrewThreadPitch()); + EXPECT_DOUBLE_EQ(-2*GZ_PI, joint.ThreadPitch()); + + // Set and check thread pitch const double threadPitch = 0.1; + joint.SetScrewThreadPitch(threadPitch); + EXPECT_DOUBLE_EQ(threadPitch, joint.ScrewThreadPitch()); + EXPECT_DOUBLE_EQ(-2*GZ_PI / threadPitch, joint.ThreadPitch()); joint.SetThreadPitch(threadPitch); EXPECT_DOUBLE_EQ(threadPitch, joint.ThreadPitch()); + EXPECT_DOUBLE_EQ(-2*GZ_PI / threadPitch, joint.ScrewThreadPitch()); EXPECT_EQ(0u, joint.SensorCount()); EXPECT_EQ(nullptr, joint.SensorByIndex(0)); diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 0587457b6..bf415de82 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -158,6 +158,49 @@ TEST(DOMJoint, Complete) } } +////////////////////////////////////////////////// +TEST(DOMJoint, ScrewThreadPitch) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "joint_screw_thread_pitch.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + + // Get the first model + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + + EXPECT_EQ(2u, model->LinkCount()); + EXPECT_TRUE(model->LinkNameExists("child_link")); + EXPECT_TRUE(model->LinkNameExists("parent_link")); + + EXPECT_EQ(5u, model->JointCount()); + ASSERT_TRUE(model->JointNameExists("default_param")); + ASSERT_TRUE(model->JointNameExists("both_params")); + ASSERT_TRUE(model->JointNameExists("new_param")); + ASSERT_TRUE(model->JointNameExists("old_param")); + ASSERT_TRUE(model->JointNameExists("param_precedence")); + + EXPECT_DOUBLE_EQ(1.0, + model->JointByName("default_param")->ScrewThreadPitch()); + EXPECT_DOUBLE_EQ(0.5, model->JointByName("both_params")->ScrewThreadPitch()); + EXPECT_DOUBLE_EQ(0.5, model->JointByName("new_param")->ScrewThreadPitch()); + EXPECT_NEAR(0.5, model->JointByName("old_param")->ScrewThreadPitch(), 1e-3); + EXPECT_DOUBLE_EQ(0.5, + model->JointByName("param_precedence")->ScrewThreadPitch()); + + EXPECT_DOUBLE_EQ(-2*GZ_PI, + model->JointByName("default_param")->ThreadPitch()); + EXPECT_NEAR(-12.566, model->JointByName("both_params")->ThreadPitch(), 1e-3); + EXPECT_NEAR(-12.566, model->JointByName("new_param")->ThreadPitch(), 1e-3); + EXPECT_DOUBLE_EQ(-12.566, model->JointByName("old_param")->ThreadPitch()); + EXPECT_NEAR(-12.566, model->JointByName("param_precedence")->ThreadPitch(), + 1e-3); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadJointParentWorld) { diff --git a/test/sdf/joint_screw_thread_pitch.sdf b/test/sdf/joint_screw_thread_pitch.sdf new file mode 100644 index 000000000..c3f22271c --- /dev/null +++ b/test/sdf/joint_screw_thread_pitch.sdf @@ -0,0 +1,52 @@ + + + + + 0 0 1 0 0 0 + + + 0 0 10 0 0 0 + + + + 0 0 1 + + child_link + parent_link + + + + 0 0 1 + + child_link + parent_link + 0.5 + -12.566 + + + + 0 0 1 + + child_link + parent_link + 0.5 + + + + 0 0 1 + + child_link + parent_link + -12.566 + + + + 0 0 1 + + child_link + parent_link + 0.5 + 0.5 + + + From c703deef9d29d036585695c688d0c57f9226833c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 20 Oct 2022 11:33:11 -0700 Subject: [PATCH 05/31] Check World joints in checkJointParentChildNames (#1189) This refactors the checkModelJointParentChildNames lambda function into a template function that works for both sdf::Model and sdf::World objects, after adding the World::NameExistsInFrameAttachedToGraph API. An example world and test is included. Signed-off-by: Steve Peters --- include/sdf/World.hh | 12 ++ python/src/sdf/pyModel.cc | 2 +- python/src/sdf/pyWorld.cc | 4 + python/test/pyModel_TEST.py | 4 + python/test/pyWorld_TEST.py | 4 + src/Model_TEST.cc | 7 + src/World.cc | 10 ++ src/World_TEST.cc | 7 + src/parser.cc | 138 +++++++++--------- test/integration/joint_dom.cc | 26 +++- ..._invalid_resolved_parent_same_as_child.sdf | 27 ++++ 11 files changed, 172 insertions(+), 69 deletions(-) create mode 100644 test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf diff --git a/include/sdf/World.hh b/include/sdf/World.hh index d99dc4992..0afc85472 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -487,6 +487,18 @@ namespace sdf public: sdf::ElementPtr ToElement( const OutputConfig &_config = OutputConfig::GlobalConfig()) const; + /// \brief Check if a given name exists in the FrameAttachedTo graph at the + /// scope of the world. + /// \param[in] _name Name of the implicit or explicit frame to check. + /// To check for a frame in a nested model, prefix the frame name with + /// the sequence of nested models containing this frame, delimited by "::". + /// \return True if the frame name is found in the FrameAttachedTo graph. + /// False otherwise, or if the frame graph is invalid. + /// \note This function assumes the world has a valid FrameAttachedTo graph. + /// It will return false if the graph is invalid. + public: bool NameExistsInFrameAttachedToGraph( + const std::string &_name) const; + /// \brief Get the plugins attached to this object. /// \return A vector of Plugin, which will be empty if there are no /// plugins. diff --git a/python/src/sdf/pyModel.cc b/python/src/sdf/pyModel.cc index e38d3b17c..fce94d0b9 100644 --- a/python/src/sdf/pyModel.cc +++ b/python/src/sdf/pyModel.cc @@ -175,7 +175,7 @@ void defineModel(pybind11::object module) .def("name_exists_in_frame_attached_to_graph", &sdf::Model::NameExistsInFrameAttachedToGraph, "Check if a given name exists in the FrameAttachedTo graph at the " - "scope of the model..") + "scope of the model.") .def("add_link", &sdf::Model::AddLink, "Add a link to the model.") .def("add_joint", &sdf::Model::AddJoint, diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index bc2619877..41b930fc4 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -107,6 +107,10 @@ void defineWorld(pybind11::object module) "index.") .def("model_name_exists", &sdf::World::ModelNameExists, "Get whether a model name exists.") + .def("name_exists_in_frame_attached_to_graph", + &sdf::World::NameExistsInFrameAttachedToGraph, + "Check if a given name exists in the FrameAttachedTo graph at the " + "scope of the world.") .def("add_model", &sdf::World::AddModel, "Add a model to the world.") // .def("add_actor", &sdf::World::AddActor, diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index ce8d7e5b4..126c1b579 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -145,6 +145,10 @@ def test_default_construction(self): self.assertEqual(errors[1].message(), "PoseRelativeToGraph error: scope does not point to a valid graph.") + # model doesn't have graphs, so no names should exist in graphs + self.assertFalse(model.name_exists_in_frame_attached_to_graph("")); + self.assertFalse(model.name_exists_in_frame_attached_to_graph("link")); + def test_copy_construction(self): model = Model() diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 63fe26a01..08c62ab12 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -86,6 +86,10 @@ def test_default_construction(self): self.assertEqual(errors[1].message(), "PoseRelativeToGraph error: scope does not point to a valid graph.") + # world doesn't have graphs, so no names should exist in graphs + self.assertFalse(world.name_exists_in_frame_attached_to_graph("")); + self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")); + def test_copy_construction(self): world = World() diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 0da219e48..14e26bf5c 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -154,6 +154,10 @@ TEST(DOMModel, Construction) errors[1].Message().find( "PoseRelativeToGraph error: scope does not point to a valid graph")) << errors[1]; + + // model doesn't have graphs, so no names should exist in graphs + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("link")); } ///////////////////////////////////////////////// @@ -229,6 +233,9 @@ TEST(DOMModel, AddLink) EXPECT_EQ(1u, model.LinkCount()); EXPECT_FALSE(model.AddLink(link)); EXPECT_EQ(1u, model.LinkCount()); + // model doesn't have graphs, so no names should exist in graphs + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("link1")); model.ClearLinks(); EXPECT_EQ(0u, model.LinkCount()); diff --git a/src/World.cc b/src/World.cc index 05dccde19..aacb571c9 100644 --- a/src/World.cc +++ b/src/World.cc @@ -1091,6 +1091,16 @@ void World::ClearFrames() this->dataPtr->frames.clear(); } +///////////////////////////////////////////////// +bool World::NameExistsInFrameAttachedToGraph(const std::string &_name) const +{ + if (!this->dataPtr->frameAttachedToGraph) + return false; + + return this->dataPtr->frameAttachedToGraph.VertexIdByName(_name) + != gz::math::graph::kNullId; +} + ///////////////////////////////////////////////// bool World::AddModel(const Model &_model) { diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 88679e4e5..9e9174a4f 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -93,6 +93,10 @@ TEST(DOMWorld, Construction) EXPECT_NE(std::string::npos, errors[1].Message().find( "PoseRelativeToGraph error: scope does not point to a valid graph")); + + // world doesn't have graphs, so no names should exist in graphs + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("model1")); } ///////////////////////////////////////////////// @@ -384,6 +388,9 @@ TEST(DOMWorld, AddModel) EXPECT_EQ(1u, world.ModelCount()); EXPECT_FALSE(world.AddModel(model)); EXPECT_EQ(1u, world.ModelCount()); + // world doesn't have graphs, so no names should exist in graphs + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("model1")); world.ClearModels(); EXPECT_EQ(0u, world.ModelCount()); diff --git a/src/parser.cc b/src/parser.cc index f5cd07786..218e9239e 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -2502,87 +2502,92 @@ bool checkJointParentChildNames(const sdf::Root *_root) } ////////////////////////////////////////////////// -void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) +template +void checkScopedJointParentChildNames( + const TPtr _scope, const std::string &_scopeType, Errors &errors) { - auto checkModelJointParentChildNames = []( - const sdf::Model *_model, Errors &errors) -> void + for (uint64_t j = 0; j < _scope->JointCount(); ++j) { - for (uint64_t j = 0; j < _model->JointCount(); ++j) - { - auto joint = _model->JointByIndex(j); + auto joint = _scope->JointByIndex(j); - const std::string &parentName = joint->ParentName(); - const std::string parentLocalName = sdf::SplitName(parentName).second; + const std::string &parentName = joint->ParentName(); + const std::string parentLocalName = sdf::SplitName(parentName).second; - if (parentName != "world" && parentLocalName != "__model__" && - !_model->NameExistsInFrameAttachedToGraph(parentName)) - { - errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, - "parent frame with name[" + parentName + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); - } + if (parentName != "world" && parentLocalName != "__model__" && + !_scope->NameExistsInFrameAttachedToGraph(parentName)) + { + errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, + "parent frame with name[" + parentName + + "] specified by joint with name[" + joint->Name() + + "] not found in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - const std::string &childName = joint->ChildName(); - const std::string childLocalName = sdf::SplitName(childName).second; - if (childName == "world") - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "invalid child name[world] specified by joint with name[" + - joint->Name() + "] in model with name[" + _model->Name() + "]."}); - } + const std::string &childName = joint->ChildName(); + const std::string childLocalName = sdf::SplitName(childName).second; + if (childName == "world") + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "invalid child name[world] specified by joint with name[" + + joint->Name() + "] in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - if (childLocalName != "__model__" && - !_model->NameExistsInFrameAttachedToGraph(childName)) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "child frame with name[" + childName + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); - } + if (childLocalName != "__model__" && + !_scope->NameExistsInFrameAttachedToGraph(childName)) + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "child frame with name[" + childName + + "] specified by joint with name[" + joint->Name() + + "] not found in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - if (childName == joint->Name()) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] must not specify its own name as the child frame."}); - } + if (childName == joint->Name()) + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] must not specify its own name as the child frame."}); + } - if (parentName == joint->Name()) - { - errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] must not specify its own name as the parent frame."}); - } + if (parentName == joint->Name()) + { + errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] must not specify its own name as the parent frame."}); + } - // Check that parent and child frames resolve to different links - std::string resolvedChildName; - std::string resolvedParentName; + // Check that parent and child frames resolve to different links + std::string resolvedChildName; + std::string resolvedParentName; - auto resolveErrors = joint->ResolveChildLink(resolvedChildName); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + auto resolveErrors = joint->ResolveChildLink(resolvedChildName); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - resolveErrors = joint->ResolveParentLink(resolvedParentName); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + resolveErrors = joint->ResolveParentLink(resolvedParentName); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - if (resolvedChildName == resolvedParentName) - { - errors.push_back({ErrorCode::JOINT_PARENT_SAME_AS_CHILD, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] specified parent frame [" + parentName + - "] and child frame [" + childName + - "] that both resolve to [" + resolvedChildName + - "], but they should resolve to different values."}); - } + if (resolvedChildName == resolvedParentName) + { + errors.push_back({ErrorCode::JOINT_PARENT_SAME_AS_CHILD, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] specified parent frame [" + parentName + + "] and child frame [" + childName + + "] that both resolve to [" + resolvedChildName + + "], but they should resolve to different values."}); } - }; + } +} +////////////////////////////////////////////////// +void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) +{ if (_root->Model()) { - checkModelJointParentChildNames(_root->Model(), _errors); + checkScopedJointParentChildNames(_root->Model(), "model", _errors); } for (uint64_t w = 0; w < _root->WorldCount(); ++w) @@ -2591,8 +2596,9 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) for (uint64_t m = 0; m < world->ModelCount(); ++m) { auto model = world->ModelByIndex(m); - checkModelJointParentChildNames(model, _errors); + checkScopedJointParentChildNames(model, "model", _errors); } + checkScopedJointParentChildNames(world, "world", _errors); } } diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index bf415de82..552d91ad0 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -530,7 +530,8 @@ TEST(DOMJoint, LoadWorldJointChildFrame) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; using Pose = gz::math::Pose3d; @@ -645,13 +646,34 @@ TEST(DOMJoint, WorldJointInvalidChildWorld) auto errors = root.Load(testFile); for (auto e : errors) std::cout << e << std::endl; - ASSERT_EQ(2u, errors.size()); + ASSERT_EQ(3u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( "Joint with name[J2] specified invalid child link [world]")); } +///////////////////////////////////////////////// +TEST(DOMJoint, WorldJointInvalidResolvedParentSameAsChild) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", + "world_joint_invalid_resolved_parent_same_as_child.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + std::cerr << errors << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_PARENT_SAME_AS_CHILD); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "joint with name[J] in world with name[" + "joint_invalid_resolved_parent_same_as_child.sdf] specified parent " + "frame [child_model] and child frame [child_frame] that both resolve " + "to [child_model::L]")); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadJointPoseRelativeTo) { diff --git a/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf b/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf new file mode 100644 index 000000000..aabdd0ad4 --- /dev/null +++ b/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf @@ -0,0 +1,27 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 0 10 0 0 0 + + + + 0 1 0 0 0 0 + + + 10 0 0 0 0 0 + child_model + child_frame + + + From 5558b7ce887fa86804fcf9e4545af8c2b7be233b Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 20 Oct 2022 12:13:11 -0700 Subject: [PATCH 06/31] Added Root::WorldByName (#1121) Signed-off-by: Nate Koenig Co-authored-by: Addisu Z. Taddese Co-authored-by: Steve Peters --- include/sdf/Root.hh | 14 ++++++++++++++ src/Root.cc | 21 +++++++++++++++++++++ src/Root_TEST.cc | 26 ++++++++++++++++++++++++++ 3 files changed, 61 insertions(+) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 11e14502c..1d88defcc 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -146,6 +146,20 @@ namespace sdf /// \sa uint64_t WorldCount() const public: World *WorldByIndex(const uint64_t _index); + /// \brief Get a world based on a name. + /// \param[in] _name Name of the world. + /// \return Pointer to the world. Nullptr if a world with the given name + /// does not exist. + /// \sa bool WorldNameExists(const std::string &_name) const + public: const World *WorldByName(const std::string &_name) const; + + /// \brief Get a world based on a name. + /// \param[in] _name Name of the world. + /// \return Pointer to the world. Nullptr if a world with the given name + /// does not exist. + /// \sa bool WorldNameExists(const std::string &_name) const + public: World *WorldByName(const std::string &_name); + /// \brief Get whether a world name exists. /// \param[in] _name Name of the world to check. /// \return True if there exists a world with the given name. diff --git a/src/Root.cc b/src/Root.cc index 118d783c0..facd460c2 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -435,6 +435,27 @@ bool Root::WorldNameExists(const std::string &_name) const return false; } +///////////////////////////////////////////////// +const World *Root::WorldByName(const std::string &_name) const +{ + for (const sdf::World &w : this->dataPtr->worlds) + { + if (w.Name() == _name) + { + return &w; + } + } + + return nullptr; +} + +///////////////////////////////////////////////// +World *Root::WorldByName(const std::string &_name) +{ + return const_cast( + static_cast(this)->WorldByName(_name)); +} + ///////////////////////////////////////////////// const Model *Root::Model() const { diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 7988b9ecf..fd169a7c3 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -35,6 +35,7 @@ TEST(DOMRoot, Construction) EXPECT_EQ(nullptr, root.Element()); EXPECT_EQ(SDF_VERSION, root.Version()); EXPECT_FALSE(root.WorldNameExists("default")); + EXPECT_TRUE(root.WorldByName("default") == nullptr); EXPECT_FALSE(root.WorldNameExists("")); EXPECT_EQ(0u, root.WorldCount()); EXPECT_TRUE(root.WorldByIndex(0) == nullptr); @@ -66,6 +67,7 @@ TEST(DOMRoot, MoveAssignmentOperator) EXPECT_EQ("test_root", root2.Version()); } +///////////////////////////////////////////////// TEST(DOMRoot, WorldNamesFromFile) { const auto path = sdf::testing::TestFile("sdf", "basic_shapes.sdf"); @@ -604,3 +606,27 @@ TEST(DOMRoot, CopyConstructor) testFrame1(root3); } } + +///////////////////////////////////////////////// +TEST(DOMRoot, WorldByName) +{ + sdf::Root root; + EXPECT_EQ(0u, root.WorldCount()); + + sdf::World world; + world.SetName("world1"); + EXPECT_TRUE(root.AddWorld(world).empty()); + EXPECT_EQ(1u, root.WorldCount()); + + EXPECT_TRUE(root.WorldNameExists("world1")); + const sdf::World *wPtr = root.WorldByName("world1"); + EXPECT_NE(nullptr, wPtr); + + // Modify the world + sdf::World *w = root.WorldByName("world1"); + ASSERT_NE(nullptr, w); + EXPECT_EQ("world1", w->Name()); + w->SetName("world2"); + ASSERT_TRUE(root.WorldNameExists("world2")); + EXPECT_EQ("world2", root.WorldByName("world2")->Name()); +} From b5fb1ea374bcd2934a39cfab708b87fc63845e7c Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Thu, 20 Oct 2022 21:14:04 +0200 Subject: [PATCH 07/31] Rename python bindings import library for Windows (#1165) The naming collision is caused by the both the import library of sdformat library and the import library of the pybind11 bindings library are called sdformat13.lib https://github.com/gazebosim/sdformat/issues/1150 Thanks to Martin Pecka and Silvio Traversaro. Signed-off-by: Jose Luis Rivero Co-authored-by: Steve Peters --- python/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e536c749f..cad2adff0 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -98,6 +98,12 @@ target_compile_definitions(${BINDINGS_MODULE_NAME} PRIVATE set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES OUTPUT_NAME "sdformat${PROJECT_VERSION_MAJOR}") +# Deal with naming collision on Windows. It is caused by the both the import +# library of sdformat library and the import library of the pybind11 bindings +# library are called sdformat13.lib. +# See https://github.com/gazebosim/sdformat/issues/1150 +set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES + ARCHIVE_OUTPUT_NAME "python_sdformat${PROJECT_VERSION_MAJOR}") configure_build_install_location(${BINDINGS_MODULE_NAME}) From 5797d03f3cbef58f8561e60ae7d6c0f33a9ded44 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 20 Oct 2022 21:15:25 +0200 Subject: [PATCH 08/31] Fix GZ_PYTHON_INSTALL_PATH value (#1183) Fix GZ_PYTHON_INSTALL_PATH value if USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION is ON and USE_DIST_PACKAGES_FOR_PYTHON is OFF. Based on code in gz-math. Signed-off-by: Silvio --- python/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index cad2adff0..c096a1dd9 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -22,6 +22,9 @@ if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) if(USE_DIST_PACKAGES_FOR_PYTHON) string(REPLACE "site-packages" "dist-packages" GZ_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) + else() + # custom cmake command is returning dist-packages + string(REPLACE "dist-packages" "site-packages" GZ_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) endif() else() # If not a system installation, respect local paths From 527fd3859e6f95241c4f31f3689f5d3698829a70 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 21 Oct 2022 04:16:21 +0900 Subject: [PATCH 09/31] Python: add OpticalFrameID APIs to pyCamera (#1184) Signed-off-by: ahcorde --- python/src/sdf/pyCamera.cc | 10 ++++++++++ python/test/pyCamera_TEST.py | 4 ++++ 2 files changed, 14 insertions(+) diff --git a/python/src/sdf/pyCamera.cc b/python/src/sdf/pyCamera.cc index 842ec23d7..87668387f 100644 --- a/python/src/sdf/pyCamera.cc +++ b/python/src/sdf/pyCamera.cc @@ -171,6 +171,16 @@ void defineCamera(pybind11::object module) "Set the name of the coordinate frame relative to which this " "object's pose is expressed. An empty value indicates that the frame " "is relative to the parent link.") + .def("optical_frame_id", &sdf::Camera::OpticalFrameId, + "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.") + .def("set_optical_frame_id", &sdf::Camera::SetOpticalFrameId, + "Set the name of the coordinate frame relative to which this " + "object's camera_info is expressed.") .def("lens_type", &sdf::Camera::LensType, "Get the lens type. This is the type of the lens mapping. " "Supported values are gnomonical, stereographic, equidistant, " diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index 40000b7f0..a842eed29 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -131,6 +131,10 @@ def test_default_construction(self): cam.set_pose_relative_to("/frame") self.assertEqual("/frame", cam.pose_relative_to()) + self.assertFalse(cam.optical_frame_id()); + cam.set_optical_frame_id("/optical_frame"); + self.assertEqual("/optical_frame", cam.optical_frame_id()); + self.assertEqual("stereographic", cam.lens_type()) cam.set_lens_type("custom") self.assertEqual("custom", cam.lens_type()) From 2d203ccc5b96feee44c02d12538fbccbdded00d4 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 20 Oct 2022 14:57:07 -0700 Subject: [PATCH 10/31] Prepare for 13.2.0 release (#1191) Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 24 ++++++++++++++++++++++++ 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4c2d65423..f4e6ee7b3 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.1.0) +project (sdformat13 VERSION 13.2.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software diff --git a/Changelog.md b/Changelog.md index 365932c3e..df4e2fa3d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,29 @@ ## libsdformat 13.X +### libsdformat 13.2.0 (2022-10-20) + +1. sdf/1.10/joint.sdf: add `screw_thread_pitch` + * [Pull request #1168](https://github.com/gazebosim/sdformat/pull/1168) + +1. sdf/1.10: support //world/joint specification + * [Pull request #1117](https://github.com/gazebosim/sdformat/pull/1117) + * [Pull request #1189](https://github.com/gazebosim/sdformat/pull/1189) + +1. Model: add sdf::Errors output to API methods + * [Pull request #1122](https://github.com/gazebosim/sdformat/pull/1122) + +1. Added Root::WorldByName + * [Pull request #1121](https://github.com/gazebosim/sdformat/pull/1121) + +1. Python: add OpticalFrameID APIs to pyCamera + * [Pull request #1184](https://github.com/gazebosim/sdformat/pull/1184) + +1. Fix `GZ_PYTHON_INSTALL_PATH` value + * [Pull request #1183](https://github.com/gazebosim/sdformat/pull/1183) + +1. Rename python bindings import library for Windows + * [Pull request #1165](https://github.com/gazebosim/sdformat/pull/1165) + ### libsdformat 13.1.0 (2022-10-12) 1. Add test helper python package for encapsulating versioned python packages From 9374f0ed9df05328b02c3ad0c518e842465532b9 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 4 Nov 2022 20:48:54 -0700 Subject: [PATCH 11/31] Fix static URDF models with fixed joints (#1193) * Add test for static URDF model with fixed joints This demonstrates a bug that prevents models with fixed joint reduction from being declared as static. * SDFExtension: add isSetStaticFlag variable Add a boolean flag to indicate when setStaticFlag has been set, since all the other variables in SDFExtension have these flags. * SDFExtension: follow rule of zero Delete destructor and copy constructor. Signed-off-by: Steve Peters --- src/SDFExtension.cc | 50 +------------------- src/SDFExtension.hh | 8 +--- src/parser_urdf.cc | 18 ++++--- test/integration/fixed_joint_static.urdf | 38 +++++++++++++++ test/integration/urdf_gazebo_extensions.cc | 41 ++++++++++++++++ test/integration/urdf_gazebo_extensions.urdf | 5 ++ 6 files changed, 97 insertions(+), 63 deletions(-) create mode 100644 test/integration/fixed_joint_static.urdf diff --git a/src/SDFExtension.cc b/src/SDFExtension.cc index cd72f41fb..53016779a 100644 --- a/src/SDFExtension.cc +++ b/src/SDFExtension.cc @@ -25,6 +25,7 @@ SDFExtension::SDFExtension() this->material.clear(); this->visual_blobs.clear(); this->collision_blobs.clear(); + this->isSetStaticFlag = false; this->setStaticFlag = false; this->isGravity = false; this->gravity = true; @@ -67,52 +68,3 @@ SDFExtension::SDFExtension() this->provideFeedback = false; this->implicitSpringDamper = false; } - -///////////////////////////////////////////////// -SDFExtension::SDFExtension(const SDFExtension &_ge) -{ - this->material = _ge.material; - this->visual_blobs = _ge.visual_blobs; - this->collision_blobs = _ge.collision_blobs; - this->setStaticFlag = _ge.setStaticFlag; - this->isGravity = _ge.isGravity; - this->gravity = _ge.gravity; - this->isDampingFactor = _ge.isDampingFactor; - this->isMaxContacts = _ge.isMaxContacts; - this->isMaxVel = _ge.isMaxVel; - this->isMinDepth = _ge.isMinDepth; - this->fdir1 = _ge.fdir1; - this->isMu1 = _ge.isMu1; - this->isMu2 = _ge.isMu2; - this->isKp = _ge.isKp; - this->isKd = _ge.isKd; - this->selfCollide = _ge.selfCollide; - this->isLaserRetro = _ge.isLaserRetro; - this->isSpringReference = _ge.isSpringReference; - this->isSpringStiffness = _ge.isSpringStiffness; - this->isStopCfm = _ge.isStopCfm; - this->isStopErp = _ge.isStopErp; - this->isFudgeFactor = _ge.isFudgeFactor; - this->isProvideFeedback = _ge.isProvideFeedback; - this->isImplicitSpringDamper = _ge.isImplicitSpringDamper; - this->provideFeedback = _ge.provideFeedback; - this->implicitSpringDamper = _ge.implicitSpringDamper; - this->oldLinkName = _ge.oldLinkName; - this->reductionTransform = _ge.reductionTransform; - this->blobs = _ge.blobs; - - this->dampingFactor = _ge.dampingFactor; - this->maxContacts = _ge.maxContacts; - this->maxVel = _ge.maxVel; - this->minDepth = _ge.minDepth; - this->mu1 = _ge.mu1; - this->mu2 = _ge.mu2; - this->kp = _ge.kp; - this->kd = _ge.kd; - this->laserRetro = _ge.laserRetro; - this->springReference = _ge.springReference; - this->springStiffness = _ge.springStiffness; - this->stopCfm = _ge.stopCfm; - this->stopErp = _ge.stopErp; - this->fudgeFactor = _ge.fudgeFactor; -} diff --git a/src/SDFExtension.hh b/src/SDFExtension.hh index b48d104f0..47a4203c3 100644 --- a/src/SDFExtension.hh +++ b/src/SDFExtension.hh @@ -42,13 +42,6 @@ namespace sdf /// \brief Constructor public: SDFExtension(); - /// \brief Copy constructor - /// \param[in] _ge SDFExtension to copy. - public: SDFExtension(const SDFExtension &_ge); - - /// \brief Destructor - public: virtual ~SDFExtension() = default; - // for reducing fixed joints and removing links public: std::string oldLinkName; public: ignition::math::Pose3d reductionTransform; @@ -87,6 +80,7 @@ namespace sdf public: std::vector > collision_blobs; // body, default off + public: bool isSetStaticFlag; public: bool setStaticFlag; public: bool isGravity; public: bool gravity; diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index e6f07def2..871daf1a9 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -1349,6 +1349,7 @@ void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml) else if (childElem->ValueStr() == "static") { std::string valueStr = GetKeyValueAsString(childElem); + sdf->isSetStaticFlag = true; // default of setting static flag is false if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" || @@ -2284,14 +2285,17 @@ void InsertSDFExtensionRobot(TiXmlElement *_elem) for (std::vector::iterator ge = sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge) { - // insert static flag - if ((*ge)->setStaticFlag) + if ((*ge)->isSetStaticFlag) { - AddKeyValue(_elem, "static", "true"); - } - else - { - AddKeyValue(_elem, "static", "false"); + // insert static flag + if ((*ge)->setStaticFlag) + { + AddKeyValue(_elem, "static", "true"); + } + else + { + AddKeyValue(_elem, "static", "false"); + } } // copy extension containing blobs and without reference diff --git a/test/integration/fixed_joint_static.urdf b/test/integration/fixed_joint_static.urdf new file mode 100644 index 000000000..8058ea6c8 --- /dev/null +++ b/test/integration/fixed_joint_static.urdf @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index b75ea6f2c..1c08c8483 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -39,6 +39,10 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) auto modelDom = root.ModelByIndex(0); ASSERT_NE(nullptr, modelDom); + + // Verify that model is not static + EXPECT_FALSE(modelDom->Static()); + sdf::ElementPtr model = modelDom->Element(); for (sdf::ElementPtr joint = model->GetElement("joint"); joint; joint = joint->GetNextElement("joint")) @@ -398,6 +402,43 @@ TEST(SDFParser, FixedJointSimple) auto model = root.ModelByIndex(0); ASSERT_NE(nullptr, model); EXPECT_EQ("fixed_joint_simple", model->Name()); + EXPECT_FALSE(model->Static()); + + 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")); +} + +///////////////////////////////////////////////// +TEST(SDFParser, FixedJointStatic) +{ + const std::string urdfTestFile = + sdf::testing::TestFile("integration", "fixed_joint_static.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_TRUE(model->Static()); EXPECT_EQ(1u, model->LinkCount()); EXPECT_TRUE(model->LinkNameExists("base")); diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index bfbde44e2..9f29e3464 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -1,6 +1,11 @@ + + + 0 + + From 7d312cb6da0bb7541f4a3c24b4105c453e589397 Mon Sep 17 00:00:00 2001 From: Ben Wolsieffer Date: Wed, 9 Nov 2022 02:22:19 -0500 Subject: [PATCH 12/31] Don't assume CMAKE_INSTALL_*DIR variables are relative (#1190) The CMAKE_INSTALL_*DIR variables may be absolute, in which case they must not be concatenated with CMAKE_INSTALL_PREFIX. If an absolute path is needed, CMAKE_INSTALL_FULL_*DIR must be used. In other cases, more complex logic may be required to make packages relocatable in the case that CMAKE_INSTALL_*DIR is relative. This patch replaces the custom LIB_INSTALL_DIR and BIN_INSTALL_DIR variables with the standard GNUInstallDirs variables throughout the code. The custom variables are still honored if specified manually. Signed-off-by: Ben Wolsieffer Co-authored-by: Steve Peters --- CMakeLists.txt | 30 +++++++++++++++++++++--------- cmake/SDFUtils.cmake | 8 ++++---- cmake/sdf_config.cmake.in | 8 ++++---- cmake/sdformat_pc.in | 4 ++-- 4 files changed, 31 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 80ed65a9a..aaa19b051 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,13 +70,20 @@ if(LD_LIBRARY_PATH) endif() set (INCLUDE_INSTALL_DIR "${CMAKE_INSTALL_INCLUDEDIR}/sdformat-${SDF_VERSION}/sdf") -set (LIB_INSTALL_DIR ${CMAKE_INSTALL_LIBDIR} CACHE STRING "Installation directory for libraries (relative to CMAKE_INSTALL_PREFIX)") -set (BIN_INSTALL_DIR ${CMAKE_INSTALL_BINDIR} CACHE STRING "Installation directory for binaries (relative to CMAKE_INSTALL_PREFIX)") set (USE_FULL_RPATH OFF CACHE BOOL "Set to true to enable full rpath") +set (LIB_INSTALL_DIR "" CACHE STRING "Installation directory for libraries (relative to CMAKE_INSTALL_PREFIX)") +set (BIN_INSTALL_DIR "" CACHE STRING "Installation directory for binaries (relative to CMAKE_INSTALL_PREFIX)") +if (NOT LIB_INSTALL_DIR STREQUAL "") + set(CMAKE_INSTALL_LIBDIR "${LIB_INSTALL_DIR}") +endif() +if (NOT BIN_INSTALL_DIR STREQUAL "") + set(CMAKE_INSTALL_BINDIR "${BIN_INSTALL_DIR}") +endif() + set(PKG_NAME SDFormat) set(sdf_target sdformat${SDF_MAJOR_VERSION}) -set(sdf_config_install_dir "${LIB_INSTALL_DIR}/cmake/${PROJECT_NAME_LOWER}/") +set(sdf_config_install_dir "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME_LOWER}/") set(sdf_import_target_name ${PROJECT_EXPORT_NAME}::${sdf_target}) set(sdf_target_output_filename "${sdf_target}-targets.cmake") @@ -89,16 +96,16 @@ if (USE_FULL_RPATH) # (but later on when installing) set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}") + set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_FULL_LIBDIR}") # add the automatically determined parts of the RPATH # which point to directories outside the build tree to the install RPATH set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) # the RPATH to be used when installing, but only if its not a system directory - list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}" isSystemDir) + list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_FULL_LIBDIR}" isSystemDir) if("${isSystemDir}" STREQUAL "-1") - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}") + set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_FULL_LIBDIR}") endif("${isSystemDir}" STREQUAL "-1") endif() @@ -304,12 +311,16 @@ else (build_errors) ######################################## # Make the package config file - set(PC_CONFIG_INSTALL_DIR ${LIB_INSTALL_DIR}/pkgconfig) + set(PC_CONFIG_INSTALL_DIR ${CMAKE_INSTALL_FULL_LIBDIR}/pkgconfig) file(RELATIVE_PATH PC_CONFIG_RELATIVE_PATH_TO_PREFIX - "${CMAKE_INSTALL_PREFIX}/${PC_CONFIG_INSTALL_DIR}" + "${PC_CONFIG_INSTALL_DIR}" "${CMAKE_INSTALL_PREFIX}" ) + string(REPLACE "${CMAKE_INSTALL_PREFIX}" "\${prefix}" + SDFORMAT_PKG_CONFIG_LIBDIR "${CMAKE_INSTALL_FULL_LIBDIR}") + string(REPLACE "${CMAKE_INSTALL_PREFIX}" "\${prefix}" + SDFORMAT_PKG_CONFIG_INCLUDEDIR "${CMAKE_INSTALL_FULL_INCLUDEDIR}") 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 @@ -325,7 +336,7 @@ else (build_errors) set(sdf_config_input "${CMAKE_CURRENT_SOURCE_DIR}/cmake/sdf_config.cmake.in") set(sdf_config_output "cmake/${sdf_target}-config.cmake") set(sdf_version_output "cmake/${sdf_target}-config-version.cmake") - set(sdf_config_install_dir "${LIB_INSTALL_DIR}/cmake/${PROJECT_NAME_LOWER}/") + set(sdf_config_install_dir "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME_LOWER}/") #-------------------------------------- # Configure and install the config file @@ -333,6 +344,7 @@ else (build_errors) ${sdf_config_input} ${sdf_config_output} INSTALL_DESTINATION ${sdf_config_install_dir} + PATH_VARS CMAKE_INSTALL_LIBDIR CMAKE_INSTALL_INCLUDEDIR NO_CHECK_REQUIRED_COMPONENTS_MACRO) #-------------------------------------- diff --git a/cmake/SDFUtils.cmake b/cmake/SDFUtils.cmake index df9931deb..6521c9eaf 100644 --- a/cmake/SDFUtils.cmake +++ b/cmake/SDFUtils.cmake @@ -79,9 +79,9 @@ macro (sdf_install_library _name) install ( TARGETS ${_name} EXPORT ${_name} - ARCHIVE DESTINATION ${LIB_INSTALL_DIR} - LIBRARY DESTINATION ${LIB_INSTALL_DIR} - RUNTIME DESTINATION ${BIN_INSTALL_DIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} COMPONENT shlib) # Export and install target @@ -101,7 +101,7 @@ endmacro () ################################################# macro (sdf_install_executable _name) set_target_properties(${_name} PROPERTIES VERSION ${SDF_VERSION_FULL}) - install (TARGETS ${_name} DESTINATION ${BIN_INSTALL_DIR}) + install (TARGETS ${_name} DESTINATION ${CMAKE_INSTALL_BINDIR}) endmacro () ################################################# diff --git a/cmake/sdf_config.cmake.in b/cmake/sdf_config.cmake.in index e51abeefd..357e359e5 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 "${PACKAGE_PREFIX_DIR}/include/sdformat-@SDF_VERSION@") +list(APPEND @PKG_NAME@_INCLUDE_DIRS "@PACKAGE_CMAKE_INSTALL_INCLUDEDIR@/sdformat-@SDF_VERSION@") -list(APPEND @PKG_NAME@_CFLAGS "-I${PACKAGE_PREFIX_DIR}/include/sdformat-@SDF_VERSION@") +list(APPEND @PKG_NAME@_CFLAGS "-I@PACKAGE_CMAKE_INSTALL_INCLUDEDIR@/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 "${PACKAGE_PREFIX_DIR}/@CMAKE_INSTALL_LIBDIR@") +list(APPEND @PKG_NAME@_LIBRARY_DIRS "@PACKAGE_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${PACKAGE_PREFIX_DIR}/@LIB_INSTALL_DIR@") +list(APPEND @PKG_NAME@_LDFLAGS "-L@PACKAGE_CMAKE_INSTALL_LIBDIR@") diff --git a/cmake/sdformat_pc.in b/cmake/sdformat_pc.in index 9ccfa4fbc..1e8f59bdd 100644 --- a/cmake/sdformat_pc.in +++ b/cmake/sdformat_pc.in @@ -1,6 +1,6 @@ prefix=${pcfiledir}/@PC_CONFIG_RELATIVE_PATH_TO_PREFIX@ -libdir=${prefix}/@LIB_INSTALL_DIR@ -includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ +libdir=@SDFORMAT_PKG_CONFIG_LIBDIR@ +includedir=@SDFORMAT_PKG_CONFIG_INCLUDEDIR@ Name: SDF Description: Robot Modeling Language (SDF) From 255c59e1cca0eaf7b9d5f7e7df61fc9cbe948cfd Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 9 Nov 2022 11:45:19 -0800 Subject: [PATCH 13/31] Prepare for 9.9.1 Release (#1197) Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aaa19b051..667d45658 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ set (SDF_PROTOCOL_VERSION 1.7) set (SDF_MAJOR_VERSION 9) set (SDF_MINOR_VERSION 9) -set (SDF_PATCH_VERSION 0) +set (SDF_PATCH_VERSION 1) set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}) set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}) diff --git a/Changelog.md b/Changelog.md index a6b42f213..3a4a0f23f 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,13 @@ ## libsdformat 9.X +### libsdformat 9.9.1 (2022-11-08) + +1. Fix static URDF models with fixed joints + * [Pull request #1193](https://github.com/gazebosim/sdformat/pull/1193) + +1. Don't assume `CMAKE_INSTALL_*DIR` variables are relative + * [Pull request #1190](https://github.com/gazebosim/sdformat/pull/1190) + ### libsdformat 9.9.0 (2022-09-07) 1. sdf/camera.sdf: fields for projection matrix From 7c89e64e0a5757d15f1c447aca72ebf65c1f3f75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 14 Nov 2022 20:01:08 +0100 Subject: [PATCH 14/31] Added camera info topic to Camera (#1198) * Added camera info topic to Camera Signed-off-by: ahcorde --- include/sdf/Camera.hh | 8 ++++++++ sdf/1.7/camera.sdf | 4 ++++ src/Camera.cc | 18 ++++++++++++++++++ src/Camera_TEST.cc | 4 ++++ test/integration/link_dom.cc | 1 + test/sdf/sensors.sdf | 1 + 6 files changed, 36 insertions(+) diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 4a3274ff8..7605e2e93 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -105,6 +105,14 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(ElementPtr _sdf); + /// \brief Get the camera info topic + /// \return Topic for the camera info + public: std::string CameraInfoTopic() const; + + /// \brief Set the camera info topic + /// \param[in] _cameraInfoTopic Topic for the camera info. + public: void SetCameraInfoTopic(const std::string& _cameraInfoTopic); + /// \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/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf index de99908c2..88005cb10 100644 --- a/sdf/1.7/camera.sdf +++ b/sdf/1.7/camera.sdf @@ -5,6 +5,10 @@ An optional name for the camera. + + Name of the camera info + + Horizontal field of view diff --git a/src/Camera.cc b/src/Camera.cc index c089810d4..cd4e0deef 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -51,6 +51,9 @@ class sdf::CameraPrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; + /// \brief Camera info topic. + public: std::string cameraInfoTopic = ""; + /// \brief Name of the camera. public: std::string name = ""; @@ -253,6 +256,9 @@ Errors Camera::Load(ElementPtr _sdf) // Read the camera's name loadName(_sdf, this->dataPtr->name); + this->dataPtr->cameraInfoTopic = _sdf->Get("camera_info_topic", + this->dataPtr->cameraInfoTopic).first; + this->dataPtr->hfov = _sdf->Get("horizontal_fov", this->dataPtr->hfov).first; @@ -431,6 +437,18 @@ Errors Camera::Load(ElementPtr _sdf) return errors; } +///////////////////////////////////////////////// +std::string Camera::CameraInfoTopic() const +{ + return this->dataPtr->cameraInfoTopic; +} + +///////////////////////////////////////////////// +void Camera::SetCameraInfoTopic(const std::string &_cameraInfoTopic) +{ + this->dataPtr->cameraInfoTopic = _cameraInfoTopic; +} + ///////////////////////////////////////////////// sdf::ElementPtr Camera::Element() const { diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 56711e087..1b9680c91 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -26,6 +26,10 @@ TEST(DOMCamera, Construction) cam.SetName("my_camera"); EXPECT_EQ("my_camera", cam.Name()); + EXPECT_EQ("", cam.CameraInfoTopic()); + cam.SetCameraInfoTopic("/camera/camera_info"); + EXPECT_EQ("/camera/camera_info", cam.CameraInfoTopic()); + EXPECT_DOUBLE_EQ(1.047, cam.HorizontalFov().Radian()); cam.SetHorizontalFov(1.45); EXPECT_DOUBLE_EQ(1.45, cam.HorizontalFov().Radian()); diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 7806cbe62..b2d225390 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -273,6 +273,7 @@ TEST(DOMLink, Sensors) EXPECT_EQ(ignition::math::Pose3d(0.1, 0.2, 0.3, 0, 0, 0), camSensor->RawPose()); EXPECT_DOUBLE_EQ(0.75, camSensor->HorizontalFov().Radian()); + EXPECT_EQ("/camera_sensor/camera_info", camSensor->CameraInfoTopic()); EXPECT_EQ(640u, camSensor->ImageWidth()); EXPECT_EQ(480u, camSensor->ImageHeight()); EXPECT_EQ(sdf::PixelFormatType::RGB_INT8, camSensor->PixelFormat()); diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index 8ea6ef043..81a071b25 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -25,6 +25,7 @@ false 0.1 0.2 0.3 0 0 0 + /camera_sensor/camera_info .75 640 From c6282bda4521bf0d37079cb0cca86543ffa15681 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Nov 2022 13:29:20 +0100 Subject: [PATCH 15/31] Updated camera to sdf definitions and update ToElement Signed-off-by: ahcorde --- sdf/1.8/camera.sdf | 4 ++++ sdf/1.9/camera.sdf | 4 ++++ src/Camera.cc | 6 ++++++ src/Camera_TEST.cc | 6 ++++++ 4 files changed, 20 insertions(+) diff --git a/sdf/1.8/camera.sdf b/sdf/1.8/camera.sdf index ec0bcafaa..fb33b088a 100644 --- a/sdf/1.8/camera.sdf +++ b/sdf/1.8/camera.sdf @@ -5,6 +5,10 @@ An optional name for the camera. + + Name of the camera info + + Horizontal field of view diff --git a/sdf/1.9/camera.sdf b/sdf/1.9/camera.sdf index 708e1d6eb..3c7f6ee95 100644 --- a/sdf/1.9/camera.sdf +++ b/sdf/1.9/camera.sdf @@ -5,6 +5,10 @@ An optional name for the camera. + + Name of the camera info + + If the camera will be triggered by a topic diff --git a/src/Camera.cc b/src/Camera.cc index 4741edcea..ba075ae8c 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -1212,6 +1212,12 @@ sdf::ElementPtr Camera::ToElement() const imageElem->GetElement("format")->Set(this->PixelFormatStr()); imageElem->GetElement("anti_aliasing")->Set( this->AntiAliasingValue()); + elem->GetElement("camera_info_topic")->Set( + this->CameraInfoTopic()); + elem->GetElement("trigger_topic")->Set( + this->TriggerTopic()); + elem->GetElement("triggered")->Set( + this->Triggered()); sdf::ElementPtr clipElem = elem->GetElement("clip"); clipElem->GetElement("near")->Set(this->NearClip()); diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 9ba232544..cc64e01a1 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -273,6 +273,9 @@ TEST(DOMCamera, ToElement) cam.SetSaveFrames(true); cam.SetSaveFramesPath("/tmp"); cam.SetOpticalFrameId("/optical_frame"); + cam.SetCameraInfoTopic("/camera_info_test"); + cam.SetTriggerTopic("/trigger_topic_test"); + cam.SetTriggered(true); sdf::ElementPtr camElem = cam.ToElement(); EXPECT_NE(nullptr, camElem); @@ -293,6 +296,9 @@ TEST(DOMCamera, ToElement) EXPECT_TRUE(cam2.SaveFrames()); EXPECT_EQ("/tmp", cam2.SaveFramesPath()); EXPECT_EQ("/optical_frame", cam2.OpticalFrameId()); + EXPECT_EQ("/camera_info_test", cam2.CameraInfoTopic()); + EXPECT_EQ("/trigger_topic_test", cam2.TriggerTopic()); + EXPECT_TRUE(cam2.Triggered()); // make changes to DOM and verify ToElement produces updated values cam2.SetNearClip(0.33); From 74fe572194ccf2c945d82d6337ac0a5b57d4789e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Nov 2022 13:55:10 +0100 Subject: [PATCH 16/31] Add camera info topic to 1.10 and Python interfaces Signed-off-by: ahcorde --- python/src/sdf/pyCamera.cc | 4 ++++ python/test/pyCamera_TEST.py | 4 ++++ sdf/1.10/camera.sdf | 4 ++++ src/Camera.cc | 2 +- 4 files changed, 13 insertions(+), 1 deletion(-) diff --git a/python/src/sdf/pyCamera.cc b/python/src/sdf/pyCamera.cc index 87668387f..32bac95d3 100644 --- a/python/src/sdf/pyCamera.cc +++ b/python/src/sdf/pyCamera.cc @@ -42,6 +42,10 @@ void defineCamera(pybind11::object module) "Get the name of the camera.") .def("set_name", &sdf::Camera::SetName, "Set the name of the camera.") + .def("set_camera_info_topic", &sdf::Camera::SetCameraInfoTopic, + "Set the camera info topic") + .def("camera_info_topic", &sdf::Camera::CameraInfoTopic, + "Get the camera info topic.") .def("triggered", &sdf::Camera::Triggered, "Get whether the camera is triggered by a topic.") .def("set_triggered", &sdf::Camera::SetTriggered, diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index a842eed29..e21e3f4dc 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -35,6 +35,10 @@ def test_default_construction(self): cam.set_trigger_topic("my_camera/trigger") self.assertEqual("my_camera/trigger", cam.trigger_topic()) + self.assertEqual("", cam.camera_info_topic()); + cam.set_camera_info_topic("/camera/camera_info"); + self.assertEqual("/camera/camera_info", cam.camera_info_topic()); + self.assertAlmostEqual(1.047, cam.horizontal_fov().radian()) cam.set_horizontal_fov(Angle(1.45)) self.assertAlmostEqual(1.45, cam.horizontal_fov().radian()) diff --git a/sdf/1.10/camera.sdf b/sdf/1.10/camera.sdf index 708e1d6eb..08f406218 100644 --- a/sdf/1.10/camera.sdf +++ b/sdf/1.10/camera.sdf @@ -9,6 +9,10 @@ If the camera will be triggered by a topic + + Name of the camera info + + Name of the topic that will trigger the camera if enabled diff --git a/src/Camera.cc b/src/Camera.cc index d957b3a77..ba20484ec 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -258,7 +258,7 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->cameraInfoTopic = _sdf->Get("camera_info_topic", this->dataPtr->cameraInfoTopic).first; - this->dataPtr->hfov = _sdf->Get("horizontal_fov", + this->dataPtr->hfov = _sdf->Get("horizontal_fov", this->dataPtr->hfov).first; // Read the distortion From 438e05b7aab79923b4fc6a84eaffb286e0d8bb0b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 16 Nov 2022 15:55:47 -0800 Subject: [PATCH 17/31] Check JointAxis expressed-in values during Load (#1195) Currently the //axis*/xyz/@expressed_in values are not checked that they refer to valid frames. This adds a check in Root::Load just after checking that the Joint parent and child names are valid. The JOINT_AXIS_EXPRESSED_IN_INVALID Error type is added and used when checking //xyz/@expressed_in. * Add test for invalid @expressed_in values Signed-off-by: Steve Peters Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- include/sdf/Error.hh | 4 ++ include/sdf/parser.hh | 9 +++ src/Root.cc | 3 + src/parser.cc | 58 ++++++++++++++++++++ test/integration/joint_axis_dom.cc | 20 +++++++ test/sdf/joint_axis_invalid_expressed_in.sdf | 16 ++++++ 6 files changed, 110 insertions(+) create mode 100644 test/sdf/joint_axis_invalid_expressed_in.sdf diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index b3c58f2a1..68896241a 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -168,6 +168,10 @@ namespace sdf /// \brief Generic warning saved as error due to WarningsPolicy config WARNING, + + /// \brief The joint axis expressed-in value does not match the name of an + /// existing frame in the current scope. + JOINT_AXIS_EXPRESSED_IN_INVALID, }; class SDFORMAT_VISIBLE Error diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index 03df56462..31864a9fd 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -416,6 +416,15 @@ namespace sdf SDFORMAT_VISIBLE void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors); + /// \brief Check that all joint axes in contained joints specify xyz + /// expressed-in names that match the names of valid frames. + /// This checks recursively and should check the files exhaustively + /// rather than terminating early when the first error is found. + /// \param[in] _root SDF Root object to check recursively. + /// \param[out] _errors Detected errors will be appended to this variable. + SDFORMAT_VISIBLE + void checkJointAxisExpressedInValues(const sdf::Root *_root, Errors &_errors); + /// \brief For the world and each model, check that the attached_to graphs /// build without errors and have no cycles. /// Confirm that following directed edges from each vertex in the graph diff --git a/src/Root.cc b/src/Root.cc index facd460c2..7e57e5673 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -386,6 +386,9 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) // different frames. checkJointParentChildNames(this, errors); + // Check that //axis*/xyz/@expressed_in values specify valid frames. + checkJointAxisExpressedInValues(this, errors); + return errors; } diff --git a/src/parser.cc b/src/parser.cc index 218e9239e..a82e37e12 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -28,6 +28,7 @@ #include "sdf/Filesystem.hh" #include "sdf/Frame.hh" #include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" #include "sdf/Link.hh" #include "sdf/Model.hh" #include "sdf/Param.hh" @@ -2602,6 +2603,63 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) } } +////////////////////////////////////////////////// +template +void checkScopedJointAxisExpressedInValues( + const TPtr _scope, const std::string &_scopeType, Errors &errors) +{ + for (uint64_t j = 0; j < _scope->JointCount(); ++j) + { + auto joint = _scope->JointByIndex(j); + for (uint64_t a = 0; a < 2; ++a) + { + auto axis = joint->Axis(a); + if (axis) + { + const std::string &xyzExpressedIn = axis->XyzExpressedIn(); + const std::string xyzExpressedInLocal = + sdf::SplitName(xyzExpressedIn).second; + + // If a frame name is specfied, check that the frame exists. + if (!xyzExpressedIn.empty() && + !_scope->NameExistsInFrameAttachedToGraph(xyzExpressedIn)) + { + errors.push_back({ErrorCode::JOINT_AXIS_EXPRESSED_IN_INVALID, + "axis xyz expressed-in frame with name[" + xyzExpressedIn + + "] specified by joint with name[" + joint->Name() + + "] not found in " + _scopeType + " with name[" + _scope->Name() + + "]."}); + } + + // Also try resolving the axis to its default frame. + gz::math::Vector3d xyz; + auto xyzErrors = axis->ResolveXyz(xyz); + errors.insert(errors.end(), xyzErrors.begin(), xyzErrors.end()); + } + } + } +} + +////////////////////////////////////////////////// +void checkJointAxisExpressedInValues(const sdf::Root *_root, Errors &_errors) +{ + if (_root->Model()) + { + checkScopedJointAxisExpressedInValues(_root->Model(), "model", _errors); + } + + for (uint64_t w = 0; w < _root->WorldCount(); ++w) + { + auto world = _root->WorldByIndex(w); + for (uint64_t m = 0; m < world->ModelCount(); ++m) + { + auto model = world->ModelByIndex(m); + checkScopedJointAxisExpressedInValues(model, "model", _errors); + } + checkScopedJointAxisExpressedInValues(world, "world", _errors); + } +} + ////////////////////////////////////////////////// bool shouldValidateElement(sdf::ElementPtr _elem) { diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc index b1c102e28..d8986adfa 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -219,6 +219,26 @@ TEST(DOMJointAxis, XyzExpressedIn) EXPECT_EQ(nullptr, model->FrameByIndex(0)); } +///////////////////////////////////////////////// +TEST(DOMJointAxis, InvalidExpressedIn) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "joint_axis_invalid_expressed_in.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_AXIS_EXPRESSED_IN_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "axis xyz expressed-in frame with name[invalid] specified by joint with" + " name[joint] not found in model with" + " name[joint_axis_invalid_expressed_in]")); +} + ////////////////////////////////////////////////// TEST(DOMJointAxis, InfiniteLimits) { diff --git a/test/sdf/joint_axis_invalid_expressed_in.sdf b/test/sdf/joint_axis_invalid_expressed_in.sdf new file mode 100644 index 000000000..c9524f6cd --- /dev/null +++ b/test/sdf/joint_axis_invalid_expressed_in.sdf @@ -0,0 +1,16 @@ + + + + + 0 0 1 0 0 0 + + + 0 0 3 0 0 0 + world + link + + 0 0 1 + + + + From dc88f3a2d2f5e915c9f5a975da5c0b2ac339e706 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 17 Nov 2022 09:42:50 +0100 Subject: [PATCH 18/31] Change default camera_info_topic value to __default__ (#1201) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- sdf/1.7/camera.sdf | 2 +- src/Camera.cc | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/sdf/1.7/camera.sdf b/sdf/1.7/camera.sdf index 88005cb10..9bb88c875 100644 --- a/sdf/1.7/camera.sdf +++ b/sdf/1.7/camera.sdf @@ -5,7 +5,7 @@ An optional name for the camera. - + Name of the camera info diff --git a/src/Camera.cc b/src/Camera.cc index cd4e0deef..aa390fc11 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -258,6 +258,8 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->cameraInfoTopic = _sdf->Get("camera_info_topic", this->dataPtr->cameraInfoTopic).first; + if (this->dataPtr->cameraInfoTopic == "__default__") + this->dataPtr->cameraInfoTopic = ""; this->dataPtr->hfov = _sdf->Get("horizontal_fov", this->dataPtr->hfov).first; From ba97ad3e0072299c8befd90ab125151afe454cde Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 17 Nov 2022 11:43:02 -0800 Subject: [PATCH 19/31] Prepare for 9.10.0~pre1 prerelease (#1202) Signed-off-by: Steve Peters --- CMakeLists.txt | 6 +++--- Changelog.md | 6 ++++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 667d45658..04bda9d4f 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 9) -set (SDF_PATCH_VERSION 1) +set (SDF_MINOR_VERSION 10) +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 3a4a0f23f..5089fc688 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,11 @@ ## libsdformat 9.X +### libsdformat 9.10.0 (2022-11-17) + +1. Added camera info topic to Camera + * [Pull request #1198](https://github.com/gazebosim/sdformat/pull/1198) + * [Pull request #1201](https://github.com/gazebosim/sdformat/pull/1201) + ### libsdformat 9.9.1 (2022-11-08) 1. Fix static URDF models with fixed joints From 8fcbe1ee938421957d0edea95889d98d8868191e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 21 Nov 2022 18:24:28 +0100 Subject: [PATCH 20/31] Added HasLensProjection (#1203) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- include/sdf/Camera.hh | 4 ++++ python/src/sdf/pyCamera.cc | 2 ++ python/test/pyCamera_TEST.py | 1 + src/Camera.cc | 27 ++++++++++++++++++++++++++- src/Camera_TEST.cc | 1 + 5 files changed, 34 insertions(+), 1 deletion(-) diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 5c3b75c92..5c46827c8 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -566,6 +566,10 @@ namespace sdf /// \return True if the camera has instrinsics values set, false otherwise public: bool HasLensIntrinsics() const; + /// \brief Get whether or not the camera has projection values set + /// \return True if the camera has projection values set, false otherwise + public: bool HasLensProjection() const; + /// \brief Create and return an SDF element filled with data from this /// camera. /// Note that parameter passing functionality is not captured with this diff --git a/python/src/sdf/pyCamera.cc b/python/src/sdf/pyCamera.cc index 32bac95d3..e91683d5e 100644 --- a/python/src/sdf/pyCamera.cc +++ b/python/src/sdf/pyCamera.cc @@ -267,6 +267,8 @@ void defineCamera(pybind11::object module) "Set the visibility mask of a camera") .def("has_lens_intrinsics", &sdf::Camera::HasLensIntrinsics, "Get whether or not the camera has instrinsics values set") + .def("has_lens_projection", &sdf::Camera::HasLensProjection, + "Get whether or not the camera has proejction values set") .def("__copy__", [](const sdf::Camera &self) { return sdf::Camera(self); }) diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index e21e3f4dc..6daca4cbe 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -196,6 +196,7 @@ def test_default_construction(self): self.assertAlmostEqual(2.3, cam.lens_intrinsics_skew()) self.assertTrue(cam.has_lens_intrinsics()) + self.assertFalse(cam.has_lens_projection()) self.assertEqual(4294967295, cam.visibility_mask()) cam.set_visibility_mask(123) diff --git a/src/Camera.cc b/src/Camera.cc index ba20484ec..c0bc3cc85 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -208,6 +208,9 @@ class sdf::Camera::Implementation /// \brief lens instrinsics s. public: double lensIntrinsicsS{1.0}; + /// \brief True if this camera has custom projection values + public: bool hasProjection = false; + /// \brief True if this camera has custom intrinsics values public: bool hasIntrinsics = false; @@ -441,7 +444,7 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->lensProjectionTx).first; this->dataPtr->lensProjectionTy = projection->Get("ty", this->dataPtr->lensProjectionTy).first; - + this->dataPtr->hasProjection = true; } } @@ -1055,6 +1058,7 @@ double Camera::LensProjectionFx() const void Camera::SetLensProjectionFx(double _fx_p) { this->dataPtr->lensProjectionFx = _fx_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1067,6 +1071,7 @@ double Camera::LensProjectionFy() const void Camera::SetLensProjectionFy(double _fy_p) { this->dataPtr->lensProjectionFy = _fy_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1079,6 +1084,7 @@ double Camera::LensProjectionCx() const void Camera::SetLensProjectionCx(double _cx_p) { this->dataPtr->lensProjectionCx = _cx_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1091,6 +1097,7 @@ double Camera::LensProjectionCy() const void Camera::SetLensProjectionCy(double _cy_p) { this->dataPtr->lensProjectionCy = _cy_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1103,6 +1110,7 @@ double Camera::LensProjectionTx() const void Camera::SetLensProjectionTx(double _tx) { this->dataPtr->lensProjectionTx = _tx; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1115,6 +1123,7 @@ double Camera::LensProjectionTy() const void Camera::SetLensProjectionTy(double _ty) { this->dataPtr->lensProjectionTy = _ty; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1190,6 +1199,12 @@ bool Camera::HasLensIntrinsics() const return this->dataPtr->hasIntrinsics; } +///////////////////////////////////////////////// +bool Camera::HasLensProjection() const +{ + return this->dataPtr->hasProjection; +} + ///////////////////////////////////////////////// sdf::ElementPtr Camera::ToElement() const { @@ -1296,6 +1311,16 @@ sdf::ElementPtr Camera::ToElement() const intrinsicsElem->GetElement("cy")->Set(this->LensIntrinsicsCy()); intrinsicsElem->GetElement("s")->Set(this->LensIntrinsicsSkew()); } + if (this->HasLensProjection()) + { + sdf::ElementPtr projectionElem = lensElem->GetElement("projection"); + projectionElem->GetElement("p_fx")->Set(this->LensProjectionFx()); + projectionElem->GetElement("p_fy")->Set(this->LensProjectionFy()); + projectionElem->GetElement("p_cx")->Set(this->LensProjectionCx()); + projectionElem->GetElement("p_cy")->Set(this->LensProjectionCy()); + projectionElem->GetElement("tx")->Set(this->LensProjectionTx()); + projectionElem->GetElement("ty")->Set(this->LensProjectionTy()); + } if (this->HasSegmentationType()) { diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 9140ecc8b..68b61beac 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -219,6 +219,7 @@ TEST(DOMCamera, Construction) EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); EXPECT_TRUE(cam.HasLensIntrinsics()); + EXPECT_TRUE(cam.HasLensProjection()); EXPECT_EQ(UINT32_MAX, cam.VisibilityMask()); cam.SetVisibilityMask(123u); From 9a7c42ab2d0a0fd20773ced8c1c1f4ab137f6137 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 21 Nov 2022 18:24:28 +0100 Subject: [PATCH 21/31] Added HasLensProjection (#1203) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- include/sdf/Camera.hh | 4 ++++ src/Camera.cc | 17 ++++++++++++++++- src/Camera_TEST.cc | 1 + 3 files changed, 21 insertions(+), 1 deletion(-) diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 7605e2e93..e5d27566c 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -553,6 +553,10 @@ namespace sdf /// \param[in] _mask visibility mask public: void SetVisibilityMask(uint32_t _mask); + /// \brief Get whether or not the camera has projection values set + /// \return True if the camera has projection values set, false otherwise + public: bool HasLensProjection() const; + /// \brief Private data pointer. private: CameraPrivate *dataPtr = nullptr; }; diff --git a/src/Camera.cc b/src/Camera.cc index aa390fc11..572f882fa 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -186,6 +186,9 @@ class sdf::CameraPrivate /// \brief lens instrinsics s. public: double lensIntrinsicsS{1.0}; + /// \brief True if this camera has custom projection values + public: bool hasProjection = false; + /// \brief Visibility mask of a camera. Defaults to 0xFFFFFFFF public: uint32_t visibilityMask{4294967295u}; }; @@ -426,7 +429,7 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->lensProjectionTx).first; this->dataPtr->lensProjectionTy = projection->Get("ty", this->dataPtr->lensProjectionTy).first; - + this->dataPtr->hasProjection = true; } } @@ -972,6 +975,7 @@ double Camera::LensProjectionFx() const void Camera::SetLensProjectionFx(double _fx_p) { this->dataPtr->lensProjectionFx = _fx_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -984,6 +988,7 @@ double Camera::LensProjectionFy() const void Camera::SetLensProjectionFy(double _fy_p) { this->dataPtr->lensProjectionFy = _fy_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -996,6 +1001,7 @@ double Camera::LensProjectionCx() const void Camera::SetLensProjectionCx(double _cx_p) { this->dataPtr->lensProjectionCx = _cx_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1008,6 +1014,7 @@ double Camera::LensProjectionCy() const void Camera::SetLensProjectionCy(double _cy_p) { this->dataPtr->lensProjectionCy = _cy_p; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1020,6 +1027,7 @@ double Camera::LensProjectionTx() const void Camera::SetLensProjectionTx(double _tx) { this->dataPtr->lensProjectionTx = _tx; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1032,6 +1040,7 @@ double Camera::LensProjectionTy() const void Camera::SetLensProjectionTy(double _ty) { this->dataPtr->lensProjectionTy = _ty; + this->dataPtr->hasProjection = true; } ///////////////////////////////////////////////// @@ -1099,3 +1108,9 @@ void Camera::SetVisibilityMask(uint32_t _mask) { this->dataPtr->visibilityMask = _mask; } + +///////////////////////////////////////////////// +bool Camera::HasLensProjection() const +{ + return this->dataPtr->hasProjection; +} diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 1b9680c91..448a8fd6d 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -191,6 +191,7 @@ TEST(DOMCamera, Construction) EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); EXPECT_EQ(4294967295u, cam.VisibilityMask()); + EXPECT_TRUE(cam.HasLensProjection()); cam.SetVisibilityMask(123u); EXPECT_EQ(123u, cam.VisibilityMask()); From 702dc371e2881c8b1755823f79682f1e32df6113 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 25 Nov 2022 17:15:54 +0800 Subject: [PATCH 22/31] ParamPassing: sdfwarns to sdf::Errors when warnings policy set to sdf::EnforcementPolicy::ERR (#1135) Co-authored-by: Addisu Z. Taddese Signed-off-by: Marco A. Gutierrez --- src/ParamPassing.cc | 59 ++++++++++++------- src/ParamPassing.hh | 17 +++++- src/ParamPassing_TEST.cc | 123 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 174 insertions(+), 25 deletions(-) diff --git a/src/ParamPassing.cc b/src/ParamPassing.cc index cea6e8b77..1b80d09cf 100644 --- a/src/ParamPassing.cc +++ b/src/ParamPassing.cc @@ -24,6 +24,7 @@ #include "ParamPassing.hh" #include "parser_private.hh" +#include "Utils.hh" #include "XmlUtils.hh" namespace sdf @@ -176,11 +177,11 @@ void updateParams(const ParserConfig &_config, } else if (actionStr == "modify") { - modify(childElemXml, elem, _errors); + modify(childElemXml, _config, elem, _errors); } else if (actionStr == "remove") { - remove(childElemXml, elem, _errors); + remove(childElemXml, _config, elem, _errors); } else if (actionStr == "replace") { @@ -292,6 +293,8 @@ bool isValidAction(const std::string &_action) ////////////////////////////////////////////////// ElementPtr getElementByName(const ElementPtr _elem, const tinyxml2::XMLElement *_xml, + const sdf::ParserConfig _config, + sdf::Errors &_errors, const bool _isModifyAction) { std::string elemName = _xml->Name(); @@ -324,10 +327,14 @@ ElementPtr getElementByName(const ElementPtr _elem, else if (elem->HasAttribute("name") && elem->GetAttribute("name")->GetRequired()) { - sdfwarn << "The original element [" << elemName << "] contains the " - << "attribute 'name' but none was provided in the element modifier." - << " The assumed element to be modified is: <" << elemName - << " name='" << elem->Get("name") << "'>\n"; + std::stringstream ss; + ss << "The original element [" << elemName << "] contains the " + << "attribute 'name' but none was provided in the element modifier." + << " The assumed element to be modified is: <" << elemName + << " name='" << elem->Get("name") << "'>"; + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, _errors); } return elem; @@ -397,7 +404,7 @@ void handleIndividualChildActions(const ParserConfig &_config, if (actionStr == "remove") { - ElementPtr e = getElementByName(_elem, xmlChild); + ElementPtr e = getElementByName(_elem, xmlChild, _config, _errors); if (e == nullptr) { _errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -409,14 +416,14 @@ void handleIndividualChildActions(const ParserConfig &_config, } else { - remove(xmlChild, e, _errors); + remove(xmlChild, _config, e, _errors); } continue; } else if (actionStr == "modify") { - ElementPtr e = getElementByName(_elem, xmlChild, true); + ElementPtr e = getElementByName(_elem, xmlChild, _config, _errors, true); if (e == nullptr) { _errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -428,7 +435,7 @@ void handleIndividualChildActions(const ParserConfig &_config, } else { - modify(xmlChild, e, _errors); + modify(xmlChild, _config, e, _errors); } continue; @@ -472,7 +479,7 @@ void handleIndividualChildActions(const ParserConfig &_config, } else if (actionStr == "replace") { - ElementPtr e = getElementByName(_elem, xmlChild); + ElementPtr e = getElementByName(_elem, xmlChild, _config, _errors); if (e == nullptr) { _errors.push_back({ErrorCode::ELEMENT_MISSING, @@ -562,7 +569,8 @@ void modifyAttributes(tinyxml2::XMLElement *_xml, ////////////////////////////////////////////////// void modifyChildren(tinyxml2::XMLElement *_xml, - ElementPtr _elem, Errors &_errors) + const sdf::ParserConfig &_config, ElementPtr _elem, + Errors &_errors) { for (tinyxml2::XMLElement *xmlChild = _xml->FirstChildElement(); xmlChild; @@ -579,7 +587,8 @@ void modifyChildren(tinyxml2::XMLElement *_xml, continue; } - ElementPtr elemChild = getElementByName(_elem, xmlChild, true); + ElementPtr elemChild = getElementByName(_elem, xmlChild, + _config, _errors, true); ParamPtr paramChild = elemChild->GetValue(); if (xmlChild->GetText()) @@ -609,21 +618,26 @@ void modifyChildren(tinyxml2::XMLElement *_xml, else { // sdf has child elements but no children were specified in xml - sdfwarn << "No modifications for element " - << ElementToString(xmlChild) - << " provided, skipping modification for:\n" - << ElementToString(_xml) << "\n"; + std::stringstream ss; + ss << "No modifications for element " + << ElementToString(xmlChild) + << " provided, skipping modification for:\n" + << ElementToString(_xml); + Error err(ErrorCode::WARNING, ss.str()); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, _errors); } } else { - modify(xmlChild, elemChild, _errors); + modify(xmlChild, _config, elemChild, _errors); } } } ////////////////////////////////////////////////// -void modify(tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors) +void modify(tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + ElementPtr _elem, Errors &_errors) { modifyAttributes(_xml, _elem, _errors); @@ -643,12 +657,13 @@ void modify(tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors) else { // modify children elements - modifyChildren(_xml, _elem, _errors); + modifyChildren(_xml, _config, _elem, _errors); } } ////////////////////////////////////////////////// -void remove(const tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors) +void remove(const tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + ElementPtr _elem, Errors &_errors) { if (_xml->NoChildren()) { @@ -663,7 +678,7 @@ void remove(const tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors) xmlChild; xmlChild = xmlChild->NextSiblingElement()) { - elemChild = getElementByName(_elem, xmlChild); + elemChild = getElementByName(_elem, xmlChild, _config, _errors); if (elemChild == nullptr) { const tinyxml2::XMLElement *xmlParent = _xml->Parent()->ToElement(); diff --git a/src/ParamPassing.hh b/src/ParamPassing.hh index 1572f6dda..ac077debb 100644 --- a/src/ParamPassing.hh +++ b/src/ParamPassing.hh @@ -85,6 +85,8 @@ namespace sdf /// \param[in] _elem The sdf (parent) element used to find the matching /// child element described in xml /// \param[in] _xml The xml element to find + /// \param[in] _config Custom parser configuration. + /// \param[out] _errors Vector of errros. /// \param[in] _isModifyAction Is true if the action is modify, the /// attribute 'name' may not be in the sdf element (i.e., may be a /// modified/added attribute such as //camera) @@ -92,6 +94,8 @@ namespace sdf /// element could not be found ElementPtr getElementByName(const ElementPtr _elem, const tinyxml2::XMLElement *_xml, + const sdf::ParserConfig _config, + sdf::Errors &_errors, const bool _isModifyAction = false); /// \brief Initialize an sdf element description from the xml element @@ -143,24 +147,31 @@ namespace sdf /// \brief Modifies the children elements of the included model /// \param[in] _xml Pointer to the xml element which contains the elements /// to be modified + /// \param[in] _config Custom parser configuration. /// \param[out] _elem The element from the included model to modify + /// \param[out] _errors Vector of errors. void modifyChildren(tinyxml2::XMLElement *_xml, - ElementPtr _elem, Errors &_errors); + const sdf::ParserConfig &_config, ElementPtr _elem, + Errors &_errors); /// \brief Modifies element values and/or attributes of an element from the /// included model /// \param[in] _xml Pointer to the xml element which contains the elements /// to be modified + /// \param[in] _config Custom parser configuration. /// \param[out] _elem The element from the included model to modify /// \param[out] _errors Captures errors found during parsing - void modify(tinyxml2::XMLElement *_xml, ElementPtr _elem, Errors &_errors); + void modify(tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + ElementPtr _elem, Errors &_errors); /// \brief Removes an element specified in xml /// \param[in] _xml Pointer to the xml element(s) to be removed from _elem + /// \param[in] _config Custom parser configuration. /// \param[out] _elem The element from the included model to remove /// elements from /// \param[out] _errors Captures errors found during parsing - void remove(const tinyxml2::XMLElement *_xml, ElementPtr _elem, + void remove(const tinyxml2::XMLElement *_xml, + const sdf::ParserConfig &_config, ElementPtr _elem, Errors &_errors); /// \brief Replace an element with another element diff --git a/src/ParamPassing_TEST.cc b/src/ParamPassing_TEST.cc index f9c9a5704..00f9ff48a 100644 --- a/src/ParamPassing_TEST.cc +++ b/src/ParamPassing_TEST.cc @@ -20,6 +20,7 @@ #include "ParamPassing.hh" #include "sdf/Element.hh" #include "sdf/parser.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(ParamPassing, GetElement) @@ -91,3 +92,125 @@ TEST(ParamPassing, GetElement) "model::test_link::test_visual"); EXPECT_EQ(nullptr, paramPassElem); } + +//////////////////////////////////////// +// Test warnings outputs for GetElementByName +TEST(ParamPassing, GetElementByNameWarningOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + + sdf::Errors errors; + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::readString(stream.str(), parserConfig, sdfParsed, errors); + EXPECT_TRUE(errors.empty()); + + std::ostringstream stream2; + stream2 << " " + << " "; + tinyxml2::XMLDocument doc; + doc.Parse(stream2.str().c_str()); + + sdf::ParamPassing::getElementByName(sdfParsed->Root(), + doc.FirstChildElement("model"), + parserConfig, + errors); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "The original element [model] contains the attribute 'name' but none was" + " provided in the element modifier. The assumed element to be modified " + "is: ")); + errors.clear(); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::WARN); + sdf::ParamPassing::getElementByName(sdfParsed->Root(), + doc.FirstChildElement("model"), + parserConfig, + errors); + EXPECT_TRUE(errors.empty()); + // Check the warning has been printed + EXPECT_NE(std::string::npos, buffer.str().find( + "The original element [model] contains the attribute 'name' but none " + "was provided in the element modifier. The assumed element to be " + "modified is: ")); +} + +//////////////////////////////////////// +// Test warnings outputs for GetElementByName +TEST(ParamPassing, ModifyChildrenNameWarningOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + std::ostringstream stream; + stream << "" + << "" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + + sdf::Errors errors; + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::readString(stream.str(), parserConfig, sdfParsed, errors); + EXPECT_TRUE(errors.empty()); + + std::ostringstream stream2; + stream2 << "" + << " " + << " " + << ""; + tinyxml2::XMLDocument doc; + doc.Parse(stream2.str().c_str()); + + sdf::ParamPassing::modifyChildren( + doc.FirstChildElement("sdf"), + parserConfig, + sdfParsed->Root(), + errors); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "No modifications for element \n provided, " + "skipping modification for:\n\n" + " \n")); + errors.clear(); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::WARN); + sdf::ParamPassing::modifyChildren( + doc.FirstChildElement("sdf"), + parserConfig, + sdfParsed->Root(), + errors); + EXPECT_TRUE(errors.empty()); + // Check the warning has been printed + EXPECT_NE(std::string::npos, buffer.str().find( + "No modifications for element \n provided, " + "skipping modification for:\n\n" + " \n")); +} From 686476b290a4d49c99ec49d2a022b7c0df77067e Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 30 Nov 2022 11:47:24 +0800 Subject: [PATCH 23/31] ign -> gz Migrate Ignition Headers : sdformat (#1118) * Migrate namespace usage * Require ign-math 6.13 * Backport ign->gz comment/Changelog changes Signed-off-by: methylDragon Signed-off-by: Steve Peters Signed-off-by: Nate Koenig Co-authored-by: Steve Peters Co-authored-by: Nate Koenig Co-authored-by: Nate Koenig --- Changelog.md | 90 +++++++-------- cmake/SearchForStuff.cmake | 2 +- doc/header.html | 2 +- doc/mainpage.html | 2 +- include/sdf/Actor.hh | 18 +-- include/sdf/Atmosphere.hh | 6 +- include/sdf/Box.hh | 20 ++-- include/sdf/Camera.hh | 28 ++--- include/sdf/Collision.hh | 14 +-- include/sdf/Cylinder.hh | 14 +-- include/sdf/Frame.hh | 8 +- include/sdf/Heightmap.hh | 10 +- include/sdf/Imu.hh | 8 +- include/sdf/Joint.hh | 14 +-- include/sdf/JointAxis.hh | 14 +-- include/sdf/Lidar.hh | 18 +-- include/sdf/Light.hh | 36 +++--- include/sdf/Link.hh | 26 ++--- include/sdf/Material.hh | 16 +-- include/sdf/Mesh.hh | 6 +- include/sdf/Model.hh | 14 +-- include/sdf/NavSat.hh | 2 +- include/sdf/Param.hh | 32 +++--- include/sdf/Plane.hh | 30 ++--- include/sdf/Polyline.hh | 10 +- include/sdf/Scene.hh | 10 +- include/sdf/SemanticPose.hh | 8 +- include/sdf/Sensor.hh | 14 +-- include/sdf/Sky.hh | 10 +- include/sdf/Sphere.hh | 14 +-- include/sdf/Types.hh | 2 +- include/sdf/Visual.hh | 14 +-- include/sdf/World.hh | 18 +-- src/Actor.cc | 20 ++-- src/Actor_TEST.cc | 14 +-- src/AirPressure.cc | 2 +- src/Atmosphere.cc | 12 +- src/Atmosphere_TEST.cc | 2 +- src/Box.cc | 16 +-- src/Box_TEST.cc | 24 ++-- src/Camera.cc | 38 +++---- src/Camera_TEST.cc | 12 +- src/Collision.cc | 12 +- src/Collision_TEST.cc | 20 ++-- src/Cylinder.cc | 6 +- src/Frame.cc | 8 +- src/FrameSemantics.cc | 38 +++---- src/FrameSemantics.hh | 18 +-- src/FrameSemantics_TEST.cc | 34 +++--- src/Frame_TEST.cc | 12 +- src/Geometry_TEST.cc | 22 ++-- src/Heightmap.cc | 16 +-- src/Heightmap_TEST.cc | 32 +++--- src/Imu.cc | 16 +-- src/Imu_TEST.cc | 12 +- src/Joint.cc | 12 +- src/JointAxis.cc | 18 +-- src/JointAxis_TEST.cc | 20 ++-- src/Joint_TEST.cc | 40 +++---- src/Lidar.cc | 2 +- src/Lidar_TEST.cc | 10 +- src/Light.cc | 52 ++++----- src/Light_TEST.cc | 98 ++++++++--------- src/Link.cc | 38 +++---- src/Link_TEST.cc | 34 +++--- src/Material.cc | 34 +++--- src/Material_TEST.cc | 106 +++++++++--------- src/Mesh.cc | 8 +- src/Mesh_TEST.cc | 16 +-- src/Model.cc | 22 ++-- src/Model_TEST.cc | 10 +- src/NavSat.cc | 2 +- src/Noise.cc | 14 +-- src/Param.cc | 64 +++++------ src/Param_TEST.cc | 28 ++--- src/Pbr.cc | 8 +- src/Pbr_TEST.cc | 2 +- src/Physics.cc | 2 +- src/Plane.cc | 28 ++--- src/Plane_TEST.cc | 50 ++++----- src/Polyline.cc | 12 +- src/Polyline_TEST.cc | 8 +- src/SDFExtension.hh | 4 +- src/SDF_TEST.cc | 70 ++++++------ src/Scene.cc | 20 ++-- src/Scene_TEST.cc | 52 ++++----- src/SemanticPose.cc | 12 +- src/SemanticPose_TEST.cc | 22 ++-- src/Sensor.cc | 14 +-- src/Sensor_TEST.cc | 38 +++---- src/Sky.cc | 18 +-- src/Sky_TEST.cc | 44 ++++---- src/Sphere.cc | 6 +- src/Utils.cc | 6 +- src/Utils.hh | 6 +- src/Utils_TEST.cc | 22 ++-- src/Visual.cc | 12 +- src/Visual_TEST.cc | 32 +++--- src/World.cc | 32 +++--- src/World_TEST.cc | 60 +++++----- src/ign.cc | 6 +- src/parser.cc | 12 +- src/parser_TEST.cc | 4 +- src/parser_urdf.cc | 80 +++++++------- test/env.hh | 2 +- test/integration/actor_dom.cc | 4 +- test/integration/collision_dom.cc | 6 +- test/integration/converter.cc | 8 +- test/integration/fixed_joint_reduction.cc | 122 ++++++++++----------- test/integration/frame.cc | 70 ++++++------ test/integration/geometry_dom.cc | 36 +++--- test/integration/includes.cc | 14 +-- test/integration/joint_axis_dom.cc | 10 +- test/integration/joint_dom.cc | 20 ++-- test/integration/light_dom.cc | 30 ++--- test/integration/link_dom.cc | 74 ++++++------- test/integration/link_light.cc | 14 +-- test/integration/locale_fix_cxx.cc | 4 +- test/integration/material_pbr.cc | 18 +-- test/integration/model_dom.cc | 10 +- test/integration/nested_model.cc | 114 +++++++++---------- test/integration/scene_dom.cc | 10 +- test/integration/urdf_gazebo_extensions.cc | 28 ++--- test/integration/urdf_joint_parameters.cc | 4 +- test/integration/visual_dom.cc | 14 +-- test/integration/world_dom.cc | 12 +- 126 files changed, 1425 insertions(+), 1425 deletions(-) diff --git a/Changelog.md b/Changelog.md index 5089fc688..d7d670d4f 100644 --- a/Changelog.md +++ b/Changelog.md @@ -50,143 +50,143 @@ ### libsdformat 9.8.0 (2022-04-26) 1. Polyline geometry DOM - * [Pull request #1000](https://github.com/ignitionrobotics/sdformat/pull/1000) + * [Pull request #1000](https://github.com/gazebosim/sdformat/pull/1000) 1. Added `` to `` - * [Pull request #985](https://github.com/ignitionrobotics/sdformat/pull/985) + * [Pull request #985](https://github.com/gazebosim/sdformat/pull/985) 1. Backport ``ign sdf --inertial-stats`` - * [Pull request #958](https://github.com/ignitionrobotics/sdformat/pull/958) + * [Pull request #958](https://github.com/gazebosim/sdformat/pull/958) 1. Add L16 pixel format to Camera pixel format conversion function - * [Pull request #487](https://github.com/ignitionrobotics/sdformat/pull/487) + * [Pull request #487](https://github.com/gazebosim/sdformat/pull/487) 1. Anti-aliasing element for `` - * [Pull request #909](https://github.com/ignitionrobotics/sdformat/pull/909) + * [Pull request #909](https://github.com/gazebosim/sdformat/pull/909) 1. Fix loading nested include with custom attributes - * [Pull request #789](https://github.com/ignitionrobotics/sdformat/pull/789) + * [Pull request #789](https://github.com/gazebosim/sdformat/pull/789) 1. add enable_orientation to 1.6 spec - * [Pull request #686](https://github.com/ignitionrobotics/sdformat/pull/686) + * [Pull request #686](https://github.com/gazebosim/sdformat/pull/686) 1. Fix xyz and rpy offsets in fixed joint reduction - * [Pull request #500](https://github.com/ignitionrobotics/sdformat/pull/500) + * [Pull request #500](https://github.com/gazebosim/sdformat/pull/500) 1. 👩‍🌾 Remove bitbucket-pipelines and backport labeler / triage - * [Pull request #674](https://github.com/ignitionrobotics/sdformat/pull/674) + * [Pull request #674](https://github.com/gazebosim/sdformat/pull/674) 1. Create CODEOWNERS with azeey and scpeters - * [Pull request #650](https://github.com/ignitionrobotics/sdformat/pull/650) + * [Pull request #650](https://github.com/gazebosim/sdformat/pull/650) 1. Use Ubuntu bionic in CI - * [Pull request #626](https://github.com/ignitionrobotics/sdformat/pull/626) + * [Pull request #626](https://github.com/gazebosim/sdformat/pull/626) 1. Translate poses of nested models inside other nested models (sdf6) - * [Pull request #596](https://github.com/ignitionrobotics/sdformat/pull/596) + * [Pull request #596](https://github.com/gazebosim/sdformat/pull/596) 1. Fix flattening logic for nested model names (sdf6) - * [Pull request #597](https://github.com/ignitionrobotics/sdformat/pull/597) + * [Pull request #597](https://github.com/gazebosim/sdformat/pull/597) 1. Parse rpyOffset as radians - * [Pull request #497](https://github.com/ignitionrobotics/sdformat/pull/497) + * [Pull request #497](https://github.com/gazebosim/sdformat/pull/497) 1. BitBucket - * [Pull request #258](https://github.com/ignitionrobotics/sdformat/pull/258) - * [Pull request #237](https://github.com/ignitionrobotics/sdformat/pull/237) + * [Pull request #258](https://github.com/gazebosim/sdformat/pull/258) + * [Pull request #237](https://github.com/gazebosim/sdformat/pull/237) ### libsdformat 9.7.0 (2021-11-03) 1. Make exception for plugins when checking for name uniqueness - * [Pull request #733](https://github.com/ignitionrobotics/sdformat/pull/733) + * [Pull request #733](https://github.com/gazebosim/sdformat/pull/733) 1. Backport test utilities from sdf10 - * [Pull request #731](https://github.com/ignitionrobotics/sdformat/pull/731) + * [Pull request #731](https://github.com/gazebosim/sdformat/pull/731) 1. Added Force Torque Noise functions + Unit tests - * [Pull request #669](https://github.com/ignitionrobotics/sdformat/pull/669) + * [Pull request #669](https://github.com/gazebosim/sdformat/pull/669) 1. Add Joint DOM API to access joint sensors - * [Pull request #517](https://github.com/ignitionrobotics/sdformat/pull/517) + * [Pull request #517](https://github.com/gazebosim/sdformat/pull/517) 1. Add force torque sensor - * [Pull request #393](https://github.com/ignitionrobotics/sdformat/pull/393) + * [Pull request #393](https://github.com/gazebosim/sdformat/pull/393) ### libsdformat 9.6.1 (2021-09-07) 1. Parse URDF continuous joint effort/velocity limits - * [Pull request #684](https://github.com/ignitionrobotics/sdformat/pull/684) + * [Pull request #684](https://github.com/gazebosim/sdformat/pull/684) 1. Add a codecheck make target - * [Pull request #682](https://github.com/ignitionrobotics/sdformat/pull/682) + * [Pull request #682](https://github.com/gazebosim/sdformat/pull/682) 1. Refactor sdf::readXml - * [Pull request #681](https://github.com/ignitionrobotics/sdformat/pull/681) + * [Pull request #681](https://github.com/gazebosim/sdformat/pull/681) 1. Upgrade cpplint and fix new errors - * [Pull request #680](https://github.com/ignitionrobotics/sdformat/pull/680) + * [Pull request #680](https://github.com/gazebosim/sdformat/pull/680) 1. BUG: add missing plugin element to include - * [Pull request #675](https://github.com/ignitionrobotics/sdformat/pull/675) + * [Pull request #675](https://github.com/gazebosim/sdformat/pull/675) 1. Added comment reminder to update functions - * [Pull request #677](https://github.com/ignitionrobotics/sdformat/pull/677) + * [Pull request #677](https://github.com/gazebosim/sdformat/pull/677) ### libsdformat 9.6.0 (2021-08-18) 1. Adds `enable_metrics` flag to Sensor. - * [Pull request #665](https://github.com/ignitionrobotics/sdformat/pull/665) + * [Pull request #665](https://github.com/gazebosim/sdformat/pull/665) 1. Add GPS / NavSat sensor DOM to sdf9 - * [Pull request #453](https://github.com/ignitionrobotics/sdformat/pull/453) + * [Pull request #453](https://github.com/gazebosim/sdformat/pull/453) 1. Support parsing elements that are not part of the schema - * [Pull request #638](https://github.com/ignitionrobotics/sdformat/pull/638) + * [Pull request #638](https://github.com/gazebosim/sdformat/pull/638) 1. Add lightmap to 1.7 spec and PBR material DOM - * [Pull request #429](https://github.com/ignitionrobotics/sdformat/pull/429) + * [Pull request #429](https://github.com/gazebosim/sdformat/pull/429) 1. Fix urdf link extension tags - * [Pull request #628](https://github.com/ignitionrobotics/sdformat/pull/628) + * [Pull request #628](https://github.com/gazebosim/sdformat/pull/628) 1. Updated material spec - * [Pull request #644](https://github.com/ignitionrobotics/sdformat/pull/644) + * [Pull request #644](https://github.com/gazebosim/sdformat/pull/644) 1. Minor fix to Migration guide - * [Pull request #630](https://github.com/ignitionrobotics/sdformat/pull/630) + * [Pull request #630](https://github.com/gazebosim/sdformat/pull/630) 1. Error: move << operator from .hh to .cc file - * [Pull request #625](https://github.com/ignitionrobotics/sdformat/pull/625) + * [Pull request #625](https://github.com/gazebosim/sdformat/pull/625) 1. Update build system to allow overriding CXX flags and using clang on Linux - * [Pull request #621](https://github.com/ignitionrobotics/sdformat/pull/621) + * [Pull request #621](https://github.com/gazebosim/sdformat/pull/621) 1. Add Element::FindElement as an alternative to Element::GetElement - * [Pull request #620](https://github.com/ignitionrobotics/sdformat/pull/620) + * [Pull request #620](https://github.com/gazebosim/sdformat/pull/620) 1. Add ValidateGraphs methods to Model/World (sdf9) - * [Pull request #602](https://github.com/ignitionrobotics/sdformat/pull/602) + * [Pull request #602](https://github.com/gazebosim/sdformat/pull/602) 1. Fix ABI break - * [Pull request #605](https://github.com/ignitionrobotics/sdformat/pull/605) + * [Pull request #605](https://github.com/gazebosim/sdformat/pull/605) 1. Making PrintValues() and ToString() able to not print default elements - * [Pull request #575](https://github.com/ignitionrobotics/sdformat/pull/575) + * [Pull request #575](https://github.com/gazebosim/sdformat/pull/575) 1. Add API for determining if an element was set by the user - * [Pull request #542](https://github.com/ignitionrobotics/sdformat/pull/542) + * [Pull request #542](https://github.com/gazebosim/sdformat/pull/542) 1. Methods for removing attributes from an element - * [Pull request #555](https://github.com/ignitionrobotics/sdformat/pull/555) + * [Pull request #555](https://github.com/gazebosim/sdformat/pull/555) 1. Improve docs of collision_bitmask. - * [Pull request #521](https://github.com/ignitionrobotics/sdformat/pull/521) + * [Pull request #521](https://github.com/gazebosim/sdformat/pull/521) 1. Add camera type aliases to docs. - * [Pull request #514](https://github.com/ignitionrobotics/sdformat/pull/514) + * [Pull request #514](https://github.com/gazebosim/sdformat/pull/514) 1. Add action-ignition-ci - * [Pull request #501](https://github.com/ignitionrobotics/sdformat/pull/452) + * [Pull request #501](https://github.com/gazebosim/sdformat/pull/452) ### libsdformat 9.5.0 (2021-02-11) diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake index 3ae8d19c6..1e89a1291 100644 --- a/cmake/SearchForStuff.cmake +++ b/cmake/SearchForStuff.cmake @@ -142,7 +142,7 @@ set(IGN_CMAKE_VER ${ignition-cmake2_VERSION_MAJOR}) ######################################## # Find ignition math # Set a variable for generating ProjectConfig.cmake -find_package(ignition-math6 QUIET) +find_package(ignition-math6 6.13 QUIET) if (NOT ignition-math6_FOUND) message(STATUS "Looking for ignition-math6-config.cmake - not found") BUILD_ERROR ("Missing: Ignition math (libignition-math6-dev)") diff --git a/doc/header.html b/doc/header.html index 30f514517..5e99f7fdf 100644 --- a/doc/header.html +++ b/doc/header.html @@ -41,7 +41,7 @@

Tutorials
Download
--> -
Report Documentation Issues
+
Report Documentation Issues
diff --git a/doc/mainpage.html b/doc/mainpage.html index 16bab2e47..26ba00c8f 100644 --- a/doc/mainpage.html +++ b/doc/mainpage.html @@ -5,7 +5,7 @@ Desctiption Format API. The code reference is divided into the groups below. Should you find problems with this documentation - typos, unclear phrases, or insufficient detail - please create a new GitHub issue. + href="https://github.com/gazebosim/sdformat/issues/new">new GitHub issue. Include sufficient detail to quickly locate the problematic documentation, and set the issue's fields accordingly: Assignee - blank; Kind - bug; Priority - minor; Version - blank. diff --git a/include/sdf/Actor.hh b/include/sdf/Actor.hh index c36ed9ae0..3846c656f 100644 --- a/include/sdf/Actor.hh +++ b/include/sdf/Actor.hh @@ -20,7 +20,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/Types.hh" @@ -175,11 +175,11 @@ namespace sdf /// \brief Get the pose to be reached. /// \return Pose to be reached. - public: ignition::math::Pose3d Pose() const; + public: gz::math::Pose3d Pose() const; /// \brief Set the pose to be reached. /// \param[in] _pose Pose to be reached. - public: void SetPose(const ignition::math::Pose3d &_pose); + public: void SetPose(const gz::math::Pose3d &_pose); /// \brief Copy waypoint from an Waypoint instance. /// \param[in] _waypoint The waypoint to set values from. @@ -326,14 +326,14 @@ namespace sdf /// global coordinate frame. /// \return The pose of the actor. /// \deprecated See RawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the actor. - /// \sa const ignition::math::Pose3d &Pose() const + /// \sa const gz::math::Pose3d &Pose() const /// \param[in] _pose The new actor pose. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the actor. This is the pose of the actor @@ -341,12 +341,12 @@ namespace sdf /// typically used to express the position and rotation of an actor in a /// global coordinate frame. /// \return The pose of the actor. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the actor. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The new actor pose. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is diff --git a/include/sdf/Atmosphere.hh b/include/sdf/Atmosphere.hh index af3e73566..c8a0a2839 100644 --- a/include/sdf/Atmosphere.hh +++ b/include/sdf/Atmosphere.hh @@ -17,7 +17,7 @@ #ifndef SDF_ATMOSPHERE_HH_ #define SDF_ATMOSPHERE_HH_ -#include +#include #include "sdf/Element.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -87,11 +87,11 @@ namespace sdf /// \brief Get the temperature at sea level. /// \return The temperature at sea level. - public: ignition::math::Temperature Temperature() const; + public: gz::math::Temperature Temperature() const; /// \brief Set the temperature at sea level. /// \param[in] _temp The temperature at sea level. - public: void SetTemperature(const ignition::math::Temperature &_temp); + public: void SetTemperature(const gz::math::Temperature &_temp); /// \brief Get the temperature gradient with respect to increasing /// altitude in units of K/m. diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 62b5a003e..af31382aa 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -17,8 +17,8 @@ #ifndef SDF_BOX_HH_ #define SDF_BOX_HH_ -#include -#include +#include +#include #include #include #include @@ -69,11 +69,11 @@ namespace sdf /// \brief Get the box size in meters. /// \return Size of the box in meters. - public: ignition::math::Vector3d Size() const; + public: gz::math::Vector3d Size() const; /// \brief Set the box size in meters. /// \param[in] _size Size of the box in meters. - public: void SetSize(const ignition::math::Vector3d &_size); + public: void SetSize(const gz::math::Vector3d &_size); /// \brief Get a pointer to the SDF element that was used during /// load. @@ -81,13 +81,13 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Get the Ignition Math representation of this Box. - /// \return A const reference to an ignition::math::Boxd object. - public: const ignition::math::Boxd &Shape() const; + /// \brief Get the Gazebo Math representation of this Box. + /// \return A const reference to a gz::math::Boxd object. + public: const gz::math::Boxd &Shape() const; - /// \brief Get a mutable Ignition Math representation of this Box. - /// \return A reference to an ignition::math::Boxd object. - public: ignition::math::Boxd &Shape(); + /// \brief Get a mutable Gazebo Math representation of this Box. + /// \return A reference to a gz::math::Boxd object. + public: gz::math::Boxd &Shape(); /// \brief Private data pointer. private: BoxPrivate *dataPtr; diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index e5d27566c..ae5aee061 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -18,7 +18,7 @@ #define SDF_CAMERA_HH_ #include -#include +#include #include #include @@ -35,7 +35,7 @@ namespace sdf /// \enum PixelFormatType /// \brief The set of pixel formats. This list should match - /// ignition::common::Image::PixelFormatType. + /// gz::common::Image::PixelFormatType. enum class PixelFormatType { UNKNOWN_PIXEL_FORMAT = 0, @@ -129,11 +129,11 @@ namespace sdf /// \brief Get the horizontal field of view in radians. /// \return The horizontal field of view in radians. - public: ignition::math::Angle HorizontalFov() const; + public: gz::math::Angle HorizontalFov() const; /// \brief Set the horizontal field of view in radians. /// \param[in] _hfov The horizontal field of view in radians. - public: void SetHorizontalFov(const ignition::math::Angle &_hfov); + public: void SetHorizontalFov(const gz::math::Angle &_hfov); /// \brief Get the image width in pixels. /// \return The image width in pixels. @@ -302,36 +302,36 @@ namespace sdf /// \brief Get the distortion center or principal point. /// \return Distortion center or principal point. - public: const ignition::math::Vector2d &DistortionCenter() const; + public: const gz::math::Vector2d &DistortionCenter() const; /// \brief Set the distortion center or principal point. /// \param[in] _center Distortion center or principal point. public: void SetDistortionCenter( - const ignition::math::Vector2d &_center) const; + const gz::math::Vector2d &_center) const; /// \brief Get the pose of the camer. This is the pose of the camera /// as specified in SDF ( ... ). /// \return The pose of the link. /// \deprecated See RawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the camera. - /// \sa const ignition::math::Pose3d &Pose() const + /// \sa const gz::math::Pose3d &Pose() const /// \param[in] _pose The new camera pose. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the camer. This is the pose of the camera /// as specified in SDF ( ... ). /// \return The pose of the link. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the camera. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The new camera pose. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is @@ -429,12 +429,12 @@ namespace sdf /// \brief Get lens cutoff angle. Everything outside of the specified /// angle will be hidden. /// \return The lens cutoff angle. - public: ignition::math::Angle LensCutoffAngle() const; + public: gz::math::Angle LensCutoffAngle() const; /// \brief Set lens cutoff angle. Everything outside of the specified /// angle will be hidden. /// \param[in] _angle The lens cutoff angle. - public: void SetLensCutoffAngle(const ignition::math::Angle &_angle); + public: void SetLensCutoffAngle(const gz::math::Angle &_angle); /// \brief Get environment texture size. This is the resolution of the /// environment cube map used to draw the world. diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index a61d723a9..fb3e585bb 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -106,26 +106,26 @@ namespace sdf /// ( ... ). /// \return The pose of the collision object. /// \deprecated See RawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the collision object. - /// \sa const ignition::math::Pose3d &Pose() const + /// \sa const gz::math::Pose3d &Pose() const /// \param[in] _pose The pose of the collision object. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the collision object. This is the pose of the /// collison as specified in SDF /// ( ... ). /// \return The pose of the collision object. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the collision object. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The pose of the collision object. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 166a7a72f..1919bf5e9 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -17,7 +17,7 @@ #ifndef SDF_CYLINDER_HH_ #define SDF_CYLINDER_HH_ -#include +#include #include #include #include @@ -89,13 +89,13 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Get the Ignition Math representation of this Cylinder. - /// \return A const reference to an ignition::math::Cylinderd object. - public: const ignition::math::Cylinderd &Shape() const; + /// \brief Get the Gazebo Math representation of this Cylinder. + /// \return A const reference to a gz::math::Sphered object. + public: const gz::math::Cylinderd &Shape() const; - /// \brief Get a mutable Ignition Math representation of this Cylinder. - /// \return A reference to an ignition::math::Cylinderd object. - public: ignition::math::Cylinderd &Shape(); + /// \brief Get a mutable Gazebo Math representation of this Cylinder. + /// \return A reference to a gz::math::Cylinderd object. + public: gz::math::Cylinderd &Shape(); /// \brief Private data pointer. private: CylinderPrivate *dataPtr; diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index e5097e8ca..d35c428ca 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -107,13 +107,13 @@ namespace sdf /// ( ... ). /// Use SemanticPose to compute the pose relative to a specific frame. /// \return The pose of the frame object. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the raw pose of the frame object. This is interpreted /// relative to the frame named in the //pose/@relative_to attribute. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The pose of the frame object. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// frame's pose is expressed. An empty value indicates that the frame is diff --git a/include/sdf/Heightmap.hh b/include/sdf/Heightmap.hh index 9a24ec1e5..5f07491ff 100644 --- a/include/sdf/Heightmap.hh +++ b/include/sdf/Heightmap.hh @@ -18,7 +18,7 @@ #define SDF_HEIGHTMAP_HH_ #include -#include +#include #include #include #include @@ -216,19 +216,19 @@ namespace sdf /// \brief Get the heightmap's scaling factor. /// \return The heightmap's size. - public: ignition::math::Vector3d Size() const; + public: gz::math::Vector3d Size() const; /// \brief Set the heightmap's scaling factor. Defaults to 1x1x1. /// \return The heightmap's size factor. - public: void SetSize(const ignition::math::Vector3d &_size); + public: void SetSize(const gz::math::Vector3d &_size); /// \brief Get the heightmap's position offset. /// \return The heightmap's position offset. - public: ignition::math::Vector3d Position() const; + public: gz::math::Vector3d Position() const; /// \brief Set the heightmap's position offset. /// \return The heightmap's position offset. - public: void SetPosition(const ignition::math::Vector3d &_position); + public: void SetPosition(const gz::math::Vector3d &_position); /// \brief Get whether the heightmap uses terrain paging. /// \return True if the heightmap uses terrain paging. diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh index 1ff9c2f89..f4ca39674 100644 --- a/include/sdf/Imu.hh +++ b/include/sdf/Imu.hh @@ -138,7 +138,7 @@ namespace sdf /// X-axis. grav_dir_x is defined in the coordinate frame as defined by /// the parent_frame element. /// \return The gravity direction. - public: ignition::math::Vector3d &GravityDirX() const; + public: gz::math::Vector3d &GravityDirX() const; /// \brief Used when localization is set to GRAV_UP or GRAV_DOWN, a /// projection of this vector into a plane that is orthogonal to the @@ -146,7 +146,7 @@ namespace sdf /// X-axis. grav_dir_x is defined in the coordinate frame as defined by /// the parent_frame element. /// \param[in] _grav The gravity direction. - public: void SetGravityDirX(const ignition::math::Vector3d &_grav) const; + public: void SetGravityDirX(const gz::math::Vector3d &_grav) const; /// \brief Get the name of parent frame which the GravityDirX vector is /// defined relative to. It can be any valid fully scoped link name or the @@ -235,11 +235,11 @@ namespace sdf /// +z:up). /// Example sdf: parent_frame="local", custom_rpy="-0.5*M_PI 0 -0.5*M_PI" /// \return Custom RPY vectory - public: const ignition::math::Vector3d &CustomRpy() const; + public: const gz::math::Vector3d &CustomRpy() const; /// \brief See CustomRpy() const. /// \param[in] Custom RPY vectory - public: void SetCustomRpy(const ignition::math::Vector3d &_rpy) const; + public: void SetCustomRpy(const gz::math::Vector3d &_rpy) const; /// \brief Get the name of parent frame which the custom_rpy transform is /// defined relative to. It can be any valid fully scoped link name or the diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 4a97d12e8..76a9927f0 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -172,14 +172,14 @@ namespace sdf /// \return The pose of the joint. This is the raw pose value, as set in /// the SDF file. /// \deprecated See RawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the joint. - /// \sa const ignition::math::Pose3d &Pose() const; + /// \sa const gz::math::Pose3d &Pose() const; /// \param[in] _pose The pose of the joint. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the joint. This is the pose of the joint @@ -187,12 +187,12 @@ namespace sdf /// Transformations have not been applied to the return value. /// \return The pose of the joint. This is the raw pose value, as set in /// the SDF file. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the joint. - /// \sa const ignition::math::Pose3d &RawPose() const; + /// \sa const gz::math::Pose3d &RawPose() const; /// \param[in] _pose The pose of the joint. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index eae4810e0..df3b6b703 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" @@ -85,16 +85,16 @@ namespace sdf /// \brief Get the x,y,z components of the axis unit vector. /// The axis is expressed in the joint frame unless UseParentModelFrame /// is true. The vector should be normalized. - /// The default value is ignition::math::Vector3d::UnitZ which equals + /// The default value is gz::math::Vector3d::UnitZ which equals /// (0, 0, 1). /// \return The x,y,z components of the axis unit vector. - /// \sa void SetXyz(const ignition::math::Vector3d &_xyz) - public: ignition::math::Vector3d Xyz() const; + /// \sa void SetXyz(const gz::math::Vector3d &_xyz) + public: gz::math::Vector3d Xyz() const; /// \brief Set the x,y,z components of the axis unit vector. /// \param[in] _xyz The x,y,z components of the axis unit vector. - /// \sa ignition::math::Vector3d Xyz() const - public: void SetXyz(const ignition::math::Vector3d &_xyz); + /// \sa gz::math::Vector3d Xyz() const + public: void SetXyz(const gz::math::Vector3d &_xyz); /// \brief Get whether to interpret the axis xyz value in the parent model /// frame instead of joint frame. The default value is false. @@ -251,7 +251,7 @@ namespace sdf /// xml parent object, which is always a joint frame. /// \return Errors. public: Errors ResolveXyz( - ignition::math::Vector3d &_xyz, + gz::math::Vector3d &_xyz, const std::string &_resolveTo = "") const; /// \brief Get a pointer to the SDF element that was used during diff --git a/include/sdf/Lidar.hh b/include/sdf/Lidar.hh index f04a1538e..44fbff47d 100644 --- a/include/sdf/Lidar.hh +++ b/include/sdf/Lidar.hh @@ -22,7 +22,7 @@ #include #include -#include +#include namespace sdf { @@ -161,19 +161,19 @@ namespace sdf /// \brief Get the minimum angle for horizontal scan. /// \return Minimum angle for horizontal scan. - public: ignition::math::Angle HorizontalScanMinAngle() const; + public: gz::math::Angle HorizontalScanMinAngle() const; /// \brief Set the minimum angle for horizontal scan. /// \param[in] Minimum angle for horizontal scan. - public: void SetHorizontalScanMinAngle(const ignition::math::Angle &_min); + public: void SetHorizontalScanMinAngle(const gz::math::Angle &_min); /// \brief Get the maximum angle for horizontal scan. /// \return Maximum angle for horizontal scan. - public: ignition::math::Angle HorizontalScanMaxAngle() const; + public: gz::math::Angle HorizontalScanMaxAngle() const; /// \brief Set the maximum angle for horizontal scan. /// \param[in] Maximum angle for horizontal scan. - public: void SetHorizontalScanMaxAngle(const ignition::math::Angle &_max); + public: void SetHorizontalScanMaxAngle(const gz::math::Angle &_max); /// \brief Get the number of lidar rays vertically to generate per laser /// sweep. @@ -195,19 +195,19 @@ namespace sdf /// \brief Get the minimum angle for vertical scan. /// \return Minimum angle for vertical scan. - public: ignition::math::Angle VerticalScanMinAngle() const; + public: gz::math::Angle VerticalScanMinAngle() const; /// \brief Set the minimum angle for vertical scan. /// \param[in] Minimum angle for vertical scan. - public: void SetVerticalScanMinAngle(const ignition::math::Angle &_min); + public: void SetVerticalScanMinAngle(const gz::math::Angle &_min); /// \brief Get the maximum angle for vertical scan. /// \return Maximum angle for vertical scan. - public: ignition::math::Angle VerticalScanMaxAngle() const; + public: gz::math::Angle VerticalScanMaxAngle() const; /// \brief Set the maximum angle for vertical scan. /// \param[in] Maximum angle for vertical scan. - public: void SetVerticalScanMaxAngle(const ignition::math::Angle &_max); + public: void SetVerticalScanMaxAngle(const gz::math::Angle &_max); /// \brief Get minimum distance for each lidar ray. /// \return Minimum distance for each lidar ray. diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index 96bb9ebf3..60f81e613 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" @@ -115,14 +115,14 @@ namespace sdf /// global coordinate frame. /// \return The pose of the light. /// \deprecated See RawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the light. - /// \sa const ignition::math::Pose3d &Pose() const + /// \sa const gz::math::Pose3d &Pose() const /// \param[in] _pose The new light pose. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the light. This is the pose of the light @@ -130,12 +130,12 @@ namespace sdf /// typically used to express the position and rotation of a light in a /// global coordinate frame. /// \return The pose of the light. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the light. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The new light pose. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is @@ -182,25 +182,25 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \return Diffuse color. - public: ignition::math::Color Diffuse() const; + public: gz::math::Color Diffuse() const; /// \brief Set the diffuse color. The diffuse color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Diffuse color. - public: void SetDiffuse(const ignition::math::Color &_color) const; + public: void SetDiffuse(const gz::math::Color &_color) const; /// \brief Get the specular color. The specular color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \return Specular color. - public: ignition::math::Color Specular() const; + public: gz::math::Color Specular() const; /// \brief Set the specular color. The specular color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Specular color. - public: void SetSpecular(const ignition::math::Color &_color) const; + public: void SetSpecular(const gz::math::Color &_color) const; /// \brief Get the range of the light source in meters. /// \return Range of the light source in meters. @@ -248,32 +248,32 @@ namespace sdf /// for spot and directional light types. The default value is /// [0, 0, -1]. /// \return Light source direction. - public: ignition::math::Vector3d Direction() const; + public: gz::math::Vector3d Direction() const; /// \brief Set the direction of the light source. This only has meaning /// for spot and directional light types. /// \param[in] _dir Light source direction. - public: void SetDirection(const ignition::math::Vector3d &_dir); + public: void SetDirection(const gz::math::Vector3d &_dir); /// \brief Get the angle covered by the bright inner cone. /// \return The angle covered by the bright inner cone. /// \note This function only has meaning for a spot light. - public: ignition::math::Angle SpotInnerAngle() const; + public: gz::math::Angle SpotInnerAngle() const; /// \brief Set the angle covered by the bright inner cone. /// \param[in] _angle The angle covered by the bright inner cone. /// \note This function only has meaning for a spot light. - public: void SetSpotInnerAngle(const ignition::math::Angle &_angle); + public: void SetSpotInnerAngle(const gz::math::Angle &_angle); /// \brief Get the angle covered by the outer cone. /// \return The angle covered by the outer cone. /// \note This function only has meaning for a spot light. - public: ignition::math::Angle SpotOuterAngle() const; + public: gz::math::Angle SpotOuterAngle() const; /// \brief Set the angle covered by the outer cone. /// \param[in] _angle The angle covered by the outer cone. /// \note This function only has meaning for a spot light. - public: void SetSpotOuterAngle(const ignition::math::Angle &_angle); + public: void SetSpotOuterAngle(const gz::math::Angle &_angle); /// \brief Get the rate of falloff between the inner and outer cones. /// A value of 1.0 is a linear falloff, less than 1.0 is a slower falloff, diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index fc54da7b6..3731f82df 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -176,21 +176,21 @@ namespace sdf /// a pose for the inertial reference frame. The units for mass is /// kilograms with a default value of 1kg. The 3x3 rotational inertia /// matrix is symmetric and only 6 above-diagonal elements of this matrix - /// are specified the Interial's ignition::math::MassMatrix3 property. + /// are specified the Interial's gz::math::MassMatrix3 property. /// /// The origin of the inertial reference frame needs to be at the center /// of mass expressed in this link's frame. /// The axes of the inertial reference frame do not need to /// be aligned with the principal axes of the inertia. /// \return The link's inertial value. - /// \sa void SetInertial(const ignition::math::Inertiald &_inertial) - public: const ignition::math::Inertiald &Inertial() const; + /// \sa void SetInertial(const gz::math::Inertiald &_inertial) + public: const gz::math::Inertiald &Inertial() const; /// \brief Set the inertial value for this link. /// \param[in] _inertial The link's inertial value. /// \return True if the inertial is valid, false otherwise. - /// \sa const ignition::math::Inertiald &Inertial() const - public: bool SetInertial(const ignition::math::Inertiald &_inertial); + /// \sa const gz::math::Inertiald &Inertial() const + public: bool SetInertial(const gz::math::Inertiald &_inertial); /// \brief Resolve the Inertial to a specified frame. /// If there are any errors resolving the Inertial, the output will not @@ -199,32 +199,32 @@ namespace sdf /// \param[in] _resolveTo The Inertial will be resolved with respect to this /// frame. If unset or empty, the default resolve-to frame will be used. /// \return Errors in resolving pose. - public: Errors ResolveInertial(ignition::math::Inertiald &_inertial, + public: Errors ResolveInertial(gz::math::Inertiald &_inertial, const std::string &_resolveTo = "") const; /// \brief Get the pose of the link. This is the pose of the link /// as specified in SDF ( ... ). /// \return The pose of the link. /// \deprecated See RawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the link. - /// \sa const ignition::math::Pose3d &Pose() const + /// \sa const gz::math::Pose3d &Pose() const /// \param[in] _pose The new link pose. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the link. This is the pose of the link /// as specified in SDF ( ... ). /// \return The pose of the link. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the link. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The new link pose. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is diff --git a/include/sdf/Material.hh b/include/sdf/Material.hh index eeddd86de..00f5ce49e 100644 --- a/include/sdf/Material.hh +++ b/include/sdf/Material.hh @@ -80,37 +80,37 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \return Ambient color. - public: ignition::math::Color Ambient() const; + public: gz::math::Color Ambient() const; /// \brief Set 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]. /// \param[in] _color Ambient color. - public: void SetAmbient(const ignition::math::Color &_color) const; + public: void SetAmbient(const gz::math::Color &_color) const; /// \brief Get the diffuse color. The diffuse color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \return Diffuse color. - public: ignition::math::Color Diffuse() const; + public: gz::math::Color Diffuse() const; /// \brief Set the diffuse color. The diffuse color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Diffuse color. - public: void SetDiffuse(const ignition::math::Color &_color) const; + public: void SetDiffuse(const gz::math::Color &_color) const; /// \brief Get the specular color. The specular color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \return Specular color. - public: ignition::math::Color Specular() const; + public: gz::math::Color Specular() const; /// \brief Set the specular color. The specular color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Specular color. - public: void SetSpecular(const ignition::math::Color &_color) const; + public: void SetSpecular(const gz::math::Color &_color) const; /// \brief Get the specular exponent. /// \return Specular exponent. @@ -124,13 +124,13 @@ namespace sdf /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \return Emissive color. - public: ignition::math::Color Emissive() const; + public: gz::math::Color Emissive() const; /// \brief Set the emissive color. The emissive color is /// specified by a set of three numbers representing red/green/blue, /// each in the range of [0,1]. /// \param[in] _color Emissive color. - public: void SetEmissive(const ignition::math::Color &_color) const; + public: void SetEmissive(const gz::math::Color &_color) const; /// \brief Get whether dynamic lighting is enabled. The default /// value is true. diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 1e61d8c2c..84677a141 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -18,7 +18,7 @@ #define SDF_MESH_HH_ #include -#include +#include #include #include #include @@ -86,11 +86,11 @@ namespace sdf /// \brief Get the mesh's scale factor. /// \return The mesh's scale factor. - public: ignition::math::Vector3d Scale() const; + public: gz::math::Vector3d Scale() const; /// \brief Set the mesh's scale factor. /// \return The mesh's scale factor. - public: void SetScale(const ignition::math::Vector3d &_scale); + public: void SetScale(const gz::math::Vector3d &_scale); /// \brief A submesh, contained with the mesh at the specified URI, may /// optionally be specified. If specified, this submesh should be used diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 454eb99c3..d05de5936 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -234,14 +234,14 @@ namespace sdf /// global coordinate frame. /// \return The pose of the model. /// \deprecated See RawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the model. - /// \sa const ignition::math::Pose3d &Pose() const + /// \sa const gz::math::Pose3d &Pose() const /// \param[in] _pose The new model pose. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the model. This is the pose of the model @@ -249,12 +249,12 @@ namespace sdf /// typically used to express the position and rotation of a model in a /// global coordinate frame. /// \return The pose of the model. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the model. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The new model pose. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the model's canonical link /// \return An immutable pointer to the canonical link diff --git a/include/sdf/NavSat.hh b/include/sdf/NavSat.hh index d22c93a6b..d79fe4911 100644 --- a/include/sdf/NavSat.hh +++ b/include/sdf/NavSat.hh @@ -22,7 +22,7 @@ #include #include -#include +#include namespace sdf { diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index fb097411f..cabd8f762 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -30,7 +30,7 @@ #include #include -#include +#include #include "sdf/Console.hh" #include "sdf/sdf_config.h" @@ -248,13 +248,13 @@ namespace sdf /// ParamPrivate::TypeToString and ParamPrivate::ValueFromStringImpl public: typedef std::variant ParamVariant; + gz::math::Angle, + gz::math::Color, + gz::math::Vector2i, + gz::math::Vector2d, + gz::math::Vector3d, + gz::math::Quaterniond, + gz::math::Pose3d> ParamVariant; /// \brief This parameter's value public: ParamVariant value; @@ -301,19 +301,19 @@ namespace sdf return "float"; else if constexpr (std::is_same_v) return "time"; - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) return "angle"; - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) return "color"; - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) return "vector2i"; - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) return "vector2d"; - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) return "vector3"; - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) return "quaternion"; - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) return "pose"; else return ""; @@ -376,7 +376,7 @@ namespace sdf { // this section for handling bool types is to keep backward behavior // TODO(anyone) remove for Fortress. For more details: - // https://github.com/ignitionrobotics/sdformat/pull/638 + // https://github.com/gazebosim/sdformat/pull/638 valueStr = lowercase(valueStr); std::stringstream tmp; diff --git a/include/sdf/Plane.hh b/include/sdf/Plane.hh index fea04517a..cddf8e2e9 100644 --- a/include/sdf/Plane.hh +++ b/include/sdf/Plane.hh @@ -17,9 +17,9 @@ #ifndef SDF_PLANE_HH_ #define SDF_PLANE_HH_ -#include -#include -#include +#include +#include +#include #include #include #include @@ -73,21 +73,21 @@ namespace sdf /// for a Visual or Collision object, then the normal is specified in the /// Visual or Collision frame, respectively. /// \return The plane normal vector. - public: ignition::math::Vector3d Normal() const; + public: gz::math::Vector3d Normal() const; - /// \brief Set the plane normal vector. The _normal vector will be - /// normalized. See ignition::math::Vector3d Normal() for more information + /// \brief Set the plane normal vector. The normal vector will be + /// normalized. See gz::math::Vector3d Normal() for more information /// about the normal vector, such as the frame in which it is specified. /// \param[in] _normal The plane normal vector. - public: void SetNormal(const ignition::math::Vector3d &_normal); + public: void SetNormal(const gz::math::Vector3d &_normal); /// \brief Get the plane size in meters. /// \return The plane size in meters. - public: ignition::math::Vector2d Size() const; + public: gz::math::Vector2d Size() const; /// \brief Set the plane size in meters. /// \param[in] _size The plane size in meters. - public: void SetSize(const ignition::math::Vector2d &_size); + public: void SetSize(const gz::math::Vector2d &_size); /// \brief Get a pointer to the SDF element that was used during /// load. @@ -95,13 +95,13 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Get the Ignition Math representation of this Plane. - /// \return A const reference to an ignition::math::Planed object. - public: const ignition::math::Planed &Shape() const; + /// \brief Get the Gazebo Math representation of this Plane. + /// \return A const reference to a gz::math::Planed object. + public: const gz::math::Planed &Shape() const; - /// \brief Get a mutable Ignition Math representation of this Plane. - /// \return A reference to an ignition::math::Planed object. - public: ignition::math::Planed &Shape(); + /// \brief Get a mutable Gazebo Math representation of this Plane. + /// \return A reference to a gz::math::Planed object. + public: gz::math::Planed &Shape(); /// \brief Private data pointer. private: PlanePrivate *dataPtr; diff --git a/include/sdf/Polyline.hh b/include/sdf/Polyline.hh index 253a7407c..f1e4de3b2 100644 --- a/include/sdf/Polyline.hh +++ b/include/sdf/Polyline.hh @@ -19,7 +19,7 @@ #include -#include +#include #include #include #include @@ -82,16 +82,16 @@ namespace sdf /// \brief Get a point by its index. /// \return Constant pointer to the point. - public: const ignition::math::Vector2d *PointByIndex(uint64_t _index) const; + public: const gz::math::Vector2d *PointByIndex(uint64_t _index) const; /// \brief Get a point by its index. /// \return Mutable pointer to the point. - public: ignition::math::Vector2d *PointByIndex(uint64_t _index); + public: gz::math::Vector2d *PointByIndex(uint64_t _index); /// \brief Add a point to the polyline. /// \param[in] _point 2D point to add. /// \return True for success. - public: bool AddPoint(const ignition::math::Vector2d &_point); + public: bool AddPoint(const gz::math::Vector2d &_point); /// \brief Remove all points from the polyline. public: void ClearPoints(); @@ -99,7 +99,7 @@ namespace sdf /// \brief Get the polyline's points. Each point has 2D coordinates in /// meters. /// \return The polyline's points. - public: const std::vector &Points() const; + public: const std::vector &Points() const; /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Scene.hh b/include/sdf/Scene.hh index 27dc1a326..e53274217 100644 --- a/include/sdf/Scene.hh +++ b/include/sdf/Scene.hh @@ -17,7 +17,7 @@ #ifndef SDF_SCENE_HH_ #define SDF_SCENE_HH_ -#include +#include #include "sdf/Element.hh" #include "sdf/Sky.hh" @@ -70,19 +70,19 @@ namespace sdf /// \brief Get the ambient color of the scene /// \return Scene ambient color - public: ignition::math::Color Ambient() const; + public: gz::math::Color Ambient() const; /// \brief Set the ambient color of the scene /// \param[in] _ambient Ambient color to set to - public: void SetAmbient(const ignition::math::Color &_ambient); + public: void SetAmbient(const gz::math::Color &_ambient); /// \brief Get the background color of the scene /// \return Scene background color - public: ignition::math::Color Background() const; + public: gz::math::Color Background() const; /// \brief Set the background color of the scene /// \param[in] _background Background color to set to - public: void SetBackground(const ignition::math::Color &_background); + public: void SetBackground(const gz::math::Color &_background); /// \brief Get whether grid is enabled /// \return True if grid is enabled diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 6baa76035..b9004ad4a 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -54,7 +54,7 @@ namespace sdf { /// \brief Get the raw Pose3 transform. /// \return The raw Pose3 transform. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is @@ -69,7 +69,7 @@ namespace sdf /// \param[in] _resolveTo The pose will be resolved with respect to this /// frame. If unset or empty, the default resolve-to frame will be used. /// \return Errors in resolving pose. - public: Errors Resolve(ignition::math::Pose3d &_pose, + public: Errors Resolve(gz::math::Pose3d &_pose, const std::string &_resolveTo = "") const; /// \brief Private constructor. @@ -80,7 +80,7 @@ namespace sdf /// if no frame is specified. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: SemanticPose( - const ignition::math::Pose3d &_pose, + const gz::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, std::weak_ptr _graph); diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index d66320d04..24f65b353 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" @@ -174,14 +174,14 @@ namespace sdf /// global coordinate frame. /// \return The pose of the sensor. /// \deprecated See RawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the sensor. - /// \sa const ignition::math::Pose3d &Pose() const + /// \sa const gz::math::Pose3d &Pose() const /// \param[in] _pose The new sensor pose. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the sensor. This is the pose of the sensor @@ -189,12 +189,12 @@ namespace sdf /// typically used to express the position and rotation of a sensor in a /// global coordinate frame. /// \return The pose of the sensor. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the sensor. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The new sensor pose. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is diff --git a/include/sdf/Sky.hh b/include/sdf/Sky.hh index 84f327a24..eb3d2e554 100644 --- a/include/sdf/Sky.hh +++ b/include/sdf/Sky.hh @@ -18,7 +18,7 @@ #ifndef SDF_SKY_HH_ #define SDF_SKY_HH_ -#include +#include #include "sdf/Element.hh" #include "sdf/Types.hh" @@ -94,11 +94,11 @@ namespace sdf /// \brief Get cloud direction angle (angle around up axis) /// \return cloud direction angle in world frame - public: ignition::math::Angle CloudDirection() const; + public: gz::math::Angle CloudDirection() const; /// \brief Set cloud direction angle (angle around up axis) /// \param[in] _angle Cloud direction angle in world frame. - public: void SetCloudDirection(const ignition::math::Angle &_angle); + public: void SetCloudDirection(const gz::math::Angle &_angle); /// \brief Get cloud humidity /// \return cloud humidity [0..1] @@ -118,11 +118,11 @@ namespace sdf /// \brief Get cloud ambient color /// \return cloud ambient color - public: ignition::math::Color CloudAmbient() const; + public: gz::math::Color CloudAmbient() const; /// \brief Set cloud ambient color /// \param[in] _ambient cloud ambient color - public: void SetCloudAmbient(const ignition::math::Color &_ambient); + public: void SetCloudAmbient(const gz::math::Color &_ambient); /// \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 diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 1fff9ec0c..cc12655e3 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -17,7 +17,7 @@ #ifndef SDF_SPHERE_HH_ #define SDF_SPHERE_HH_ -#include +#include #include #include @@ -76,13 +76,13 @@ namespace sdf /// \param[in] _radius The radius of the sphere in meters. public: void SetRadius(const double _radius); - /// \brief Get the Ignition Math representation of this Sphere. - /// \return A const reference to an ignition::math::Sphered object. - public: const ignition::math::Sphered &Shape() const; + /// \brief Get the Gazebo Math representation of this Sphere. + /// \return A const reference to a gz::math::Sphered object. + public: const gz::math::Sphered &Shape() const; - /// \brief Get a mutable Ignition Math representation of this Sphere. - /// \return A reference to an ignition::math::Sphered object. - public: ignition::math::Sphered &Shape(); + /// \brief Get a mutable Gazebo Math representation of this Sphere. + /// \return A reference to a gz::math::Sphered object. + public: gz::math::Sphered &Shape(); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh index 7e0b8ea7d..cb0286d57 100644 --- a/include/sdf/Types.hh +++ b/include/sdf/Types.hh @@ -96,7 +96,7 @@ namespace sdf /// \param[in] _g Green value (range 0 to 1 /// \param[in] _b Blue value (range 0 to 1 /// \param[in] _a Alpha value (0=transparent, 1=opaque) - /// \deprecated Use ignition::math::Color + /// \deprecated Use gz::math::Color public: Color(float _r = 0.0f, float _g = 0.0f, float _b = 0.0f, float _a = 1.0f) SDF_DEPRECATED(6.0) : r(_r), g(_g), b(_b), a(_a) diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index cd93aceea..1f7322dbd 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Box.hh" #include "sdf/Cylinder.hh" #include "sdf/Element.hh" @@ -115,26 +115,26 @@ namespace sdf /// ( ... ). /// \return The pose of the visual object. /// \deprecated See SetRawPose. - public: const ignition::math::Pose3d &Pose() const + public: const gz::math::Pose3d &Pose() const SDF_DEPRECATED(9.0); /// \brief Set the pose of the visual object. - /// \sa const ignition::math::Pose3d &Pose() const + /// \sa const gz::math::Pose3d &Pose() const /// \param[in] _pose The pose of the visual object. /// \deprecated See SetRawPose. - public: void SetPose(const ignition::math::Pose3d &_pose) + public: void SetPose(const gz::math::Pose3d &_pose) SDF_DEPRECATED(9.0); /// \brief Get the pose of the visual object. This is the pose of the /// visual as specified in SDF /// ( ... ). /// \return The pose of the visual object. - public: const ignition::math::Pose3d &RawPose() const; + public: const gz::math::Pose3d &RawPose() const; /// \brief Set the pose of the visual object. - /// \sa const ignition::math::Pose3d &RawPose() const + /// \sa const gz::math::Pose3d &RawPose() const /// \param[in] _pose The pose of the visual object. - public: void SetRawPose(const ignition::math::Pose3d &_pose); + public: void SetRawPose(const gz::math::Pose3d &_pose); /// \brief Get the name of the coordinate frame relative to which this /// object's pose is expressed. An empty value indicates that the frame is diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 3e333a104..b2a1662ca 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -18,7 +18,7 @@ #define SDF_WORLD_HH_ #include -#include +#include #include "sdf/Atmosphere.hh" #include "sdf/Element.hh" @@ -105,26 +105,26 @@ namespace sdf /// \brief Get the wind linear velocity in the global/world coordinate /// frame. Units are meters per second \f$(\frac{m}{s})\f$ /// \return Linear velocity of wind in the global/world coordinate frame. - /// \sa void SetWindLinearVelocity(const ignition::math::Vector3d &_wind) - public: ignition::math::Vector3d WindLinearVelocity() const; + /// \sa void SetWindLinearVelocity(const gz::math::Vector3d &_wind) + public: gz::math::Vector3d WindLinearVelocity() const; /// \brief Set the wind linear velocity in the global/world coordinate /// frame. Units are meters per second \f$(\frac{m}{s})\f$ /// \param[in] _wind The new linear velocity of wind. - /// \sa ignition::math::Vector3d WindLinearVelocity() const - public: void SetWindLinearVelocity(const ignition::math::Vector3d &_wind); + /// \sa gz::math::Vector3d WindLinearVelocity() const + public: void SetWindLinearVelocity(const gz::math::Vector3d &_wind); /// \brief Get the acceleration due to gravity. The default value is /// Earth's standard gravity at sea level, which equals /// [0, 0, -9.80665] \f$(\frac{m}{s^2})\f$ /// \return Gravity vector in meters per second squared /// \f$(\frac{m}{s^2})\f$ - public: ignition::math::Vector3d Gravity() const; + public: gz::math::Vector3d Gravity() const; /// \brief Set the acceleration due to gravity. Units are meters per /// second squared \f$(\frac{m}{s^2})\f$ /// \param[in] _gravity The new gravity vector. - public: void SetGravity(const ignition::math::Vector3d &_gravity); + public: void SetGravity(const gz::math::Vector3d &_gravity); /// \brief Get the magnetic vector in Tesla, expressed in /// a coordinate frame defined by the SphericalCoordinates property. @@ -132,7 +132,7 @@ namespace sdf /// element. /// \return Magnetic field vector. /// \sa SphericalCoordinates - public: ignition::math::Vector3d MagneticField() const; + public: gz::math::Vector3d MagneticField() const; /// \brief Set the magnetic vector in Tesla, expressed in /// a coordinate frame defined by the SphericalCoordinate. @@ -140,7 +140,7 @@ namespace sdf /// element. /// \param[in] _mag The new magnetic field vector. /// \sa SphericalCoordinates - public: void SetMagneticField(const ignition::math::Vector3d &_mag); + public: void SetMagneticField(const gz::math::Vector3d &_mag); /// \brief Get the number of models. /// \return Number of models contained in this World object. diff --git a/src/Actor.cc b/src/Actor.cc index 1ff483879..2da499829 100644 --- a/src/Actor.cc +++ b/src/Actor.cc @@ -16,7 +16,7 @@ */ #include #include -#include +#include #include "sdf/Actor.hh" #include "sdf/Error.hh" #include "Utils.hh" @@ -49,7 +49,7 @@ class sdf::WaypointPrivate public: double time = 0.0; /// \brief Pose to be reached. - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; }; /// \brief Trajectory private data. @@ -75,7 +75,7 @@ class sdf::ActorPrivate public: std::string name = "__default__"; /// \brief Pose of the actor. - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the actor. public: std::string poseRelativeTo = ""; @@ -313,7 +313,7 @@ Errors Waypoint::Load(ElementPtr _sdf) } this->dataPtr->time = timeValue.first; - std::pair posePair = _sdf->Get + std::pair posePair = _sdf->Get ("pose", this->dataPtr->pose); if (!posePair.second) { @@ -338,13 +338,13 @@ void Waypoint::SetTime(double _time) } ///////////////////////////////////////////////// -ignition::math::Pose3d Waypoint::Pose() const +gz::math::Pose3d Waypoint::Pose() const { return this->dataPtr->pose; } ///////////////////////////////////////////////// -void Waypoint::SetPose(const ignition::math::Pose3d &_pose) +void Waypoint::SetPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } @@ -643,13 +643,13 @@ void Actor::SetName(const std::string &_name) } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Actor::Pose() const +const gz::math::Pose3d &Actor::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Actor::RawPose() const +const gz::math::Pose3d &Actor::RawPose() const { return this->dataPtr->pose; } @@ -667,13 +667,13 @@ const std::string &Actor::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Actor::SetPose(const ignition::math::Pose3d &_pose) +void Actor::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Actor::SetRawPose(const ignition::math::Pose3d &_pose) +void Actor::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } diff --git a/src/Actor_TEST.cc b/src/Actor_TEST.cc index bac434933..90e98a646 100644 --- a/src/Actor_TEST.cc +++ b/src/Actor_TEST.cc @@ -16,7 +16,7 @@ */ #include -#include +#include #include "sdf/Actor.hh" ///////////////////////////////////////////////// @@ -145,7 +145,7 @@ TEST(DOMActor, DefaultConstruction) { sdf::Actor actor; EXPECT_EQ("__default__", actor.Name()); - EXPECT_EQ(ignition::math::Pose3d::Zero, actor.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, actor.RawPose()); EXPECT_EQ("", actor.PoseRelativeTo()); EXPECT_EQ(nullptr, actor.Element()); EXPECT_EQ("__default__", actor.SkinFilename()); @@ -363,7 +363,7 @@ TEST(DOMWaypoint, DefaultConstruction) { sdf::Waypoint waypoint; EXPECT_DOUBLE_EQ(waypoint.Time(), 0.0); - EXPECT_EQ(waypoint.Pose(), ignition::math::Pose3d::Zero); + EXPECT_EQ(waypoint.Pose(), gz::math::Pose3d::Zero); } ////////////////////////////////////////////////// @@ -395,7 +395,7 @@ TEST(DOMWaypoint, CopyAssignmentOperator) TEST(DOMWaypoint, MoveConstructor) { sdf::Waypoint waypoint1; - ignition::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); + gz::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); waypoint1.SetTime(1.23); waypoint1.SetPose(pose1); @@ -408,7 +408,7 @@ TEST(DOMWaypoint, MoveConstructor) TEST(DOMWaypoint, MoveAssignment) { sdf::Waypoint waypoint1; - ignition::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); + gz::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); waypoint1.SetTime(1.23); waypoint1.SetPose(pose1); @@ -422,11 +422,11 @@ TEST(DOMWaypoint, MoveAssignment) TEST(DOMWaypoint, CopyAssignmentAfterMove) { sdf::Waypoint waypoint1; - ignition::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); + gz::math::Pose3d pose1(3, 2, 1, 0, IGN_PI, 0); waypoint1.SetTime(1.23); waypoint1.SetPose(pose1); sdf::Waypoint waypoint2; - ignition::math::Pose3d pose2(1, 2, 3, 1, 2, IGN_PI); + gz::math::Pose3d pose2(1, 2, 3, 1, 2, IGN_PI); waypoint2.SetTime(3.45); waypoint2.SetPose(pose2); diff --git a/src/AirPressure.cc b/src/AirPressure.cc index 667fb164f..db7ecbdda 100644 --- a/src/AirPressure.cc +++ b/src/AirPressure.cc @@ -120,7 +120,7 @@ bool AirPressure::operator!=(const AirPressure &_air) const bool AirPressure::operator==(const AirPressure &_air) const { return this->dataPtr->noise == _air.dataPtr->noise && - ignition::math::equal(this->dataPtr->referenceAltitude, + gz::math::equal(this->dataPtr->referenceAltitude, _air.dataPtr->referenceAltitude); } diff --git a/src/Atmosphere.cc b/src/Atmosphere.cc index b8901957d..408e15efe 100644 --- a/src/Atmosphere.cc +++ b/src/Atmosphere.cc @@ -16,7 +16,7 @@ */ #include -#include +#include #include "sdf/Atmosphere.hh" using namespace sdf; @@ -28,7 +28,7 @@ class sdf::AtmospherePrivate public: AtmosphereType type {AtmosphereType::ADIABATIC}; /// \brief Temperature at sea level in kelvins. - public: ignition::math::Temperature temperature {288.15}; + public: gz::math::Temperature temperature {288.15}; /// \brief Temperature gradient with respect to increasing altitude at sea /// level in units of K/m. @@ -135,13 +135,13 @@ void Atmosphere::SetType(const AtmosphereType _type) } ////////////////////////////////////////////////// -ignition::math::Temperature Atmosphere::Temperature() const +gz::math::Temperature Atmosphere::Temperature() const { return this->dataPtr->temperature; } ////////////////////////////////////////////////// -void Atmosphere::SetTemperature(const ignition::math::Temperature &_temp) +void Atmosphere::SetTemperature(const gz::math::Temperature &_temp) { this->dataPtr->temperature = _temp; } @@ -175,8 +175,8 @@ bool Atmosphere::operator==(const Atmosphere &_atmosphere) { return this->dataPtr->type == _atmosphere.dataPtr->type && this->dataPtr->temperature == _atmosphere.dataPtr->temperature && - ignition::math::equal(this->dataPtr->temperatureGradient, + gz::math::equal(this->dataPtr->temperatureGradient, _atmosphere.dataPtr->temperatureGradient) && - ignition::math::equal(this->dataPtr->pressure, + gz::math::equal(this->dataPtr->pressure, _atmosphere.dataPtr->pressure); } diff --git a/src/Atmosphere_TEST.cc b/src/Atmosphere_TEST.cc index a947d987e..c444aea7e 100644 --- a/src/Atmosphere_TEST.cc +++ b/src/Atmosphere_TEST.cc @@ -16,7 +16,7 @@ */ #include -#include +#include #include "sdf/World.hh" ///////////////////////////////////////////////// diff --git a/src/Box.cc b/src/Box.cc index d16404c59..6b37b0044 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ -#include +#include #include "sdf/Box.hh" using namespace sdf; @@ -23,7 +23,7 @@ using namespace sdf; class sdf::BoxPrivate { // Size of the box - public: ignition::math::Boxd box{ignition::math::Vector3d::One}; + public: gz::math::Boxd box{gz::math::Vector3d::One}; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -97,8 +97,8 @@ Errors Box::Load(ElementPtr _sdf) if (_sdf->HasElement("size")) { - std::pair pair = - _sdf->Get("size", this->dataPtr->box.Size()); + std::pair pair = + _sdf->Get("size", this->dataPtr->box.Size()); if (!pair.second) { @@ -119,13 +119,13 @@ Errors Box::Load(ElementPtr _sdf) } ////////////////////////////////////////////////// -ignition::math::Vector3d Box::Size() const +gz::math::Vector3d Box::Size() const { return this->dataPtr->box.Size(); } ////////////////////////////////////////////////// -void Box::SetSize(const ignition::math::Vector3d &_size) +void Box::SetSize(const gz::math::Vector3d &_size) { this->dataPtr->box.SetSize(_size); } @@ -137,13 +137,13 @@ sdf::ElementPtr Box::Element() const } ///////////////////////////////////////////////// -const ignition::math::Boxd &Box::Shape() const +const gz::math::Boxd &Box::Shape() const { return this->dataPtr->box; } ///////////////////////////////////////////////// -ignition::math::Boxd &Box::Shape() +gz::math::Boxd &Box::Shape() { return this->dataPtr->box; } diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index b77bd23a7..a9448858c 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -25,16 +25,16 @@ TEST(DOMBox, Construction) sdf::Box box; EXPECT_EQ(nullptr, box.Element()); - EXPECT_EQ(ignition::math::Vector3d::One, box.Size()); + EXPECT_EQ(gz::math::Vector3d::One, box.Size()); - box.SetSize(ignition::math::Vector3d::Zero); - EXPECT_EQ(ignition::math::Vector3d::Zero, box.Size()); + box.SetSize(gz::math::Vector3d::Zero); + EXPECT_EQ(gz::math::Vector3d::Zero, box.Size()); } ///////////////////////////////////////////////// TEST(DOMBox, MoveConstructor) { - const ignition::math::Vector3d size(1, 2, 3); + const gz::math::Vector3d size(1, 2, 3); sdf::Box box; box.SetSize(size); @@ -49,7 +49,7 @@ TEST(DOMBox, MoveConstructor) ///////////////////////////////////////////////// TEST(DOMBox, CopyConstructor) { - const ignition::math::Vector3d size(0.1, 0.2, 0.3); + const gz::math::Vector3d size(0.1, 0.2, 0.3); sdf::Box box; box.SetSize(size); @@ -61,7 +61,7 @@ TEST(DOMBox, CopyConstructor) ///////////////////////////////////////////////// TEST(DOMBox, CopyAssigmentOperator) { - const ignition::math::Vector3d size(0.2, 0.3, 0.4); + const gz::math::Vector3d size(0.2, 0.3, 0.4); sdf::Box box; box.SetSize(size); @@ -74,7 +74,7 @@ TEST(DOMBox, CopyAssigmentOperator) ///////////////////////////////////////////////// TEST(DOMBox, MoveAssignmentConstructor) { - const ignition::math::Vector3d size(1, 2, 3); + const gz::math::Vector3d size(1, 2, 3); sdf::Box box; box.SetSize(size); @@ -87,8 +87,8 @@ TEST(DOMBox, MoveAssignmentConstructor) ///////////////////////////////////////////////// TEST(DOMBox, CopyAssignmentAfterMove) { - const ignition::math::Vector3d size1(1, 2, 3); - const ignition::math::Vector3d size2(4, 5, 6); + const gz::math::Vector3d size1(1, 2, 3); + const gz::math::Vector3d size2(4, 5, 6); sdf::Box box1; box1.SetSize(size1); @@ -138,8 +138,8 @@ TEST(DOMBox, Load) TEST(DOMBox, Shape) { sdf::Box box; - EXPECT_EQ(ignition::math::Vector3d::One, box.Size()); + EXPECT_EQ(gz::math::Vector3d::One, box.Size()); - box.Shape().SetSize(ignition::math::Vector3d(1, 2, 3)); - EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), box.Size()); + box.Shape().SetSize(gz::math::Vector3d(1, 2, 3)); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), box.Size()); } diff --git a/src/Camera.cc b/src/Camera.cc index 572f882fa..c05ea0fa5 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -58,7 +58,7 @@ class sdf::CameraPrivate public: std::string name = ""; /// \brief Horizontal fied of view. - public: ignition::math::Angle hfov{1.047}; + public: gz::math::Angle hfov{1.047}; /// \brief Image width. public: uint32_t imageWidth{320}; @@ -118,10 +118,10 @@ class sdf::CameraPrivate public: double distortionP2{0.0}; /// \brief The distortion center or principal point - public: ignition::math::Vector2d distortionCenter{0.5, 0.5}; + public: gz::math::Vector2d distortionCenter{0.5, 0.5}; /// \brief Pose of the link - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; @@ -148,7 +148,7 @@ class sdf::CameraPrivate public: std::string lensFun{"tan"}; /// \brief Lens cutoff angle. - public: ignition::math::Angle lensCutoffAngle{IGN_PI_2}; + public: gz::math::Angle lensCutoffAngle{IGN_PI_2}; /// \brief lens environment texture size. public: int lensEnvTextureSize{256}; @@ -264,7 +264,7 @@ Errors Camera::Load(ElementPtr _sdf) if (this->dataPtr->cameraInfoTopic == "__default__") this->dataPtr->cameraInfoTopic = ""; - this->dataPtr->hfov = _sdf->Get("horizontal_fov", + this->dataPtr->hfov = _sdf->Get("horizontal_fov", this->dataPtr->hfov).first; // Read the distortion @@ -283,7 +283,7 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->distortionP2 = elem->Get("p2", this->dataPtr->distortionP2).first; - this->dataPtr->distortionCenter = elem->Get( + this->dataPtr->distortionCenter = elem->Get( "center", this->dataPtr->distortionCenter).first; } @@ -380,7 +380,7 @@ Errors Camera::Load(ElementPtr _sdf) this->dataPtr->lensType).first; this->dataPtr->lensScaleToHfov = elem->Get("scale_to_hfov", this->dataPtr->lensScaleToHfov).first; - this->dataPtr->lensCutoffAngle = elem->Get( + this->dataPtr->lensCutoffAngle = elem->Get( "cutoff_angle", this->dataPtr->lensCutoffAngle).first; this->dataPtr->lensEnvTextureSize = elem->Get("env_texture_size", this->dataPtr->lensEnvTextureSize).first; @@ -473,13 +473,13 @@ void Camera::SetName(const std::string &_name) } ///////////////////////////////////////////////// -ignition::math::Angle Camera::HorizontalFov() const +gz::math::Angle Camera::HorizontalFov() const { return this->dataPtr->hfov; } ///////////////////////////////////////////////// -void Camera::SetHorizontalFov(const ignition::math::Angle &_hfov) +void Camera::SetHorizontalFov(const gz::math::Angle &_hfov) { this->dataPtr->hfov = _hfov; } @@ -662,8 +662,8 @@ bool Camera::operator==(const Camera &_cam) const this->ImageWidth() == _cam.ImageWidth() && this->ImageHeight() == _cam.ImageHeight() && this->PixelFormat() == _cam.PixelFormat() && - ignition::math::equal(this->NearClip(), _cam.NearClip()) && - ignition::math::equal(this->FarClip(), _cam.FarClip()) && + gz::math::equal(this->NearClip(), _cam.NearClip()) && + gz::math::equal(this->FarClip(), _cam.FarClip()) && this->SaveFrames() == _cam.SaveFrames() && this->SaveFramesPath() == _cam.SaveFramesPath() && this->ImageNoise() == _cam.ImageNoise() && @@ -749,26 +749,26 @@ void Camera::SetDistortionP2(double _p2) } ////////////////////////////////////////////////// -const ignition::math::Vector2d &Camera::DistortionCenter() const +const gz::math::Vector2d &Camera::DistortionCenter() const { return this->dataPtr->distortionCenter; } ////////////////////////////////////////////////// void Camera::SetDistortionCenter( - const ignition::math::Vector2d &_center) const + const gz::math::Vector2d &_center) const { this->dataPtr->distortionCenter = _center; } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Camera::Pose() const +const gz::math::Pose3d &Camera::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Camera::RawPose() const +const gz::math::Pose3d &Camera::RawPose() const { return this->dataPtr->pose; } @@ -786,13 +786,13 @@ const std::string &Camera::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Camera::SetPose(const ignition::math::Pose3d &_pose) +void Camera::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Camera::SetRawPose(const ignition::math::Pose3d &_pose) +void Camera::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } @@ -894,13 +894,13 @@ void Camera::SetLensFunction(const std::string &_fun) } ///////////////////////////////////////////////// -ignition::math::Angle Camera::LensCutoffAngle() const +gz::math::Angle Camera::LensCutoffAngle() const { return this->dataPtr->lensCutoffAngle; } ///////////////////////////////////////////////// -void Camera::SetLensCutoffAngle(const ignition::math::Angle &_angle) +void Camera::SetLensCutoffAngle(const gz::math::Angle &_angle) { this->dataPtr->lensCutoffAngle = _angle; } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 448a8fd6d..ab7d4aa11 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -98,13 +98,13 @@ TEST(DOMCamera, Construction) cam.SetDistortionP2(0.2); EXPECT_DOUBLE_EQ(0.2, cam.DistortionP2()); - EXPECT_EQ(ignition::math::Vector2d(0.5, 0.5), cam.DistortionCenter()); - cam.SetDistortionCenter(ignition::math::Vector2d(0.1, 0.2)); - EXPECT_EQ(ignition::math::Vector2d(0.1, 0.2), cam.DistortionCenter()); + EXPECT_EQ(gz::math::Vector2d(0.5, 0.5), cam.DistortionCenter()); + cam.SetDistortionCenter(gz::math::Vector2d(0.1, 0.2)); + EXPECT_EQ(gz::math::Vector2d(0.1, 0.2), cam.DistortionCenter()); - EXPECT_EQ(ignition::math::Pose3d::Zero, cam.RawPose()); - cam.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), cam.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, cam.RawPose()); + cam.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), cam.RawPose()); EXPECT_TRUE(cam.PoseRelativeTo().empty()); cam.SetPoseRelativeTo("/frame"); diff --git a/src/Collision.cc b/src/Collision.cc index 47f4c33e5..163374f88 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -16,7 +16,7 @@ */ #include #include -#include +#include #include "sdf/Collision.hh" #include "sdf/Error.hh" #include "sdf/Geometry.hh" @@ -32,7 +32,7 @@ class sdf::CollisionPrivate public: std::string name = ""; /// \brief Pose of the collision object - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; @@ -176,13 +176,13 @@ void Collision::SetSurface(const sdf::Surface &_surface) } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Collision::Pose() const +const gz::math::Pose3d &Collision::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Collision::RawPose() const +const gz::math::Pose3d &Collision::RawPose() const { return this->dataPtr->pose; } @@ -200,13 +200,13 @@ const std::string &Collision::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Collision::SetPose(const ignition::math::Pose3d &_pose) +void Collision::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Collision::SetRawPose(const ignition::math::Pose3d &_pose) +void Collision::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index c830f15aa..686a12bb0 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -30,19 +30,19 @@ TEST(DOMcollision, Construction) collision.SetName("test_collison"); EXPECT_EQ(collision.Name(), "test_collison"); - EXPECT_EQ(ignition::math::Pose3d::Zero, collision.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, collision.RawPose()); EXPECT_TRUE(collision.PoseRelativeTo().empty()); { auto semanticPose = collision.SemanticPose(); EXPECT_EQ(collision.RawPose(), semanticPose.RawPose()); EXPECT_TRUE(semanticPose.RelativeTo().empty()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } collision.SetRawPose({-10, -20, -30, IGN_PI, IGN_PI, IGN_PI}); - EXPECT_EQ(ignition::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), + EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), collision.RawPose()); collision.SetPoseRelativeTo("link"); @@ -51,7 +51,7 @@ TEST(DOMcollision, Construction) auto semanticPose = collision.SemanticPose(); EXPECT_EQ(collision.RawPose(), semanticPose.RawPose()); EXPECT_EQ("link", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } @@ -74,7 +74,7 @@ TEST(DOMCollision, MoveConstructor) collision.SetRawPose({-10, -20, -30, IGN_PI, IGN_PI, IGN_PI}); sdf::Collision collision2(std::move(collision)); - EXPECT_EQ(ignition::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), + EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), collision2.RawPose()); } @@ -85,7 +85,7 @@ TEST(DOMCollision, CopyConstructor) collision.SetRawPose({-10, -20, -30, IGN_PI, IGN_PI, IGN_PI}); sdf::Collision collision2(collision); - EXPECT_EQ(ignition::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), + EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), collision2.RawPose()); } @@ -97,7 +97,7 @@ TEST(DOMCollision, MoveAssignment) sdf::Collision collision2; collision2 = std::move(collision); - EXPECT_EQ(ignition::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), + EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), collision2.RawPose()); } @@ -109,7 +109,7 @@ TEST(DOMCollision, CopyAssignment) sdf::Collision collision2; collision2 = collision; - EXPECT_EQ(ignition::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), + EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), collision2.RawPose()); } @@ -128,9 +128,9 @@ TEST(DOMCollision, CopyAssignmentAfterMove) collision1 = collision2; collision2 = tmp; - EXPECT_EQ(ignition::math::Pose3d(-20, -30, -40, IGN_PI, IGN_PI, IGN_PI), + EXPECT_EQ(gz::math::Pose3d(-20, -30, -40, IGN_PI, IGN_PI, IGN_PI), collision1.RawPose()); - EXPECT_EQ(ignition::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), + EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), collision2.RawPose()); } diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 0f2fe3cb3..3104760bc 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -22,7 +22,7 @@ using namespace sdf; class sdf::CylinderPrivate { // A cylinder with a length and radius if 1 meter. - public: ignition::math::Cylinderd cylinder{1.0, 1.0}; + public: gz::math::Cylinderd cylinder{1.0, 1.0}; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -167,13 +167,13 @@ sdf::ElementPtr Cylinder::Element() const } ///////////////////////////////////////////////// -const ignition::math::Cylinderd &Cylinder::Shape() const +const gz::math::Cylinderd &Cylinder::Shape() const { return this->dataPtr->cylinder; } ///////////////////////////////////////////////// -ignition::math::Cylinderd &Cylinder::Shape() +gz::math::Cylinderd &Cylinder::Shape() { return this->dataPtr->cylinder; } diff --git a/src/Frame.cc b/src/Frame.cc index c6a5737df..d17773898 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -15,7 +15,7 @@ * */ #include -#include +#include #include "sdf/Frame.hh" #include "sdf/Error.hh" #include "sdf/Types.hh" @@ -33,7 +33,7 @@ class sdf::FramePrivate public: std::string attachedTo = ""; /// \brief Pose of the frame object - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Name of the relative-to frame. public: std::string poseRelativeTo = ""; @@ -164,7 +164,7 @@ const std::string &Frame::AttachedTo() const } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Frame::RawPose() const +const gz::math::Pose3d &Frame::RawPose() const { return this->dataPtr->pose; } @@ -182,7 +182,7 @@ void Frame::SetAttachedTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Frame::SetRawPose(const ignition::math::Pose3d &_pose) +void Frame::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 86ac7ae53..0ae02d5d5 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -51,16 +51,16 @@ inline namespace SDF_VERSION_NAMESPACE { /// source to the starting vertex, or a NullVertex paired with an empty /// vector if a cycle or vertex with multiple incoming edges are detected. template -std::pair &, - std::vector< ignition::math::graph::DirectedEdge > > +std::pair &, + std::vector< gz::math::graph::DirectedEdge > > FindSourceVertex( - const ignition::math::graph::DirectedGraph &_graph, - const ignition::math::graph::VertexId _id, + const gz::math::graph::DirectedGraph &_graph, + const gz::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = ignition::math::graph::DirectedEdge; - using Vertex = ignition::math::graph::Vertex; - using VertexId = ignition::math::graph::VertexId; + using DirectedEdge = gz::math::graph::DirectedEdge; + using Vertex = gz::math::graph::Vertex; + using VertexId = gz::math::graph::VertexId; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -115,16 +115,16 @@ FindSourceVertex( /// sink to the starting vertex, or a NullVertex paired with an empty /// vector if a cycle or vertex with multiple outgoing edges are detected. template -std::pair &, - std::vector< ignition::math::graph::DirectedEdge > > +std::pair &, + std::vector< gz::math::graph::DirectedEdge > > FindSinkVertex( - const ignition::math::graph::DirectedGraph &_graph, - const ignition::math::graph::VertexId _id, + const gz::math::graph::DirectedGraph &_graph, + const gz::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = ignition::math::graph::DirectedEdge; - using Vertex = ignition::math::graph::Vertex; - using VertexId = ignition::math::graph::VertexId; + using DirectedEdge = gz::math::graph::DirectedEdge; + using Vertex = gz::math::graph::Vertex; + using VertexId = gz::math::graph::VertexId; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -1271,7 +1271,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) // check graph for cycles by resolving pose of each vertex relative to root for (auto const &namePair : _in.map) { - ignition::math::Pose3d pose; + gz::math::Pose3d pose; Errors e = resolvePoseRelativeToRoot(pose, _in, namePair.first); errors.insert(errors.end(), e.begin(), e.end()); } @@ -1360,7 +1360,7 @@ Errors resolveFrameAttachedToBody( ///////////////////////////////////////////////// Errors resolvePoseRelativeToRoot( - ignition::math::Pose3d &_pose, + gz::math::Pose3d &_pose, const PoseRelativeToGraph &_graph, const std::string &_vertexName) { @@ -1398,7 +1398,7 @@ Errors resolvePoseRelativeToRoot( return errors; } - ignition::math::Pose3d pose; + gz::math::Pose3d pose; for (auto const &edge : incomingVertexEdges.second) { pose = edge.Data() * pose; @@ -1414,14 +1414,14 @@ Errors resolvePoseRelativeToRoot( ///////////////////////////////////////////////// Errors resolvePose( - ignition::math::Pose3d &_pose, + gz::math::Pose3d &_pose, const PoseRelativeToGraph &_graph, const std::string &_frameName, const std::string &_resolveTo) { Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameName); - ignition::math::Pose3d poseR; + gz::math::Pose3d poseR; Errors errorsR = resolvePoseRelativeToRoot(poseR, _graph, _resolveTo); errors.insert(errors.end(), errorsR.begin(), errorsR.end()); diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index a6c1a59df..8f50dd19f 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include "sdf/Error.hh" #include "sdf/Types.hh" @@ -73,11 +73,11 @@ namespace sdf /// \brief A DirectedGraph with a vertex for each frame and edges pointing /// to the frame to which another frame is attached. Each vertex stores /// its FrameType and each edge can store a boolean value. - using GraphType = ignition::math::graph::DirectedGraph; + using GraphType = gz::math::graph::DirectedGraph; GraphType graph; /// \brief A Map from Vertex names to Vertex Ids. - using MapType = std::map; + using MapType = std::map; MapType map; /// \brief Name of scope vertex, either __model__ or world. @@ -92,12 +92,12 @@ namespace sdf /// When well-formed, it should form a directed tree with a root vertex /// named __model__ or world. Each vertex stores its FrameType and each edge /// stores the Pose3 between those frames. - using Pose3d = ignition::math::Pose3d; - using GraphType = ignition::math::graph::DirectedGraph; + using Pose3d = gz::math::Pose3d; + using GraphType = gz::math::graph::DirectedGraph; GraphType graph; /// \brief A Map from Vertex names to Vertex Ids. - using MapType = std::map; + using MapType = std::map; MapType map; /// \brief Name of source vertex, either __model__ or world. @@ -167,7 +167,7 @@ namespace sdf /// \param[in] _vertexName Name of vertex whose pose is to be computed. /// \return Errors. Errors resolvePoseRelativeToRoot( - ignition::math::Pose3d &_pose, + gz::math::Pose3d &_pose, const PoseRelativeToGraph &_graph, const std::string &_vertexName); @@ -179,7 +179,7 @@ namespace sdf /// to be resolved. /// \return Errors. Errors resolvePose( - ignition::math::Pose3d &_pose, + gz::math::Pose3d &_pose, const PoseRelativeToGraph &_graph, const std::string &_frameName, const std::string &_resolveTo); diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index 5f38c4df0..6457c5d56 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/Frame.hh" @@ -224,43 +224,43 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) EXPECT_EQ(1u, graph.map.count("F4")); // Test resolvePoseRelativeToRoot for each frame. - ignition::math::Pose3d pose; + gz::math::Pose3d pose; EXPECT_TRUE(sdf::resolvePoseRelativeToRoot(pose, graph, "__model__").empty()); - EXPECT_EQ(ignition::math::Pose3d::Zero, pose); + EXPECT_EQ(gz::math::Pose3d::Zero, pose); EXPECT_TRUE(sdf::resolvePoseRelativeToRoot(pose, graph, "P").empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), pose); EXPECT_TRUE(sdf::resolvePoseRelativeToRoot(pose, graph, "F1").empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 1, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 0, 1, 0, 0, 0), pose); EXPECT_TRUE(sdf::resolvePoseRelativeToRoot(pose, graph, "C").empty()); - EXPECT_EQ(ignition::math::Pose3d(2, 0, 0, 0, IGN_PI/2, 0), pose); + EXPECT_EQ(gz::math::Pose3d(2, 0, 0, 0, IGN_PI/2, 0), pose); EXPECT_TRUE(sdf::resolvePoseRelativeToRoot(pose, graph, "F2").empty()); - EXPECT_EQ(ignition::math::Pose3d(4, 0, 0, 0, IGN_PI/2, 0), pose); + EXPECT_EQ(gz::math::Pose3d(4, 0, 0, 0, IGN_PI/2, 0), pose); EXPECT_TRUE(sdf::resolvePoseRelativeToRoot(pose, graph, "J").empty()); - EXPECT_EQ(ignition::math::Pose3d(2, 3, 0, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(2, 3, 0, 0, 0, 0), pose); EXPECT_TRUE(sdf::resolvePoseRelativeToRoot(pose, graph, "F3").empty()); - EXPECT_EQ(ignition::math::Pose3d(2, 3, 3, 0, IGN_PI/2, 0), pose); + EXPECT_EQ(gz::math::Pose3d(2, 3, 3, 0, IGN_PI/2, 0), pose); EXPECT_TRUE(sdf::resolvePoseRelativeToRoot(pose, graph, "F4").empty()); - EXPECT_EQ(ignition::math::Pose3d(6, 3, 3, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(6, 3, 3, 0, 0, 0), pose); // Test resolvePose for each frame with its relative_to value. // Numbers should match the raw pose value in the model file. EXPECT_TRUE(sdf::resolvePose(pose, graph, "P", "__model__").empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), pose); EXPECT_TRUE(sdf::resolvePose(pose, graph, "C", "__model__").empty()); - EXPECT_EQ(ignition::math::Pose3d(2, 0, 0, 0, IGN_PI/2, 0), pose); + EXPECT_EQ(gz::math::Pose3d(2, 0, 0, 0, IGN_PI/2, 0), pose); EXPECT_TRUE(sdf::resolvePose(pose, graph, "J", "C").empty()); - EXPECT_EQ(ignition::math::Pose3d(0, 3, 0, 0, -IGN_PI/2, 0), pose); + EXPECT_EQ(gz::math::Pose3d(0, 3, 0, 0, -IGN_PI/2, 0), pose); EXPECT_TRUE(sdf::resolvePose(pose, graph, "F1", "P").empty()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 1, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(0, 0, 1, 0, 0, 0), pose); EXPECT_TRUE(sdf::resolvePose(pose, graph, "F2", "C").empty()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 2, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(0, 0, 2, 0, 0, 0), pose); EXPECT_TRUE(sdf::resolvePose(pose, graph, "F3", "J").empty()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 3, 0, IGN_PI/2, 0), pose); + EXPECT_EQ(gz::math::Pose3d(0, 0, 3, 0, IGN_PI/2, 0), pose); EXPECT_TRUE(sdf::resolvePose(pose, graph, "F4", "F3").empty()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 4, 0, -IGN_PI/2, 0), pose); + EXPECT_EQ(gz::math::Pose3d(0, 0, 4, 0, -IGN_PI/2, 0), pose); // Try to resolve invalid frame names errors = sdf::resolvePose(pose, graph, "invalid", "__model__"); diff --git a/src/Frame_TEST.cc b/src/Frame_TEST.cc index 42c806573..d2a2f80b3 100644 --- a/src/Frame_TEST.cc +++ b/src/Frame_TEST.cc @@ -30,13 +30,13 @@ TEST(DOMframe, Construction) EXPECT_EQ(frame.Name(), "test_frame"); EXPECT_TRUE(frame.AttachedTo().empty()); - EXPECT_EQ(ignition::math::Pose3d::Zero, frame.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, frame.RawPose()); EXPECT_TRUE(frame.PoseRelativeTo().empty()); { auto semanticPose = frame.SemanticPose(); - EXPECT_EQ(ignition::math::Pose3d::Zero, semanticPose.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, semanticPose.RawPose()); EXPECT_TRUE(semanticPose.RelativeTo().empty()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } @@ -45,13 +45,13 @@ TEST(DOMframe, Construction) EXPECT_EQ("attachment", frame.AttachedTo()); frame.SetRawPose({-10, -20, -30, IGN_PI, IGN_PI, IGN_PI}); - EXPECT_EQ(ignition::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), + EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, IGN_PI, IGN_PI, IGN_PI), frame.RawPose()); { auto semanticPose = frame.SemanticPose(); EXPECT_EQ(frame.RawPose(), semanticPose.RawPose()); EXPECT_EQ("attachment", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } @@ -62,7 +62,7 @@ TEST(DOMframe, Construction) auto semanticPose = frame.SemanticPose(); EXPECT_EQ(frame.RawPose(), semanticPose.RawPose()); EXPECT_EQ("link", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 3008fec8c..3164baaf9 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -63,7 +63,7 @@ TEST(DOMGeometry, CopyConstructor) sdf::Geometry geometry; geometry.SetType(sdf::GeometryType::BOX); sdf::Box boxShape; - boxShape.SetSize(ignition::math::Vector3d(1, 2, 3)); + boxShape.SetSize(gz::math::Vector3d(1, 2, 3)); geometry.SetBoxShape(boxShape); sdf::Geometry geometry2(geometry); @@ -76,7 +76,7 @@ TEST(DOMGeometry, AssignmentOperator) sdf::Geometry geometry; geometry.SetType(sdf::GeometryType::BOX); sdf::Box boxShape; - boxShape.SetSize(ignition::math::Vector3d(1, 2, 3)); + boxShape.SetSize(gz::math::Vector3d(1, 2, 3)); geometry.SetBoxShape(boxShape); sdf::Geometry geometry2; @@ -90,7 +90,7 @@ TEST(DOMGeometry, MoveAssignmentOperator) sdf::Geometry geometry; geometry.SetType(sdf::GeometryType::BOX); sdf::Box boxShape; - boxShape.SetSize(ignition::math::Vector3d(1, 2, 3)); + boxShape.SetSize(gz::math::Vector3d(1, 2, 3)); geometry.SetBoxShape(boxShape); sdf::Geometry geometry2; @@ -145,12 +145,12 @@ TEST(DOMGeometry, Box) geom.SetType(sdf::GeometryType::BOX); sdf::Box boxShape; - boxShape.SetSize(ignition::math::Vector3d(1, 2, 3)); + boxShape.SetSize(gz::math::Vector3d(1, 2, 3)); geom.SetBoxShape(boxShape); EXPECT_EQ(sdf::GeometryType::BOX, geom.Type()); EXPECT_NE(nullptr, geom.BoxShape()); - EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), geom.BoxShape()->Size()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), geom.BoxShape()->Size()); } ///////////////////////////////////////////////// @@ -192,7 +192,7 @@ TEST(DOMGeometry, Mesh) geom.SetType(sdf::GeometryType::MESH); sdf::Mesh meshShape; - meshShape.SetScale(ignition::math::Vector3d(1, 2, 3)); + meshShape.SetScale(gz::math::Vector3d(1, 2, 3)); meshShape.SetUri("banana"); meshShape.SetSubmesh("orange"); meshShape.SetCenterSubmesh(true); @@ -200,7 +200,7 @@ TEST(DOMGeometry, Mesh) EXPECT_EQ(sdf::GeometryType::MESH, geom.Type()); EXPECT_NE(nullptr, geom.MeshShape()); - EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), geom.MeshShape()->Scale()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), geom.MeshShape()->Scale()); EXPECT_EQ("banana", geom.MeshShape()->Uri()); EXPECT_EQ("orange", geom.MeshShape()->Submesh()); EXPECT_TRUE(geom.MeshShape()->CenterSubmesh()); @@ -213,12 +213,12 @@ TEST(DOMGeometry, Plane) geom.SetType(sdf::GeometryType::PLANE); sdf::Plane planeShape; - planeShape.SetNormal(ignition::math::Vector3d::UnitX); - planeShape.SetSize(ignition::math::Vector2d(9, 8)); + planeShape.SetNormal(gz::math::Vector3d::UnitX); + planeShape.SetSize(gz::math::Vector2d(9, 8)); geom.SetPlaneShape(planeShape); EXPECT_EQ(sdf::GeometryType::PLANE, geom.Type()); EXPECT_NE(nullptr, geom.PlaneShape()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, geom.PlaneShape()->Normal()); - EXPECT_EQ(ignition::math::Vector2d(9, 8), geom.PlaneShape()->Size()); + EXPECT_EQ(gz::math::Vector3d::UnitX, geom.PlaneShape()->Normal()); + EXPECT_EQ(gz::math::Vector2d(9, 8), geom.PlaneShape()->Size()); } diff --git a/src/Heightmap.cc b/src/Heightmap.cc index 70b161fb7..d2c289a84 100644 --- a/src/Heightmap.cc +++ b/src/Heightmap.cc @@ -61,10 +61,10 @@ class sdf::HeightmapPrivate public: std::string filePath{""}; /// \brief The heightmap's size. - public: ignition::math::Vector3d size{1, 1, 1}; + public: gz::math::Vector3d size{1, 1, 1}; /// \brief Position offset. - public: ignition::math::Vector3d position{0, 0, 0}; + public: gz::math::Vector3d position{0, 0, 0}; /// \brief Whether to use terrain paging. public: bool useTerrainPaging{false}; @@ -416,10 +416,10 @@ Errors Heightmap::Load(ElementPtr _sdf) "Heightmap geometry is missing a child element."}); } - this->dataPtr->size = _sdf->Get("size", + this->dataPtr->size = _sdf->Get("size", this->dataPtr->size).first; - this->dataPtr->position = _sdf->Get("pos", + this->dataPtr->position = _sdf->Get("pos", this->dataPtr->position).first; this->dataPtr->useTerrainPaging = _sdf->Get("use_terrain_paging", @@ -471,25 +471,25 @@ void Heightmap::SetFilePath(const std::string &_filePath) } ////////////////////////////////////////////////// -ignition::math::Vector3d Heightmap::Size() const +gz::math::Vector3d Heightmap::Size() const { return this->dataPtr->size; } ////////////////////////////////////////////////// -void Heightmap::SetSize(const ignition::math::Vector3d &_size) +void Heightmap::SetSize(const gz::math::Vector3d &_size) { this->dataPtr->size = _size; } ////////////////////////////////////////////////// -ignition::math::Vector3d Heightmap::Position() const +gz::math::Vector3d Heightmap::Position() const { return this->dataPtr->position; } ////////////////////////////////////////////////// -void Heightmap::SetPosition(const ignition::math::Vector3d &_position) +void Heightmap::SetPosition(const gz::math::Vector3d &_position) { this->dataPtr->position = _position; } diff --git a/src/Heightmap_TEST.cc b/src/Heightmap_TEST.cc index 41d0774c3..01ba2e5ec 100644 --- a/src/Heightmap_TEST.cc +++ b/src/Heightmap_TEST.cc @@ -26,8 +26,8 @@ TEST(DOMHeightmap, Construction) EXPECT_EQ(std::string(), heightmap.FilePath()); EXPECT_EQ(std::string(), heightmap.Uri()); - EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), heightmap.Size()); - EXPECT_EQ(ignition::math::Vector3d::Zero, heightmap.Position()); + EXPECT_EQ(gz::math::Vector3d(1, 1, 1), heightmap.Size()); + EXPECT_EQ(gz::math::Vector3d::Zero, heightmap.Position()); EXPECT_FALSE(heightmap.UseTerrainPaging()); EXPECT_EQ(2u, heightmap.Sampling()); EXPECT_EQ(0u, heightmap.TextureCount()); @@ -63,8 +63,8 @@ TEST(DOMHeightmap, MoveConstructor) sdf::Heightmap heightmap2(std::move(heightmap)); EXPECT_EQ("banana", heightmap2.Uri()); EXPECT_EQ("/pear", heightmap2.FilePath()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); EXPECT_TRUE(heightmap2.UseTerrainPaging()); EXPECT_EQ(123u, heightmap2.Sampling()); @@ -101,8 +101,8 @@ TEST(DOMHeightmap, CopyConstructor) sdf::Heightmap heightmap2(heightmap); EXPECT_EQ("banana", heightmap2.Uri()); EXPECT_EQ("/pear", heightmap2.FilePath()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); EXPECT_TRUE(heightmap2.UseTerrainPaging()); EXPECT_EQ(123u, heightmap2.Sampling()); @@ -140,8 +140,8 @@ TEST(DOMHeightmap, CopyAssignmentOperator) heightmap2 = heightmap; EXPECT_EQ("banana", heightmap2.Uri()); EXPECT_EQ("/pear", heightmap2.FilePath()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); EXPECT_TRUE(heightmap2.UseTerrainPaging()); EXPECT_EQ(123u, heightmap2.Sampling()); @@ -181,8 +181,8 @@ TEST(DOMHeightmap, MoveAssignmentOperator) heightmap2 = std::move(heightmap); EXPECT_EQ("banana", heightmap2.Uri()); EXPECT_EQ("/pear", heightmap2.FilePath()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 0.3), heightmap2.Size()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), heightmap2.Position()); EXPECT_TRUE(heightmap2.UseTerrainPaging()); EXPECT_EQ(123u, heightmap2.Sampling()); @@ -292,13 +292,13 @@ TEST(DOMHeightmap, Set) heightmap.SetFilePath("/mypath"); EXPECT_EQ("/mypath", heightmap.FilePath()); - EXPECT_EQ(ignition::math::Vector3d::One, heightmap.Size()); - heightmap.SetSize(ignition::math::Vector3d(0.2, 1.4, 7.8)); - EXPECT_EQ(ignition::math::Vector3d(0.2, 1.4, 7.8), heightmap.Size()); + EXPECT_EQ(gz::math::Vector3d::One, heightmap.Size()); + heightmap.SetSize(gz::math::Vector3d(0.2, 1.4, 7.8)); + EXPECT_EQ(gz::math::Vector3d(0.2, 1.4, 7.8), heightmap.Size()); - EXPECT_EQ(ignition::math::Vector3d::Zero, heightmap.Position()); - heightmap.SetPosition(ignition::math::Vector3d(0.2, 1.4, 7.8)); - EXPECT_EQ(ignition::math::Vector3d(0.2, 1.4, 7.8), heightmap.Position()); + EXPECT_EQ(gz::math::Vector3d::Zero, heightmap.Position()); + heightmap.SetPosition(gz::math::Vector3d(0.2, 1.4, 7.8)); + EXPECT_EQ(gz::math::Vector3d(0.2, 1.4, 7.8), heightmap.Position()); EXPECT_FALSE(heightmap.UseTerrainPaging()); heightmap.SetUseTerrainPaging(true); diff --git a/src/Imu.cc b/src/Imu.cc index 70b33b9e7..a7acd3afa 100644 --- a/src/Imu.cc +++ b/src/Imu.cc @@ -48,7 +48,7 @@ class sdf::ImuPrivate public: Noise angularVelZNoise; /// \brief The gravity dir - public: ignition::math::Vector3d gravityDirX{ignition::math::Vector3d::UnitX}; + public: gz::math::Vector3d gravityDirX{gz::math::Vector3d::UnitX}; /// \brief Name of the parent frame for the gravityDirX vector. public: std::string gravityDirXParentFrame; @@ -57,7 +57,7 @@ class sdf::ImuPrivate public: std::string localization = "CUSTOM"; /// \brief Custom RPY - public: ignition::math::Vector3d customRpy; + public: gz::math::Vector3d customRpy; /// \brief Name of the parent frame for the customRpy vector. public: std::string customRpyParentFrame; @@ -193,7 +193,7 @@ Errors Imu::Load(ElementPtr _sdf) if (elem->HasElement("grav_dir_x")) { - this->dataPtr->gravityDirX = elem->Get( + this->dataPtr->gravityDirX = elem->Get( "grav_dir_x", this->dataPtr->gravityDirX).first; this->dataPtr->gravityDirXParentFrame = elem->GetElement("grav_dir_x")->Get("parent_frame", @@ -202,7 +202,7 @@ Errors Imu::Load(ElementPtr _sdf) if (elem->HasElement("custom_rpy")) { - this->dataPtr->customRpy = elem->Get( + this->dataPtr->customRpy = elem->Get( "custom_rpy", this->dataPtr->customRpy).first; this->dataPtr->customRpyParentFrame = elem->GetElement("custom_rpy")->Get("parent_frame", @@ -318,7 +318,7 @@ void Imu::SetAngularVelocityZNoise(const Noise &_noise) } ////////////////////////////////////////////////// -ignition::math::Vector3d &Imu::GravityDirX() const +gz::math::Vector3d &Imu::GravityDirX() const { return this->dataPtr->gravityDirX; } @@ -336,7 +336,7 @@ void Imu::SetGravityDirXParentFrame(const std::string &_frame) const } ////////////////////////////////////////////////// -void Imu::SetGravityDirX(const ignition::math::Vector3d &_grav) const +void Imu::SetGravityDirX(const gz::math::Vector3d &_grav) const { this->dataPtr->gravityDirX = _grav; } @@ -354,14 +354,14 @@ void Imu::SetLocalization(const std::string &_localization) } ////////////////////////////////////////////////// -const ignition::math::Vector3d &Imu::CustomRpy() const +const gz::math::Vector3d &Imu::CustomRpy() const { return this->dataPtr->customRpy; } ////////////////////////////////////////////////// void Imu::SetCustomRpy( - const ignition::math::Vector3d &_rpy) const + const gz::math::Vector3d &_rpy) const { this->dataPtr->customRpy = _rpy; } diff --git a/src/Imu_TEST.cc b/src/Imu_TEST.cc index 2d76c20ca..f9966b71f 100644 --- a/src/Imu_TEST.cc +++ b/src/Imu_TEST.cc @@ -55,17 +55,17 @@ TEST(DOMImu, Construction) imu.SetAngularVelocityZNoise(noise); EXPECT_EQ(noise, imu.AngularVelocityZNoise()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, imu.GravityDirX()); - imu.SetGravityDirX(ignition::math::Vector3d::Zero); - EXPECT_EQ(ignition::math::Vector3d::Zero, imu.GravityDirX()); + EXPECT_EQ(gz::math::Vector3d::UnitX, imu.GravityDirX()); + imu.SetGravityDirX(gz::math::Vector3d::Zero); + EXPECT_EQ(gz::math::Vector3d::Zero, imu.GravityDirX()); EXPECT_TRUE(imu.GravityDirXParentFrame().empty()); imu.SetGravityDirXParentFrame("my_frame"); EXPECT_EQ("my_frame", imu.GravityDirXParentFrame()); - EXPECT_EQ(ignition::math::Vector3d::Zero, imu.CustomRpy()); - imu.SetCustomRpy(ignition::math::Vector3d::UnitZ); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, imu.CustomRpy()); + EXPECT_EQ(gz::math::Vector3d::Zero, imu.CustomRpy()); + imu.SetCustomRpy(gz::math::Vector3d::UnitZ); + EXPECT_EQ(gz::math::Vector3d::UnitZ, imu.CustomRpy()); EXPECT_TRUE(imu.CustomRpyParentFrame().empty()); imu.SetCustomRpyParentFrame("other_frame"); diff --git a/src/Joint.cc b/src/Joint.cc index f33e8e90d..ecf9c9997 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include "sdf/Error.hh" #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" @@ -59,7 +59,7 @@ class sdf::JointPrivate public: JointType type = JointType::INVALID; /// \brief Pose of the joint - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; @@ -338,13 +338,13 @@ void Joint::SetAxis(const unsigned int _index, const JointAxis &_axis) } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Joint::Pose() const +const gz::math::Pose3d &Joint::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Joint::RawPose() const +const gz::math::Pose3d &Joint::RawPose() const { return this->dataPtr->pose; } @@ -362,13 +362,13 @@ const std::string &Joint::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Joint::SetPose(const ignition::math::Pose3d &_pose) +void Joint::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Joint::SetRawPose(const ignition::math::Pose3d &_pose) +void Joint::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } diff --git a/src/JointAxis.cc b/src/JointAxis.cc index bedc16b18..908714ec1 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -14,8 +14,8 @@ * limitations under the License. * */ -#include -#include +#include +#include #include "sdf/Error.hh" #include "sdf/JointAxis.hh" #include "FrameSemantics.hh" @@ -31,7 +31,7 @@ class sdf::JointAxisPrivate /// The axis is expressed in the joint frame unless the /// use_parent_model_frame flag is set to true. The vector should be /// normalized. - public: ignition::math::Vector3d xyz = ignition::math::Vector3d::UnitZ; + public: gz::math::Vector3d xyz = gz::math::Vector3d::UnitZ; /// \brief Frame in which xyz is expressed in. public: std::string xyzExpressedIn = ""; @@ -136,8 +136,8 @@ Errors JointAxis::Load(ElementPtr _sdf) // Read the xyz values. if (_sdf->HasElement("xyz")) { - this->dataPtr->xyz = _sdf->Get("xyz", - ignition::math::Vector3d::UnitZ).first; + this->dataPtr->xyz = _sdf->Get("xyz", + gz::math::Vector3d::UnitZ).first; auto e = _sdf->GetElement("xyz"); if (e->HasAttribute("expressed_in")) { @@ -203,13 +203,13 @@ void JointAxis::SetInitialPosition(const double _pos) } ///////////////////////////////////////////////// -ignition::math::Vector3d JointAxis::Xyz() const +gz::math::Vector3d JointAxis::Xyz() const { return this->dataPtr->xyz; } ///////////////////////////////////////////////// -void JointAxis::SetXyz(const ignition::math::Vector3d &_xyz) +void JointAxis::SetXyz(const gz::math::Vector3d &_xyz) { this->dataPtr->xyz = _xyz; } @@ -371,7 +371,7 @@ void JointAxis::SetPoseRelativeToGraph( ///////////////////////////////////////////////// Errors JointAxis::ResolveXyz( - ignition::math::Vector3d &_xyz, + gz::math::Vector3d &_xyz, const std::string &_resolveTo) const { Errors errors; @@ -403,7 +403,7 @@ Errors JointAxis::ResolveXyz( resolveTo = this->dataPtr->xmlParentName; } - ignition::math::Pose3d pose; + gz::math::Pose3d pose; errors = resolvePose(pose, *graph, axisExpressedIn, resolveTo); if (errors.empty()) diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index a62b2928d..0af6775ca 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -24,7 +24,7 @@ TEST(DOMJointAxis, Construction) sdf::JointAxis axis; EXPECT_EQ(nullptr, axis.Element()); EXPECT_DOUBLE_EQ(0.0, axis.InitialPosition()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, axis.Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, axis.Xyz()); EXPECT_TRUE(axis.XyzExpressedIn().empty()); EXPECT_DOUBLE_EQ(0.0, axis.Damping()); EXPECT_DOUBLE_EQ(0.0, axis.Friction()); @@ -40,14 +40,14 @@ TEST(DOMJointAxis, Construction) axis.SetInitialPosition(1.2); EXPECT_DOUBLE_EQ(1.2, axis.InitialPosition()); - axis.SetXyz(ignition::math::Vector3d(0, 1, 0)); - EXPECT_EQ(ignition::math::Vector3d::UnitY, axis.Xyz()); + axis.SetXyz(gz::math::Vector3d(0, 1, 0)); + EXPECT_EQ(gz::math::Vector3d::UnitY, axis.Xyz()); axis.SetXyzExpressedIn("__model__"); EXPECT_EQ("__model__", axis.XyzExpressedIn()); // expect errors when trying to resolve axis without graph - ignition::math::Vector3d vec3; + gz::math::Vector3d vec3; EXPECT_FALSE(axis.ResolveXyz(vec3).empty()); axis.SetDamping(0.2); @@ -85,7 +85,7 @@ TEST(DOMJointAxis, Construction) TEST(DOMJointAxis, CopyConstructor) { sdf::JointAxis jointAxis; - jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0)); + jointAxis.SetXyz(gz::math::Vector3d(0, 1, 0)); sdf::JointAxis jointAxisCopy(jointAxis); EXPECT_EQ(jointAxis.Xyz(), jointAxisCopy.Xyz()); @@ -95,7 +95,7 @@ TEST(DOMJointAxis, CopyConstructor) TEST(DOMJointAxis, AssignmentOperator) { sdf::JointAxis jointAxis; - jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0)); + jointAxis.SetXyz(gz::math::Vector3d(0, 1, 0)); sdf::JointAxis jointAxisCopy; jointAxisCopy = jointAxis; @@ -105,7 +105,7 @@ TEST(DOMJointAxis, AssignmentOperator) ///////////////////////////////////////////////// TEST(DOMJointAxis, MoveConstructor) { - ignition::math::Vector3d axis{0, 1, 0}; + gz::math::Vector3d axis{0, 1, 0}; sdf::JointAxis jointAxis; jointAxis.SetXyz(axis); @@ -116,7 +116,7 @@ TEST(DOMJointAxis, MoveConstructor) ///////////////////////////////////////////////// TEST(DOMJointAxis, MoveAssignmentOperator) { - ignition::math::Vector3d axis{0, 1, 0}; + gz::math::Vector3d axis{0, 1, 0}; sdf::JointAxis jointAxis; jointAxis.SetXyz(axis); @@ -128,11 +128,11 @@ TEST(DOMJointAxis, MoveAssignmentOperator) ///////////////////////////////////////////////// TEST(DOMJointAxis, CopyAssignmentAfterMove) { - ignition::math::Vector3d axis1{0, 1, 0}; + gz::math::Vector3d axis1{0, 1, 0}; sdf::JointAxis jointAxis1; jointAxis1.SetXyz(axis1); - ignition::math::Vector3d axis2{1, 0, 0}; + gz::math::Vector3d axis2{1, 0, 0}; sdf::JointAxis jointAxis2; jointAxis2.SetXyz(axis2); diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index fae9c7ad4..ff1ee94a3 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -16,7 +16,7 @@ */ #include -#include +#include #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" @@ -28,20 +28,20 @@ TEST(DOMJoint, Construction) EXPECT_EQ(sdf::JointType::INVALID, joint.Type()); EXPECT_TRUE(joint.ParentLinkName().empty()); EXPECT_TRUE(joint.ChildLinkName().empty()); - EXPECT_EQ(ignition::math::Pose3d::Zero, joint.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, joint.RawPose()); EXPECT_TRUE(joint.PoseRelativeTo().empty()); EXPECT_EQ(nullptr, joint.Element()); { auto semanticPose = joint.SemanticPose(); - EXPECT_EQ(ignition::math::Pose3d::Zero, semanticPose.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, semanticPose.RawPose()); EXPECT_TRUE(semanticPose.RelativeTo().empty()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose without graph EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } joint.SetRawPose({-1, -2, -3, IGN_PI, IGN_PI, 0}); - EXPECT_EQ(ignition::math::Pose3d(-1, -2, -3, IGN_PI, IGN_PI, 0), + EXPECT_EQ(gz::math::Pose3d(-1, -2, -3, IGN_PI, IGN_PI, 0), joint.RawPose()); joint.SetPoseRelativeTo("link"); @@ -50,7 +50,7 @@ TEST(DOMJoint, Construction) auto semanticPose = joint.SemanticPose(); EXPECT_EQ(joint.RawPose(), semanticPose.RawPose()); EXPECT_EQ("link", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose without graph EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } @@ -84,10 +84,10 @@ TEST(DOMJoint, Construction) EXPECT_EQ(nullptr, joint.Axis(0)); EXPECT_EQ(nullptr, joint.Axis(1)); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + axis.SetXyz(gz::math::Vector3d(1, 0, 0)); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + axis1.SetXyz(gz::math::Vector3d(0, 1, 0)); joint.SetAxis(1, axis1); ASSERT_TRUE(nullptr != joint.Axis(0)); ASSERT_TRUE(nullptr != joint.Axis(1)); @@ -113,10 +113,10 @@ TEST(DOMJoint, MoveConstructor) sdf::Joint joint; joint.SetName("test_joint"); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + axis.SetXyz(gz::math::Vector3d(1, 0, 0)); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + axis1.SetXyz(gz::math::Vector3d(0, 1, 0)); joint.SetAxis(1, axis1); sdf::Joint joint2(std::move(joint)); @@ -134,10 +134,10 @@ TEST(DOMJoint, CopyConstructor) sdf::Joint joint; joint.SetName("test_joint"); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + axis.SetXyz(gz::math::Vector3d(1, 0, 0)); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + axis1.SetXyz(gz::math::Vector3d(0, 1, 0)); joint.SetAxis(1, axis1); sdf::Joint joint2(joint); @@ -161,10 +161,10 @@ TEST(DOMJoint, MoveAssignment) sdf::Joint joint; joint.SetName("test_joint"); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + axis.SetXyz(gz::math::Vector3d(1, 0, 0)); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + axis1.SetXyz(gz::math::Vector3d(0, 1, 0)); joint.SetAxis(1, axis1); sdf::Joint joint2; @@ -183,10 +183,10 @@ TEST(DOMJoint, CopyAssignment) sdf::Joint joint; joint.SetName("test_joint"); sdf::JointAxis axis; - axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + axis.SetXyz(gz::math::Vector3d(1, 0, 0)); joint.SetAxis(0, axis); sdf::JointAxis axis1; - axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + axis1.SetXyz(gz::math::Vector3d(0, 1, 0)); joint.SetAxis(1, axis1); sdf::Joint joint2; @@ -211,19 +211,19 @@ TEST(DOMJoint, CopyAssignmentAfterMove) sdf::Joint joint1; joint1.SetName("test_joint1"); sdf::JointAxis joint1Axis; - joint1Axis.SetXyz(ignition::math::Vector3d(1, 0, 0)); + joint1Axis.SetXyz(gz::math::Vector3d(1, 0, 0)); joint1.SetAxis(0, joint1Axis); sdf::JointAxis joint1Axis1; - joint1Axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)); + joint1Axis1.SetXyz(gz::math::Vector3d(0, 1, 0)); joint1.SetAxis(1, joint1Axis1); sdf::Joint joint2; joint2.SetName("test_joint2"); sdf::JointAxis joint2Axis; - joint2Axis.SetXyz(ignition::math::Vector3d(0, 0, 1)); + joint2Axis.SetXyz(gz::math::Vector3d(0, 0, 1)); joint2.SetAxis(0, joint2Axis); sdf::JointAxis joint2Axis1; - joint2Axis1.SetXyz(ignition::math::Vector3d(-1, 0, 0)); + joint2Axis1.SetXyz(gz::math::Vector3d(-1, 0, 0)); joint2.SetAxis(1, joint2Axis1); // This is similar to what std::swap does except it uses std::move for each diff --git a/src/Lidar.cc b/src/Lidar.cc index 103cccdea..88f12a219 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -17,7 +17,7 @@ #include "sdf/Lidar.hh" using namespace sdf; -using namespace ignition; +using namespace gz; /// \brief Private lidar data. class sdf::LidarPrivate diff --git a/src/Lidar_TEST.cc b/src/Lidar_TEST.cc index 8dea4494d..4c963d8cb 100644 --- a/src/Lidar_TEST.cc +++ b/src/Lidar_TEST.cc @@ -16,7 +16,7 @@ */ #include -#include +#include #include "sdf/Lidar.hh" ///////////////////////////////////////////////// @@ -36,18 +36,18 @@ TEST(DOMLidar, Set) EXPECT_EQ(lidar.HorizontalScanSamples(), 123u); lidar.SetHorizontalScanResolution(0.45); EXPECT_DOUBLE_EQ(lidar.HorizontalScanResolution(), 0.45); - lidar.SetHorizontalScanMinAngle(ignition::math::Angle(0.67)); + lidar.SetHorizontalScanMinAngle(gz::math::Angle(0.67)); EXPECT_DOUBLE_EQ(*(lidar.HorizontalScanMinAngle()), 0.67); - lidar.SetHorizontalScanMaxAngle(ignition::math::Angle(0.89)); + lidar.SetHorizontalScanMaxAngle(gz::math::Angle(0.89)); EXPECT_DOUBLE_EQ(*(lidar.HorizontalScanMaxAngle()), 0.89); lidar.SetVerticalScanSamples(98); EXPECT_EQ(lidar.VerticalScanSamples(), 98u); lidar.SetVerticalScanResolution(0.76); EXPECT_DOUBLE_EQ(lidar.VerticalScanResolution(), 0.76); - lidar.SetVerticalScanMinAngle(ignition::math::Angle(0.54)); + lidar.SetVerticalScanMinAngle(gz::math::Angle(0.54)); EXPECT_DOUBLE_EQ(*(lidar.VerticalScanMinAngle()), 0.54); - lidar.SetVerticalScanMaxAngle(ignition::math::Angle(0.321)); + lidar.SetVerticalScanMaxAngle(gz::math::Angle(0.321)); EXPECT_DOUBLE_EQ(*(lidar.VerticalScanMaxAngle()), 0.321); lidar.SetRangeMin(1.2); diff --git a/src/Light.cc b/src/Light.cc index f2944d3b4..3a9a223f7 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -15,7 +15,7 @@ * */ #include -#include +#include #include "sdf/Error.hh" #include "sdf/Light.hh" #include "Utils.hh" @@ -29,7 +29,7 @@ class sdf::LightPrivate public: std::string name = ""; /// \brief Pose of the light - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; @@ -62,19 +62,19 @@ class sdf::LightPrivate public: double quadraticAttenuation = 0.0; /// \brief Direction of the light source. - public: ignition::math::Vector3d direction {0, 0, -1}; + public: gz::math::Vector3d direction {0, 0, -1}; /// \brief Light diffuse color. - public: ignition::math::Color diffuse; + public: gz::math::Color diffuse; /// \brief Light specular color. - public: ignition::math::Color specular; + public: gz::math::Color specular; /// \brief Spot light inner angle. - public: ignition::math::Angle spotInnerAngle {0.0}; + public: gz::math::Angle spotInnerAngle {0.0}; /// \brief Spot light outer angle. - public: ignition::math::Angle spotOuterAngle {0.0}; + public: gz::math::Angle spotOuterAngle {0.0}; /// \brief Spot light falloff. public: double spotFalloff = 0.0; @@ -193,10 +193,10 @@ Errors Light::Load(ElementPtr _sdf) this->dataPtr->castShadows = _sdf->Get("cast_shadows", this->dataPtr->castShadows).first; - this->dataPtr->diffuse = _sdf->Get("diffuse", + this->dataPtr->diffuse = _sdf->Get("diffuse", this->dataPtr->diffuse).first; - this->dataPtr->specular = _sdf->Get("specular", + this->dataPtr->specular = _sdf->Get("specular", this->dataPtr->specular).first; sdf::ElementPtr attenuationElem = _sdf->GetElement("attenuation"); @@ -225,7 +225,7 @@ Errors Light::Load(ElementPtr _sdf) if (this->dataPtr->type == LightType::SPOT || this->dataPtr->type == LightType::DIRECTIONAL) { - std::pair dirPair = + std::pair dirPair = _sdf->Get<>("direction", this->dataPtr->direction); if (!dirPair.second) @@ -286,13 +286,13 @@ void Light::SetName(const std::string &_name) const } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Light::Pose() const +const gz::math::Pose3d &Light::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Light::RawPose() const +const gz::math::Pose3d &Light::RawPose() const { return this->dataPtr->pose; } @@ -310,13 +310,13 @@ const std::string &Light::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Light::SetPose(const ignition::math::Pose3d &_pose) +void Light::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Light::SetRawPose(const ignition::math::Pose3d &_pose) +void Light::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } @@ -375,25 +375,25 @@ void Light::SetCastShadows(const bool _cast) } ///////////////////////////////////////////////// -ignition::math::Color Light::Diffuse() const +gz::math::Color Light::Diffuse() const { return this->dataPtr->diffuse; } ///////////////////////////////////////////////// -void Light::SetDiffuse(const ignition::math::Color &_color) const +void Light::SetDiffuse(const gz::math::Color &_color) const { this->dataPtr->diffuse = _color; } ///////////////////////////////////////////////// -ignition::math::Color Light::Specular() const +gz::math::Color Light::Specular() const { return this->dataPtr->specular; } ///////////////////////////////////////////////// -void Light::SetSpecular(const ignition::math::Color &_color) const +void Light::SetSpecular(const gz::math::Color &_color) const { this->dataPtr->specular = _color; } @@ -419,7 +419,7 @@ double Light::LinearAttenuationFactor() const ///////////////////////////////////////////////// void Light::SetLinearAttenuationFactor(const double _factor) { - this->dataPtr->linearAttenuation = ignition::math::clamp(_factor, 0.0, 1.0); + this->dataPtr->linearAttenuation = gz::math::clamp(_factor, 0.0, 1.0); } ///////////////////////////////////////////////// @@ -432,7 +432,7 @@ double Light::ConstantAttenuationFactor() const void Light::SetConstantAttenuationFactor(const double _factor) { this->dataPtr->constantAttenuation = - ignition::math::clamp(_factor, 0.0, 1.0); + gz::math::clamp(_factor, 0.0, 1.0); } ///////////////////////////////////////////////// @@ -448,37 +448,37 @@ void Light::SetQuadraticAttenuationFactor(const double _factor) } ///////////////////////////////////////////////// -ignition::math::Vector3d Light::Direction() const +gz::math::Vector3d Light::Direction() const { return this->dataPtr->direction; } ///////////////////////////////////////////////// -void Light::SetDirection(const ignition::math::Vector3d &_dir) +void Light::SetDirection(const gz::math::Vector3d &_dir) { this->dataPtr->direction = _dir; } ///////////////////////////////////////////////// -ignition::math::Angle Light::SpotInnerAngle() const +gz::math::Angle Light::SpotInnerAngle() const { return this->dataPtr->spotInnerAngle; } ///////////////////////////////////////////////// -void Light::SetSpotInnerAngle(const ignition::math::Angle &_angle) +void Light::SetSpotInnerAngle(const gz::math::Angle &_angle) { this->dataPtr->spotInnerAngle.Radian(std::max(0.0, _angle.Radian())); } ///////////////////////////////////////////////// -ignition::math::Angle Light::SpotOuterAngle() const +gz::math::Angle Light::SpotOuterAngle() const { return this->dataPtr->spotOuterAngle; } ///////////////////////////////////////////////// -void Light::SetSpotOuterAngle(const ignition::math::Angle &_angle) +void Light::SetSpotOuterAngle(const gz::math::Angle &_angle) { this->dataPtr->spotOuterAngle.Radian(std::max(0.0, _angle.Radian())); } diff --git a/src/Light_TEST.cc b/src/Light_TEST.cc index 238b411a0..37bf9e13a 100644 --- a/src/Light_TEST.cc +++ b/src/Light_TEST.cc @@ -16,7 +16,7 @@ */ #include -#include +#include #include "sdf/Light.hh" ///////////////////////////////////////////////// @@ -30,19 +30,19 @@ TEST(DOMLight, DefaultConstruction) light.SetName("test_light"); EXPECT_EQ("test_light", light.Name()); - EXPECT_EQ(ignition::math::Pose3d::Zero, light.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, light.RawPose()); EXPECT_TRUE(light.PoseRelativeTo().empty()); { auto semanticPose = light.SemanticPose(); EXPECT_EQ(light.RawPose(), semanticPose.RawPose()); EXPECT_TRUE(semanticPose.RelativeTo().empty()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } light.SetRawPose({1, 2, 3, 0, 0, IGN_PI}); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, IGN_PI), light.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, IGN_PI), light.RawPose()); light.SetPoseRelativeTo("world"); EXPECT_EQ("world", light.PoseRelativeTo()); @@ -50,7 +50,7 @@ TEST(DOMLight, DefaultConstruction) auto semanticPose = light.SemanticPose(); EXPECT_EQ(light.RawPose(), semanticPose.RawPose()); EXPECT_EQ("world", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } @@ -59,13 +59,13 @@ TEST(DOMLight, DefaultConstruction) light.SetCastShadows(true); EXPECT_TRUE(light.CastShadows()); - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), light.Diffuse()); - light.SetDiffuse(ignition::math::Color(0.1f, 0.2f, 0.3f, 1.0)); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 1), light.Diffuse()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), light.Diffuse()); + light.SetDiffuse(gz::math::Color(0.1f, 0.2f, 0.3f, 1.0)); + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f, 1), light.Diffuse()); - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), light.Specular()); - light.SetSpecular(ignition::math::Color(0.4f, 0.6f, 0.7f, 1.0)); - EXPECT_EQ(ignition::math::Color(0.4f, 0.6f, 0.7f, 1), light.Specular()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), light.Specular()); + light.SetSpecular(gz::math::Color(0.4f, 0.6f, 0.7f, 1.0)); + EXPECT_EQ(gz::math::Color(0.4f, 0.6f, 0.7f, 1), light.Specular()); EXPECT_DOUBLE_EQ(10.0, light.AttenuationRange()); light.SetAttenuationRange(1.2); @@ -83,17 +83,17 @@ TEST(DOMLight, DefaultConstruction) light.SetQuadraticAttenuationFactor(1.1); EXPECT_DOUBLE_EQ(1.1, light.QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), light.Direction()); + EXPECT_EQ(gz::math::Vector3d(0, 0, -1), light.Direction()); light.SetDirection({0.4, 0.2, 0}); - EXPECT_EQ(ignition::math::Vector3d(0.4, 0.2, 0), light.Direction()); + EXPECT_EQ(gz::math::Vector3d(0.4, 0.2, 0), light.Direction()); - EXPECT_EQ(ignition::math::Angle(0.0), light.SpotInnerAngle()); + EXPECT_EQ(gz::math::Angle(0.0), light.SpotInnerAngle()); light.SetSpotInnerAngle(1.4); - EXPECT_EQ(ignition::math::Angle(1.4), light.SpotInnerAngle()); + EXPECT_EQ(gz::math::Angle(1.4), light.SpotInnerAngle()); - EXPECT_EQ(ignition::math::Angle(0.0), light.SpotOuterAngle()); + EXPECT_EQ(gz::math::Angle(0.0), light.SpotOuterAngle()); light.SetSpotOuterAngle(0.2); - EXPECT_EQ(ignition::math::Angle(0.2), light.SpotOuterAngle()); + EXPECT_EQ(gz::math::Angle(0.2), light.SpotOuterAngle()); EXPECT_DOUBLE_EQ(0.0, light.SpotFalloff()); light.SetSpotFalloff(4.3); @@ -109,8 +109,8 @@ TEST(DOMLight, CopyConstructor) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); - light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); - light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); + light.SetDiffuse(gz::math::Color(0.4f, 0.5f, 0.6f, 1.0)); + light.SetSpecular(gz::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); light.SetLinearAttenuationFactor(0.1); light.SetConstantAttenuationFactor(0.5); @@ -123,18 +123,18 @@ TEST(DOMLight, CopyConstructor) sdf::Light light2(light); EXPECT_EQ("test_copy_light", light2.Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, light2.Type()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); EXPECT_DOUBLE_EQ(0.1, light2.LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.5, light2.ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, light2.QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 1), light2.Direction()); - EXPECT_EQ(ignition::math::Angle(1.9), light2.SpotInnerAngle()); - EXPECT_EQ(ignition::math::Angle(3.3), light2.SpotOuterAngle()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 1), light2.Direction()); + EXPECT_EQ(gz::math::Angle(1.9), light2.SpotInnerAngle()); + EXPECT_EQ(gz::math::Angle(3.3), light2.SpotOuterAngle()); EXPECT_DOUBLE_EQ(0.9, light2.SpotFalloff()); } @@ -147,8 +147,8 @@ TEST(DOMLight, CopyAssignmentOperator) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); - light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); - light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); + light.SetDiffuse(gz::math::Color(0.4f, 0.5f, 0.6f, 1.0)); + light.SetSpecular(gz::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); light.SetLinearAttenuationFactor(0.1); light.SetConstantAttenuationFactor(0.5); @@ -162,18 +162,18 @@ TEST(DOMLight, CopyAssignmentOperator) light2 = light; EXPECT_EQ("test_light_assignment", light2.Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, light2.Type()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); EXPECT_DOUBLE_EQ(0.1, light2.LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.5, light2.ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, light2.QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 1), light2.Direction()); - EXPECT_EQ(ignition::math::Angle(1.9), light2.SpotInnerAngle()); - EXPECT_EQ(ignition::math::Angle(3.3), light2.SpotOuterAngle()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 1), light2.Direction()); + EXPECT_EQ(gz::math::Angle(1.9), light2.SpotInnerAngle()); + EXPECT_EQ(gz::math::Angle(3.3), light2.SpotOuterAngle()); EXPECT_DOUBLE_EQ(0.9, light2.SpotFalloff()); } @@ -186,8 +186,8 @@ TEST(DOMLight, MoveConstructor) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); - light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); - light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); + light.SetDiffuse(gz::math::Color(0.4f, 0.5f, 0.6f, 1.0)); + light.SetSpecular(gz::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); light.SetLinearAttenuationFactor(0.1); light.SetConstantAttenuationFactor(0.5); @@ -200,18 +200,18 @@ TEST(DOMLight, MoveConstructor) sdf::Light light2(std::move(light)); EXPECT_EQ("test_light_assignment", light2.Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, light2.Type()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); EXPECT_DOUBLE_EQ(0.1, light2.LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.5, light2.ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, light2.QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 1), light2.Direction()); - EXPECT_EQ(ignition::math::Angle(1.9), light2.SpotInnerAngle()); - EXPECT_EQ(ignition::math::Angle(3.3), light2.SpotOuterAngle()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 1), light2.Direction()); + EXPECT_EQ(gz::math::Angle(1.9), light2.SpotInnerAngle()); + EXPECT_EQ(gz::math::Angle(3.3), light2.SpotOuterAngle()); EXPECT_DOUBLE_EQ(0.9, light2.SpotFalloff()); } @@ -224,8 +224,8 @@ TEST(DOMLight, MoveAssignment) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("ground_plane"); light.SetCastShadows(true); - light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); - light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); + light.SetDiffuse(gz::math::Color(0.4f, 0.5f, 0.6f, 1.0)); + light.SetSpecular(gz::math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); light.SetLinearAttenuationFactor(0.1); light.SetConstantAttenuationFactor(0.5); @@ -239,18 +239,18 @@ TEST(DOMLight, MoveAssignment) light2 = std::move(light); EXPECT_EQ("test_light_assignment", light2.Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, light2.Type()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(3, 2, 1, 0, IGN_PI, 0), light2.RawPose()); EXPECT_EQ("ground_plane", light2.PoseRelativeTo()); EXPECT_TRUE(light2.CastShadows()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 1), light2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.8f, 0.9f, 0.1f, 1), light2.Specular()); EXPECT_DOUBLE_EQ(3.2, light2.AttenuationRange()); EXPECT_DOUBLE_EQ(0.1, light2.LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.5, light2.ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, light2.QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 1), light2.Direction()); - EXPECT_EQ(ignition::math::Angle(1.9), light2.SpotInnerAngle()); - EXPECT_EQ(ignition::math::Angle(3.3), light2.SpotOuterAngle()); + EXPECT_EQ(gz::math::Vector3d(0.1, 0.2, 1), light2.Direction()); + EXPECT_EQ(gz::math::Angle(1.9), light2.SpotInnerAngle()); + EXPECT_EQ(gz::math::Angle(3.3), light2.SpotOuterAngle()); EXPECT_DOUBLE_EQ(0.9, light2.SpotFalloff()); } diff --git a/src/Link.cc b/src/Link.cc index 1a2d965c1..d816199c3 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -17,9 +17,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include "sdf/Collision.hh" #include "sdf/Error.hh" @@ -38,7 +38,7 @@ class sdf::LinkPrivate public: std::string name = ""; /// \brief Pose of the link - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; @@ -56,9 +56,9 @@ class sdf::LinkPrivate public: std::vector sensors; /// \brief The inertial information for this link. - public: ignition::math::Inertiald inertial {{1.0, - ignition::math::Vector3d::One, ignition::math::Vector3d::Zero}, - ignition::math::Pose3d::Zero}; + public: gz::math::Inertiald inertial {{1.0, + gz::math::Vector3d::One, gz::math::Vector3d::Zero}, + gz::math::Pose3d::Zero}; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -163,9 +163,9 @@ Errors Link::Load(ElementPtr _sdf) this->dataPtr->sensors); errors.insert(errors.end(), sensorLoadErrors.begin(), sensorLoadErrors.end()); - ignition::math::Vector3d xxyyzz = ignition::math::Vector3d::One; - ignition::math::Vector3d xyxzyz = ignition::math::Vector3d::Zero; - ignition::math::Pose3d inertiaPose; + gz::math::Vector3d xxyyzz = gz::math::Vector3d::One; + gz::math::Vector3d xyxzyz = gz::math::Vector3d::Zero; + gz::math::Pose3d inertiaPose; std::string inertiaFrame = ""; double mass = 1.0; @@ -193,7 +193,7 @@ Errors Link::Load(ElementPtr _sdf) } } if (!this->dataPtr->inertial.SetMassMatrix( - ignition::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) + gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) { errors.push_back({ErrorCode::LINK_INERTIA_INVALID, "A link named " + @@ -337,13 +337,13 @@ const Sensor *Link::SensorByName(const std::string &_name) const } ///////////////////////////////////////////////// -const ignition::math::Inertiald &Link::Inertial() const +const gz::math::Inertiald &Link::Inertial() const { return this->dataPtr->inertial; } ///////////////////////////////////////////////// -bool Link::SetInertial(const ignition::math::Inertiald &_inertial) +bool Link::SetInertial(const gz::math::Inertiald &_inertial) { this->dataPtr->inertial = _inertial; return _inertial.MassMatrix().IsValid(); @@ -351,10 +351,10 @@ bool Link::SetInertial(const ignition::math::Inertiald &_inertial) ///////////////////////////////////////////////// Errors Link::ResolveInertial( - ignition::math::Inertiald &_inertial, + gz::math::Inertiald &_inertial, const std::string &_resolveTo) const { - ignition::math::Pose3d linkPose; + gz::math::Pose3d linkPose; auto errors = this->SemanticPose().Resolve(linkPose, _resolveTo); if (errors.empty()) { @@ -365,13 +365,13 @@ Errors Link::ResolveInertial( } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Link::Pose() const +const gz::math::Pose3d &Link::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Link::RawPose() const +const gz::math::Pose3d &Link::RawPose() const { return this->dataPtr->pose; } @@ -389,13 +389,13 @@ const std::string &Link::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Link::SetPose(const ignition::math::Pose3d &_pose) +void Link::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Link::SetRawPose(const ignition::math::Pose3d &_pose) +void Link::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index fd460e6df..e1ad1e6a1 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -16,9 +16,9 @@ */ #include -#include -#include -#include +#include +#include +#include #include "sdf/Collision.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" @@ -59,19 +59,19 @@ TEST(DOMLink, Construction) EXPECT_FALSE(link.SensorNameExists("")); EXPECT_FALSE(link.SensorNameExists("default")); - EXPECT_EQ(ignition::math::Pose3d::Zero, link.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, link.RawPose()); EXPECT_TRUE(link.PoseRelativeTo().empty()); { auto semanticPose = link.SemanticPose(); - EXPECT_EQ(ignition::math::Pose3d::Zero, semanticPose.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, semanticPose.RawPose()); EXPECT_TRUE(semanticPose.RelativeTo().empty()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } link.SetRawPose({10, 20, 30, 0, IGN_PI, 0}); - EXPECT_EQ(ignition::math::Pose3d(10, 20, 30, 0, IGN_PI, 0), link.RawPose()); + EXPECT_EQ(gz::math::Pose3d(10, 20, 30, 0, IGN_PI, 0), link.RawPose()); link.SetPoseRelativeTo("model"); EXPECT_EQ("model", link.PoseRelativeTo()); @@ -79,13 +79,13 @@ TEST(DOMLink, Construction) auto semanticPose = link.SemanticPose(); EXPECT_EQ(link.RawPose(), semanticPose.RawPose()); EXPECT_EQ("model", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } // Get the default inertial - const ignition::math::Inertiald inertial = link.Inertial(); + const gz::math::Inertiald inertial = link.Inertial(); EXPECT_DOUBLE_EQ(1.0, inertial.MassMatrix().Mass()); EXPECT_DOUBLE_EQ(1.0, inertial.MassMatrix().DiagonalMoments().X()); EXPECT_DOUBLE_EQ(1.0, inertial.MassMatrix().DiagonalMoments().Y()); @@ -101,11 +101,11 @@ TEST(DOMLink, Construction) EXPECT_FALSE(link.CollisionNameExists("")); EXPECT_FALSE(link.CollisionNameExists("default")); - ignition::math::Inertiald inertial2 { + gz::math::Inertiald inertial2 { {2.3, - ignition::math::Vector3d(1.4, 2.3, 3.2), - ignition::math::Vector3d(0.1, 0.2, 0.3)}, - ignition::math::Pose3d(1, 2, 3, 0, 0, 0)}; + gz::math::Vector3d(1.4, 2.3, 3.2), + gz::math::Vector3d(0.1, 0.2, 0.3)}, + gz::math::Pose3d(1, 2, 3, 0, 0, 0)}; EXPECT_TRUE(link.SetInertial(inertial2)); @@ -187,10 +187,10 @@ TEST(DOMLink, InvalidInertia) EXPECT_EQ(nullptr, link.Element()); EXPECT_TRUE(link.Name().empty()); - ignition::math::Inertiald invalidInertial { - {2.3, ignition::math::Vector3d(0.1, 0.2, 0.3), - ignition::math::Vector3d(1.2, 2.3, 3.4)}, - ignition::math::Pose3d(1, 2, 3, 0, 0, 0)}; + gz::math::Inertiald invalidInertial { + {2.3, gz::math::Vector3d(0.1, 0.2, 0.3), + gz::math::Vector3d(1.2, 2.3, 3.4)}, + gz::math::Pose3d(1, 2, 3, 0, 0, 0)}; EXPECT_FALSE(link.SetInertial(invalidInertial)); diff --git a/src/Material.cc b/src/Material.cc index 19a0d8db2..a709d3c75 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -16,7 +16,7 @@ */ #include #include -#include +#include #include "sdf/Types.hh" #include "sdf/Material.hh" @@ -46,19 +46,19 @@ class sdf::MaterialPrivate public: bool doubleSided = false; /// \brief Ambient color - public: ignition::math::Color ambient {0, 0, 0, 1}; + public: gz::math::Color ambient {0, 0, 0, 1}; /// \brief Diffuse color - public: ignition::math::Color diffuse {0, 0, 0, 1}; + public: gz::math::Color diffuse {0, 0, 0, 1}; /// \brief Specular color - public: ignition::math::Color specular {0, 0, 0, 1}; + public: gz::math::Color specular {0, 0, 0, 1}; /// \brief Specular exponent public: double shininess {0}; /// \brief Emissive color - public: ignition::math::Color emissive {0, 0, 0, 1}; + public: gz::math::Color emissive {0, 0, 0, 1}; /// \brief Physically Based Rendering (PBR) properties public: std::unique_ptr pbr; @@ -211,19 +211,19 @@ Errors Material::Load(sdf::ElementPtr _sdf) } } - this->dataPtr->ambient = _sdf->Get("ambient", + this->dataPtr->ambient = _sdf->Get("ambient", this->dataPtr->ambient).first; - this->dataPtr->diffuse = _sdf->Get("diffuse", + this->dataPtr->diffuse = _sdf->Get("diffuse", this->dataPtr->diffuse).first; - this->dataPtr->specular = _sdf->Get("specular", + this->dataPtr->specular = _sdf->Get("specular", this->dataPtr->specular).first; this->dataPtr->shininess = _sdf->Get("shininess", this->dataPtr->shininess).first; - this->dataPtr->emissive = _sdf->Get("emissive", + this->dataPtr->emissive = _sdf->Get("emissive", this->dataPtr->emissive).first; this->dataPtr->lighting = _sdf->Get("lighting", @@ -244,37 +244,37 @@ Errors Material::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -ignition::math::Color Material::Ambient() const +gz::math::Color Material::Ambient() const { return this->dataPtr->ambient; } ////////////////////////////////////////////////// -void Material::SetAmbient(const ignition::math::Color &_color) const +void Material::SetAmbient(const gz::math::Color &_color) const { this->dataPtr->ambient = _color; } ////////////////////////////////////////////////// -ignition::math::Color Material::Diffuse() const +gz::math::Color Material::Diffuse() const { return this->dataPtr->diffuse; } ////////////////////////////////////////////////// -void Material::SetDiffuse(const ignition::math::Color &_color) const +void Material::SetDiffuse(const gz::math::Color &_color) const { this->dataPtr->diffuse = _color; } ////////////////////////////////////////////////// -ignition::math::Color Material::Specular() const +gz::math::Color Material::Specular() const { return this->dataPtr->specular; } ////////////////////////////////////////////////// -void Material::SetSpecular(const ignition::math::Color &_color) const +void Material::SetSpecular(const gz::math::Color &_color) const { this->dataPtr->specular = _color; } @@ -292,13 +292,13 @@ void Material::SetShininess(const double _shininess) } ////////////////////////////////////////////////// -ignition::math::Color Material::Emissive() const +gz::math::Color Material::Emissive() const { return this->dataPtr->emissive; } ////////////////////////////////////////////////// -void Material::SetEmissive(const ignition::math::Color &_color) const +void Material::SetEmissive(const gz::math::Color &_color) const { this->dataPtr->emissive = _color; } diff --git a/src/Material_TEST.cc b/src/Material_TEST.cc index 1b7618def..feb513fe5 100644 --- a/src/Material_TEST.cc +++ b/src/Material_TEST.cc @@ -17,7 +17,7 @@ #include -#include +#include #include "sdf/Material.hh" #include "sdf/Pbr.hh" @@ -25,11 +25,11 @@ TEST(DOMMaterial, Construction) { sdf::Material material; - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Ambient()); - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Diffuse()); - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Specular()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), material.Ambient()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), material.Diffuse()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), material.Specular()); EXPECT_DOUBLE_EQ(0.0, material.Shininess()); - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Emissive()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), material.Emissive()); EXPECT_TRUE(material.Lighting()); EXPECT_FALSE(material.DoubleSided()); EXPECT_EQ(nullptr, material.Element()); @@ -45,11 +45,11 @@ TEST(DOMMaterial, Construction) TEST(DOMMaterial, MoveConstructor) { sdf::Material material; - material.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); - material.SetDiffuse(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); - material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); + material.SetAmbient(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); + material.SetDiffuse(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); + material.SetSpecular(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); material.SetShininess(5.0); - material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); + material.SetEmissive(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); material.SetLighting(false); material.SetDoubleSided(true); material.SetScriptUri("banana"); @@ -59,12 +59,12 @@ TEST(DOMMaterial, MoveConstructor) material.SetFilePath("/tmp/path"); sdf::Material material2(std::move(material)); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material2.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f), + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); + EXPECT_EQ(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f), material2.Specular()); EXPECT_DOUBLE_EQ(5.0, material2.Shininess()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material2.Emissive()); EXPECT_FALSE(material2.Lighting()); EXPECT_TRUE(material2.DoubleSided()); @@ -80,11 +80,11 @@ TEST(DOMMaterial, MoveConstructor) TEST(DOMMaterial, CopyConstructor) { sdf::Material material; - material.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); - material.SetDiffuse(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); - material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); + material.SetAmbient(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); + material.SetDiffuse(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); + material.SetSpecular(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); material.SetShininess(5.0); - material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); + material.SetEmissive(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); material.SetLighting(false); material.SetDoubleSided(true); material.SetScriptUri("banana"); @@ -94,12 +94,12 @@ TEST(DOMMaterial, CopyConstructor) material.SetFilePath("/tmp/other"); sdf::Material material2(material); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material2.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f), + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); + EXPECT_EQ(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f), material2.Specular()); EXPECT_DOUBLE_EQ(5.0, material2.Shininess()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material2.Emissive()); EXPECT_FALSE(material2.Lighting()); EXPECT_TRUE(material2.DoubleSided()); @@ -115,11 +115,11 @@ TEST(DOMMaterial, CopyConstructor) TEST(DOMMaterial, AssignmentOperator) { sdf::Material material; - material.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); - material.SetDiffuse(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); - material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); + material.SetAmbient(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); + material.SetDiffuse(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); + material.SetSpecular(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); material.SetShininess(5.0); - material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); + material.SetEmissive(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); material.SetLighting(false); material.SetDoubleSided(true); material.SetScriptUri("banana"); @@ -130,12 +130,12 @@ TEST(DOMMaterial, AssignmentOperator) sdf::Material material2; material2 = material; - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material2.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f), + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); + EXPECT_EQ(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f), material2.Specular()); EXPECT_DOUBLE_EQ(5.0, material2.Shininess()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material2.Emissive()); EXPECT_FALSE(material2.Lighting()); EXPECT_TRUE(material2.DoubleSided()); @@ -151,11 +151,11 @@ TEST(DOMMaterial, AssignmentOperator) TEST(DOMMaterial, MoveAssignmentOperator) { sdf::Material material; - material.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); - material.SetDiffuse(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); - material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); + material.SetAmbient(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); + material.SetDiffuse(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); + material.SetSpecular(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); material.SetShininess(5.0); - material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); + material.SetEmissive(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); material.SetLighting(false); material.SetDoubleSided(true); material.SetScriptUri("banana"); @@ -165,12 +165,12 @@ TEST(DOMMaterial, MoveAssignmentOperator) sdf::Material material2; material2 = std::move(material); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material2.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f), + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material2.Ambient()); + EXPECT_EQ(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material2.Diffuse()); + EXPECT_EQ(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f), material2.Specular()); EXPECT_DOUBLE_EQ(5.0, material2.Shininess()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material2.Emissive()); EXPECT_FALSE(material2.Lighting()); EXPECT_TRUE(material2.DoubleSided()); @@ -204,25 +204,25 @@ TEST(DOMAtmosphere, CopyAssignmentAfterMove) TEST(DOMMaterial, Set) { sdf::Material material; - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Ambient()); - material.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material.Ambient()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), material.Ambient()); + material.SetAmbient(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f)); + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f), material.Ambient()); - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Diffuse()); - material.SetDiffuse(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); - EXPECT_EQ(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material.Diffuse()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), material.Diffuse()); + material.SetDiffuse(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f)); + EXPECT_EQ(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f), material.Diffuse()); - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Specular()); - material.SetSpecular(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); - EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f), material.Specular()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), material.Specular()); + material.SetSpecular(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f)); + EXPECT_EQ(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f), material.Specular()); EXPECT_DOUBLE_EQ(0.0, material.Shininess()); material.SetShininess(5.0); EXPECT_DOUBLE_EQ(5.0, material.Shininess()); - EXPECT_EQ(ignition::math::Color(0, 0, 0, 1), material.Emissive()); - material.SetEmissive(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material.Emissive()); + EXPECT_EQ(gz::math::Color(0, 0, 0, 1), material.Emissive()); + material.SetEmissive(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f)); + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f), material.Emissive()); EXPECT_TRUE(material.Lighting()); material.SetLighting(false); @@ -264,10 +264,10 @@ TEST(DOMMaterial, Set) // Move the material sdf::Material moved(std::move(material)); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.5f), moved.Ambient()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.3f, 0.4f, 0.6f), moved.Diffuse()); - EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f, 0.7f), moved.Specular()); - EXPECT_EQ(ignition::math::Color(0.4f, 0.5f, 0.6f, 0.8f), moved.Emissive()); + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f, 0.5f), moved.Ambient()); + EXPECT_EQ(gz::math::Color(0.2f, 0.3f, 0.4f, 0.6f), moved.Diffuse()); + EXPECT_EQ(gz::math::Color(0.3f, 0.4f, 0.5f, 0.7f), moved.Specular()); + EXPECT_EQ(gz::math::Color(0.4f, 0.5f, 0.6f, 0.8f), moved.Emissive()); EXPECT_FALSE(moved.Lighting()); EXPECT_TRUE(moved.DoubleSided()); EXPECT_EQ("uri", moved.ScriptUri()); diff --git a/src/Mesh.cc b/src/Mesh.cc index 0e4c6d705..7f1b9fcf3 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -28,7 +28,7 @@ class sdf::MeshPrivate public: std::string filePath = ""; /// \brief The mesh's scale. - public: ignition::math::Vector3d scale {1, 1, 1}; + public: gz::math::Vector3d scale {1, 1, 1}; /// \brief The name of the submesh. public: std::string submesh = ""; @@ -143,7 +143,7 @@ Errors Mesh::Load(ElementPtr _sdf) this->dataPtr->centerSubmesh).first; } - this->dataPtr->scale = _sdf->Get("scale", + this->dataPtr->scale = _sdf->Get("scale", this->dataPtr->scale).first; return errors; @@ -180,13 +180,13 @@ void Mesh::SetFilePath(const std::string &_filePath) } ////////////////////////////////////////////////// -ignition::math::Vector3d Mesh::Scale() const +gz::math::Vector3d Mesh::Scale() const { return this->dataPtr->scale; } ////////////////////////////////////////////////// -void Mesh::SetScale(const ignition::math::Vector3d &_scale) +void Mesh::SetScale(const gz::math::Vector3d &_scale) { this->dataPtr->scale = _scale; } diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 41ecfe2c6..296861b18 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -27,7 +27,7 @@ TEST(DOMMesh, Construction) EXPECT_EQ(std::string(), mesh.FilePath()); EXPECT_EQ(std::string(), mesh.Uri()); EXPECT_EQ(std::string(), mesh.Submesh()); - EXPECT_TRUE(ignition::math::Vector3d(1, 1, 1) == mesh.Scale()); + EXPECT_TRUE(gz::math::Vector3d(1, 1, 1) == mesh.Scale()); EXPECT_FALSE(mesh.CenterSubmesh()); } @@ -44,7 +44,7 @@ TEST(DOMMesh, MoveConstructor) sdf::Mesh mesh2(std::move(mesh)); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); EXPECT_TRUE(mesh2.CenterSubmesh()); EXPECT_EQ("/pear", mesh2.FilePath()); } @@ -62,7 +62,7 @@ TEST(DOMMesh, CopyConstructor) sdf::Mesh mesh2(mesh); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); EXPECT_TRUE(mesh2.CenterSubmesh()); EXPECT_EQ("/pear", mesh2.FilePath()); } @@ -81,7 +81,7 @@ TEST(DOMMesh, CopyAssignmentOperator) mesh2 = mesh; EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); EXPECT_TRUE(mesh2.CenterSubmesh()); EXPECT_EQ("/pear", mesh2.FilePath()); } @@ -100,7 +100,7 @@ TEST(DOMMesh, MoveAssignmentOperator) mesh2 = std::move(mesh); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); EXPECT_TRUE(mesh2.CenterSubmesh()); EXPECT_EQ("/pear", mesh2.FilePath()); } @@ -138,9 +138,9 @@ TEST(DOMMesh, Set) mesh.SetSubmesh("my_submesh"); EXPECT_EQ("my_submesh", mesh.Submesh()); - EXPECT_TRUE(ignition::math::Vector3d(1, 1, 1) == mesh.Scale()); - mesh.SetScale(ignition::math::Vector3d(0.2, 1.4, 7.8)); - EXPECT_TRUE(ignition::math::Vector3d(0.2, 1.4, 7.8) == mesh.Scale()); + EXPECT_TRUE(gz::math::Vector3d(1, 1, 1) == mesh.Scale()); + mesh.SetScale(gz::math::Vector3d(0.2, 1.4, 7.8)); + EXPECT_TRUE(gz::math::Vector3d(0.2, 1.4, 7.8) == mesh.Scale()); EXPECT_FALSE(mesh.CenterSubmesh()); mesh.SetCenterSubmesh(true); diff --git a/src/Model.cc b/src/Model.cc index 7c88c2094..20edc5968 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -18,8 +18,8 @@ #include #include #include -#include -#include +#include +#include #include "sdf/Error.hh" #include "sdf/Frame.hh" #include "sdf/Joint.hh" @@ -54,7 +54,7 @@ class sdf::ModelPrivate public: std::string canonicalLink = ""; /// \brief Pose of the model - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; @@ -159,7 +159,7 @@ Errors Model::Load(ElementPtr _sdf) Errors errors; this->dataPtr->sdf = _sdf; - ignition::math::SemanticVersion sdfVersion(_sdf->OriginalVersion()); + gz::math::SemanticVersion sdfVersion(_sdf->OriginalVersion()); // Check that the provided SDF element is a // This is an error that cannot be recovered, so return an error. @@ -250,7 +250,7 @@ Errors Model::Load(ElementPtr _sdf) if (frameNames.count(linkName) > 0) { // This link has a name collision - if (sdfVersion < ignition::math::SemanticVersion(1, 7)) + if (sdfVersion < gz::math::SemanticVersion(1, 7)) { // This came from an old file, so try to workaround by renaming link linkName += "_link"; @@ -297,7 +297,7 @@ Errors Model::Load(ElementPtr _sdf) if (frameNames.count(jointName) > 0) { // This joint has a name collision - if (sdfVersion < ignition::math::SemanticVersion(1, 7)) + if (sdfVersion < gz::math::SemanticVersion(1, 7)) { // This came from an old file, so try to workaround by renaming joint jointName += "_joint"; @@ -334,7 +334,7 @@ Errors Model::Load(ElementPtr _sdf) if (frameNames.count(frameName) > 0) { // This frame has a name collision - if (sdfVersion < ignition::math::SemanticVersion(1, 7)) + if (sdfVersion < gz::math::SemanticVersion(1, 7)) { // This came from an old file, so try to workaround by renaming frame frameName += "_frame"; @@ -706,13 +706,13 @@ void Model::SetCanonicalLinkName(const std::string &_canonicalLink) } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Model::Pose() const +const gz::math::Pose3d &Model::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Model::RawPose() const +const gz::math::Pose3d &Model::RawPose() const { return this->dataPtr->pose; } @@ -730,13 +730,13 @@ const std::string &Model::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Model::SetPose(const ignition::math::Pose3d &_pose) +void Model::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Model::SetRawPose(const ignition::math::Pose3d &_pose) +void Model::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 7172d53e0..5a6f1b36a 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -16,7 +16,7 @@ */ #include -#include +#include #include "sdf/Joint.hh" #include "sdf/Link.hh" #include "sdf/Model.hh" @@ -85,19 +85,19 @@ TEST(DOMModel, Construction) EXPECT_EQ("link", model.CanonicalLinkName()); EXPECT_EQ(nullptr, model.CanonicalLink()); - EXPECT_EQ(ignition::math::Pose3d::Zero, model.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, model.RawPose()); EXPECT_TRUE(model.PoseRelativeTo().empty()); { auto semanticPose = model.SemanticPose(); EXPECT_EQ(model.RawPose(), semanticPose.RawPose()); EXPECT_TRUE(semanticPose.RelativeTo().empty()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } model.SetRawPose({1, 2, 3, 0, 0, IGN_PI}); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, IGN_PI), model.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, IGN_PI), model.RawPose()); model.SetPoseRelativeTo("world"); EXPECT_EQ("world", model.PoseRelativeTo()); @@ -105,7 +105,7 @@ TEST(DOMModel, Construction) auto semanticPose = model.SemanticPose(); EXPECT_EQ(model.RawPose(), semanticPose.RawPose()); EXPECT_EQ("world", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } diff --git a/src/NavSat.cc b/src/NavSat.cc index 088e5d937..bdc6c2d1c 100644 --- a/src/NavSat.cc +++ b/src/NavSat.cc @@ -17,7 +17,7 @@ #include "sdf/NavSat.hh" using namespace sdf; -using namespace ignition; +using namespace gz; /// \brief Private navsat data. class sdf::NavSatPrivate diff --git a/src/Noise.cc b/src/Noise.cc index d00c1b948..db0cc05c6 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -270,13 +270,13 @@ bool Noise::operator!=(const Noise &_noise) const bool Noise::operator==(const Noise &_noise) const { return this->dataPtr->type == _noise.Type() && - ignition::math::equal(this->dataPtr->mean, _noise.Mean()) && - ignition::math::equal(this->dataPtr->stdDev, _noise.StdDev()) && - ignition::math::equal(this->dataPtr->biasMean, _noise.BiasMean()) && - ignition::math::equal(this->dataPtr->biasStdDev, _noise.BiasStdDev()) && - ignition::math::equal(this->dataPtr->precision, _noise.Precision()) && - ignition::math::equal(this->dataPtr->dynamicBiasStdDev, + gz::math::equal(this->dataPtr->mean, _noise.Mean()) && + gz::math::equal(this->dataPtr->stdDev, _noise.StdDev()) && + gz::math::equal(this->dataPtr->biasMean, _noise.BiasMean()) && + gz::math::equal(this->dataPtr->biasStdDev, _noise.BiasStdDev()) && + gz::math::equal(this->dataPtr->precision, _noise.Precision()) && + gz::math::equal(this->dataPtr->dynamicBiasStdDev, _noise.DynamicBiasStdDev()) && - ignition::math::equal(this->dataPtr->dynamicBiasCorrelationTime, + gz::math::equal(this->dataPtr->dynamicBiasCorrelationTime, _noise.DynamicBiasCorrelationTime()); } diff --git a/src/Param.cc b/src/Param.cc index c808db885..ce9dc6c4d 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -169,55 +169,55 @@ bool Param::GetAny(std::any &_anyVal) const } _anyVal = ret; } - else if (this->IsType()) + else if (this->IsType()) { - ignition::math::Color ret; - if (!this->Get(ret)) + gz::math::Color ret; + if (!this->Get(ret)) { return false; } _anyVal = ret; } - else if (this->IsType()) + else if (this->IsType()) { - ignition::math::Vector3d ret; - if (!this->Get(ret)) + gz::math::Vector3d ret; + if (!this->Get(ret)) { return false; } _anyVal = ret; } - else if (this->IsType()) + else if (this->IsType()) { - ignition::math::Vector2i ret; - if (!this->Get(ret)) + gz::math::Vector2i ret; + if (!this->Get(ret)) { return false; } _anyVal = ret; } - else if (this->IsType()) + else if (this->IsType()) { - ignition::math::Vector2d ret; - if (!this->Get(ret)) + gz::math::Vector2d ret; + if (!this->Get(ret)) { return false; } _anyVal = ret; } - else if (this->IsType()) + else if (this->IsType()) { - ignition::math::Pose3d ret; - if (!this->Get(ret)) + gz::math::Pose3d ret; + if (!this->Get(ret)) { return false; } _anyVal = ret; } - else if (this->IsType()) + else if (this->IsType()) { - ignition::math::Quaterniond ret; - if (!this->Get(ret)) + gz::math::Quaterniond ret; + if (!this->Get(ret)) { return false; } @@ -373,66 +373,66 @@ bool ParamPrivate::ValueFromStringImpl(const std::string &_typeName, ss >> timetmp; _valueToSet = timetmp; } - else if (_typeName == "ignition::math::Angle" || + else if (_typeName == "gz::math::Angle" || _typeName == "angle") { StringStreamClassicLocale ss(tmp); - ignition::math::Angle angletmp; + gz::math::Angle angletmp; ss >> angletmp; _valueToSet = angletmp; } - else if (_typeName == "ignition::math::Color" || + else if (_typeName == "gz::math::Color" || _typeName == "color") { StringStreamClassicLocale ss(tmp); - ignition::math::Color colortmp; + gz::math::Color colortmp; ss >> colortmp; _valueToSet = colortmp; } - else if (_typeName == "ignition::math::Vector2i" || + else if (_typeName == "gz::math::Vector2i" || _typeName == "vector2i") { StringStreamClassicLocale ss(tmp); - ignition::math::Vector2i vectmp; + gz::math::Vector2i vectmp; ss >> vectmp; _valueToSet = vectmp; } - else if (_typeName == "ignition::math::Vector2d" || + else if (_typeName == "gz::math::Vector2d" || _typeName == "vector2d") { StringStreamClassicLocale ss(tmp); - ignition::math::Vector2d vectmp; + gz::math::Vector2d vectmp; ss >> vectmp; _valueToSet = vectmp; } - else if (_typeName == "ignition::math::Vector3d" || + else if (_typeName == "gz::math::Vector3d" || _typeName == "vector3") { StringStreamClassicLocale ss(tmp); - ignition::math::Vector3d vectmp; + gz::math::Vector3d vectmp; ss >> vectmp; _valueToSet = vectmp; } - else if (_typeName == "ignition::math::Pose3d" || + else if (_typeName == "gz::math::Pose3d" || _typeName == "pose" || _typeName == "Pose") { StringStreamClassicLocale ss(tmp); - ignition::math::Pose3d posetmp; + gz::math::Pose3d posetmp; ss >> posetmp; _valueToSet = posetmp; } - else if (_typeName == "ignition::math::Quaterniond" || + else if (_typeName == "gz::math::Quaterniond" || _typeName == "quaternion") { StringStreamClassicLocale ss(tmp); - ignition::math::Quaterniond quattmp; + gz::math::Quaterniond quattmp; ss >> quattmp; _valueToSet = quattmp; diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index 9604b595d..52f6d5e93 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -21,9 +21,9 @@ #include -#include -#include -#include +#include +#include +#include #include "sdf/Exception.hh" #include "sdf/Param.hh" @@ -110,17 +110,17 @@ TEST(Param, StringTypeGet) sdf::Param stringParam("key", "string", "", false, "description"); // pose type - ignition::math::Pose3d pose; + gz::math::Pose3d pose; EXPECT_TRUE(stringParam.SetFromString("1 1 1 0 0 0")); - EXPECT_TRUE(stringParam.Get(pose)); - EXPECT_EQ(ignition::math::Pose3d(1, 1, 1, 0, 0, 0), pose); + EXPECT_TRUE(stringParam.Get(pose)); + EXPECT_EQ(gz::math::Pose3d(1, 1, 1, 0, 0, 0), pose); EXPECT_EQ("string", stringParam.GetTypeName()); // color type - ignition::math::Color color; + gz::math::Color color; EXPECT_TRUE(stringParam.SetFromString("0 0 1 1")); - EXPECT_TRUE(stringParam.Get(color)); - EXPECT_EQ(ignition::math::Color(0, 0, 1, 1), color); + EXPECT_TRUE(stringParam.Get(color)); + EXPECT_EQ(gz::math::Color(0, 0, 1, 1), color); EXPECT_EQ("string", stringParam.GetTypeName()); } @@ -301,8 +301,8 @@ TEST(Param, uint64t) TEST(Param, UnknownType) { sdf::Param doubleParam("key", "double", "1.0", false, "description"); - ignition::math::Angle value; - EXPECT_TRUE(doubleParam.Get(value)); + gz::math::Angle value; + EXPECT_TRUE(doubleParam.Get(value)); EXPECT_DOUBLE_EQ(value.Radian(), 1.0); } @@ -310,10 +310,10 @@ TEST(Param, UnknownType) TEST(Param, Vector2i) { sdf::Param vect2iParam("key", "vector2i", "0 0", false, "description"); - ignition::math::Vector2i value; + gz::math::Vector2i value; - EXPECT_TRUE(vect2iParam.Get(value)); - EXPECT_EQ(value, ignition::math::Vector2i(0, 0)); + EXPECT_TRUE(vect2iParam.Get(value)); + EXPECT_EQ(value, gz::math::Vector2i(0, 0)); } //////////////////////////////////////////////////// diff --git a/src/Pbr.cc b/src/Pbr.cc index 1643ab1be..37f5a3761 100644 --- a/src/Pbr.cc +++ b/src/Pbr.cc @@ -16,7 +16,7 @@ */ #include #include -#include +#include #include "sdf/Model.hh" #include "sdf/Types.hh" @@ -149,11 +149,11 @@ bool PbrWorkflow::operator==(const PbrWorkflow &_workflow) const && (this->dataPtr->lightMapFilename == _workflow.dataPtr->lightMapFilename) && (this->dataPtr->ambientOcclusionMap == _workflow.dataPtr->ambientOcclusionMap) - && (ignition::math::equal( + && (gz::math::equal( this->dataPtr->metalness, _workflow.dataPtr->metalness)) - && (ignition::math::equal( + && (gz::math::equal( this->dataPtr->roughness, _workflow.dataPtr->roughness)) - && (ignition::math::equal( + && (gz::math::equal( this->dataPtr->glossiness, _workflow.dataPtr->glossiness)); } diff --git a/src/Pbr_TEST.cc b/src/Pbr_TEST.cc index 5b4c95da8..826726497 100644 --- a/src/Pbr_TEST.cc +++ b/src/Pbr_TEST.cc @@ -17,7 +17,7 @@ #include -#include +#include #include "sdf/Pbr.hh" ///////////////////////////////////////////////// diff --git a/src/Physics.cc b/src/Physics.cc index 04fbe4c91..f093bd47e 100644 --- a/src/Physics.cc +++ b/src/Physics.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ -#include +#include #include "sdf/Physics.hh" #include "Utils.hh" diff --git a/src/Plane.cc b/src/Plane.cc index 157410b18..3cbdff530 100644 --- a/src/Plane.cc +++ b/src/Plane.cc @@ -14,8 +14,8 @@ * limitations under the License. * */ -#include -#include +#include +#include #include "sdf/Plane.hh" using namespace sdf; @@ -25,8 +25,8 @@ class sdf::PlanePrivate { /// \brief A plane with a unit Z normal vector, size of 1x1 meters, and /// a zero offest. - public: ignition::math::Planed plane{ignition::math::Vector3d::UnitZ, - ignition::math::Vector2d::One, 0}; + public: gz::math::Planed plane{gz::math::Vector3d::UnitZ, + gz::math::Vector2d::One, 0}; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -99,8 +99,8 @@ Errors Plane::Load(ElementPtr _sdf) if (_sdf->HasElement("normal")) { - std::pair pair = - _sdf->Get("normal", + std::pair pair = + _sdf->Get("normal", this->dataPtr->plane.Normal()); if (!pair.second) @@ -120,8 +120,8 @@ Errors Plane::Load(ElementPtr _sdf) if (_sdf->HasElement("size")) { - std::pair pair = - _sdf->Get("size", this->dataPtr->plane.Size()); + std::pair pair = + _sdf->Get("size", this->dataPtr->plane.Size()); if (!pair.second) { @@ -142,25 +142,25 @@ Errors Plane::Load(ElementPtr _sdf) } ////////////////////////////////////////////////// -ignition::math::Vector3d Plane::Normal() const +gz::math::Vector3d Plane::Normal() const { return this->dataPtr->plane.Normal(); } ////////////////////////////////////////////////// -void Plane::SetNormal(const ignition::math::Vector3d &_normal) +void Plane::SetNormal(const gz::math::Vector3d &_normal) { this->dataPtr->plane.Set(_normal.Normalized(), this->dataPtr->plane.Offset()); } ////////////////////////////////////////////////// -ignition::math::Vector2d Plane::Size() const +gz::math::Vector2d Plane::Size() const { return this->dataPtr->plane.Size(); } ////////////////////////////////////////////////// -void Plane::SetSize(const ignition::math::Vector2d &_size) +void Plane::SetSize(const gz::math::Vector2d &_size) { this->dataPtr->plane.Set(this->dataPtr->plane.Normal(), _size, @@ -174,13 +174,13 @@ sdf::ElementPtr Plane::Element() const } ///////////////////////////////////////////////// -const ignition::math::Planed &Plane::Shape() const +const gz::math::Planed &Plane::Shape() const { return this->dataPtr->plane; } ///////////////////////////////////////////////// -ignition::math::Planed &Plane::Shape() +gz::math::Planed &Plane::Shape() { return this->dataPtr->plane; } diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 97fd5f780..0c100f359 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -16,8 +16,8 @@ */ #include -#include -#include +#include +#include #include "sdf/Plane.hh" ///////////////////////////////////////////////// @@ -26,17 +26,17 @@ TEST(DOMPlane, Construction) sdf::Plane plane; EXPECT_EQ(nullptr, plane.Element()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, plane.Normal()); - EXPECT_EQ(ignition::math::Vector2d::One, plane.Size()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, plane.Normal()); + EXPECT_EQ(gz::math::Vector2d::One, plane.Size()); plane.SetNormal({1, 0, 0}); - EXPECT_EQ(ignition::math::Vector3d::UnitX, plane.Normal()); + EXPECT_EQ(gz::math::Vector3d::UnitX, plane.Normal()); plane.SetNormal({1, 0, 1}); - EXPECT_EQ(ignition::math::Vector3d(0.707107, 0, 0.707107), plane.Normal()); + EXPECT_EQ(gz::math::Vector3d(0.707107, 0, 0.707107), plane.Normal()); plane.SetSize({1.2, 3.4}); - EXPECT_EQ(ignition::math::Vector2d(1.2, 3.4), plane.Size()); + EXPECT_EQ(gz::math::Vector2d(1.2, 3.4), plane.Size()); } ///////////////////////////////////////////////// @@ -47,11 +47,11 @@ TEST(DOMPlane, MoveConstructor) plane.SetSize({1.2, 3.4}); sdf::Plane plane2(std::move(plane)); - EXPECT_EQ(ignition::math::Vector3d::UnitX, plane2.Normal()); - EXPECT_EQ(ignition::math::Vector2d(1.2, 3.4), plane2.Size()); + EXPECT_EQ(gz::math::Vector3d::UnitX, plane2.Normal()); + EXPECT_EQ(gz::math::Vector2d(1.2, 3.4), plane2.Size()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, plane2.Shape().Normal()); - EXPECT_EQ(ignition::math::Vector2d(1.2, 3.4), plane2.Shape().Size()); + EXPECT_EQ(gz::math::Vector3d::UnitX, plane2.Shape().Normal()); + EXPECT_EQ(gz::math::Vector2d(1.2, 3.4), plane2.Shape().Size()); } ///////////////////////////////////////////////// @@ -62,8 +62,8 @@ TEST(DOMPlane, CopyConstructor) plane.SetSize({1.2, 3.4}); sdf::Plane plane2(plane); - EXPECT_EQ(ignition::math::Vector3d::UnitX, plane2.Normal()); - EXPECT_EQ(ignition::math::Vector2d(1.2, 3.4), plane2.Size()); + EXPECT_EQ(gz::math::Vector3d::UnitX, plane2.Normal()); + EXPECT_EQ(gz::math::Vector2d(1.2, 3.4), plane2.Size()); } ///////////////////////////////////////////////// @@ -75,8 +75,8 @@ TEST(DOMPlane, CopyAssignmentOperator) sdf::Plane plane2; plane2 = plane; - EXPECT_EQ(ignition::math::Vector3d::UnitX, plane2.Normal()); - EXPECT_EQ(ignition::math::Vector2d(1.2, 3.4), plane2.Size()); + EXPECT_EQ(gz::math::Vector3d::UnitX, plane2.Normal()); + EXPECT_EQ(gz::math::Vector2d(1.2, 3.4), plane2.Size()); } ///////////////////////////////////////////////// @@ -88,18 +88,18 @@ TEST(DOMPlane, MoveAssignmentOperator) sdf::Plane plane2; plane2 = std::move(plane); - EXPECT_EQ(ignition::math::Vector3d::UnitX, plane2.Normal()); - EXPECT_EQ(ignition::math::Vector2d(1.2, 3.4), plane2.Size()); + EXPECT_EQ(gz::math::Vector3d::UnitX, plane2.Normal()); + EXPECT_EQ(gz::math::Vector2d(1.2, 3.4), plane2.Size()); } ///////////////////////////////////////////////// TEST(DOMPlane, CopyAssignmentAfterMove) { sdf::Plane plane1; - plane1.SetNormal(ignition::math::Vector3d::UnitX); + plane1.SetNormal(gz::math::Vector3d::UnitX); sdf::Plane plane2; - plane2.SetNormal(ignition::math::Vector3d::UnitY); + plane2.SetNormal(gz::math::Vector3d::UnitY); // This is similar to what std::swap does except it uses std::move for each // assignment @@ -107,8 +107,8 @@ TEST(DOMPlane, CopyAssignmentAfterMove) plane1 = plane2; plane2 = tmp; - EXPECT_EQ(ignition::math::Vector3d::UnitY, plane1.Normal()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, plane2.Normal()); + EXPECT_EQ(gz::math::Vector3d::UnitY, plane1.Normal()); + EXPECT_EQ(gz::math::Vector3d::UnitX, plane2.Normal()); } ///////////////////////////////////////////////// @@ -147,7 +147,7 @@ TEST(DOMPlane, Load) normalDesc->AddValue("vector3", "0 0 1", "1", "normal"); sdf->AddElementDescription(normalDesc); sdf::ElementPtr normalElem = sdf->AddElement("normal"); - normalElem->Set({1, 0, 0}); + normalElem->Set({1, 0, 0}); // Missing element sdf->SetName("plane"); @@ -161,9 +161,9 @@ TEST(DOMPlane, Load) TEST(DOMPlane, Shape) { sdf::Plane plane; - EXPECT_EQ(ignition::math::Vector2d::One, plane.Size()); + EXPECT_EQ(gz::math::Vector2d::One, plane.Size()); - plane.Shape().Set(plane.Shape().Normal(), ignition::math::Vector2d(1, 2), + plane.Shape().Set(plane.Shape().Normal(), gz::math::Vector2d(1, 2), plane.Shape().Offset()); - EXPECT_EQ(ignition::math::Vector2d(1, 2), plane.Size()); + EXPECT_EQ(gz::math::Vector2d(1, 2), plane.Size()); } diff --git a/src/Polyline.cc b/src/Polyline.cc index ac0e9f657..46aeb1502 100644 --- a/src/Polyline.cc +++ b/src/Polyline.cc @@ -27,7 +27,7 @@ class sdf::PolylinePrivate public: double height{1.0}; /// \brief 2D points. - public: std::vector points; + public: std::vector points; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -121,7 +121,7 @@ Errors Polyline::Load(ElementPtr _sdf) pointElem != nullptr; pointElem = pointElem->GetNextElement("point")) { - this->dataPtr->points.push_back(pointElem->Get()); + this->dataPtr->points.push_back(pointElem->Get()); } return errors; @@ -146,7 +146,7 @@ uint64_t Polyline::PointCount() const } ///////////////////////////////////////////////// -const ignition::math::Vector2d *Polyline::PointByIndex(uint64_t _index) const +const gz::math::Vector2d *Polyline::PointByIndex(uint64_t _index) const { if (_index < this->dataPtr->points.size()) return &this->dataPtr->points[_index]; @@ -154,7 +154,7 @@ const ignition::math::Vector2d *Polyline::PointByIndex(uint64_t _index) const } ///////////////////////////////////////////////// -ignition::math::Vector2d *Polyline::PointByIndex(uint64_t _index) +gz::math::Vector2d *Polyline::PointByIndex(uint64_t _index) { if (_index < this->dataPtr->points.size()) return &this->dataPtr->points[_index]; @@ -162,7 +162,7 @@ ignition::math::Vector2d *Polyline::PointByIndex(uint64_t _index) } ///////////////////////////////////////////////// -bool Polyline::AddPoint(const ignition::math::Vector2d &_point) +bool Polyline::AddPoint(const gz::math::Vector2d &_point) { if (this->dataPtr->points.size() == this->dataPtr->points.max_size()) { @@ -179,7 +179,7 @@ void Polyline::ClearPoints() } ///////////////////////////////////////////////// -const std::vector &Polyline::Points() const +const std::vector &Polyline::Points() const { return this->dataPtr->points; } diff --git a/src/Polyline_TEST.cc b/src/Polyline_TEST.cc index 1dead60a4..0932fb6a7 100644 --- a/src/Polyline_TEST.cc +++ b/src/Polyline_TEST.cc @@ -125,10 +125,10 @@ TEST(DOMPolyline, Points) sdf::Polyline polyline; EXPECT_EQ(0u, polyline.PointCount()); - ignition::math::Vector2d p1{1, 2}; - ignition::math::Vector2d p2{3, 4}; - ignition::math::Vector2d p3{5, 6}; - ignition::math::Vector2d p4{7, 8}; + gz::math::Vector2d p1{1, 2}; + gz::math::Vector2d p2{3, 4}; + gz::math::Vector2d p3{5, 6}; + gz::math::Vector2d p4{7, 8}; EXPECT_TRUE(polyline.AddPoint(p1)); EXPECT_TRUE(polyline.AddPoint(p2)); diff --git a/src/SDFExtension.hh b/src/SDFExtension.hh index 47a4203c3..36b7adf3f 100644 --- a/src/SDFExtension.hh +++ b/src/SDFExtension.hh @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include "sdf/Types.hh" @@ -44,7 +44,7 @@ namespace sdf // for reducing fixed joints and removing links public: std::string oldLinkName; - public: ignition::math::Pose3d reductionTransform; + public: gz::math::Pose3d reductionTransform; // visual material public: std::string material; diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc index a947cf55c..03741fb53 100644 --- a/src/SDF_TEST.cc +++ b/src/SDF_TEST.cc @@ -17,7 +17,7 @@ #include #include -#include +#include #include "sdf/sdf.hh" @@ -25,10 +25,10 @@ class SDFUpdateFixture { public: std::string GetName() const {return this->name;} public: bool GetFlag() const {return this->flag;} - public: ignition::math::Pose3d GetPose() const {return this->pose;} + public: gz::math::Pose3d GetPose() const {return this->pose;} public: std::string name; public: bool flag; - public: ignition::math::Pose3d pose; + public: gz::math::Pose3d pose; }; //////////////////////////////////////////////////// @@ -104,7 +104,7 @@ TEST(SDF, UpdateElement) // Read pose element value ASSERT_TRUE(modelElem->HasElement("pose")); sdf::ParamPtr poseParam = modelElem->GetElement("pose")->GetValue(); - EXPECT_TRUE(poseParam->IsType()); + EXPECT_TRUE(poseParam->IsType()); // Set test class variables based on sdf values // Set parameter update functions to test class accessors @@ -115,7 +115,7 @@ TEST(SDF, UpdateElement) poseParam->SetUpdateFunc(std::bind(&SDFUpdateFixture::GetPose, &fixture)); bool flagCheck; - ignition::math::Pose3d poseCheck; + gz::math::Pose3d poseCheck; int i; for (i = 0; i < 4; i++) { @@ -307,41 +307,41 @@ TEST(SDF, EmptyValues) EXPECT_EQ(elem->Get(emptyString), "hello"); elem.reset(new sdf::Element()); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Vector2d()); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Vector2d()); elem->AddValue("vector2d", "1 2", "0", "description"); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Vector2d(1, 2)); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Vector2d(1, 2)); elem.reset(new sdf::Element()); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Vector3d()); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Vector3d()); elem->AddValue("vector3", "1 2 3", "0", "description"); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Vector3d(1, 2, 3)); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Vector3d(1, 2, 3)); elem.reset(new sdf::Element()); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Quaterniond()); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Quaterniond()); elem->AddValue("quaternion", "1 2 3", "0", "description"); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Quaterniond(-2.14159, 1.14159, -0.141593)); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Quaterniond(-2.14159, 1.14159, -0.141593)); elem.reset(new sdf::Element()); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Pose3d()); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Pose3d()); elem->AddValue("pose", "1.0 2.0 3.0 4.0 5.0 6.0", "0", "description"); - EXPECT_EQ(elem->Get(emptyString).Pos(), - ignition::math::Pose3d(1, 2, 3, 4, 5, 6).Pos()); - EXPECT_EQ(elem->Get(emptyString).Rot().Euler(), - ignition::math::Pose3d(1, 2, 3, 4, 5, 6).Rot().Euler()); + EXPECT_EQ(elem->Get(emptyString).Pos(), + gz::math::Pose3d(1, 2, 3, 4, 5, 6).Pos()); + EXPECT_EQ(elem->Get(emptyString).Rot().Euler(), + gz::math::Pose3d(1, 2, 3, 4, 5, 6).Rot().Euler()); elem.reset(new sdf::Element()); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Color()); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Color()); elem->AddValue("color", ".1 .2 .3 1.0", "0", "description"); - EXPECT_EQ(elem->Get(emptyString), - ignition::math::Color(.1f, .2f, .3f, 1.0f)); + EXPECT_EQ(elem->Get(emptyString), + gz::math::Color(.1f, .2f, .3f, 1.0f)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), sdf::Time()); @@ -414,8 +414,8 @@ TEST(SDF, GetAny) std::any anyValue = poseElem->GetAny(); try { - EXPECT_EQ(std::any_cast(anyValue), - ignition::math::Pose3d(0, 1, 2, 0, 0, 0)); + EXPECT_EQ(std::any_cast(anyValue), + gz::math::Pose3d(0, 1, 2, 0, 0, 0)); } catch(std::bad_any_cast &/*_e*/) { @@ -432,8 +432,8 @@ TEST(SDF, GetAny) std::any anyValue = worldElem->GetElement("gravity")->GetAny(); try { - EXPECT_EQ(std::any_cast(anyValue), - ignition::math::Vector3d(0, 0, -7.1)); + EXPECT_EQ(std::any_cast(anyValue), + gz::math::Vector3d(0, 0, -7.1)); } catch(std::bad_any_cast &/*_e*/) { @@ -476,8 +476,8 @@ TEST(SDF, GetAny) std::any anyValue = worldElem->GetElement("gravity")->GetAny(); try { - EXPECT_EQ(std::any_cast(anyValue), - ignition::math::Vector3d(0, 0, -7.1)); + EXPECT_EQ(std::any_cast(anyValue), + gz::math::Vector3d(0, 0, -7.1)); } catch(std::bad_any_cast &/*_e*/) { @@ -513,8 +513,8 @@ TEST(SDF, GetAny) std::any anyValue = materialElem->GetElement("ambient")->GetAny(); try { - EXPECT_EQ(std::any_cast(anyValue), - ignition::math::Color(0.1f, 0.1f, 0.1f, 1.0f)); + EXPECT_EQ(std::any_cast(anyValue), + gz::math::Color(0.1f, 0.1f, 0.1f, 1.0f)); } catch(std::bad_any_cast &/*_e*/) { diff --git a/src/Scene.cc b/src/Scene.cc index de532b156..8c50de211 100644 --- a/src/Scene.cc +++ b/src/Scene.cc @@ -42,12 +42,12 @@ class sdf::ScenePrivate public: bool originVisual = true; /// \brief Ambient light color of the scene. - public: ignition::math::Color ambient = - ignition::math::Color(0.4f, 0.4f, 0.4f); + public: gz::math::Color ambient = + gz::math::Color(0.4f, 0.4f, 0.4f); /// \brief Background color of the scene. - public: ignition::math::Color background = - ignition::math::Color(0.7f, 0.7f, .7f); + public: gz::math::Color background = + gz::math::Color(0.7f, 0.7f, .7f); /// \brief Pointer to the sky properties. public: std::unique_ptr sky; @@ -128,11 +128,11 @@ Errors Scene::Load(ElementPtr _sdf) } // Get the ambient property - this->dataPtr->ambient = _sdf->Get("ambient", + this->dataPtr->ambient = _sdf->Get("ambient", this->dataPtr->ambient).first; // Get the background color property - this->dataPtr->background = _sdf->Get("background", + this->dataPtr->background = _sdf->Get("background", this->dataPtr->background).first; // Get the grid property @@ -159,25 +159,25 @@ Errors Scene::Load(ElementPtr _sdf) } ///////////////////////////////////////////////// -ignition::math::Color Scene::Ambient() const +gz::math::Color Scene::Ambient() const { return this->dataPtr->ambient; } ///////////////////////////////////////////////// -void Scene::SetAmbient(const ignition::math::Color &_ambient) +void Scene::SetAmbient(const gz::math::Color &_ambient) { this->dataPtr->ambient = _ambient; } ///////////////////////////////////////////////// -ignition::math::Color Scene::Background() const +gz::math::Color Scene::Background() const { return this->dataPtr->background; } ///////////////////////////////////////////////// -void Scene::SetBackground(const ignition::math::Color &_background) +void Scene::SetBackground(const gz::math::Color &_background) { this->dataPtr->background = _background; } diff --git a/src/Scene_TEST.cc b/src/Scene_TEST.cc index 0e10ece0d..ea20999b8 100644 --- a/src/Scene_TEST.cc +++ b/src/Scene_TEST.cc @@ -22,8 +22,8 @@ TEST(DOMScene, Construction) { sdf::Scene scene; - EXPECT_EQ(ignition::math::Color(0.4f, 0.4f, 0.4f), scene.Ambient()); - EXPECT_EQ(ignition::math::Color(0.7f, 0.7f, 0.7f), scene.Background()); + EXPECT_EQ(gz::math::Color(0.4f, 0.4f, 0.4f), scene.Ambient()); + EXPECT_EQ(gz::math::Color(0.7f, 0.7f, 0.7f), scene.Background()); EXPECT_TRUE(scene.Grid()); EXPECT_TRUE(scene.Shadows()); EXPECT_TRUE(scene.OriginVisual()); @@ -37,8 +37,8 @@ TEST(DOMScene, CopyConstruction) sdf::Scene scene; scene.Load(sdf); - scene.SetAmbient(ignition::math::Color::Blue); - scene.SetBackground(ignition::math::Color::Red); + scene.SetAmbient(gz::math::Color::Blue); + scene.SetBackground(gz::math::Color::Red); scene.SetGrid(false); scene.SetShadows(false); scene.SetOriginVisual(false); @@ -46,8 +46,8 @@ TEST(DOMScene, CopyConstruction) scene.SetSky(sky); sdf::Scene scene2(scene); - EXPECT_EQ(ignition::math::Color::Blue, scene2.Ambient()); - EXPECT_EQ(ignition::math::Color::Red, scene2.Background()); + EXPECT_EQ(gz::math::Color::Blue, scene2.Ambient()); + EXPECT_EQ(gz::math::Color::Red, scene2.Background()); EXPECT_FALSE(scene2.Grid()); EXPECT_FALSE(scene2.Shadows()); EXPECT_FALSE(scene2.OriginVisual()); @@ -61,8 +61,8 @@ TEST(DOMScene, CopyConstruction) TEST(DOMScene, MoveConstruction) { sdf::Scene scene; - scene.SetAmbient(ignition::math::Color::Blue); - scene.SetBackground(ignition::math::Color::Red); + scene.SetAmbient(gz::math::Color::Blue); + scene.SetBackground(gz::math::Color::Red); scene.SetGrid(false); scene.SetShadows(false); scene.SetOriginVisual(false); @@ -70,8 +70,8 @@ TEST(DOMScene, MoveConstruction) scene.SetSky(sky); sdf::Scene scene2(std::move(scene)); - EXPECT_EQ(ignition::math::Color::Blue, scene2.Ambient()); - EXPECT_EQ(ignition::math::Color::Red, scene2.Background()); + EXPECT_EQ(gz::math::Color::Blue, scene2.Ambient()); + EXPECT_EQ(gz::math::Color::Red, scene2.Background()); EXPECT_FALSE(scene2.Grid()); EXPECT_FALSE(scene2.Shadows()); EXPECT_FALSE(scene2.OriginVisual()); @@ -82,8 +82,8 @@ TEST(DOMScene, MoveConstruction) TEST(DOMScene, MoveAssignmentOperator) { sdf::Scene scene; - scene.SetAmbient(ignition::math::Color::Green); - scene.SetBackground(ignition::math::Color::White); + scene.SetAmbient(gz::math::Color::Green); + scene.SetBackground(gz::math::Color::White); scene.SetGrid(false); scene.SetShadows(false); scene.SetOriginVisual(false); @@ -92,8 +92,8 @@ TEST(DOMScene, MoveAssignmentOperator) sdf::Scene scene2; scene2 = std::move(scene); - EXPECT_EQ(ignition::math::Color::Green, scene2.Ambient()); - EXPECT_EQ(ignition::math::Color::White, scene2.Background()); + EXPECT_EQ(gz::math::Color::Green, scene2.Ambient()); + EXPECT_EQ(gz::math::Color::White, scene2.Background()); EXPECT_FALSE(scene2.Grid()); EXPECT_FALSE(scene2.Shadows()); EXPECT_FALSE(scene2.OriginVisual()); @@ -104,8 +104,8 @@ TEST(DOMScene, MoveAssignmentOperator) TEST(DOMScene, AssignmentOperator) { sdf::Scene scene; - scene.SetAmbient(ignition::math::Color::Red); - scene.SetBackground(ignition::math::Color(0.2f, 0.3f, 0.4f)); + scene.SetAmbient(gz::math::Color::Red); + scene.SetBackground(gz::math::Color(0.2f, 0.3f, 0.4f)); scene.SetGrid(false); scene.SetShadows(false); scene.SetOriginVisual(false); @@ -114,8 +114,8 @@ TEST(DOMScene, AssignmentOperator) sdf::Scene scene2; scene2 = scene; - EXPECT_EQ(ignition::math::Color::Red, scene2.Ambient()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.3f, 0.4f), scene2.Background()); + EXPECT_EQ(gz::math::Color::Red, scene2.Ambient()); + EXPECT_EQ(gz::math::Color(0.2f, 0.3f, 0.4f), scene2.Background()); EXPECT_FALSE(scene2.Grid()); EXPECT_FALSE(scene2.Shadows()); EXPECT_FALSE(scene2.OriginVisual()); @@ -126,10 +126,10 @@ TEST(DOMScene, AssignmentOperator) TEST(DOMScene, CopyAssignmentAfterMove) { sdf::Scene scene1; - scene1.SetAmbient(ignition::math::Color::Red); + scene1.SetAmbient(gz::math::Color::Red); sdf::Scene scene2; - scene2.SetAmbient(ignition::math::Color::Green); + scene2.SetAmbient(gz::math::Color::Green); // This is similar to what std::swap does except it uses std::move for each // assignment @@ -137,19 +137,19 @@ TEST(DOMScene, CopyAssignmentAfterMove) scene1 = scene2; scene2 = tmp; - EXPECT_EQ(ignition::math::Color::Green, scene1.Ambient()); - EXPECT_EQ(ignition::math::Color::Red, scene2.Ambient()); + EXPECT_EQ(gz::math::Color::Green, scene1.Ambient()); + EXPECT_EQ(gz::math::Color::Red, scene2.Ambient()); } ///////////////////////////////////////////////// TEST(DOMScene, Set) { sdf::Scene scene; - scene.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f)); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f), scene.Ambient()); + scene.SetAmbient(gz::math::Color(0.1f, 0.2f, 0.3f)); + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f), scene.Ambient()); - scene.SetBackground(ignition::math::Color(0.1f, 0.2f, 0.3f)); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f), scene.Ambient()); + scene.SetBackground(gz::math::Color(0.1f, 0.2f, 0.3f)); + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f), scene.Ambient()); scene.SetGrid(true); EXPECT_TRUE(scene.Grid()); diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index eefc7d51c..867d596eb 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -15,7 +15,7 @@ * */ #include -#include +#include #include "sdf/Assert.hh" #include "sdf/SemanticPose.hh" #include "sdf/Error.hh" @@ -31,7 +31,7 @@ inline namespace SDF_VERSION_NAMESPACE { class SemanticPosePrivate { /// \brief Raw pose of the SemanticPose object. - public: ignition::math::Pose3d rawPose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d rawPose = gz::math::Pose3d::Zero; /// \brief Name of the relative-to frame. public: std::string relativeTo = ""; @@ -45,7 +45,7 @@ class SemanticPosePrivate ///////////////////////////////////////////////// SemanticPose::SemanticPose( - const ignition::math::Pose3d &_pose, + const gz::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, const std::weak_ptr _graph) @@ -85,7 +85,7 @@ SemanticPose &SemanticPose::operator=(const SemanticPose &_semanticPose) } ///////////////////////////////////////////////// -const ignition::math::Pose3d &SemanticPose::RawPose() const +const gz::math::Pose3d &SemanticPose::RawPose() const { return this->dataPtr->rawPose; } @@ -98,7 +98,7 @@ const std::string &SemanticPose::RelativeTo() const ///////////////////////////////////////////////// Errors SemanticPose::Resolve( - ignition::math::Pose3d &_pose, + gz::math::Pose3d &_pose, const std::string &_resolveTo) const { Errors errors; @@ -123,7 +123,7 @@ Errors SemanticPose::Resolve( resolveTo = this->dataPtr->defaultResolveTo; } - ignition::math::Pose3d pose; + gz::math::Pose3d pose; errors = resolvePose(pose, *graph, relativeTo, resolveTo); pose *= this->RawPose(); diff --git a/src/SemanticPose_TEST.cc b/src/SemanticPose_TEST.cc index 828626438..d5f558c38 100644 --- a/src/SemanticPose_TEST.cc +++ b/src/SemanticPose_TEST.cc @@ -16,7 +16,7 @@ */ #include -#include +#include #include "sdf/Link.hh" #include "sdf/SemanticPose.hh" @@ -24,7 +24,7 @@ TEST(SemanticPose, Construction) { sdf::Link link; - ignition::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); + gz::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); link.SetRawPose(rawPose); const sdf::SemanticPose &semPose = link.SemanticPose(); EXPECT_EQ(rawPose, semPose.RawPose()); @@ -34,7 +34,7 @@ TEST(SemanticPose, Construction) TEST(SemanticPose, CopyConstructor) { sdf::Link link; - ignition::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); + gz::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); link.SetRawPose(rawPose); sdf::SemanticPose semPose1 = link.SemanticPose(); @@ -46,14 +46,14 @@ TEST(SemanticPose, CopyConstructor) TEST(SemanticPose, CopyAssignmentOperator) { sdf::Link link; - ignition::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); + gz::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); link.SetRawPose(rawPose); sdf::SemanticPose semPose1 = link.SemanticPose(); // Create another SemanticPose object from another Link; sdf::Link link2; sdf::SemanticPose semPose2 = link2.SemanticPose(); - EXPECT_EQ(ignition::math::Pose3d::Zero, semPose2.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, semPose2.RawPose()); semPose2 = semPose1; EXPECT_EQ(rawPose, semPose2.RawPose()); @@ -63,7 +63,7 @@ TEST(SemanticPose, CopyAssignmentOperator) TEST(SemanticPose, MoveConstructor) { sdf::Link link; - ignition::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); + gz::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); link.SetRawPose(rawPose); sdf::SemanticPose semPose1 = link.SemanticPose(); @@ -75,14 +75,14 @@ TEST(SemanticPose, MoveConstructor) TEST(SemanticPose, MoveAssignmentOperator) { sdf::Link link; - ignition::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); + gz::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); link.SetRawPose(rawPose); sdf::SemanticPose semPose1 = link.SemanticPose(); // Create another SemanticPose object from another Link; sdf::Link link2; sdf::SemanticPose semPose2 = link2.SemanticPose(); - EXPECT_EQ(ignition::math::Pose3d::Zero, semPose2.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, semPose2.RawPose()); semPose2 = std::move(semPose1); EXPECT_EQ(rawPose, semPose2.RawPose()); @@ -92,14 +92,14 @@ TEST(SemanticPose, MoveAssignmentOperator) TEST(SemanticPose, CopyAssignmentAfterMove) { sdf::Link link; - ignition::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); + gz::math::Pose3d rawPose(1, 0, 0, 0, 0, 0); link.SetRawPose(rawPose); sdf::SemanticPose semPose1 = link.SemanticPose(); // Create another SemanticPose object from another Link; sdf::Link link2; sdf::SemanticPose semPose2 = link2.SemanticPose(); - EXPECT_EQ(ignition::math::Pose3d::Zero, semPose2.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, semPose2.RawPose()); // This is similar to what std::swap does except it uses std::move for each // assignment @@ -107,6 +107,6 @@ TEST(SemanticPose, CopyAssignmentAfterMove) semPose1 = semPose2; semPose2 = tmp; - EXPECT_EQ(ignition::math::Pose3d::Zero, semPose1.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, semPose1.RawPose()); EXPECT_EQ(rawPose, semPose2.RawPose()); } diff --git a/src/Sensor.cc b/src/Sensor.cc index c6a4036f5..778343a54 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include "sdf/AirPressure.hh" #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" @@ -130,7 +130,7 @@ class sdf::SensorPrivate public: std::string topic = ""; /// \brief Pose of the sensor - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; @@ -228,7 +228,7 @@ bool Sensor::operator==(const Sensor &_sensor) const this->RawPose() != _sensor.RawPose() || this->PoseRelativeTo() != _sensor.PoseRelativeTo() || this->EnableMetrics() != _sensor.EnableMetrics() || - !ignition::math::equal(this->UpdateRate(), _sensor.UpdateRate())) + !gz::math::equal(this->UpdateRate(), _sensor.UpdateRate())) { return false; } @@ -479,13 +479,13 @@ void Sensor::SetTopic(const std::string &_topic) } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Sensor::Pose() const +const gz::math::Pose3d &Sensor::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Sensor::RawPose() const +const gz::math::Pose3d &Sensor::RawPose() const { return this->dataPtr->pose; } @@ -503,13 +503,13 @@ const std::string &Sensor::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Sensor::SetPose(const ignition::math::Pose3d &_pose) +void Sensor::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Sensor::SetRawPose(const ignition::math::Pose3d &_pose) +void Sensor::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index c74ca3a46..815b4b307 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -35,19 +35,19 @@ TEST(DOMSensor, Construction) sensor.SetType(sdf::SensorType::ALTIMETER); EXPECT_EQ(sdf::SensorType::ALTIMETER, sensor.Type()); - EXPECT_EQ(ignition::math::Pose3d::Zero, sensor.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, sensor.RawPose()); EXPECT_TRUE(sensor.PoseRelativeTo().empty()); { auto semanticPose = sensor.SemanticPose(); EXPECT_EQ(sensor.RawPose(), semanticPose.RawPose()); EXPECT_TRUE(semanticPose.RelativeTo().empty()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } - sensor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); + sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); sensor.SetPoseRelativeTo("a_frame"); EXPECT_EQ("a_frame", sensor.PoseRelativeTo()); @@ -55,7 +55,7 @@ TEST(DOMSensor, Construction) auto semanticPose = sensor.SemanticPose(); EXPECT_EQ(sensor.RawPose(), semanticPose.RawPose()); EXPECT_EQ("a_frame", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } @@ -72,7 +72,7 @@ TEST(DOMSensor, Construction) TEST(DOMSensor, MoveConstructor) { sdf::Sensor sensor; - sensor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); sensor.SetUpdateRate(0.123); @@ -86,7 +86,7 @@ TEST(DOMSensor, MoveConstructor) sdf::Sensor sensor2(std::move(sensor)); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), @@ -98,7 +98,7 @@ TEST(DOMSensor, MoveConstructor) TEST(DOMSensor, CopyConstructor) { sdf::Sensor sensor; - sensor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); sensor.SetUpdateRate(0.123); @@ -112,14 +112,14 @@ TEST(DOMSensor, CopyConstructor) sdf::Sensor sensor2(sensor); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor.Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); EXPECT_EQ("a_frame", sensor.PoseRelativeTo()); ASSERT_TRUE(nullptr != sensor.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor.MagnetometerSensor()->XNoise().Mean()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), @@ -131,7 +131,7 @@ TEST(DOMSensor, CopyConstructor) TEST(DOMSensor, MoveAssignment) { sdf::Sensor sensor; - sensor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); @@ -145,7 +145,7 @@ TEST(DOMSensor, MoveAssignment) sensor2 = std::move(sensor); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), @@ -156,7 +156,7 @@ TEST(DOMSensor, MoveAssignment) TEST(DOMSensor, CopyAssignment) { sdf::Sensor sensor; - sensor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); @@ -170,14 +170,14 @@ TEST(DOMSensor, CopyAssignment) sensor2 = sensor; EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor.Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); EXPECT_EQ("a_frame", sensor.PoseRelativeTo()); ASSERT_TRUE(nullptr != sensor.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor.MagnetometerSensor()->XNoise().Mean()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), @@ -188,12 +188,12 @@ TEST(DOMSensor, CopyAssignment) TEST(DOMSensor, CopyAssignmentAfterMove) { sdf::Sensor sensor1; - sensor1.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor1.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor1.SetType(sdf::SensorType::MAGNETOMETER); sensor1.SetPoseRelativeTo("frame1"); sdf::Sensor sensor2; - sensor2.SetRawPose(ignition::math::Pose3d(4, 5, 6, 0, 0, 0)); + sensor2.SetRawPose(gz::math::Pose3d(4, 5, 6, 0, 0, 0)); sensor2.SetType(sdf::SensorType::CAMERA); sensor2.SetPoseRelativeTo("frame2"); @@ -204,11 +204,11 @@ TEST(DOMSensor, CopyAssignmentAfterMove) sensor2 = tmp; EXPECT_EQ(sdf::SensorType::CAMERA, sensor1.Type()); - EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), sensor1.RawPose()); + EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 0), sensor1.RawPose()); EXPECT_EQ("frame2", sensor1.PoseRelativeTo()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("frame1", sensor2.PoseRelativeTo()); } diff --git a/src/Sky.cc b/src/Sky.cc index 2ce2ffb0a..3d97bb098 100644 --- a/src/Sky.cc +++ b/src/Sky.cc @@ -35,7 +35,7 @@ class sdf::SkyPrivate public: double cloudSpeed = 0.6; /// \brief Cloud direction. - public: ignition::math::Angle cloudDirection; + public: gz::math::Angle cloudDirection; /// \brief Cloud humidity public: double cloudHumidity = 0.5; @@ -44,8 +44,8 @@ class sdf::SkyPrivate public: double cloudMeanSize = 0.5; /// \brief Cloud ambient color - public: ignition::math::Color cloudAmbient = - ignition::math::Color(0.8f, 0.8f, 0.8f); + public: gz::math::Color cloudAmbient = + gz::math::Color(0.8f, 0.8f, 0.8f); /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -138,13 +138,13 @@ void Sky::SetCloudSpeed(double _speed) } ///////////////////////////////////////////////// -ignition::math::Angle Sky::CloudDirection() const +gz::math::Angle Sky::CloudDirection() const { return this->dataPtr->cloudDirection; } ///////////////////////////////////////////////// -void Sky::SetCloudDirection(const ignition::math::Angle &_angle) +void Sky::SetCloudDirection(const gz::math::Angle &_angle) { this->dataPtr->cloudDirection = _angle; } @@ -174,13 +174,13 @@ void Sky::SetCloudMeanSize(double _size) } ///////////////////////////////////////////////// -ignition::math::Color Sky::CloudAmbient() const +gz::math::Color Sky::CloudAmbient() const { return this->dataPtr->cloudAmbient; } ///////////////////////////////////////////////// -void Sky::SetCloudAmbient(const ignition::math::Color &_ambient) +void Sky::SetCloudAmbient(const gz::math::Color &_ambient) { this->dataPtr->cloudAmbient = _ambient; } @@ -214,14 +214,14 @@ Errors Sky::Load(ElementPtr _sdf) this->dataPtr->cloudSpeed = cloudElem->Get("speed", this->dataPtr->cloudSpeed).first; this->dataPtr->cloudDirection = - cloudElem->Get("direction", + cloudElem->Get("direction", this->dataPtr->cloudDirection).first; this->dataPtr->cloudHumidity = cloudElem->Get("humidity", this->dataPtr->cloudHumidity).first; this->dataPtr->cloudMeanSize = cloudElem->Get("mean_size", this->dataPtr->cloudMeanSize).first; this->dataPtr->cloudAmbient = - cloudElem->Get("ambient", + cloudElem->Get("ambient", this->dataPtr->cloudAmbient).first; } diff --git a/src/Sky_TEST.cc b/src/Sky_TEST.cc index 03461bc43..1d1f7fb0f 100644 --- a/src/Sky_TEST.cc +++ b/src/Sky_TEST.cc @@ -26,10 +26,10 @@ TEST(DOMSky, Construction) EXPECT_DOUBLE_EQ(6.0, sky.Sunrise()); EXPECT_DOUBLE_EQ(20.0, sky.Sunset()); EXPECT_DOUBLE_EQ(0.6, sky.CloudSpeed()); - EXPECT_EQ(ignition::math::Angle(), sky.CloudDirection()); + EXPECT_EQ(gz::math::Angle(), sky.CloudDirection()); EXPECT_DOUBLE_EQ(0.5, sky.CloudHumidity()); EXPECT_DOUBLE_EQ(0.5, sky.CloudMeanSize()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f), + EXPECT_EQ(gz::math::Color(0.8f, 0.8f, 0.8f), sky.CloudAmbient()); } @@ -44,20 +44,20 @@ TEST(DOMSky, CopyConstruction) sky.SetSunrise(5.0); sky.SetSunset(15.0); sky.SetCloudSpeed(0.3); - sky.SetCloudDirection(ignition::math::Angle(1.2)); + sky.SetCloudDirection(gz::math::Angle(1.2)); sky.SetCloudHumidity(0.9); sky.SetCloudMeanSize(0.123); - sky.SetCloudAmbient(ignition::math::Color::Blue); + sky.SetCloudAmbient(gz::math::Color::Blue); sdf::Sky sky2(sky); EXPECT_DOUBLE_EQ(1.0, sky2.Time()); EXPECT_DOUBLE_EQ(5.0, sky2.Sunrise()); EXPECT_DOUBLE_EQ(15.0, sky2.Sunset()); EXPECT_DOUBLE_EQ(0.3, sky2.CloudSpeed()); - EXPECT_EQ(ignition::math::Angle(1.2), sky2.CloudDirection()); + EXPECT_EQ(gz::math::Angle(1.2), sky2.CloudDirection()); EXPECT_DOUBLE_EQ(0.9, sky2.CloudHumidity()); EXPECT_DOUBLE_EQ(0.123, sky2.CloudMeanSize()); - EXPECT_EQ(ignition::math::Color::Blue, sky2.CloudAmbient()); + EXPECT_EQ(gz::math::Color::Blue, sky2.CloudAmbient()); EXPECT_NE(nullptr, sky2.Element()); EXPECT_EQ(sky.Element(), sky2.Element()); @@ -71,20 +71,20 @@ TEST(DOMSky, MoveConstruction) sky.SetSunrise(5.0); sky.SetSunset(15.0); sky.SetCloudSpeed(0.3); - sky.SetCloudDirection(ignition::math::Angle(1.2)); + sky.SetCloudDirection(gz::math::Angle(1.2)); sky.SetCloudHumidity(0.9); sky.SetCloudMeanSize(0.123); - sky.SetCloudAmbient(ignition::math::Color::Blue); + sky.SetCloudAmbient(gz::math::Color::Blue); sdf::Sky sky2(std::move(sky)); EXPECT_DOUBLE_EQ(1.0, sky2.Time()); EXPECT_DOUBLE_EQ(5.0, sky2.Sunrise()); EXPECT_DOUBLE_EQ(15.0, sky2.Sunset()); EXPECT_DOUBLE_EQ(0.3, sky2.CloudSpeed()); - EXPECT_EQ(ignition::math::Angle(1.2), sky2.CloudDirection()); + EXPECT_EQ(gz::math::Angle(1.2), sky2.CloudDirection()); EXPECT_DOUBLE_EQ(0.9, sky2.CloudHumidity()); EXPECT_DOUBLE_EQ(0.123, sky2.CloudMeanSize()); - EXPECT_EQ(ignition::math::Color::Blue, sky2.CloudAmbient()); + EXPECT_EQ(gz::math::Color::Blue, sky2.CloudAmbient()); } ///////////////////////////////////////////////// @@ -95,10 +95,10 @@ TEST(DOMSky, MoveAssignmentOperator) sky.SetSunrise(5.0); sky.SetSunset(15.0); sky.SetCloudSpeed(0.3); - sky.SetCloudDirection(ignition::math::Angle(1.2)); + sky.SetCloudDirection(gz::math::Angle(1.2)); sky.SetCloudHumidity(0.9); sky.SetCloudMeanSize(0.123); - sky.SetCloudAmbient(ignition::math::Color::Blue); + sky.SetCloudAmbient(gz::math::Color::Blue); sdf::Sky sky2; sky2 = std::move(sky); @@ -106,10 +106,10 @@ TEST(DOMSky, MoveAssignmentOperator) EXPECT_DOUBLE_EQ(5.0, sky2.Sunrise()); EXPECT_DOUBLE_EQ(15.0, sky2.Sunset()); EXPECT_DOUBLE_EQ(0.3, sky2.CloudSpeed()); - EXPECT_EQ(ignition::math::Angle(1.2), sky2.CloudDirection()); + EXPECT_EQ(gz::math::Angle(1.2), sky2.CloudDirection()); EXPECT_DOUBLE_EQ(0.9, sky2.CloudHumidity()); EXPECT_DOUBLE_EQ(0.123, sky2.CloudMeanSize()); - EXPECT_EQ(ignition::math::Color::Blue, sky2.CloudAmbient()); + EXPECT_EQ(gz::math::Color::Blue, sky2.CloudAmbient()); } ///////////////////////////////////////////////// @@ -120,10 +120,10 @@ TEST(DOMSky, AssignmentOperator) sky.SetSunrise(5.0); sky.SetSunset(15.0); sky.SetCloudSpeed(0.3); - sky.SetCloudDirection(ignition::math::Angle(1.2)); + sky.SetCloudDirection(gz::math::Angle(1.2)); sky.SetCloudHumidity(0.9); sky.SetCloudMeanSize(0.123); - sky.SetCloudAmbient(ignition::math::Color::Blue); + sky.SetCloudAmbient(gz::math::Color::Blue); sdf::Sky sky2; sky2 = sky; @@ -131,10 +131,10 @@ TEST(DOMSky, AssignmentOperator) EXPECT_DOUBLE_EQ(5.0, sky2.Sunrise()); EXPECT_DOUBLE_EQ(15.0, sky2.Sunset()); EXPECT_DOUBLE_EQ(0.3, sky2.CloudSpeed()); - EXPECT_EQ(ignition::math::Angle(1.2), sky2.CloudDirection()); + EXPECT_EQ(gz::math::Angle(1.2), sky2.CloudDirection()); EXPECT_DOUBLE_EQ(0.9, sky2.CloudHumidity()); EXPECT_DOUBLE_EQ(0.123, sky2.CloudMeanSize()); - EXPECT_EQ(ignition::math::Color::Blue, sky2.CloudAmbient()); + EXPECT_EQ(gz::math::Color::Blue, sky2.CloudAmbient()); } ///////////////////////////////////////////////// @@ -173,8 +173,8 @@ TEST(DOMSky, Set) sky.SetCloudSpeed(0.3); EXPECT_DOUBLE_EQ(0.3, sky.CloudSpeed()); - sky.SetCloudDirection(ignition::math::Angle(1.2)); - EXPECT_EQ(ignition::math::Angle(1.2), sky.CloudDirection()); + sky.SetCloudDirection(gz::math::Angle(1.2)); + EXPECT_EQ(gz::math::Angle(1.2), sky.CloudDirection()); sky.SetCloudHumidity(0.9); EXPECT_DOUBLE_EQ(0.9, sky.CloudHumidity()); @@ -182,7 +182,7 @@ TEST(DOMSky, Set) sky.SetCloudMeanSize(0.123); EXPECT_DOUBLE_EQ(0.123, sky.CloudMeanSize()); - sky.SetCloudAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f)); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f), + sky.SetCloudAmbient(gz::math::Color(0.1f, 0.2f, 0.3f)); + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f), sky.CloudAmbient()); } diff --git a/src/Sphere.cc b/src/Sphere.cc index 2195dc724..51dcba842 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -22,7 +22,7 @@ using namespace sdf; class sdf::SpherePrivate { /// \brief Representation of the sphere - public: ignition::math::Sphered sphere{1.0}; + public: gz::math::Sphered sphere{1.0}; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -128,13 +128,13 @@ void Sphere::SetRadius(const double _radius) } ///////////////////////////////////////////////// -const ignition::math::Sphered &Sphere::Shape() const +const gz::math::Sphered &Sphere::Shape() const { return this->dataPtr->sphere; } ///////////////////////////////////////////////// -ignition::math::Sphered &Sphere::Shape() +gz::math::Sphered &Sphere::Shape() { return this->dataPtr->sphere; } diff --git a/src/Utils.cc b/src/Utils.cc index 9ed658039..95eedf6c0 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -43,7 +43,7 @@ bool loadName(sdf::ElementPtr _sdf, std::string &_name) } ///////////////////////////////////////////////// -bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, +bool loadPose(sdf::ElementPtr _sdf, gz::math::Pose3d &_pose, std::string &_frame) { sdf::ElementPtr sdf = _sdf; @@ -60,8 +60,8 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, sdf->Get("relative_to", ""); // Read the pose value. - std::pair posePair = - sdf->Get("", ignition::math::Pose3d::Zero); + std::pair posePair = + sdf->Get("", gz::math::Pose3d::Zero); // Set output, but only if the return value is true. if (posePair.second) diff --git a/src/Utils.hh b/src/Utils.hh index 9c69bb699..964f6544d 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -48,12 +48,12 @@ namespace sdf /// function parameters) the pose value and coordinate frame. /// \param[in] _sdf Pointer to an SDF element that is a pose element. /// \param[out] _pose Value of the pose element. The default value is - /// ignition::math::Pose3d::Zero. + /// gz::math::Pose3d::Zero. /// \param[out] _frame Value of the frame attribute. The default value is /// an empty string. - /// \return True if the pose element contained an ignition::math::Pose3d + /// \return True if the pose element contained an gz::math::Pose3d /// value. - bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, + bool loadPose(sdf::ElementPtr _sdf, gz::math::Pose3d &_pose, std::string &_frame); /// \brief Load all objects of a specific sdf element type. No error diff --git a/src/Utils_TEST.cc b/src/Utils_TEST.cc index d06271fd7..67aeb976b 100644 --- a/src/Utils_TEST.cc +++ b/src/Utils_TEST.cc @@ -17,7 +17,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "Utils.hh" @@ -29,11 +29,11 @@ TEST(DOMUtils, PoseDefaultValues) element->AddValue("pose", "0 0 0 0 0 0", true); element->AddAttribute("relative_to", "string", "", false); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; std::string frame; EXPECT_TRUE(sdf::loadPose(element, pose, frame)); - EXPECT_EQ(ignition::math::Pose3d::Zero, pose); + EXPECT_EQ(gz::math::Pose3d::Zero, pose); EXPECT_TRUE(frame.empty()); } @@ -44,11 +44,11 @@ TEST(DOMUtils, PoseNoFrame) element->SetName("pose"); element->AddValue("pose", "0 0 0 0 0 0", true); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; std::string frame; EXPECT_TRUE(sdf::loadPose(element, pose, frame)); - EXPECT_EQ(ignition::math::Pose3d::Zero, pose); + EXPECT_EQ(gz::math::Pose3d::Zero, pose); EXPECT_TRUE(frame.empty()); } @@ -59,11 +59,11 @@ TEST(DOMUtils, PoseNoValue) element->SetName("pose"); element->AddValue("pose", "", true); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; std::string frame; EXPECT_TRUE(sdf::loadPose(element, pose, frame)); - EXPECT_EQ(ignition::math::Pose3d::Zero, pose); + EXPECT_EQ(gz::math::Pose3d::Zero, pose); EXPECT_TRUE(frame.empty()); } @@ -76,11 +76,11 @@ TEST(DOMUtils, PoseWithFrame) element->AddAttribute("relative_to", "string", "", false); element->GetAttribute("relative_to")->SetFromString("frame_name"); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; std::string frame; EXPECT_TRUE(sdf::loadPose(element, pose, frame)); - EXPECT_EQ(ignition::math::Pose3d::Zero, pose); + EXPECT_EQ(gz::math::Pose3d::Zero, pose); EXPECT_EQ("frame_name", frame); } @@ -94,11 +94,11 @@ TEST(DOMUtils, PoseWithValue) element->GetAttribute("relative_to")->SetFromString("another frame"); element->GetValue()->SetFromString("1 2 3 0.1 0.2 0.3"); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; std::string frame; EXPECT_TRUE(sdf::loadPose(element, pose, frame)); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), pose); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), pose); EXPECT_EQ("another frame", frame); } diff --git a/src/Visual.cc b/src/Visual.cc index d5e848068..a6c0e3266 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -16,7 +16,7 @@ */ #include #include -#include +#include #include "sdf/Error.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" @@ -47,7 +47,7 @@ class sdf::VisualPrivate public: float transparency = 0.0; /// \brief Pose of the visual object - public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero; + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; /// \brief Frame of the pose. public: std::string poseRelativeTo = ""; @@ -247,13 +247,13 @@ void Visual::SetTransparency(float _transparency) } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Visual::Pose() const +const gz::math::Pose3d &Visual::Pose() const { return this->RawPose(); } ///////////////////////////////////////////////// -const ignition::math::Pose3d &Visual::RawPose() const +const gz::math::Pose3d &Visual::RawPose() const { return this->dataPtr->pose; } @@ -271,13 +271,13 @@ const std::string &Visual::PoseRelativeTo() const } ///////////////////////////////////////////////// -void Visual::SetPose(const ignition::math::Pose3d &_pose) +void Visual::SetPose(const gz::math::Pose3d &_pose) { this->SetRawPose(_pose); } ///////////////////////////////////////////////// -void Visual::SetRawPose(const ignition::math::Pose3d &_pose) +void Visual::SetRawPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } diff --git a/src/Visual_TEST.cc b/src/Visual_TEST.cc index 98b70855f..9f1d5e7d6 100644 --- a/src/Visual_TEST.cc +++ b/src/Visual_TEST.cc @@ -39,19 +39,19 @@ TEST(DOMVisual, Construction) visual.SetTransparency(0.34f); EXPECT_FLOAT_EQ(0.34f, visual.Transparency()); - EXPECT_EQ(ignition::math::Pose3d::Zero, visual.RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, visual.RawPose()); EXPECT_TRUE(visual.PoseRelativeTo().empty()); { auto semanticPose = visual.SemanticPose(); EXPECT_EQ(visual.RawPose(), semanticPose.RawPose()); EXPECT_TRUE(semanticPose.RelativeTo().empty()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } visual.SetRawPose({0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2}); - EXPECT_EQ(ignition::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), + EXPECT_EQ(gz::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), visual.RawPose()); visual.SetPoseRelativeTo("link"); @@ -60,7 +60,7 @@ TEST(DOMVisual, Construction) auto semanticPose = visual.SemanticPose(); EXPECT_EQ(visual.RawPose(), semanticPose.RawPose()); EXPECT_EQ("link", semanticPose.RelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // expect errors when trying to resolve pose EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } @@ -102,7 +102,7 @@ TEST(DOMVisual, CopyConstructor) EXPECT_EQ("test_visual", visual.Name()); EXPECT_FALSE(visual.CastShadows()); EXPECT_FLOAT_EQ(0.345f, visual.Transparency()); - EXPECT_EQ(ignition::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), + EXPECT_EQ(gz::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), visual.RawPose()); EXPECT_EQ("link", visual.PoseRelativeTo()); ASSERT_TRUE(nullptr != visual.Material()); @@ -112,7 +112,7 @@ TEST(DOMVisual, CopyConstructor) EXPECT_EQ("test_visual", visual2.Name()); EXPECT_FALSE(visual2.CastShadows()); EXPECT_FLOAT_EQ(0.345f, visual2.Transparency()); - EXPECT_EQ(ignition::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), + EXPECT_EQ(gz::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), visual2.RawPose()); EXPECT_EQ("link", visual2.PoseRelativeTo()); ASSERT_TRUE(nullptr != visual2.Material()); @@ -142,7 +142,7 @@ TEST(DOMVisual, CopyAssignmentOperator) EXPECT_EQ("test_visual", visual.Name()); EXPECT_FALSE(visual.CastShadows()); EXPECT_FLOAT_EQ(0.345f, visual.Transparency()); - EXPECT_EQ(ignition::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), + EXPECT_EQ(gz::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), visual.RawPose()); EXPECT_EQ("link", visual.PoseRelativeTo()); ASSERT_TRUE(nullptr != visual.Material()); @@ -152,7 +152,7 @@ TEST(DOMVisual, CopyAssignmentOperator) EXPECT_EQ("test_visual", visual2.Name()); EXPECT_FALSE(visual2.CastShadows()); EXPECT_FLOAT_EQ(0.345f, visual2.Transparency()); - EXPECT_EQ(ignition::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), + EXPECT_EQ(gz::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), visual2.RawPose()); EXPECT_EQ("link", visual2.PoseRelativeTo()); ASSERT_TRUE(nullptr != visual2.Material()); @@ -181,7 +181,7 @@ TEST(DOMVisual, MoveConstructor) EXPECT_EQ("test_visual", visual2.Name()); EXPECT_FALSE(visual2.CastShadows()); EXPECT_FLOAT_EQ(0.345f, visual2.Transparency()); - EXPECT_EQ(ignition::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), + EXPECT_EQ(gz::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), visual2.RawPose()); EXPECT_EQ("link", visual2.PoseRelativeTo()); ASSERT_TRUE(nullptr != visual2.Material()); @@ -211,7 +211,7 @@ TEST(DOMVisual, MoveAssignmentOperator) EXPECT_EQ("test_visual", visual2.Name()); EXPECT_FALSE(visual2.CastShadows()); EXPECT_FLOAT_EQ(0.345f, visual2.Transparency()); - EXPECT_EQ(ignition::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), + EXPECT_EQ(gz::math::Pose3d(0, -20, 30, IGN_PI_2, -IGN_PI, IGN_PI_2), visual2.RawPose()); EXPECT_EQ("link", visual2.PoseRelativeTo()); ASSERT_TRUE(nullptr != visual2.Material()); @@ -262,16 +262,16 @@ TEST(DOMVisual, SetMaterial) EXPECT_TRUE(visual.Name().empty()); sdf::Material material; - material.SetAmbient(ignition::math::Color(0, 0.5, 0)); - material.SetDiffuse(ignition::math::Color(1, 0, 0)); - material.SetSpecular(ignition::math::Color(0.f, 0.1f, 0.9f)); + material.SetAmbient(gz::math::Color(0, 0.5, 0)); + material.SetDiffuse(gz::math::Color(1, 0, 0)); + material.SetSpecular(gz::math::Color(0.f, 0.1f, 0.9f)); visual.SetMaterial(material); ASSERT_NE(nullptr, visual.Material()); - EXPECT_EQ(ignition::math::Color(0, 0.5, 0), visual.Material()->Ambient()); - EXPECT_EQ(ignition::math::Color(1, 0, 0), visual.Material()->Diffuse()); - EXPECT_EQ(ignition::math::Color(0.f, 0.1f, 0.9f), + EXPECT_EQ(gz::math::Color(0, 0.5, 0), visual.Material()->Ambient()); + EXPECT_EQ(gz::math::Color(1, 0, 0), visual.Material()->Diffuse()); + EXPECT_EQ(gz::math::Color(0.f, 0.1f, 0.9f), visual.Material()->Specular()); } diff --git a/src/World.cc b/src/World.cc index 1c1ba5e8e..346f327fb 100644 --- a/src/World.cc +++ b/src/World.cc @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include "sdf/Actor.hh" #include "sdf/Frame.hh" @@ -51,8 +51,8 @@ class sdf::WorldPrivate public: std::string audioDevice = "default"; /// \brief Gravity vector. - public: ignition::math::Vector3d gravity = - ignition::math::Vector3d(0, 0, -9.80665); + public: gz::math::Vector3d gravity = + gz::math::Vector3d(0, 0, -9.80665); /// \brief Pointer to Gui parameters. public: std::unique_ptr gui; @@ -70,8 +70,8 @@ class sdf::WorldPrivate public: std::vector actors; /// \brief Magnetic field. - public: ignition::math::Vector3d magneticField = - ignition::math::Vector3d(5.5645e-6, 22.8758e-6, -42.3884e-6); + public: gz::math::Vector3d magneticField = + gz::math::Vector3d(5.5645e-6, 22.8758e-6, -42.3884e-6); /// \brief The models specified in this world. public: std::vector models; @@ -86,8 +86,8 @@ class sdf::WorldPrivate public: sdf::ElementPtr sdf; /// \brief Linear velocity of wind. - public: ignition::math::Vector3d windLinearVelocity = - ignition::math::Vector3d::Zero; + public: gz::math::Vector3d windLinearVelocity = + gz::math::Vector3d::Zero; /// \brief Frame Attached-To Graph constructed during Load. public: std::shared_ptr frameAttachedToGraph; @@ -233,7 +233,7 @@ Errors World::Load(sdf::ElementPtr _sdf) { sdf::ElementPtr elem = _sdf->GetElement("wind"); this->dataPtr->windLinearVelocity = - elem->Get("linear_velocity", + elem->Get("linear_velocity", this->dataPtr->windLinearVelocity).first; } @@ -248,12 +248,12 @@ Errors World::Load(sdf::ElementPtr _sdf) } // Read gravity. - this->dataPtr->gravity = _sdf->Get("gravity", + this->dataPtr->gravity = _sdf->Get("gravity", this->dataPtr->gravity).first; // Read the magnetic field. this->dataPtr->magneticField = - _sdf->Get("magnetic_field", + _sdf->Get("magnetic_field", this->dataPtr->magneticField).first; for (const auto &[name, size] : @@ -446,37 +446,37 @@ void World::SetAudioDevice(const std::string &_device) } ///////////////////////////////////////////////// -ignition::math::Vector3d World::WindLinearVelocity() const +gz::math::Vector3d World::WindLinearVelocity() const { return this->dataPtr->windLinearVelocity; } ///////////////////////////////////////////////// -void World::SetWindLinearVelocity(const ignition::math::Vector3d &_wind) +void World::SetWindLinearVelocity(const gz::math::Vector3d &_wind) { this->dataPtr->windLinearVelocity = _wind; } ///////////////////////////////////////////////// -ignition::math::Vector3d World::Gravity() const +gz::math::Vector3d World::Gravity() const { return this->dataPtr->gravity; } ///////////////////////////////////////////////// -void World::SetGravity(const ignition::math::Vector3d &_gravity) +void World::SetGravity(const gz::math::Vector3d &_gravity) { this->dataPtr->gravity = _gravity; } ///////////////////////////////////////////////// -ignition::math::Vector3d World::MagneticField() const +gz::math::Vector3d World::MagneticField() const { return this->dataPtr->magneticField; } ///////////////////////////////////////////////// -void World::SetMagneticField(const ignition::math::Vector3d &_mag) +void World::SetMagneticField(const gz::math::Vector3d &_mag) { this->dataPtr->magneticField = _mag; } diff --git a/src/World_TEST.cc b/src/World_TEST.cc index ba9b52a31..5975ae048 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -16,8 +16,8 @@ */ #include -#include -#include +#include +#include #include "sdf/World.hh" ///////////////////////////////////////////////// @@ -26,11 +26,11 @@ TEST(DOMWorld, Construction) sdf::World world; EXPECT_EQ(nullptr, world.Element()); EXPECT_TRUE(world.Name().empty()); - EXPECT_EQ(ignition::math::Vector3d(0, 0, -9.80665), world.Gravity()); - EXPECT_EQ(ignition::math::Vector3d(5.5645e-6, 22.8758e-6, -42.3884e-6), + EXPECT_EQ(gz::math::Vector3d(0, 0, -9.80665), world.Gravity()); + EXPECT_EQ(gz::math::Vector3d(5.5645e-6, 22.8758e-6, -42.3884e-6), world.MagneticField()); EXPECT_STREQ("default", world.AudioDevice().c_str()); - EXPECT_EQ(ignition::math::Vector3d::Zero, world.WindLinearVelocity()); + EXPECT_EQ(gz::math::Vector3d::Zero, world.WindLinearVelocity()); EXPECT_EQ(0u, world.ModelCount()); EXPECT_EQ(nullptr, world.ModelByIndex(0)); @@ -94,7 +94,7 @@ TEST(DOMWorld, CopyConstructor) ASSERT_TRUE(nullptr != world.Atmosphere()); EXPECT_DOUBLE_EQ(0.1, world.Atmosphere()->Pressure()); EXPECT_EQ("test_audio_device", world.AudioDevice()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, world.Gravity()); + EXPECT_EQ(gz::math::Vector3d::UnitX, world.Gravity()); ASSERT_TRUE(nullptr != world.Gui()); EXPECT_EQ(gui.Fullscreen(), world.Gui()->Fullscreen()); @@ -102,14 +102,14 @@ TEST(DOMWorld, CopyConstructor) ASSERT_TRUE(nullptr != world.Scene()); EXPECT_EQ(scene.Grid(), world.Scene()->Grid()); - EXPECT_EQ(ignition::math::Vector3d::UnitY, world.MagneticField()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, world.WindLinearVelocity()); + EXPECT_EQ(gz::math::Vector3d::UnitY, world.MagneticField()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, world.WindLinearVelocity()); EXPECT_EQ("test_world", world.Name()); ASSERT_TRUE(nullptr != world2.Atmosphere()); EXPECT_DOUBLE_EQ(0.1, world2.Atmosphere()->Pressure()); EXPECT_EQ("test_audio_device", world2.AudioDevice()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, world2.Gravity()); + EXPECT_EQ(gz::math::Vector3d::UnitX, world2.Gravity()); ASSERT_TRUE(nullptr != world2.Gui()); EXPECT_EQ(gui.Fullscreen(), world2.Gui()->Fullscreen()); @@ -117,8 +117,8 @@ TEST(DOMWorld, CopyConstructor) ASSERT_TRUE(nullptr != world2.Scene()); EXPECT_EQ(scene.Grid(), world2.Scene()->Grid()); - EXPECT_EQ(ignition::math::Vector3d::UnitY, world2.MagneticField()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, world2.WindLinearVelocity()); + EXPECT_EQ(gz::math::Vector3d::UnitY, world2.MagneticField()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, world2.WindLinearVelocity()); EXPECT_EQ("test_world", world2.Name()); } @@ -151,7 +151,7 @@ TEST(DOMWorld, CopyAssignmentOperator) ASSERT_TRUE(nullptr != world.Atmosphere()); EXPECT_DOUBLE_EQ(0.1, world.Atmosphere()->Pressure()); EXPECT_EQ("test_audio_device", world.AudioDevice()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, world.Gravity()); + EXPECT_EQ(gz::math::Vector3d::UnitX, world.Gravity()); ASSERT_TRUE(nullptr != world.Gui()); EXPECT_EQ(gui.Fullscreen(), world.Gui()->Fullscreen()); @@ -159,14 +159,14 @@ TEST(DOMWorld, CopyAssignmentOperator) ASSERT_TRUE(nullptr != world.Scene()); EXPECT_EQ(scene.Grid(), world.Scene()->Grid()); - EXPECT_EQ(ignition::math::Vector3d::UnitY, world.MagneticField()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, world.WindLinearVelocity()); + EXPECT_EQ(gz::math::Vector3d::UnitY, world.MagneticField()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, world.WindLinearVelocity()); EXPECT_EQ("test_world", world.Name()); ASSERT_TRUE(nullptr != world2.Atmosphere()); EXPECT_DOUBLE_EQ(0.1, world2.Atmosphere()->Pressure()); EXPECT_EQ("test_audio_device", world2.AudioDevice()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, world2.Gravity()); + EXPECT_EQ(gz::math::Vector3d::UnitX, world2.Gravity()); ASSERT_TRUE(nullptr != world2.Gui()); EXPECT_EQ(gui.Fullscreen(), world2.Gui()->Fullscreen()); @@ -174,8 +174,8 @@ TEST(DOMWorld, CopyAssignmentOperator) ASSERT_TRUE(nullptr != world2.Scene()); EXPECT_EQ(scene.Grid(), world2.Scene()->Grid()); - EXPECT_EQ(ignition::math::Vector3d::UnitY, world2.MagneticField()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, world2.WindLinearVelocity()); + EXPECT_EQ(gz::math::Vector3d::UnitY, world2.MagneticField()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, world2.WindLinearVelocity()); EXPECT_EQ("test_world", world2.Name()); } @@ -207,7 +207,7 @@ TEST(DOMWorld, MoveConstructor) ASSERT_TRUE(nullptr != world2.Atmosphere()); EXPECT_DOUBLE_EQ(0.1, world2.Atmosphere()->Pressure()); EXPECT_EQ("test_audio_device", world2.AudioDevice()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, world2.Gravity()); + EXPECT_EQ(gz::math::Vector3d::UnitX, world2.Gravity()); ASSERT_TRUE(nullptr != world2.Gui()); EXPECT_EQ(gui.Fullscreen(), world2.Gui()->Fullscreen()); @@ -215,8 +215,8 @@ TEST(DOMWorld, MoveConstructor) ASSERT_TRUE(nullptr != world2.Scene()); EXPECT_EQ(scene.Grid(), world2.Scene()->Grid()); - EXPECT_EQ(ignition::math::Vector3d::UnitY, world2.MagneticField()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, world2.WindLinearVelocity()); + EXPECT_EQ(gz::math::Vector3d::UnitY, world2.MagneticField()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, world2.WindLinearVelocity()); EXPECT_EQ("test_world", world2.Name()); } @@ -249,7 +249,7 @@ TEST(DOMWorld, MoveAssignmentOperator) ASSERT_TRUE(nullptr != world2.Atmosphere()); EXPECT_DOUBLE_EQ(0.1, world2.Atmosphere()->Pressure()); EXPECT_EQ("test_audio_device", world2.AudioDevice()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, world2.Gravity()); + EXPECT_EQ(gz::math::Vector3d::UnitX, world2.Gravity()); ASSERT_TRUE(nullptr != world2.Gui()); EXPECT_EQ(gui.Fullscreen(), world2.Gui()->Fullscreen()); @@ -257,8 +257,8 @@ TEST(DOMWorld, MoveAssignmentOperator) ASSERT_TRUE(nullptr != world2.Scene()); EXPECT_EQ(scene.Grid(), world2.Scene()->Grid()); - EXPECT_EQ(ignition::math::Vector3d::UnitY, world2.MagneticField()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, world2.WindLinearVelocity()); + EXPECT_EQ(gz::math::Vector3d::UnitY, world2.MagneticField()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, world2.WindLinearVelocity()); EXPECT_EQ("test_world", world2.Name()); } @@ -294,13 +294,13 @@ TEST(DOMWorld, Set) EXPECT_EQ("/dev/audio", world.AudioDevice()); world.SetWindLinearVelocity({0, 1 , 2}); - EXPECT_EQ(ignition::math::Vector3d(0, 1, 2), world.WindLinearVelocity()); + EXPECT_EQ(gz::math::Vector3d(0, 1, 2), world.WindLinearVelocity()); world.SetGravity({1, -2, 4}); - EXPECT_EQ(ignition::math::Vector3d(1, -2, 4), world.Gravity()); + EXPECT_EQ(gz::math::Vector3d(1, -2, 4), world.Gravity()); world.SetMagneticField({1.2, -2.3, 4.5}); - EXPECT_EQ(ignition::math::Vector3d(1.2, -2.3, 4.5), world.MagneticField()); + EXPECT_EQ(gz::math::Vector3d(1.2, -2.3, 4.5), world.MagneticField()); } ///////////////////////////////////////////////// @@ -324,16 +324,16 @@ TEST(DOMWorld, SetScene) EXPECT_EQ(nullptr, world.Scene()); sdf::Scene scene; - scene.SetAmbient(ignition::math::Color::Blue); - scene.SetBackground(ignition::math::Color::Red); + scene.SetAmbient(gz::math::Color::Blue); + scene.SetBackground(gz::math::Color::Red); scene.SetGrid(true); scene.SetShadows(true); scene.SetOriginVisual(true); world.SetScene(scene); ASSERT_NE(nullptr, world.Scene()); - EXPECT_EQ(ignition::math::Color::Blue, world.Scene()->Ambient()); - EXPECT_EQ(ignition::math::Color::Red, world.Scene()->Background()); + EXPECT_EQ(gz::math::Color::Blue, world.Scene()->Ambient()); + EXPECT_EQ(gz::math::Color::Red, world.Scene()->Background()); EXPECT_TRUE(world.Scene()->Grid()); EXPECT_TRUE(world.Scene()->Shadows()); EXPECT_TRUE(world.Scene()->OriginVisual()); diff --git a/src/ign.cc b/src/ign.cc index 06418efff..050f7d69a 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -28,7 +28,7 @@ #include "sdf/parser.hh" #include "sdf/system_util.hh" -#include "ignition/math/Inertial.hh" +#include "gz/math/Inertial.hh" #include "ign.hh" @@ -200,11 +200,11 @@ extern "C" SDFORMAT_VISIBLE int cmdInertialStats( " models will not be included." << std::endl; } - ignition::math::Inertiald totalInertial; + gz::math::Inertiald totalInertial; for (uint64_t i = 0; i < model->LinkCount(); i++) { - ignition::math::Inertiald currentLinkInertial; + gz::math::Inertiald currentLinkInertial; model->LinkByIndex(i)->ResolveInertial(currentLinkInertial, "__model__"); totalInertial += currentLinkInertial; diff --git a/src/parser.cc b/src/parser.cc index 4b108fc04..ccc3e85e0 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -20,7 +20,7 @@ #include #include -#include +#include #include "sdf/Console.hh" #include "sdf/Filesystem.hh" @@ -677,7 +677,7 @@ std::string getBestSupportedModelVersion(TiXmlElement *_modelXML, // If a match is not found, use the latest version of the element // that is not older than the SDF parser. - ignition::math::SemanticVersion sdfParserVersion(SDF_VERSION); + gz::math::SemanticVersion sdfParserVersion(SDF_VERSION); std::string bestVersionStr = "0.0"; TiXmlElement *sdfSearch = sdfXML; @@ -686,8 +686,8 @@ std::string getBestSupportedModelVersion(TiXmlElement *_modelXML, if (sdfSearch->Attribute("version")) { auto version = std::string(sdfSearch->Attribute("version")); - ignition::math::SemanticVersion modelVersion(version); - ignition::math::SemanticVersion bestVersion(bestVersionStr); + gz::math::SemanticVersion modelVersion(version); + gz::math::SemanticVersion bestVersion(bestVersionStr); if (modelVersion > bestVersion) { // this model is better than the previous one @@ -1266,8 +1266,8 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors) ElementPtr elem = modelPtr->GetFirstElement(); std::map replace; - ignition::math::Pose3d modelPose = - modelPtr->Get("pose"); + gz::math::Pose3d modelPose = + modelPtr->Get("pose"); std::string modelName = modelPtr->Get("name"); diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index 9d6f05b3c..ec6f2931f 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -172,7 +172,7 @@ TEST(Parser, addNestedModel) EXPECT_EQ(_canonicalLink, nestedModelFrame->Get("attached_to")); - using ignition::math::Pose3d; + using gz::math::Pose3d; const Pose3d pose(0, 0, 10, 0, 0, 1.57); EXPECT_EQ(pose, nestedModelFrame->Get("pose")); @@ -192,7 +192,7 @@ TEST(Parser, addNestedModel) EXPECT_EQ(_expressedIn, xyz->Get("expressed_in")); EXPECT_EQ( - ignition::math::Vector3d::UnitX, xyz->Get()); + gz::math::Vector3d::UnitX, xyz->Get()); }; // insert as 1.4, expect rotation of //joint/axis/xyz diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 871daf1a9..854165f35 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -91,7 +91,7 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt); /// in extensions when doing fixed joint reduction void ReduceSDFExtensionElementTransformReduction( std::vector::iterator _blobIt, - const ignition::math::Pose3d &_reductionTransform, + const gz::math::Pose3d &_reductionTransform, const std::string &_elementName); @@ -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, - const ignition::math::Pose3d &_currentTransform); + const gz::math::Pose3d &_currentTransform); /// insert extensions into links void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName); @@ -139,14 +139,14 @@ void CreateInertial(TiXmlElement *_elem, urdf::LinkConstSharedPtr _link); /// append transform (pose) to the end of the xml element void AddTransform(TiXmlElement *_elem, - const ignition::math::Pose3d &_transform); + const gz::math::Pose3d &_transform); /// create SDF from URDF link void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link); /// create SDF Link block based on URDF void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - const ignition::math::Pose3d &_currentTransform); + const gz::math::Pose3d &_currentTransform); /// reduced fixed joints: apply appropriate frame updates in joint /// inside urdf extensions when doing fixed joint reduction @@ -172,7 +172,7 @@ void ReduceSDFExtensionPluginFrameReplace( std::vector::iterator _blobIt, urdf::LinkSharedPtr _link, const std::string &_pluginName, const std::string &_elementName, - ignition::math::Pose3d _reductionTransform); + gz::math::Pose3d _reductionTransform); /// reduced fixed joints: apply appropriate frame updates in urdf /// extensions when doing fixed joint reduction @@ -220,22 +220,22 @@ urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame, urdf::Pose _parentToLinkTransform); /// reduced fixed joints: transform to parent frame -ignition::math::Pose3d TransformToParentFrame( - ignition::math::Pose3d _transformInLinkFrame, +gz::math::Pose3d TransformToParentFrame( + gz::math::Pose3d _transformInLinkFrame, urdf::Pose _parentToLinkTransform); /// reduced fixed joints: transform to parent frame -ignition::math::Pose3d TransformToParentFrame( - ignition::math::Pose3d _transformInLinkFrame, - ignition::math::Pose3d _parentToLinkTransform); +gz::math::Pose3d TransformToParentFrame( + gz::math::Pose3d _transformInLinkFrame, + gz::math::Pose3d _parentToLinkTransform); /// reduced fixed joints: utility to copy between urdf::Pose and /// math::Pose -ignition::math::Pose3d CopyPose(urdf::Pose _pose); +gz::math::Pose3d CopyPose(urdf::Pose _pose); /// reduced fixed joints: utility to copy between urdf::Pose and /// math::Pose -urdf::Pose CopyPose(ignition::math::Pose3d _pose); +urdf::Pose CopyPose(gz::math::Pose3d _pose); //////////////////////////////////////////////////////////////////////////////// bool URDF2SDF::IsURDF(const std::string &_filename) @@ -1176,9 +1176,9 @@ void AddKeyValue(TiXmlElement *_elem, const std::string &_key, } //////////////////////////////////////////////////////////////////////////////// -void AddTransform(TiXmlElement *_elem, const ignition::math::Pose3d &_transform) +void AddTransform(TiXmlElement *_elem, const gz::math::Pose3d &_transform) { - ignition::math::Vector3d e = _transform.Rot().Euler(); + gz::math::Vector3d e = _transform.Rot().Euler(); double cpose[6] = { _transform.Pos().X(), _transform.Pos().Y(), _transform.Pos().Z(), e.X(), e.Y(), e.Z() }; @@ -2423,28 +2423,28 @@ void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry) urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame, urdf::Pose _parentToLinkTransform) { - // transform to ignition::math::Pose3d then call TransformToParentFrame - ignition::math::Pose3d p1 = CopyPose(_transformInLinkFrame); - ignition::math::Pose3d p2 = CopyPose(_parentToLinkTransform); + // transform to gz::math::Pose3d then call TransformToParentFrame + gz::math::Pose3d p1 = CopyPose(_transformInLinkFrame); + gz::math::Pose3d p2 = CopyPose(_parentToLinkTransform); return CopyPose(TransformToParentFrame(p1, p2)); } //////////////////////////////////////////////////////////////////////////////// -ignition::math::Pose3d TransformToParentFrame( - ignition::math::Pose3d _transformInLinkFrame, +gz::math::Pose3d TransformToParentFrame( + gz::math::Pose3d _transformInLinkFrame, urdf::Pose _parentToLinkTransform) { - // transform to ignition::math::Pose3d then call TransformToParentFrame - ignition::math::Pose3d p2 = CopyPose(_parentToLinkTransform); + // transform to gz::math::Pose3d then call TransformToParentFrame + gz::math::Pose3d p2 = CopyPose(_parentToLinkTransform); return TransformToParentFrame(_transformInLinkFrame, p2); } //////////////////////////////////////////////////////////////////////////////// -ignition::math::Pose3d TransformToParentFrame( - ignition::math::Pose3d _transformInLinkFrame, - ignition::math::Pose3d _parentToLinkTransform) +gz::math::Pose3d TransformToParentFrame( + gz::math::Pose3d _transformInLinkFrame, + gz::math::Pose3d _parentToLinkTransform) { - ignition::math::Pose3d transformInParentLinkFrame; + gz::math::Pose3d transformInParentLinkFrame; // rotate link pose to parentLink frame transformInParentLinkFrame.Pos() = _parentToLinkTransform.Rot() * _transformInLinkFrame.Pos(); @@ -2649,7 +2649,7 @@ void CreateSDF(TiXmlElement *_root, // allow det(I) == zero, in the case of point mass geoms. // @todo: keyword "world" should be a constant defined somewhere else if (_link->name != "world" && - ((!_link->inertial) || ignition::math::equal(_link->inertial->mass, 0.0))) + ((!_link->inertial) || gz::math::equal(_link->inertial->mass, 0.0))) { if (!_link->child_links.empty()) { @@ -2686,7 +2686,7 @@ void CreateSDF(TiXmlElement *_root, (!_link->parent_joint || !FixedJointShouldBeReduced(_link->parent_joint))) { - CreateLink(_root, _link, ignition::math::Pose3d::Zero); + CreateLink(_root, _link, gz::math::Pose3d::Zero); } // recurse into children @@ -2697,9 +2697,9 @@ void CreateSDF(TiXmlElement *_root, } //////////////////////////////////////////////////////////////////////////////// -ignition::math::Pose3d CopyPose(urdf::Pose _pose) +gz::math::Pose3d CopyPose(urdf::Pose _pose) { - ignition::math::Pose3d p; + gz::math::Pose3d p; p.Pos().X() = _pose.position.x; p.Pos().Y() = _pose.position.y; p.Pos().Z() = _pose.position.z; @@ -2711,7 +2711,7 @@ ignition::math::Pose3d CopyPose(urdf::Pose _pose) } //////////////////////////////////////////////////////////////////////////////// -urdf::Pose CopyPose(ignition::math::Pose3d _pose) +urdf::Pose CopyPose(gz::math::Pose3d _pose) { urdf::Pose p; p.position.x = _pose.Pos().X(); @@ -2727,7 +2727,7 @@ urdf::Pose CopyPose(ignition::math::Pose3d _pose) //////////////////////////////////////////////////////////////////////////////// void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - const ignition::math::Pose3d &_currentTransform) + const gz::math::Pose3d &_currentTransform) { // create new body TiXmlElement *elem = new TiXmlElement("link"); @@ -2747,7 +2747,7 @@ void CreateLink(TiXmlElement *_root, { sdfdbg << "[" << _link->name << "] has no parent joint\n"; - if (_currentTransform != ignition::math::Pose3d::Zero) + if (_currentTransform != gz::math::Pose3d::Zero) { // create origin tag for this element AddTransform(elem, _currentTransform); @@ -2875,7 +2875,7 @@ void CreateInertial(TiXmlElement *_elem, _link->inertial->origin.rotation.getRPY(roll, pitch, yaw); /// add pose - ignition::math::Pose3d pose = CopyPose(_link->inertial->origin); + gz::math::Pose3d pose = CopyPose(_link->inertial->origin); AddTransform(inertial, pose); // add mass @@ -2904,7 +2904,7 @@ void CreateInertial(TiXmlElement *_elem, //////////////////////////////////////////////////////////////////////////////// void CreateJoint(TiXmlElement *_root, urdf::LinkConstSharedPtr _link, - const ignition::math::Pose3d &/*_currentTransform*/) + const gz::math::Pose3d &/*_currentTransform*/) { // compute the joint tag std::string jtype; @@ -3309,7 +3309,7 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt) //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionElementTransformReduction( std::vector::iterator _blobIt, - const ignition::math::Pose3d &_reductionTransform, + const gz::math::Pose3d &_reductionTransform, const std::string &_elementName) { auto element = *_blobIt; @@ -3327,7 +3327,7 @@ void ReduceSDFExtensionElementTransformReduction( // sdfdbg << " " << streamIn.str() << "\n"; // } - auto pose {ignition::math::Pose3d::Zero}; + auto pose {gz::math::Pose3d::Zero}; { std::string poseText = "0 0 0 0 0 0"; @@ -3433,7 +3433,7 @@ void ReduceSDFExtensionPluginFrameReplace( std::vector::iterator _blobIt, urdf::LinkSharedPtr _link, const std::string &_pluginName, const std::string &_elementName, - ignition::math::Pose3d _reductionTransform) + gz::math::Pose3d _reductionTransform) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; @@ -3461,7 +3461,7 @@ void ReduceSDFExtensionPluginFrameReplace( { urdf::Vector3 v1 = ParseVector3(xyzKey); _reductionTransform.Pos() = - ignition::math::Vector3d(v1.x, v1.y, v1.z); + gz::math::Vector3d(v1.x, v1.y, v1.z); // remove xyzOffset and rpyOffset (*_blobIt)->RemoveChild(xyzKey); } @@ -3470,7 +3470,7 @@ void ReduceSDFExtensionPluginFrameReplace( { urdf::Vector3 rpy = ParseVector3(rpyKey); _reductionTransform.Rot() = - ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z); + gz::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z); // remove xyzOffset and rpyOffset (*_blobIt)->RemoveChild(rpyKey); } diff --git a/test/env.hh b/test/env.hh index 660569429..ca6f7eae4 100644 --- a/test/env.hh +++ b/test/env.hh @@ -15,7 +15,7 @@ * */ -// Note: this is a direct copy from ignition::common, +// Note: this is a direct copy from gz::common, // and can be removed when sdformat has common as a dependency #ifndef SDF_TEST_ENV_HH diff --git a/test/integration/actor_dom.cc b/test/integration/actor_dom.cc index f4a53e6b0..2861d9789 100644 --- a/test/integration/actor_dom.cc +++ b/test/integration/actor_dom.cc @@ -55,7 +55,7 @@ TEST(DOMActor, LoadActors) const sdf::Actor *actor1 = world->ActorByIndex(0); EXPECT_EQ("actor_1", actor1->Name()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), actor1->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), actor1->RawPose()); EXPECT_EQ("", actor1->PoseRelativeTo()); EXPECT_EQ(1u, actor1->AnimationCount()); EXPECT_NE(nullptr, actor1->AnimationByIndex(0)); @@ -83,7 +83,7 @@ TEST(DOMActor, LoadActors) const sdf::Actor *actor2 = world->ActorByIndex(1); EXPECT_EQ("actor_2", actor2->Name()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 1.1, 0, 0, 0), actor2->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 1.1, 0, 0, 0), actor2->RawPose()); EXPECT_EQ("", actor2->PoseRelativeTo()); EXPECT_EQ(3u, actor2->AnimationCount()); EXPECT_NE(nullptr, actor2->AnimationByIndex(0)); diff --git a/test/integration/collision_dom.cc b/test/integration/collision_dom.cc index 08d48fc0b..791e30184 100644 --- a/test/integration/collision_dom.cc +++ b/test/integration/collision_dom.cc @@ -79,13 +79,13 @@ TEST(DOMCollision, DoublePendulum) const sdf::Collision *plateCol = baseLink->CollisionByIndex(0); ASSERT_TRUE(plateCol != nullptr); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.01, 0, 0, 0), plateCol->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0.01, 0, 0, 0), plateCol->RawPose()); EXPECT_EQ("", plateCol->PoseRelativeTo()); const sdf::Collision *poleCol = baseLink->CollisionByIndex(1); ASSERT_TRUE(poleCol != nullptr); - EXPECT_EQ(ignition::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), poleCol->RawPose()); EXPECT_EQ("", poleCol->PoseRelativeTo()); } @@ -100,7 +100,7 @@ TEST(DOMCollision, LoadModelFramesRelativeToJoint) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); diff --git a/test/integration/converter.cc b/test/integration/converter.cc index 73c13ffbe..927982c0b 100644 --- a/test/integration/converter.cc +++ b/test/integration/converter.cc @@ -140,14 +140,14 @@ void ParserStringConverter(const std::string &_version) sdf::ElementPtr gravityElem = worldElem->GetElement("gravity"); ASSERT_NE(nullptr, gravityElem); - EXPECT_EQ(gravityElem->Get(), - ignition::math::Vector3d(1, 0, -9.8)); + EXPECT_EQ(gravityElem->Get(), + gz::math::Vector3d(1, 0, -9.8)); EXPECT_EQ(_version, gravityElem->OriginalVersion()); sdf::ElementPtr magElem = worldElem->GetElement("magnetic_field"); ASSERT_NE(nullptr, magElem); - EXPECT_EQ(magElem->Get(), - ignition::math::Vector3d(1, 2, 3)); + EXPECT_EQ(magElem->Get(), + gz::math::Vector3d(1, 2, 3)); EXPECT_EQ(_version, magElem->OriginalVersion()); } diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 58e5df7ed..141a231ba 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -265,12 +265,12 @@ void FixedJointReductionCollisionVisualExtension(const std::string &_urdfFile, "file://media/materials/scripts/gazebo.material"); EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")-> - Get("ambient"), - ignition::math::Color(0, 1, 0, 1)); + Get("ambient"), + gz::math::Color(0, 1, 0, 1)); EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")-> - Get("ambient"), + Get("ambient"), sdf_child_link_1_vis->GetElement("material")-> - Get("ambient")); + Get("ambient")); // child_link_1a // @@ -316,12 +316,12 @@ void FixedJointReductionCollisionVisualExtension(const std::string &_urdfFile, // ambient unassigned should be 0, 0, 0, 1 EXPECT_EQ(urdf_child_link_1a_vis->GetElement("material")-> - Get("ambient"), - ignition::math::Color(0, 0, 0, 1)); + Get("ambient"), + gz::math::Color(0, 0, 0, 1)); EXPECT_EQ(urdf_child_link_1a_vis->GetElement("material")-> - Get("ambient"), + Get("ambient"), sdf_child_link_1a_vis->GetElement("material")-> - Get("ambient")); + Get("ambient")); // child_link_2 // @@ -363,12 +363,12 @@ void FixedJointReductionCollisionVisualExtension(const std::string &_urdfFile, // ambient unassigned should be 0, 0, 0, 1 EXPECT_EQ(urdf_child_link_2_vis->GetElement("material")-> - Get("ambient"), - ignition::math::Color(0, 0, 0, 1)); + Get("ambient"), + gz::math::Color(0, 0, 0, 1)); EXPECT_EQ(urdf_child_link_2_vis->GetElement("material")-> - Get("ambient"), + Get("ambient"), sdf_child_link_2_vis->GetElement("material")-> - Get("ambient")); + Get("ambient")); } ///////////////////////////////////////////////// @@ -492,12 +492,12 @@ void FixedJointReductionCollisionVisualExtensionEmptyRoot( "file://media/materials/scripts/gazebo.material"); EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")-> - Get("ambient"), - ignition::math::Color(0, 1, 0, 1)); + Get("ambient"), + gz::math::Color(0, 1, 0, 1)); EXPECT_EQ(urdf_child_link_1_vis->GetElement("material")-> - Get("ambient"), + Get("ambient"), sdf_child_link_1_vis->GetElement("material")-> - Get("ambient")); + Get("ambient")); } ///////////////////////////////////////////////// @@ -508,54 +508,54 @@ void FixedJointReductionEquivalence(const std::string &_file) ASSERT_TRUE(sdf::readFile(_file, robot)); std::map linkMasses; - std::map linkPoses; - std::map mapIxxIyyIzz; - std::map mapIxyIxzIyz; + std::map linkPoses; + std::map mapIxxIyyIzz; + std::map mapIxyIxzIyz; { std::string linkName = "link0"; linkMasses[linkName] = 1000; - linkPoses[linkName] = ignition::math::Pose3d(0, 0, -0.5, 0, -0, 0); - mapIxxIyyIzz[linkName] = ignition::math::Vector3d(1, 1, 1); - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(0, 0, 0); + linkPoses[linkName] = gz::math::Pose3d(0, 0, -0.5, 0, -0, 0); + mapIxxIyyIzz[linkName] = gz::math::Vector3d(1, 1, 1); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(0, 0, 0); } { std::string linkName = "link1"; linkMasses[linkName] = 100; linkPoses[linkName] = - ignition::math::Pose3d(0, -1.5, 0, -2.14159, 0.141593, 0.858407); - mapIxxIyyIzz[linkName] = ignition::math::Vector3d(2, 3, 4); - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(0, 0, 0); + gz::math::Pose3d(0, -1.5, 0, -2.14159, 0.141593, 0.858407); + mapIxxIyyIzz[linkName] = gz::math::Vector3d(2, 3, 4); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(0, 0, 0); } { std::string linkName = "link2"; linkMasses[linkName] = 200; linkPoses[linkName] = - ignition::math::Pose3d(0.2, 0.4, 1, -1.14159, -0.141593, 1.5708); - mapIxxIyyIzz[linkName] = ignition::math::Vector3d(5, 6, 7); - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(0, 0, 0); + gz::math::Pose3d(0.2, 0.4, 1, -1.14159, -0.141593, 1.5708); + mapIxxIyyIzz[linkName] = gz::math::Vector3d(5, 6, 7); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(0, 0, 0); } { std::string linkName = "link3"; linkMasses[linkName] = 400; linkPoses[linkName] = - ignition::math::Pose3d(0.1, 0.2, 0.3, -1.14159, 0.141593, 0.858407); - mapIxxIyyIzz[linkName] = ignition::math::Vector3d(8, 9, 10); - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(0, 0, 0); + gz::math::Pose3d(0.1, 0.2, 0.3, -1.14159, 0.141593, 0.858407); + mapIxxIyyIzz[linkName] = gz::math::Vector3d(8, 9, 10); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(0, 0, 0); } { std::string linkName = "link1a"; linkMasses[linkName] = 700; - linkPoses[linkName] = ignition::math::Pose3d( + linkPoses[linkName] = gz::math::Pose3d( 1.6, -2.72857, -0.342857, -2.14159, 0.141593, 0.858407); mapIxxIyyIzz[linkName] = - ignition::math::Vector3d(576.477313, 527.680922, 420.127477); + gz::math::Vector3d(576.477313, 527.680922, 420.127477); mapIxyIxzIyz[linkName] = - ignition::math::Vector3d(-65.680839, 134.562430, 264.780538); + gz::math::Vector3d(-65.680839, 134.562430, 264.780538); } // Count collisions and visuals @@ -569,8 +569,8 @@ void FixedJointReductionEquivalence(const std::string &_file) std::string linkName = link->Get("name"); sdf::ElementPtr inertial = link->GetElement("inertial"); double linkMass = inertial->Get("mass"); - ignition::math::Pose3d linkPose = - inertial->Get("pose"); + gz::math::Pose3d linkPose = + inertial->Get("pose"); sdf::ElementPtr inertia = inertial->GetElement("inertia"); double ixx = inertia->Get("ixx"); double iyy = inertia->Get("iyy"); @@ -642,60 +642,60 @@ TEST(SDFParser, FixedJointReductionSimple) ASSERT_TRUE(sdf::readFile(GetFullTestFilePath(SDF_TEST_FILE_SIMPLE), robot)); std::map linkMasses; - std::map linkPoses; - std::map mapIxxIyyIzz; - std::map mapIxyIxzIyz; + std::map linkPoses; + std::map mapIxxIyyIzz; + std::map mapIxyIxzIyz; - const ignition::math::Vector3d defaultIxxIyyIzz(7, 9, 11); + const gz::math::Vector3d defaultIxxIyyIzz(7, 9, 11); { std::string linkName = "link1"; linkMasses[linkName] = 300; - linkPoses[linkName] = ignition::math::Pose3d(); + linkPoses[linkName] = gz::math::Pose3d(); mapIxxIyyIzz[linkName] = defaultIxxIyyIzz; - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(); } { std::string linkName = "link1a"; linkMasses[linkName] = 300; - linkPoses[linkName] = ignition::math::Pose3d(); + linkPoses[linkName] = gz::math::Pose3d(); mapIxxIyyIzz[linkName] = defaultIxxIyyIzz; - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(); } { std::string linkName = "link1b"; linkMasses[linkName] = 300; - linkPoses[linkName] = ignition::math::Pose3d(); + linkPoses[linkName] = gz::math::Pose3d(); mapIxxIyyIzz[linkName] = defaultIxxIyyIzz; - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(); } { std::string linkName = "link1c"; linkMasses[linkName] = 300; - linkPoses[linkName] = ignition::math::Pose3d(0.2, 0, 0, 0, 0, 0); + linkPoses[linkName] = gz::math::Pose3d(0.2, 0, 0, 0, 0, 0); mapIxxIyyIzz[linkName] = - defaultIxxIyyIzz + ignition::math::Vector3d(0, 6, 6); - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(); + defaultIxxIyyIzz + gz::math::Vector3d(0, 6, 6); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(); } { std::string linkName = "link1d"; linkMasses[linkName] = 300; - linkPoses[linkName] = ignition::math::Pose3d(0.2, 0, 0, 0, 0, 0); + linkPoses[linkName] = gz::math::Pose3d(0.2, 0, 0, 0, 0, 0); mapIxxIyyIzz[linkName] = - defaultIxxIyyIzz + ignition::math::Vector3d(0, 6, 6); - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(); + defaultIxxIyyIzz + gz::math::Vector3d(0, 6, 6); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(); } { std::string linkName = "link1e"; linkMasses[linkName] = 300; - linkPoses[linkName] = ignition::math::Pose3d(0.2, 0, 0, 0, 0, 0); + linkPoses[linkName] = gz::math::Pose3d(0.2, 0, 0, 0, 0, 0); mapIxxIyyIzz[linkName] = - defaultIxxIyyIzz + ignition::math::Vector3d(0, 6, 6); - mapIxyIxzIyz[linkName] = ignition::math::Vector3d(); + defaultIxxIyyIzz + gz::math::Vector3d(0, 6, 6); + mapIxyIxzIyz[linkName] = gz::math::Vector3d(); } sdf::ElementPtr model = robot->Root()->GetElement("model"); @@ -705,8 +705,8 @@ TEST(SDFParser, FixedJointReductionSimple) std::string linkName = link->Get("name"); sdf::ElementPtr inertial = link->GetElement("inertial"); double linkMass = inertial->Get("mass"); - ignition::math::Pose3d linkPose = - inertial->Get("pose"); + gz::math::Pose3d linkPose = + inertial->Get("pose"); sdf::ElementPtr inertia = inertial->GetElement("inertia"); double ixx = inertia->Get("ixx"); double iyy = inertia->Get("iyy"); @@ -753,12 +753,12 @@ TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest) sdf::ElementPtr model = robot->Root()->GetElement("model"); sdf::ElementPtr plugin = model->GetElement("plugin"); - auto xyzOffset = plugin->Get("xyzOffset"); - auto rpyOffset = plugin->Get("rpyOffset"); + auto xyzOffset = plugin->Get("xyzOffset"); + auto rpyOffset = plugin->Get("rpyOffset"); auto bodyName = plugin->Get("bodyName"); EXPECT_EQ("base_link", bodyName); - EXPECT_EQ(ignition::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 1.5708), rpyOffset); + EXPECT_EQ(gz::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset); + EXPECT_EQ(gz::math::Vector3d(0, 0, 1.5708), rpyOffset); bool correctedOffset = plugin->Get("ignition::corrected_offsets"); EXPECT_TRUE(correctedOffset); diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 1cb6c0772..8524934ae 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -74,16 +74,16 @@ TEST(Frame, ModelFrame) sdf::ElementPtr poseElem = frameElem->GetElement("pose"); EXPECT_TRUE(poseElem->HasAttribute("relative_to")); EXPECT_EQ(poseElem->Get("relative_to"), "/world"); - EXPECT_EQ(poseElem->Get(), - ignition::math::Pose3d(1, 1, 0, 0, 0, 0)); + EXPECT_EQ(poseElem->Get(), + gz::math::Pose3d(1, 1, 0, 0, 0, 0)); // model pose EXPECT_TRUE(modelElem->HasElement("pose")); sdf::ElementPtr modelPoseElem = modelElem->GetElement("pose"); EXPECT_TRUE(modelPoseElem->HasAttribute("relative_to")); EXPECT_EQ(modelPoseElem->Get("relative_to"), "mframe"); - EXPECT_EQ(modelPoseElem->Get(), - ignition::math::Pose3d(1, 0, 0, 0, 0, 0)); + EXPECT_EQ(modelPoseElem->Get(), + gz::math::Pose3d(1, 0, 0, 0, 0, 0)); // link EXPECT_TRUE(modelElem->HasElement("link")); @@ -129,8 +129,8 @@ TEST(Frame, FrameDefaultPose) sdf::ElementPtr poseElem = frameElem->GetElement("pose"); EXPECT_TRUE(poseElem->HasAttribute("relative_to")); EXPECT_EQ(poseElem->Get("relative_to"), ""); - EXPECT_EQ(poseElem->Get(), - ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); + EXPECT_EQ(poseElem->Get(), + gz::math::Pose3d(0, 0, 0, 0, 0, 0)); // link EXPECT_TRUE(modelElem->HasElement("link")); @@ -176,8 +176,8 @@ TEST(Frame, NoFrame) sdf::ElementPtr poseElem = frameElem->GetElement("pose"); EXPECT_TRUE(poseElem->HasAttribute("relative_to")); EXPECT_EQ(poseElem->Get("relative_to"), ""); - EXPECT_EQ(poseElem->Get(), - ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); + EXPECT_EQ(poseElem->Get(), + gz::math::Pose3d(0, 0, 0, 0, 0, 0)); } // link @@ -192,8 +192,8 @@ TEST(Frame, NoFrame) sdf::ElementPtr poseElem = linkElem->GetElement("pose"); EXPECT_TRUE(poseElem->HasAttribute("relative_to")); EXPECT_EQ(poseElem->Get("relative_to"), ""); - EXPECT_EQ(poseElem->Get(), - ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); + EXPECT_EQ(poseElem->Get(), + gz::math::Pose3d(0, 0, 0, 0, 0, 0)); } } @@ -250,16 +250,16 @@ TEST(Frame, StateFrame) sdf::ElementPtr poseElem = frameElem->GetElement("pose"); EXPECT_TRUE(poseElem->HasAttribute("relative_to")); EXPECT_EQ(poseElem->Get("relative_to"), "/world"); - EXPECT_EQ(poseElem->Get(), - ignition::math::Pose3d(1, 0, 2, 0, 0, 0)); + EXPECT_EQ(poseElem->Get(), + gz::math::Pose3d(1, 0, 2, 0, 0, 0)); // model pose EXPECT_TRUE(modelStateElem->HasElement("pose")); sdf::ElementPtr modelPoseElem = modelStateElem->GetElement("pose"); EXPECT_TRUE(modelPoseElem->HasAttribute("relative_to")); EXPECT_EQ(modelPoseElem->Get("relative_to"), "mframe"); - EXPECT_EQ(modelPoseElem->Get(), - ignition::math::Pose3d(3, 3, 9, 0, 0, 0)); + EXPECT_EQ(modelPoseElem->Get(), + gz::math::Pose3d(3, 3, 9, 0, 0, 0)); } // link @@ -274,8 +274,8 @@ TEST(Frame, StateFrame) sdf::ElementPtr linkPoseElem = linkStateElem->GetElement("pose"); EXPECT_TRUE(linkPoseElem->HasAttribute("relative_to")); EXPECT_EQ(linkPoseElem->Get("relative_to"), "lframe"); - EXPECT_EQ(linkPoseElem->Get(), - ignition::math::Pose3d(111, 3, 0, 0, 0, 0)); + EXPECT_EQ(linkPoseElem->Get(), + gz::math::Pose3d(111, 3, 0, 0, 0, 0)); } EXPECT_TRUE(stateElem->HasElement("light")); @@ -291,8 +291,8 @@ TEST(Frame, StateFrame) sdf::ElementPtr lightPoseElem = lightStateElem->GetElement("pose"); EXPECT_TRUE(lightPoseElem->HasAttribute("relative_to")); EXPECT_EQ(lightPoseElem->Get("relative_to"), "lframe"); - EXPECT_EQ(lightPoseElem->Get(), - ignition::math::Pose3d(99, 0, 22, 0, 0, 0)); + EXPECT_EQ(lightPoseElem->Get(), + gz::math::Pose3d(99, 0, 22, 0, 0, 0)); } } @@ -337,8 +337,8 @@ TEST(Frame, IncludeRelativeTo) sdf::ElementPtr modelPoseElem = modelElem->GetElement("pose"); EXPECT_TRUE(modelPoseElem->HasAttribute("relative_to")); EXPECT_EQ(modelPoseElem->Get("relative_to"), "/world"); - EXPECT_EQ(modelPoseElem->Get(), - ignition::math::Pose3d(5, -2, 1, 0, 0, 0)); + EXPECT_EQ(modelPoseElem->Get(), + gz::math::Pose3d(5, -2, 1, 0, 0, 0)); } //////////////////////////////////////// @@ -389,8 +389,8 @@ TEST(Frame, IncludeRelativeToEmptyPose) sdf::ElementPtr modelPoseElem = modelElem->GetElement("pose"); EXPECT_TRUE(modelPoseElem->HasAttribute("relative_to")); EXPECT_EQ(modelPoseElem->Get("relative_to"), "/world"); - EXPECT_EQ(modelPoseElem->Get(), - ignition::math::Pose3d::Zero); + EXPECT_EQ(modelPoseElem->Get(), + gz::math::Pose3d::Zero); } // Check next model modelElem = modelElem->GetNextElement("model"); @@ -404,8 +404,8 @@ TEST(Frame, IncludeRelativeToEmptyPose) sdf::ElementPtr modelPoseElem = modelElem->GetElement("pose"); EXPECT_TRUE(modelPoseElem->HasAttribute("relative_to")); EXPECT_FALSE(modelPoseElem->GetAttribute("relative_to")->GetSet()); - EXPECT_EQ(modelPoseElem->Get(), - ignition::math::Pose3d::Zero); + EXPECT_EQ(modelPoseElem->Get(), + gz::math::Pose3d::Zero); } } @@ -426,7 +426,7 @@ TEST(DOMFrame, LoadModelFramesAttachedTo) EXPECT_EQ(1u, model->LinkCount()); EXPECT_NE(nullptr, model->LinkByIndex(0)); EXPECT_EQ(nullptr, model->LinkByIndex(1)); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); EXPECT_EQ("", model->PoseRelativeTo()); EXPECT_TRUE(model->LinkNameExists("L")); @@ -595,7 +595,7 @@ TEST(DOMFrame, LoadModelFramesAttachedToJoint) EXPECT_NE(nullptr, model->LinkByIndex(0)); EXPECT_NE(nullptr, model->LinkByIndex(1)); EXPECT_EQ(nullptr, model->LinkByIndex(2)); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); EXPECT_EQ("", model->PoseRelativeTo()); EXPECT_TRUE(model->LinkNameExists("P")); @@ -878,7 +878,7 @@ TEST(DOMFrame, LoadModelFramesRelativeTo) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -1039,7 +1039,7 @@ TEST(DOMFrame, LoadModelFramesRelativeToJoint) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -1231,7 +1231,7 @@ TEST(DOMFrame, LoadWorldFramesRelativeTo) EXPECT_EQ("F1", world->FrameByName("F1a")->AttachedTo()); EXPECT_TRUE(world->FrameByName("F2")->AttachedTo().empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; Pose pose; EXPECT_TRUE( world->FrameByName("world_frame")-> @@ -1317,7 +1317,7 @@ TEST(DOMFrame, WorldIncludeModel) sdf::Errors errors = root.LoadSdfString(stream.str()); EXPECT_TRUE(errors.empty()) << errors[0].Message(); - ignition::math::Pose3d expectedPoses[] = { + gz::math::Pose3d expectedPoses[] = { {0, 0, 0, 0, 0, 0}, {1, 0, 0, 0, 0, 0}, {1, 1, 0, 0, 0, 0}, @@ -1331,7 +1331,7 @@ TEST(DOMFrame, WorldIncludeModel) { const sdf::Model *model = world->ModelByIndex(i); ASSERT_NE(nullptr, model); - ignition::math::Pose3d modelPose; + gz::math::Pose3d modelPose; sdf::Errors resolveErrors = model->SemanticPose().Resolve(modelPose); EXPECT_TRUE(resolveErrors.empty()); EXPECT_EQ(expectedPoses[i], modelPose); @@ -1388,11 +1388,11 @@ TEST(Frame, IncludeFrameWithSubmodel) ASSERT_NE(nullptr, link0); EXPECT_EQ(link0->Name(), "box_with_submodel::link"); - ignition::math::Pose3d linkPose; + gz::math::Pose3d linkPose; sdf::Errors resolveErrors = link0->SemanticPose().Resolve(linkPose); EXPECT_TRUE(resolveErrors.empty()) << resolveErrors[0].Message(); EXPECT_EQ(linkPose, - ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); + gz::math::Pose3d(5, 5, 0, 0, 0, 0)); /* submodel: pose from parent is translated to model. links are the same * ... * @@ -1407,9 +1407,9 @@ TEST(Frame, IncludeFrameWithSubmodel) EXPECT_EQ(submodel->Name(), "box_with_submodel::submodel_of_box_with_submodel"); - ignition::math::Pose3d submodelPose; + gz::math::Pose3d submodelPose; resolveErrors = submodel->SemanticPose().Resolve(submodelPose); EXPECT_TRUE(resolveErrors.empty()) << resolveErrors[0].Message(); EXPECT_EQ(submodelPose, - ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); + gz::math::Pose3d(5, 5, 0, 0, 0, 0)); } diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 4b02a18e1..2ef138d97 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -61,7 +61,7 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::BOX, boxCol->Geom()->Type()); const sdf::Box *boxColGeom = boxCol->Geom()->BoxShape(); ASSERT_NE(nullptr, boxColGeom); - EXPECT_EQ(ignition::math::Vector3d(3, 4, 5), boxColGeom->Size()); + EXPECT_EQ(gz::math::Vector3d(3, 4, 5), boxColGeom->Size()); // Test box visual const sdf::Visual *boxVis = link->VisualByName("box_vis"); @@ -70,7 +70,7 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::BOX, boxVis->Geom()->Type()); const sdf::Box *boxVisGeom = boxVis->Geom()->BoxShape(); ASSERT_NE(nullptr, boxVisGeom); - EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), boxVisGeom->Size()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), boxVisGeom->Size()); // Test cylinder collision const sdf::Collision *cylinderCol = link->CollisionByName("cylinder_col"); @@ -99,8 +99,8 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::PLANE, planeCol->Geom()->Type()); const sdf::Plane *planeColGeom = planeCol->Geom()->PlaneShape(); ASSERT_NE(nullptr, planeColGeom); - EXPECT_EQ(ignition::math::Vector3d::UnitX, planeColGeom->Normal()); - EXPECT_EQ(ignition::math::Vector2d(1.4, 6.3), planeColGeom->Size()); + EXPECT_EQ(gz::math::Vector3d::UnitX, planeColGeom->Normal()); + EXPECT_EQ(gz::math::Vector2d(1.4, 6.3), planeColGeom->Size()); // Test plane visual const sdf::Visual *planeVis = link->VisualByName("plane_vis"); @@ -109,8 +109,8 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::PLANE, planeVis->Geom()->Type()); const sdf::Plane *planeVisGeom = planeVis->Geom()->PlaneShape(); ASSERT_NE(nullptr, planeVisGeom); - EXPECT_EQ(ignition::math::Vector3d::UnitY, planeVisGeom->Normal()); - EXPECT_EQ(ignition::math::Vector2d(2, 4), planeVisGeom->Size()); + EXPECT_EQ(gz::math::Vector3d::UnitY, planeVisGeom->Normal()); + EXPECT_EQ(gz::math::Vector2d(2, 4), planeVisGeom->Size()); // Test sphere collision const sdf::Collision *sphereCol = link->CollisionByName("sphere_col"); @@ -139,7 +139,7 @@ TEST(DOMGeometry, Shapes) ASSERT_NE(nullptr, meshColGeom); EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/mesh/" "mesh.dae", meshColGeom->Uri()); - EXPECT_TRUE(ignition::math::Vector3d(0.1, 0.2, 0.3) == + EXPECT_TRUE(gz::math::Vector3d(0.1, 0.2, 0.3) == meshColGeom->Scale()); EXPECT_EQ("my_submesh", meshColGeom->Submesh()); EXPECT_TRUE(meshColGeom->CenterSubmesh()); @@ -153,7 +153,7 @@ TEST(DOMGeometry, Shapes) ASSERT_NE(nullptr, meshVisGeom); EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/mesh" "/mesh.dae", meshVisGeom->Uri()); - EXPECT_TRUE(ignition::math::Vector3d(1.2, 2.3, 3.4) == + EXPECT_TRUE(gz::math::Vector3d(1.2, 2.3, 3.4) == meshVisGeom->Scale()); EXPECT_EQ("another_submesh", meshVisGeom->Submesh()); EXPECT_FALSE(meshVisGeom->CenterSubmesh()); @@ -167,8 +167,8 @@ TEST(DOMGeometry, Shapes) ASSERT_NE(nullptr, heightmapColGeom); EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" "materials/textures/heightmap.png", heightmapColGeom->Uri()); - EXPECT_EQ(ignition::math::Vector3d(500, 500, 100), heightmapColGeom->Size()); - EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), heightmapColGeom->Position()); + EXPECT_EQ(gz::math::Vector3d(500, 500, 100), heightmapColGeom->Size()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), heightmapColGeom->Position()); EXPECT_EQ(0u, heightmapColGeom->TextureCount()); EXPECT_EQ(0u, heightmapColGeom->BlendCount()); @@ -181,8 +181,8 @@ TEST(DOMGeometry, Shapes) ASSERT_NE(nullptr, heightmapVisGeom); EXPECT_EQ("https://fuel.ignitionrobotics.org/1.0/an_org/models/a_model/" "materials/textures/heightmap.png", heightmapVisGeom->Uri()); - EXPECT_EQ(ignition::math::Vector3d(500, 500, 100), heightmapVisGeom->Size()); - EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), heightmapVisGeom->Position()); + EXPECT_EQ(gz::math::Vector3d(500, 500, 100), heightmapVisGeom->Size()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), heightmapVisGeom->Position()); EXPECT_EQ(3u, heightmapVisGeom->TextureCount()); EXPECT_EQ(2u, heightmapVisGeom->BlendCount()); @@ -225,15 +225,15 @@ TEST(DOMGeometry, Shapes) ASSERT_EQ(2u, polylineColGeom.size()); EXPECT_DOUBLE_EQ(0.5, polylineColGeom[0].Height()); ASSERT_EQ(5u, polylineColGeom[0].PointCount()); - EXPECT_EQ(ignition::math::Vector2d(-0.5, -0.5), + EXPECT_EQ(gz::math::Vector2d(-0.5, -0.5), *polylineColGeom[0].PointByIndex(0)); - EXPECT_EQ(ignition::math::Vector2d(-0.5, 0.5), + EXPECT_EQ(gz::math::Vector2d(-0.5, 0.5), *polylineColGeom[0].PointByIndex(1)); EXPECT_DOUBLE_EQ(0.3, polylineColGeom[1].Height()); ASSERT_EQ(4u, polylineColGeom[1].PointCount()); - EXPECT_EQ(ignition::math::Vector2d(-0.3, -0.3), + EXPECT_EQ(gz::math::Vector2d(-0.3, -0.3), *polylineColGeom[1].PointByIndex(0)); - EXPECT_EQ(ignition::math::Vector2d(-0.3, 0.3), + EXPECT_EQ(gz::math::Vector2d(-0.3, 0.3), *polylineColGeom[1].PointByIndex(1)); // Test polyline visual @@ -246,8 +246,8 @@ TEST(DOMGeometry, Shapes) ASSERT_EQ(1u, polylineVisGeom.size()); EXPECT_DOUBLE_EQ(1.0, polylineVisGeom[0].Height()); ASSERT_EQ(3u, polylineVisGeom[0].PointCount()); - EXPECT_EQ(ignition::math::Vector2d(-0.2, -0.2), + EXPECT_EQ(gz::math::Vector2d(-0.2, -0.2), *polylineVisGeom[0].PointByIndex(0)); - EXPECT_EQ(ignition::math::Vector2d(-0.2, 0.2), + EXPECT_EQ(gz::math::Vector2d(-0.2, 0.2), *polylineVisGeom[0].PointByIndex(1)); } diff --git a/test/integration/includes.cc b/test/integration/includes.cc index 815643ed9..f7b266380 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -77,7 +77,7 @@ TEST(IncludesTest, Includes) EXPECT_EQ(actorFile, actor->FilePath()); EXPECT_EQ("actor", actor->Name()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), actor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), actor->RawPose()); EXPECT_EQ("", actor->PoseRelativeTo()); EXPECT_EQ("meshes/skin.dae", actor->SkinFilename()); EXPECT_DOUBLE_EQ(1.0, actor->SkinScale()); @@ -112,7 +112,7 @@ TEST(IncludesTest, Includes) const auto *actor1 = world->ActorByIndex(1); EXPECT_EQ("override_actor_name", actor1->Name()); - EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), actor1->RawPose()); + EXPECT_EQ(gz::math::Pose3d(7, 8, 9, 0, 0, 0), actor1->RawPose()); EXPECT_EQ("", actor1->PoseRelativeTo()); ASSERT_NE(nullptr, actor1->Element()); EXPECT_TRUE(actor1->Element()->HasElement("plugin")); @@ -127,7 +127,7 @@ TEST(IncludesTest, Includes) EXPECT_EQ("point_light", pointLight->Name()); EXPECT_EQ(sdf::LightType::POINT, pointLight->Type()); EXPECT_FALSE(pointLight->CastShadows()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 10, 0, 0, 0), pointLight->RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 10, 0, 0, 0), pointLight->RawPose()); EXPECT_EQ("world", pointLight->PoseRelativeTo()); EXPECT_DOUBLE_EQ(123.5, pointLight->AttenuationRange()); EXPECT_DOUBLE_EQ(1.0, pointLight->LinearAttenuationFactor()); @@ -140,7 +140,7 @@ TEST(IncludesTest, Includes) const auto *pointLight1 = world->LightByIndex(1); ASSERT_NE(nullptr, pointLight1); EXPECT_EQ("override_light_name", pointLight1->Name()); - EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), pointLight1->RawPose()); + EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 0), pointLight1->RawPose()); EXPECT_EQ("", pointLight1->PoseRelativeTo()); // Models @@ -175,7 +175,7 @@ TEST(IncludesTest, Includes) ASSERT_NE(nullptr, meshColGeom); EXPECT_EQ("meshes/mesh.dae", meshColGeom->Uri()); EXPECT_EQ(modelFile, meshColGeom->FilePath()); - EXPECT_TRUE(ignition::math::Vector3d(0.1, 0.2, 0.3) == + EXPECT_TRUE(gz::math::Vector3d(0.1, 0.2, 0.3) == meshColGeom->Scale()); EXPECT_EQ("my_submesh", meshColGeom->Submesh()); EXPECT_TRUE(meshColGeom->CenterSubmesh()); @@ -187,7 +187,7 @@ TEST(IncludesTest, Includes) const auto *meshVisGeom = meshVis->Geom()->MeshShape(); EXPECT_EQ(modelFile, meshVisGeom->FilePath()); EXPECT_EQ("meshes/mesh.dae", meshVisGeom->Uri()); - EXPECT_TRUE(ignition::math::Vector3d(1.2, 2.3, 3.4) == + EXPECT_TRUE(gz::math::Vector3d(1.2, 2.3, 3.4) == meshVisGeom->Scale()); EXPECT_EQ("another_submesh", meshVisGeom->Submesh()); EXPECT_FALSE(meshVisGeom->CenterSubmesh()); @@ -200,7 +200,7 @@ TEST(IncludesTest, Includes) ASSERT_NE(nullptr, model1); EXPECT_EQ("override_model_name", model1->Name()); EXPECT_TRUE(model1->Static()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), model1->RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), model1->RawPose()); EXPECT_EQ("", model1->PoseRelativeTo()); ASSERT_NE(nullptr, model1->Element()); EXPECT_TRUE(model1->Element()->HasElement("plugin")); diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc index 5169557f3..13e687079 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -79,8 +79,8 @@ TEST(DOMJointAxis, Complete) const sdf::JointAxis *axis2 = joint->Axis(1); ASSERT_NE(nullptr, axis2); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, axis->Xyz()); - EXPECT_EQ(ignition::math::Vector3d::UnitY, axis2->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, axis->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitY, axis2->Xyz()); EXPECT_EQ("__model__", axis->XyzExpressedIn()); EXPECT_TRUE(axis2->XyzExpressedIn().empty()); @@ -120,9 +120,9 @@ TEST(DOMJointAxis, XyzExpressedIn) sdf::Errors errors = root.Load(testFile); EXPECT_TRUE(errors.empty()); - using Pose = ignition::math::Pose3d; - using Quaternion = ignition::math::Quaterniond; - using Vector3 = ignition::math::Vector3d; + using Pose = gz::math::Pose3d; + using Quaternion = gz::math::Quaterniond; + using Vector3 = gz::math::Vector3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index a9290a0c9..163ec373d 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -110,8 +110,8 @@ TEST(DOMJoint, DoublePendulum) ASSERT_NE(nullptr, lowerAxis); // Check the xyz values for both axis. - EXPECT_EQ(ignition::math::Vector3d::UnitX, upperAxis->Xyz()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, lowerAxis->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitX, upperAxis->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitX, lowerAxis->Xyz()); } ////////////////////////////////////////////////// @@ -129,7 +129,7 @@ TEST(DOMJoint, Complete) const sdf::Model *model = root.ModelByIndex(0); ASSERT_NE(nullptr, model); - std::vector jointPoses = + std::vector jointPoses = { {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, @@ -166,7 +166,7 @@ TEST(DOMJoint, LoadJointParentWorld) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -233,7 +233,7 @@ TEST(DOMJoint, LoadJointPoseRelativeTo) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -419,7 +419,7 @@ TEST(DOMJoint, LoadLinkJointSameName16Valid) std::cout << e << std::endl; EXPECT_TRUE(errors.empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -497,8 +497,8 @@ TEST(DOMJoint, LoadURDFJointPoseRelativeTo) std::cout << e << std::endl; EXPECT_TRUE(errors.empty()); - using Pose = ignition::math::Pose3d; - using Vector3 = ignition::math::Vector3d; + using Pose = gz::math::Pose3d; + using Vector3 = gz::math::Vector3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -593,7 +593,7 @@ TEST(DOMJoint, Sensors) EXPECT_EQ("joint", joint->Name()); EXPECT_EQ(1u, joint->SensorCount()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // Get the force_torque sensor const sdf::Sensor *forceTorqueSensor = @@ -601,7 +601,7 @@ TEST(DOMJoint, Sensors) ASSERT_NE(nullptr, forceTorqueSensor); EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name()); EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(10, 11, 12, 0, 0, 0), forceTorqueSensor->RawPose()); auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor(); ASSERT_NE(nullptr, forceTorqueSensorObj); diff --git a/test/integration/light_dom.cc b/test/integration/light_dom.cc index 5e3d26ab4..0a3a95e04 100644 --- a/test/integration/light_dom.cc +++ b/test/integration/light_dom.cc @@ -47,13 +47,13 @@ TEST(DOMWorld, LoadLights) EXPECT_EQ("point_light", pointLight->Name()); EXPECT_EQ(sdf::LightType::POINT, pointLight->Type()); EXPECT_FALSE(pointLight->CastShadows()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 10, 0, 0, 0), pointLight->RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 10, 0, 0, 0), pointLight->RawPose()); EXPECT_EQ("world", pointLight->PoseRelativeTo()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; EXPECT_TRUE(pointLight->SemanticPose().Resolve(pose, "world").empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 10, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 2, 10, 0, 0, 0), pose); EXPECT_TRUE(pointLight->SemanticPose().Resolve(pose).empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 10, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 2, 10, 0, 0, 0), pose); EXPECT_DOUBLE_EQ(123.5, pointLight->AttenuationRange()); EXPECT_DOUBLE_EQ(1.0, pointLight->LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.0, pointLight->ConstantAttenuationFactor()); @@ -71,14 +71,14 @@ TEST(DOMWorld, LoadLights) const sdf::Light *dirLight = world->LightByIndex(2); ASSERT_NE(nullptr, dirLight); EXPECT_EQ("directional_light", dirLight->Name()); - EXPECT_EQ(ignition::math::Pose3d(0, 10, 20, 0, 0, 0), dirLight->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 10, 20, 0, 0, 0), dirLight->RawPose()); EXPECT_EQ("frame1", dirLight->PoseRelativeTo()); EXPECT_TRUE(dirLight->SemanticPose().Resolve(pose, "frame1").empty()); - EXPECT_EQ(ignition::math::Pose3d(0, 10, 20, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(0, 10, 20, 0, 0, 0), pose); EXPECT_TRUE(dirLight->SemanticPose().Resolve(pose, "world").empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 12, 23, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 12, 23, 0, 0, 0), pose); EXPECT_TRUE(dirLight->SemanticPose().Resolve(pose).empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 12, 23, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 12, 23, 0, 0, 0), pose); EXPECT_DOUBLE_EQ(0.0, dirLight->AttenuationRange()); EXPECT_DOUBLE_EQ(0.0, dirLight->LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(1.0, dirLight->ConstantAttenuationFactor()); @@ -86,17 +86,17 @@ TEST(DOMWorld, LoadLights) const sdf::Model *model = world->ModelByIndex(0); ASSERT_NE(nullptr, model); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); EXPECT_EQ("frame1", model->PoseRelativeTo()); EXPECT_TRUE(model->SemanticPose().Resolve(pose, "frame1").empty()); errors = model->SemanticPose().Resolve(pose, "frame1"); for (auto e : errors) std::cout << e.Message() << std::endl; - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), pose); EXPECT_TRUE(model->SemanticPose().Resolve(pose, "world").empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), pose); EXPECT_TRUE(model->SemanticPose().Resolve(pose).empty()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), pose); const sdf::Link *link = model->LinkByIndex(0); ASSERT_NE(nullptr, link); @@ -104,9 +104,9 @@ TEST(DOMWorld, LoadLights) ASSERT_NE(nullptr, linkLight); EXPECT_EQ("spot_light", linkLight->Name()); EXPECT_TRUE(linkLight->SemanticPose().Resolve(pose, "frame2").empty()); - EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(7, 8, 9, 0, 0, 0), pose); EXPECT_TRUE(linkLight->SemanticPose().Resolve(pose, "__model__").empty()); - EXPECT_EQ(ignition::math::Pose3d(11, 13, 15, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(11, 13, 15, 0, 0, 0), pose); EXPECT_TRUE(linkLight->SemanticPose().Resolve(pose).empty()); - EXPECT_EQ(ignition::math::Pose3d(11, 13, 15, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(11, 13, 15, 0, 0, 0), pose); } diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index b2d225390..9546b07a9 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -18,7 +18,7 @@ #include #include -#include +#include #include "sdf/AirPressure.hh" #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" @@ -126,10 +126,10 @@ TEST(DOMLink, InertialDoublePendulum) const sdf::Link *baseLink = model->LinkByIndex(0); ASSERT_NE(nullptr, baseLink); - EXPECT_EQ(ignition::math::Pose3d::Zero, baseLink->RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, baseLink->RawPose()); EXPECT_EQ("", baseLink->PoseRelativeTo()); - const ignition::math::Inertiald inertial = baseLink->Inertial(); + const gz::math::Inertiald inertial = baseLink->Inertial(); EXPECT_DOUBLE_EQ(100.0, inertial.MassMatrix().Mass()); EXPECT_DOUBLE_EQ(1.0, inertial.MassMatrix().DiagonalMoments().X()); EXPECT_DOUBLE_EQ(1.0, inertial.MassMatrix().DiagonalMoments().Y()); @@ -140,12 +140,12 @@ TEST(DOMLink, InertialDoublePendulum) const sdf::Link *upperLink = model->LinkByIndex(1); ASSERT_NE(nullptr, upperLink); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 2.1, -1.5708, 0, 0), + EXPECT_EQ(gz::math::Pose3d(0, 0, 2.1, -1.5708, 0, 0), upperLink->RawPose()); EXPECT_EQ("", upperLink->PoseRelativeTo()); EXPECT_TRUE(upperLink->EnableWind()); - const ignition::math::Inertiald inertialUpper = upperLink->Inertial(); + const gz::math::Inertiald inertialUpper = upperLink->Inertial(); EXPECT_DOUBLE_EQ(1.0, inertialUpper.MassMatrix().Mass()); EXPECT_DOUBLE_EQ(1.0, inertialUpper.MassMatrix().DiagonalMoments().X()); EXPECT_DOUBLE_EQ(1.0, inertialUpper.MassMatrix().DiagonalMoments().Y()); @@ -160,7 +160,7 @@ TEST(DOMLink, InertialDoublePendulum) const sdf::Link *lowerLink = model->LinkByIndex(2); ASSERT_TRUE(lowerLink != nullptr); - EXPECT_EQ(ignition::math::Pose3d(0.25, 1.0, 2.1, -2, 0, 0), + EXPECT_EQ(gz::math::Pose3d(0.25, 1.0, 2.1, -2, 0, 0), lowerLink->RawPose()); EXPECT_EQ("", lowerLink->PoseRelativeTo()); } @@ -181,7 +181,7 @@ TEST(DOMLink, InertialComplete) const sdf::Link *link = model->LinkByIndex(0); ASSERT_NE(nullptr, link); - const ignition::math::Inertiald inertial = link->Inertial(); + const gz::math::Inertiald inertial = link->Inertial(); EXPECT_DOUBLE_EQ(17.982, inertial.MassMatrix().Mass()); EXPECT_DOUBLE_EQ(0.125569, inertial.MassMatrix().DiagonalMoments().X()); EXPECT_DOUBLE_EQ(0.0972062, inertial.MassMatrix().DiagonalMoments().Y()); @@ -249,7 +249,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, altimeterSensor); EXPECT_EQ("altimeter_sensor", altimeterSensor->Name()); EXPECT_EQ(sdf::SensorType::ALTIMETER, altimeterSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d::Zero, altimeterSensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, altimeterSensor->RawPose()); EXPECT_FALSE(altimeterSensor->EnableMetrics()); const sdf::Altimeter *altSensor = altimeterSensor->AltimeterSensor(); ASSERT_NE(nullptr, altSensor); @@ -265,12 +265,12 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, cameraSensor); EXPECT_EQ("camera_sensor", cameraSensor->Name()); EXPECT_EQ(sdf::SensorType::CAMERA, cameraSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), cameraSensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), cameraSensor->RawPose()); EXPECT_FALSE(cameraSensor->EnableMetrics()); const sdf::Camera *camSensor = cameraSensor->CameraSensor(); ASSERT_NE(nullptr, camSensor); EXPECT_EQ("my_camera", camSensor->Name()); - EXPECT_EQ(ignition::math::Pose3d(0.1, 0.2, 0.3, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(0.1, 0.2, 0.3, 0, 0, 0), camSensor->RawPose()); EXPECT_DOUBLE_EQ(0.75, camSensor->HorizontalFov().Radian()); EXPECT_EQ("/camera_sensor/camera_info", camSensor->CameraInfoTopic()); @@ -288,7 +288,7 @@ TEST(DOMLink, Sensors) EXPECT_DOUBLE_EQ(0.3, camSensor->DistortionK3()); EXPECT_DOUBLE_EQ(0.4, camSensor->DistortionP1()); EXPECT_DOUBLE_EQ(0.5, camSensor->DistortionP2()); - EXPECT_EQ(ignition::math::Vector2d(0.2, 0.4), camSensor->DistortionCenter()); + EXPECT_EQ(gz::math::Vector2d(0.2, 0.4), camSensor->DistortionCenter()); EXPECT_EQ("custom", camSensor->LensType()); EXPECT_FALSE(camSensor->LensScaleToHfov()); EXPECT_DOUBLE_EQ(1.1, camSensor->LensC1()); @@ -310,18 +310,18 @@ TEST(DOMLink, Sensors) EXPECT_DOUBLE_EQ(0, camSensor->LensProjectionTx()); EXPECT_DOUBLE_EQ(0, camSensor->LensProjectionTy()); - ignition::math::Pose3d pose; + gz::math::Pose3d pose; // Get the contact sensor const sdf::Sensor *contactSensor = link->SensorByName("contact_sensor"); ASSERT_NE(nullptr, contactSensor); EXPECT_EQ("contact_sensor", contactSensor->Name()); EXPECT_EQ(sdf::SensorType::CONTACT, contactSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), contactSensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 0), contactSensor->RawPose()); EXPECT_TRUE(contactSensor->SemanticPose().Resolve(pose, "__model__").empty()); - EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 0), pose); EXPECT_TRUE(contactSensor->SemanticPose().Resolve(pose).empty()); - EXPECT_EQ(ignition::math::Pose3d(4, 5, 3, 0, 0, 0), pose); + EXPECT_EQ(gz::math::Pose3d(4, 5, 3, 0, 0, 0), pose); EXPECT_TRUE(contactSensor->EnableMetrics()); // Get the depth sensor @@ -329,7 +329,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, depthSensor); EXPECT_EQ("depth_sensor", depthSensor->Name()); EXPECT_EQ(sdf::SensorType::DEPTH_CAMERA, depthSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), depthSensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(7, 8, 9, 0, 0, 0), depthSensor->RawPose()); EXPECT_TRUE(depthSensor->EnableMetrics()); const sdf::Camera *depthCamSensor = depthSensor->CameraSensor(); ASSERT_NE(nullptr, depthCamSensor); @@ -340,7 +340,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, rgbdSensor); EXPECT_EQ("rgbd_sensor", rgbdSensor->Name()); EXPECT_EQ(sdf::SensorType::RGBD_CAMERA, rgbdSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(37, 38, 39, 0, 0, 0), rgbdSensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(37, 38, 39, 0, 0, 0), rgbdSensor->RawPose()); EXPECT_FALSE(rgbdSensor->EnableMetrics()); const sdf::Camera *rgbdCamSensor = rgbdSensor->CameraSensor(); ASSERT_NE(nullptr, rgbdCamSensor); @@ -351,7 +351,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, thermalSensor); EXPECT_EQ("thermal_sensor", thermalSensor->Name()); EXPECT_EQ(sdf::SensorType::THERMAL_CAMERA, thermalSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(37, 38, 39, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(37, 38, 39, 0, 0, 0), thermalSensor->RawPose()); EXPECT_FALSE(thermalSensor->EnableMetrics()); const sdf::Camera *thermalCamSensor = thermalSensor->CameraSensor(); @@ -364,7 +364,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, forceTorqueSensor); EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name()); EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(10, 11, 12, 0, 0, 0), forceTorqueSensor->RawPose()); EXPECT_FALSE(forceTorqueSensor->EnableMetrics()); @@ -373,7 +373,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, navSatSensor); EXPECT_EQ("navsat_sensor", navSatSensor->Name()); EXPECT_EQ(sdf::SensorType::NAVSAT, navSatSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(13, 14, 15, 0, 0, 0), navSatSensor->RawPose()); EXPECT_FALSE(navSatSensor->EnableMetrics()); const sdf::NavSat *navSatSensorObj = navSatSensor->NavSatSensor(); @@ -393,7 +393,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, gpuRaySensor); EXPECT_EQ("gpu_ray_sensor", gpuRaySensor->Name()); EXPECT_EQ(sdf::SensorType::GPU_LIDAR, gpuRaySensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), gpuRaySensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), gpuRaySensor->RawPose()); EXPECT_FALSE(gpuRaySensor->EnableMetrics()); const sdf::Lidar *gpuRay = gpuRaySensor->LidarSensor(); ASSERT_NE(nullptr, gpuRay); @@ -403,7 +403,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, gpuLidarSensor); EXPECT_EQ("gpu_lidar_sensor", gpuLidarSensor->Name()); EXPECT_EQ(sdf::SensorType::GPU_LIDAR, gpuLidarSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), gpuLidarSensor->RawPose()); EXPECT_FALSE(gpuLidarSensor->EnableMetrics()); const sdf::Lidar *gpuLidar = gpuLidarSensor->LidarSensor(); @@ -414,7 +414,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, imuSensor); EXPECT_EQ("imu_sensor", imuSensor->Name()); EXPECT_EQ(sdf::SensorType::IMU, imuSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), imuSensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 0), imuSensor->RawPose()); EXPECT_FALSE(imuSensor->EnableMetrics()); const sdf::Imu *imuSensorObj = imuSensor->ImuSensor(); ASSERT_NE(nullptr, imuSensorObj); @@ -465,9 +465,9 @@ TEST(DOMLink, Sensors) EXPECT_EQ("ENU", imuSensorObj->Localization()); EXPECT_EQ("linka", imuSensorObj->CustomRpyParentFrame()); - EXPECT_EQ(ignition::math::Vector3d::UnitY, imuSensorObj->CustomRpy()); + EXPECT_EQ(gz::math::Vector3d::UnitY, imuSensorObj->CustomRpy()); EXPECT_EQ("linkb", imuSensorObj->GravityDirXParentFrame()); - EXPECT_EQ(ignition::math::Vector3d::UnitZ, imuSensorObj->GravityDirX()); + EXPECT_EQ(gz::math::Vector3d::UnitZ, imuSensorObj->GravityDirX()); // Get the logical camera sensor const sdf::Sensor *logicalCameraSensor = @@ -475,7 +475,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, logicalCameraSensor); EXPECT_EQ("logical_camera_sensor", logicalCameraSensor->Name()); EXPECT_EQ(sdf::SensorType::LOGICAL_CAMERA, logicalCameraSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(7, 8, 9, 0, 0, 0), logicalCameraSensor->RawPose()); EXPECT_FALSE(logicalCameraSensor->EnableMetrics()); @@ -485,7 +485,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, magnetometerSensor); EXPECT_EQ("magnetometer_sensor", magnetometerSensor->Name()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, magnetometerSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(10, 11, 12, 0, 0, 0), magnetometerSensor->RawPose()); EXPECT_FALSE(magnetometerSensor->EnableMetrics()); const sdf::Magnetometer *magSensor = magnetometerSensor->MagnetometerSensor(); @@ -503,7 +503,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, multicameraSensor); EXPECT_EQ("multicamera_sensor", multicameraSensor->Name()); EXPECT_EQ(sdf::SensorType::MULTICAMERA, multicameraSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(13, 14, 15, 0, 0, 0), multicameraSensor->RawPose()); EXPECT_FALSE(multicameraSensor->EnableMetrics()); @@ -512,7 +512,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, raySensor); EXPECT_EQ("ray_sensor", raySensor->Name()); EXPECT_EQ(sdf::SensorType::LIDAR, raySensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), raySensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), raySensor->RawPose()); EXPECT_FALSE(raySensor->EnableMetrics()); const sdf::Lidar *ray = raySensor->LidarSensor(); ASSERT_NE(nullptr, ray); @@ -535,7 +535,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, lidarSensor); EXPECT_EQ("lidar_sensor", lidarSensor->Name()); EXPECT_EQ(sdf::SensorType::LIDAR, lidarSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), lidarSensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), lidarSensor->RawPose()); EXPECT_TRUE(lidarSensor->EnableMetrics()); const sdf::Lidar *lidar = lidarSensor->LidarSensor(); ASSERT_NE(nullptr, lidar); @@ -558,7 +558,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, rfidSensor); EXPECT_EQ("rfid_sensor", rfidSensor->Name()); EXPECT_EQ(sdf::SensorType::RFID, rfidSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), rfidSensor->RawPose()); + EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 0), rfidSensor->RawPose()); EXPECT_FALSE(rfidSensor->EnableMetrics()); // Get the rfid tag @@ -566,7 +566,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, rfidTag); EXPECT_EQ("rfid_tag", rfidTag->Name()); EXPECT_EQ(sdf::SensorType::RFIDTAG, rfidTag->Type()); - EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), rfidTag->RawPose()); + EXPECT_EQ(gz::math::Pose3d(7, 8, 9, 0, 0, 0), rfidTag->RawPose()); EXPECT_FALSE(rfidTag->EnableMetrics()); // Get the sonar sensor @@ -574,7 +574,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, sonarSensor); EXPECT_EQ("sonar_sensor", sonarSensor->Name()); EXPECT_EQ(sdf::SensorType::SONAR, sonarSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(10, 11, 12, 0, 0, 0), sonarSensor->RawPose()); EXPECT_FALSE(sonarSensor->EnableMetrics()); @@ -583,7 +583,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, wirelessReceiver); EXPECT_EQ("wireless_receiver", wirelessReceiver->Name()); EXPECT_EQ(sdf::SensorType::WIRELESS_RECEIVER, wirelessReceiver->Type()); - EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(13, 14, 15, 0, 0, 0), wirelessReceiver->RawPose()); EXPECT_FALSE(wirelessReceiver->EnableMetrics()); @@ -593,7 +593,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, wirelessTransmitter); EXPECT_EQ("wireless_transmitter", wirelessTransmitter->Name()); EXPECT_EQ(sdf::SensorType::WIRELESS_TRANSMITTER, wirelessTransmitter->Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), wirelessTransmitter->RawPose()); EXPECT_FALSE(wirelessTransmitter->EnableMetrics()); @@ -603,7 +603,7 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, airPressureSensor); EXPECT_EQ("air_pressure_sensor", airPressureSensor->Name()); EXPECT_EQ(sdf::SensorType::AIR_PRESSURE, airPressureSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(10, 20, 30, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(10, 20, 30, 0, 0, 0), airPressureSensor->RawPose()); EXPECT_FALSE(airPressureSensor->EnableMetrics()); const sdf::AirPressure *airSensor = airPressureSensor->AirPressureSensor(); @@ -623,7 +623,7 @@ TEST(DOMLink, LoadLinkPoseRelativeTo) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); diff --git a/test/integration/link_light.cc b/test/integration/link_light.cc index b4dc6d916..a873f36b1 100644 --- a/test/integration/link_light.cc +++ b/test/integration/link_light.cc @@ -17,7 +17,7 @@ #include #include -#include +#include #include "sdf/sdf.hh" #include "test_config.h" @@ -68,18 +68,18 @@ TEST(Frame, LinkLight) EXPECT_EQ(lightElem->Get("type"), "point"); EXPECT_TRUE(lightElem->HasElement("pose")); - auto pose = lightElem->Get("pose"); - EXPECT_EQ(ignition::math::Pose3d(0.1, 0, 0, 0, 0, 0), pose); + auto pose = lightElem->Get("pose"); + EXPECT_EQ(gz::math::Pose3d(0.1, 0, 0, 0, 0, 0), pose); // diffuse EXPECT_TRUE(lightElem->HasElement("diffuse")); sdf::ElementPtr diffuseElem = lightElem->GetElement("diffuse"); - EXPECT_EQ(diffuseElem->Get(), - ignition::math::Color(0.2f, 0.3f, 0.4f, 1.0f)); + EXPECT_EQ(diffuseElem->Get(), + gz::math::Color(0.2f, 0.3f, 0.4f, 1.0f)); // specular EXPECT_TRUE(lightElem->HasElement("specular")); sdf::ElementPtr specularElem = lightElem->GetElement("specular"); - EXPECT_EQ(specularElem->Get(), - ignition::math::Color(0.3f, 0.4f, 0.5f, 1.0f)); + EXPECT_EQ(specularElem->Get(), + gz::math::Color(0.3f, 0.4f, 0.5f, 1.0f)); } diff --git a/test/integration/locale_fix_cxx.cc b/test/integration/locale_fix_cxx.cc index f2fbd20df..36d74d898 100644 --- a/test/integration/locale_fix_cxx.cc +++ b/test/integration/locale_fix_cxx.cc @@ -40,8 +40,8 @@ TEST(CheckFixForLocal, CheckFixForCxxLocal) "1.5 2.5", true); // Verify that the default value is correctly parsed - ignition::math::Vector2d vectmp; - ASSERT_TRUE(param.Get(vectmp)); + gz::math::Vector2d vectmp; + ASSERT_TRUE(param.Get(vectmp)); ASSERT_DOUBLE_EQ(1.5, vectmp[0]); ASSERT_DOUBLE_EQ(2.5, vectmp[1]); diff --git a/test/integration/material_pbr.cc b/test/integration/material_pbr.cc index a315abac6..8ad0e0b98 100644 --- a/test/integration/material_pbr.cc +++ b/test/integration/material_pbr.cc @@ -56,7 +56,7 @@ TEST(Material, PbrDOM) ASSERT_NE(nullptr, material); // diffuse - EXPECT_EQ(ignition::math::Color(0.2f, 0.5f, 0.1f, 1.0f), + EXPECT_EQ(gz::math::Color(0.2f, 0.5f, 0.1f, 1.0f), material->Diffuse()); // pbr @@ -114,7 +114,7 @@ TEST(Material, PbrDOM) ASSERT_NE(nullptr, material); // diffuse - EXPECT_EQ(ignition::math::Color(0.2f, 0.5f, 0.1f, 1.0f), + EXPECT_EQ(gz::math::Color(0.2f, 0.5f, 0.1f, 1.0f), material->Diffuse()); // pbr @@ -169,7 +169,7 @@ TEST(Material, PbrDOM) ASSERT_NE(nullptr, material); // diffuse - EXPECT_EQ(ignition::math::Color(0.2f, 0.5f, 0.1f, 1.0f), + EXPECT_EQ(gz::math::Color(0.2f, 0.5f, 0.1f, 1.0f), material->Diffuse()); // pbr @@ -300,8 +300,8 @@ TEST(Material, MaterialPBR) // diffuse EXPECT_TRUE(materialElem->HasElement("diffuse")); sdf::ElementPtr diffuseElem = materialElem->GetElement("diffuse"); - EXPECT_EQ(ignition::math::Color(0.2f, 0.5f, 0.1f, 1.0f), - diffuseElem->Get()); + EXPECT_EQ(gz::math::Color(0.2f, 0.5f, 0.1f, 1.0f), + diffuseElem->Get()); // pbr EXPECT_TRUE(materialElem->HasElement("pbr")); @@ -379,8 +379,8 @@ TEST(Material, MaterialPBR) // diffuse EXPECT_TRUE(materialElem->HasElement("diffuse")); sdf::ElementPtr diffuseElem = materialElem->GetElement("diffuse"); - EXPECT_EQ(ignition::math::Color(0.2f, 0.5f, 0.1f, 1.0f), - diffuseElem->Get()); + EXPECT_EQ(gz::math::Color(0.2f, 0.5f, 0.1f, 1.0f), + diffuseElem->Get()); // pbr EXPECT_TRUE(materialElem->HasElement("pbr")); @@ -454,8 +454,8 @@ TEST(Material, MaterialPBR) // diffuse EXPECT_TRUE(materialElem->HasElement("diffuse")); sdf::ElementPtr diffuseElem = materialElem->GetElement("diffuse"); - EXPECT_EQ(ignition::math::Color(0.2f, 0.5f, 0.1f, 1.0f), - diffuseElem->Get()); + EXPECT_EQ(gz::math::Color(0.2f, 0.5f, 0.1f, 1.0f), + diffuseElem->Get()); // pbr EXPECT_TRUE(materialElem->HasElement("pbr")); diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 316088348..2a43ff588 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -18,7 +18,7 @@ #include #include -#include +#include #include "sdf/Element.hh" #include "sdf/Error.hh" #include "sdf/Filesystem.hh" @@ -128,7 +128,7 @@ TEST(DOMRoot, LoadDoublePendulum) EXPECT_FALSE(nullptr == model->LinkByIndex(1)); EXPECT_FALSE(nullptr == model->LinkByIndex(2)); EXPECT_TRUE(nullptr == model->LinkByIndex(3)); - EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), model->RawPose()); EXPECT_EQ("", model->PoseRelativeTo()); EXPECT_TRUE(model->LinkNameExists("base")); @@ -168,7 +168,7 @@ TEST(DOMRoot, NestedModel) EXPECT_NE(nullptr, model->LinkByIndex(0)); EXPECT_NE(nullptr, model->LinkByIndex(1)); EXPECT_EQ(nullptr, model->LinkByIndex(2)); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); EXPECT_EQ("", model->PoseRelativeTo()); EXPECT_TRUE(model->LinkNameExists("parent")); @@ -210,7 +210,7 @@ TEST(DOMLink, NestedModelPoseRelativeTo) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -300,7 +300,7 @@ TEST(DOMRoot, LoadCanonicalLink) EXPECT_NE(nullptr, model->LinkByIndex(0)); EXPECT_NE(nullptr, model->LinkByIndex(1)); EXPECT_EQ(nullptr, model->LinkByIndex(2)); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), model->RawPose()); EXPECT_EQ("", model->PoseRelativeTo()); EXPECT_TRUE(model->LinkNameExists("link1")); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index e25053537..1176bd294 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include "sdf/sdf.hh" @@ -102,8 +102,8 @@ TEST(NestedModel, NestedModel) sdf::ElementPtr axisElem = jointElem->GetElement("axis"); EXPECT_TRUE(axisElem->HasElement("xyz")); - EXPECT_EQ(axisElem->Get("xyz"), - ignition::math::Vector3d(1, 0, 0)); + EXPECT_EQ(axisElem->Get("xyz"), + gz::math::Vector3d(1, 0, 0)); } //////////////////////////////////////// @@ -162,8 +162,8 @@ TEST(NestedModel, State) EXPECT_TRUE(modelStateElem->HasAttribute("name")); EXPECT_EQ(modelStateElem->Get("name"), "model_00"); EXPECT_TRUE(modelStateElem->HasElement("pose")); - EXPECT_EQ(modelStateElem->Get("pose"), - ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0)); + EXPECT_EQ(modelStateElem->Get("pose"), + gz::math::Pose3d(0, 0, 0.5, 0, 0, 0)); EXPECT_TRUE(!modelStateElem->HasElement("joint")); // link sdf @@ -172,17 +172,17 @@ TEST(NestedModel, State) EXPECT_TRUE(linkStateElem->HasAttribute("name")); EXPECT_EQ(linkStateElem->Get("name"), "link_00"); EXPECT_TRUE(linkStateElem->HasElement("pose")); - EXPECT_EQ(linkStateElem->Get("pose"), - ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0)); + EXPECT_EQ(linkStateElem->Get("pose"), + gz::math::Pose3d(0, 0, 0.5, 0, 0, 0)); EXPECT_TRUE(linkStateElem->HasElement("velocity")); - EXPECT_EQ(linkStateElem->Get("velocity"), - ignition::math::Pose3d(0.001, 0, 0, 0, 0, 0)); + EXPECT_EQ(linkStateElem->Get("velocity"), + gz::math::Pose3d(0.001, 0, 0, 0, 0, 0)); EXPECT_TRUE(linkStateElem->HasElement("acceleration")); - EXPECT_EQ(linkStateElem->Get("acceleration"), - ignition::math::Pose3d(0, 0.006121, 0, 0.012288, 0, 0.001751)); + EXPECT_EQ(linkStateElem->Get("acceleration"), + gz::math::Pose3d(0, 0.006121, 0, 0.012288, 0, 0.001751)); EXPECT_TRUE(linkStateElem->HasElement("wrench")); - EXPECT_EQ(linkStateElem->Get("wrench"), - ignition::math::Pose3d(0, 0.006121, 0, 0, 0, 0)); + EXPECT_EQ(linkStateElem->Get("wrench"), + gz::math::Pose3d(0, 0.006121, 0, 0, 0, 0)); // nested model sdf EXPECT_TRUE(modelStateElem->HasElement("model")); @@ -191,8 +191,8 @@ TEST(NestedModel, State) EXPECT_TRUE(nestedModelStateElem->HasAttribute("name")); EXPECT_EQ(nestedModelStateElem->Get("name"), "model_01"); EXPECT_TRUE(nestedModelStateElem->HasElement("pose")); - EXPECT_EQ(nestedModelStateElem->Get("pose"), - ignition::math::Pose3d(1, 0, 0.5, 0, 0, 0)); + EXPECT_EQ(nestedModelStateElem->Get("pose"), + gz::math::Pose3d(1, 0, 0.5, 0, 0, 0)); EXPECT_TRUE(!nestedModelStateElem->HasElement("joint")); // nested model's link sdf @@ -202,17 +202,17 @@ TEST(NestedModel, State) EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name")); EXPECT_EQ(nestedLinkStateElem->Get("name"), "link_01"); EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); - EXPECT_EQ(nestedLinkStateElem->Get("pose"), - ignition::math::Pose3d(1.25, 0, 0.5, 0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("pose"), + gz::math::Pose3d(1.25, 0, 0.5, 0, 0, 0)); EXPECT_TRUE(nestedLinkStateElem->HasElement("velocity")); - EXPECT_EQ(nestedLinkStateElem->Get("velocity"), - ignition::math::Pose3d(0, -0.001, 0, 0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("velocity"), + gz::math::Pose3d(0, -0.001, 0, 0, 0, 0)); EXPECT_TRUE(nestedLinkStateElem->HasElement("acceleration")); - EXPECT_EQ(nestedLinkStateElem->Get("acceleration"), - ignition::math::Pose3d(0, 0.000674, 0, -0.001268, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("acceleration"), + gz::math::Pose3d(0, 0.000674, 0, -0.001268, 0, 0)); EXPECT_TRUE(nestedLinkStateElem->HasElement("wrench")); - EXPECT_EQ(nestedLinkStateElem->Get("wrench"), - ignition::math::Pose3d(0, 0.000674, 0, 0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("wrench"), + gz::math::Pose3d(0, 0.000674, 0, 0, 0, 0)); // double nested model sdf EXPECT_TRUE(nestedModelStateElem->HasElement("model")); @@ -220,8 +220,8 @@ TEST(NestedModel, State) EXPECT_TRUE(nestedModelStateElem->HasAttribute("name")); EXPECT_EQ(nestedModelStateElem->Get("name"), "model_02"); EXPECT_TRUE(nestedModelStateElem->HasElement("pose")); - EXPECT_EQ(nestedModelStateElem->Get("pose"), - ignition::math::Pose3d(1, 1, 0.5, 0, 0, 0)); + EXPECT_EQ(nestedModelStateElem->Get("pose"), + gz::math::Pose3d(1, 1, 0.5, 0, 0, 0)); EXPECT_TRUE(!nestedModelStateElem->HasElement("joint")); // double nested model's link sdf @@ -230,17 +230,17 @@ TEST(NestedModel, State) EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name")); EXPECT_EQ(nestedLinkStateElem->Get("name"), "link_02"); EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); - EXPECT_EQ(nestedLinkStateElem->Get("pose"), - ignition::math::Pose3d(1.25, 1, 0.5, 0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("pose"), + gz::math::Pose3d(1.25, 1, 0.5, 0, 0, 0)); EXPECT_TRUE(nestedLinkStateElem->HasElement("velocity")); - EXPECT_EQ(nestedLinkStateElem->Get("velocity"), - ignition::math::Pose3d(0, 0, 0.001, 0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("velocity"), + gz::math::Pose3d(0, 0, 0.001, 0, 0, 0)); EXPECT_TRUE(nestedLinkStateElem->HasElement("acceleration")); - EXPECT_EQ(nestedLinkStateElem->Get("acceleration"), - ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("acceleration"), + gz::math::Pose3d(0, 0, 0, 0, 0, 0)); EXPECT_TRUE(nestedLinkStateElem->HasElement("wrench")); - EXPECT_EQ(nestedLinkStateElem->Get("wrench"), - ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("wrench"), + gz::math::Pose3d(0, 0, 0, 0, 0, 0)); } //////////////////////////////////////// @@ -307,13 +307,13 @@ TEST(NestedModel, NestedInclude) // // 1. double_pendulum_with_base is included directly into the world // with the following pose - const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); + const gz::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); // 2. it's also included into the model named "include_with_rotation" // with the following pose - const ignition::math::Pose3d model2Pose(-10, 0, 0, 0, 0, IGN_PI/2); + const gz::math::Pose3d model2Pose(-10, 0, 0, 0, 0, IGN_PI/2); // 3. double_pendulum_with_base_14 is included into // the model named "include_with_rotation_1.4" with the following pose - const ignition::math::Pose3d model3Pose(0, 10, 0, 0, 0, IGN_PI/2); + const gz::math::Pose3d model3Pose(0, 10, 0, 0, 0, IGN_PI/2); std::ostringstream stream; std::string version = "1.5"; @@ -362,8 +362,8 @@ TEST(NestedModel, NestedInclude) // but model2Pose and model3Pose are applied to the link poses, so those // //model/pose values should be identity EXPECT_EQ(model1Pose, model1->RawPose()); - EXPECT_EQ(ignition::math::Pose3d::Zero, model2->RawPose()); - EXPECT_EQ(ignition::math::Pose3d::Zero, model3->RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, model2->RawPose()); + EXPECT_EQ(gz::math::Pose3d::Zero, model3->RawPose()); // expect empty //pose/@relative_to EXPECT_TRUE(model1->PoseRelativeTo().empty()); EXPECT_TRUE(model2->PoseRelativeTo().empty()); @@ -446,15 +446,15 @@ TEST(NestedModel, NestedInclude) ASSERT_NE(nullptr, upperAxis3); // expect //axis/xyz to be unchanged for model1 and model2 - EXPECT_EQ(ignition::math::Vector3d::UnitX, lowerAxis1->Xyz()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, upperAxis1->Xyz()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, lowerAxis2->Xyz()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, upperAxis2->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitX, lowerAxis1->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitX, upperAxis1->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitX, lowerAxis2->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitX, upperAxis2->Xyz()); // For model3, expect //axis/xyz to use the nested model frame in // //axis/xyz/@expressed_in since it came from SDFormat 1.4, which implies // //axis/xyz/@expressed_in == "__model__" inside the nested model - EXPECT_EQ(ignition::math::Vector3d::UnitX, lowerAxis3->Xyz()); - EXPECT_EQ(ignition::math::Vector3d::UnitX, upperAxis3->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitX, lowerAxis3->Xyz()); + EXPECT_EQ(gz::math::Vector3d::UnitX, upperAxis3->Xyz()); EXPECT_EQ(name + "_14::__model__", lowerAxis3->XyzExpressedIn()); EXPECT_EQ(name + "_14::__model__", upperAxis3->XyzExpressedIn()); } @@ -467,7 +467,7 @@ TEST(NestedModel, NestedModelWithFrames) const std::string modelPath = std::string(PROJECT_SOURCE_PATH) + "/test/integration/model/" + name; - const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); + const gz::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); std::ostringstream stream; std::string version = "1.7"; @@ -500,8 +500,8 @@ TEST(NestedModel, NestedModelWithFrames) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); - using ignition::math::Pose3d; - using ignition::math::Vector3d; + using gz::math::Pose3d; + using gz::math::Vector3d; // Expected poses for frames, links and joints after nesting. Pose3d frame1ExpPose = model1Pose * Pose3d(0, 0, 0, IGN_PI/2, 0, 0); Pose3d frame2ExpPose = frame1ExpPose * Pose3d(0, 0, 0, 0, IGN_PI/4, 0); @@ -627,7 +627,7 @@ TEST(NestedModel, NestedModelWithFramesDirectComparison) const std::string modelPath = std::string(PROJECT_SOURCE_PATH) + "/test/integration/model/" + name; - const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); + const gz::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); std::ostringstream stream; std::string version = "1.7"; @@ -674,7 +674,7 @@ TEST(NestedModel, TwoLevelNestedModelWithFramesDirectComparison) const std::string name = "test_nested_model_with_frames"; const std::string modelPath = modelRootPath + name; - const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); + const gz::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); std::ostringstream stream; std::string version = "1.7"; @@ -726,8 +726,8 @@ TEST(NestedModel, NestedModelWithSiblingFrames) const std::string MODEL_PATH = std::string(PROJECT_SOURCE_PATH) + "/test/integration/model/" + name; - const ignition::math::Pose3d testFramePose(0, 5, 0, 0, 0, -IGN_PI/2); - const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); + const gz::math::Pose3d testFramePose(0, 5, 0, 0, 0, -IGN_PI/2); + const gz::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); std::ostringstream stream; std::string version = "1.7"; @@ -769,8 +769,8 @@ TEST(NestedModel, NestedModelWithSiblingFrames) EXPECT_EQ("testFrame", nestedModel1Frame->PoseRelativeTo()); - using ignition::math::Pose3d; - using ignition::math::Vector3d; + using gz::math::Pose3d; + using gz::math::Vector3d; // Expected poses Pose3d nestedModel1FrameExpPose = testFramePose * model1Pose; Pose3d frame1ExpPose = @@ -825,7 +825,7 @@ TEST(NestedModel, NestedFrameOnlyModel) const std::string MODEL_PATH = std::string(PROJECT_SOURCE_PATH) + "/test/integration/model/" + name; - const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); + const gz::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); std::ostringstream stream; std::string version = "1.7"; @@ -858,8 +858,8 @@ TEST(NestedModel, NestedFrameOnlyModel) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); - using ignition::math::Pose3d; - using ignition::math::Vector3d; + using gz::math::Pose3d; + using gz::math::Vector3d; // Expected poses for frames after nesting. Pose3d frame1ExpPose = model1Pose * Pose3d(0, 0, 0, IGN_PI, 0, 0); Pose3d frame2ExpPose = frame1ExpPose * Pose3d(0, 0, 0, 0, IGN_PI, 0); diff --git a/test/integration/scene_dom.cc b/test/integration/scene_dom.cc index edcee1acb..a5b4b868c 100644 --- a/test/integration/scene_dom.cc +++ b/test/integration/scene_dom.cc @@ -19,7 +19,7 @@ #include #include -#include +#include #include "sdf/Filesystem.hh" #include "sdf/parser.hh" @@ -52,8 +52,8 @@ TEST(DOMScene, LoadScene) const sdf::Scene *scene = world->Scene(); ASSERT_NE(nullptr, scene); - EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f), scene->Ambient()); - EXPECT_EQ(ignition::math::Color(0.6f, 0.7f, 0.8f), scene->Background()); + EXPECT_EQ(gz::math::Color(0.3f, 0.4f, 0.5f), scene->Ambient()); + EXPECT_EQ(gz::math::Color(0.6f, 0.7f, 0.8f), scene->Background()); EXPECT_TRUE(scene->Grid()); EXPECT_TRUE(scene->Shadows()); EXPECT_TRUE(scene->OriginVisual()); @@ -65,8 +65,8 @@ TEST(DOMScene, LoadScene) EXPECT_DOUBLE_EQ(4.0, sky->Sunrise()); EXPECT_DOUBLE_EQ(21.0, sky->Sunset()); EXPECT_DOUBLE_EQ(1.2, sky->CloudSpeed()); - EXPECT_EQ(ignition::math::Angle(1.5), sky->CloudDirection()); + EXPECT_EQ(gz::math::Angle(1.5), sky->CloudDirection()); EXPECT_DOUBLE_EQ(0.2, sky->CloudMeanSize()); EXPECT_DOUBLE_EQ(0.9, sky->CloudHumidity()); - EXPECT_EQ(ignition::math::Color(0.1f, 0.2f, 0.3f), sky->CloudAmbient()); + EXPECT_EQ(gz::math::Color(0.1f, 0.2f, 0.3f), sky->CloudAmbient()); } diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 1c08c8483..6d406fa5d 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -183,8 +183,8 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_TRUE(element->HasElement("pose")); const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); + const auto& posePair = poseElem->Get( + "", gz::math::Pose3d::Zero); ASSERT_TRUE(posePair.second); const auto& pose = posePair.first; @@ -204,8 +204,8 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_TRUE(element->HasElement("pose")); const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); + const auto& posePair = poseElem->Get( + "", gz::math::Pose3d::Zero); ASSERT_TRUE(posePair.second); const auto& pose = posePair.first; @@ -225,8 +225,8 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_TRUE(element->HasElement("pose")); const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); + const auto& posePair = poseElem->Get( + "", gz::math::Pose3d::Zero); ASSERT_TRUE(posePair.second); const auto& pose = posePair.first; @@ -246,8 +246,8 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_TRUE(element->HasElement("pose")); const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); + const auto& posePair = poseElem->Get( + "", gz::math::Pose3d::Zero); ASSERT_TRUE(posePair.second); const auto& pose = posePair.first; @@ -267,8 +267,8 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_TRUE(element->HasElement("pose")); const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); + const auto& posePair = poseElem->Get( + "", gz::math::Pose3d::Zero); ASSERT_TRUE(posePair.second); const auto& pose = posePair.first; @@ -288,8 +288,8 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_TRUE(element->HasElement("pose")); const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); + const auto& posePair = poseElem->Get( + "", gz::math::Pose3d::Zero); ASSERT_TRUE(posePair.second); const auto& pose = posePair.first; @@ -411,7 +411,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()); @@ -447,7 +447,7 @@ TEST(SDFParser, FixedJointStatic) 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()); diff --git a/test/integration/urdf_joint_parameters.cc b/test/integration/urdf_joint_parameters.cc index e6ae97d87..75b2a343d 100644 --- a/test/integration/urdf_joint_parameters.cc +++ b/test/integration/urdf_joint_parameters.cc @@ -81,7 +81,7 @@ TEST(SDFParser, JointAxisParameters) { std::string jointName = joint->Get("name"); - ignition::math::Vector3d xyz(1, 0, 0); + gz::math::Vector3d xyz(1, 0, 0); if (jointName == "joint12" || jointName == "joint23") { xyz.Set(0, 1, 0); @@ -89,7 +89,7 @@ TEST(SDFParser, JointAxisParameters) ASSERT_TRUE(joint->HasElement("axis")); sdf::ElementPtr axis = joint->GetElement("axis"); - EXPECT_EQ(xyz, axis->Get("xyz")); + EXPECT_EQ(xyz, axis->Get("xyz")); EXPECT_FALSE(axis->Get("use_parent_model_frame")); } } diff --git a/test/integration/visual_dom.cc b/test/integration/visual_dom.cc index e882b803a..ed945d9ff 100644 --- a/test/integration/visual_dom.cc +++ b/test/integration/visual_dom.cc @@ -79,7 +79,7 @@ TEST(DOMVisual, DoublePendulum) const sdf::Visual *plateVis = baseLink->VisualByIndex(0); ASSERT_TRUE(plateVis != nullptr); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.01, 0, 0, 0), plateVis->RawPose()); + EXPECT_EQ(gz::math::Pose3d(0, 0, 0.01, 0, 0, 0), plateVis->RawPose()); EXPECT_EQ("", plateVis->PoseRelativeTo()); EXPECT_FALSE(plateVis->CastShadows()); @@ -87,7 +87,7 @@ TEST(DOMVisual, DoublePendulum) ASSERT_TRUE(poleVis != nullptr); EXPECT_TRUE(poleVis->CastShadows()); - EXPECT_EQ(ignition::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), + EXPECT_EQ(gz::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), poleVis->RawPose()); EXPECT_EQ("", poleVis->PoseRelativeTo()); } @@ -114,10 +114,10 @@ TEST(DOMVisual, Material) const sdf::Material *mat = vis1->Material(); ASSERT_NE(nullptr, mat); - EXPECT_EQ(ignition::math::Color(0.4f, 0.2f, 0.3f, 1.0f), mat->Ambient()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.5f, 0.1f, 1.0f), mat->Diffuse()); - EXPECT_EQ(ignition::math::Color(0.7f, 0.3f, 0.5f, 0.9f), mat->Specular()); - EXPECT_EQ(ignition::math::Color(1.0f, 0.0f, 0.2f, 1.0f), mat->Emissive()); + EXPECT_EQ(gz::math::Color(0.4f, 0.2f, 0.3f, 1.0f), mat->Ambient()); + EXPECT_EQ(gz::math::Color(0.2f, 0.5f, 0.1f, 1.0f), mat->Diffuse()); + EXPECT_EQ(gz::math::Color(0.7f, 0.3f, 0.5f, 0.9f), mat->Specular()); + EXPECT_EQ(gz::math::Color(1.0f, 0.0f, 0.2f, 1.0f), mat->Emissive()); EXPECT_FALSE(mat->Lighting()); EXPECT_TRUE(mat->DoubleSided()); EXPECT_EQ(sdf::ShaderType::VERTEX, mat->Shader()); @@ -232,7 +232,7 @@ TEST(DOMVisual, LoadModelFramesRelativeToJoint) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first model const sdf::Model *model = root.ModelByIndex(0); diff --git a/test/integration/world_dom.cc b/test/integration/world_dom.cc index cce13c34e..4bc792735 100644 --- a/test/integration/world_dom.cc +++ b/test/integration/world_dom.cc @@ -94,9 +94,9 @@ TEST(DOMWorld, Load) ASSERT_NE(nullptr, world->Element()); EXPECT_EQ(world->Name(), "default"); EXPECT_EQ(world->AudioDevice(), "/dev/audio"); - EXPECT_EQ(world->WindLinearVelocity(), ignition::math::Vector3d(4, 5, 6)); - EXPECT_EQ(world->Gravity(), ignition::math::Vector3d(1, 2, 3)); - EXPECT_EQ(world->MagneticField(), ignition::math::Vector3d(-1, 0.5, 10)); + EXPECT_EQ(world->WindLinearVelocity(), gz::math::Vector3d(4, 5, 6)); + EXPECT_EQ(world->Gravity(), gz::math::Vector3d(1, 2, 3)); + EXPECT_EQ(world->MagneticField(), gz::math::Vector3d(-1, 0.5, 10)); EXPECT_EQ(testFile, world->Element()->FilePath()); auto graphErrors = world->ValidateGraphs(); EXPECT_EQ(0u, graphErrors.size()); @@ -120,8 +120,8 @@ TEST(DOMWorld, Load) EXPECT_TRUE(scene->Grid()); EXPECT_TRUE(scene->Shadows()); EXPECT_TRUE(scene->OriginVisual()); - EXPECT_EQ(ignition::math::Color(0.3f, 0.4f, 0.5f), scene->Ambient()); - EXPECT_EQ(ignition::math::Color(0.6f, 0.7f, 0.8f), scene->Background()); + EXPECT_EQ(gz::math::Color(0.3f, 0.4f, 0.5f), scene->Ambient()); + EXPECT_EQ(gz::math::Color(0.6f, 0.7f, 0.8f), scene->Background()); EXPECT_EQ(testFile, scene->Element()->FilePath()); ASSERT_EQ(1u, world->PhysicsCount()); @@ -157,7 +157,7 @@ TEST(DOMWorld, LoadModelFrameSameName) std::cout << e << std::endl; EXPECT_TRUE(errors.empty()); - using Pose = ignition::math::Pose3d; + using Pose = gz::math::Pose3d; // Get the first world const sdf::World *world = root.WorldByIndex(0); From b1a33347b2e38513f37d5e534301e09227f1bcaa Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 30 Nov 2022 15:15:19 +0800 Subject: [PATCH 24/31] Converter: add sdf::Errors output to API methods (#1123) Signed-off-by: Marco A. Gutierrez Signed-off-by: Steve Peters --- include/sdf/Error.hh | 6 + include/sdf/parser.hh | 24 + src/CMakeLists.txt | 1 + src/Converter.cc | 341 +++++++++---- src/Converter.hh | 65 ++- src/Converter_TEST.cc | 1132 +++++++++++++++++++++++++++++++++++++++-- src/parser.cc | 90 +++- 7 files changed, 1466 insertions(+), 193 deletions(-) diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index 68896241a..096f7ee73 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -172,6 +172,12 @@ namespace sdf /// \brief The joint axis expressed-in value does not match the name of an /// existing frame in the current scope. JOINT_AXIS_EXPRESSED_IN_INVALID, + + /// \brief SDF conversion generic error. + CONVERSION_ERROR, + + /// \brief Generic error during parsing. + PARSING_ERROR, }; class SDFORMAT_VISIBLE Error diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index 31864a9fd..2db2a7e44 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -38,6 +38,8 @@ namespace sdf // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // + // TODO(marcoag): Deprecate function overloads that do not use sdf::Errors, + // see: https://github.com/gazebosim/sdformat/issues/1186 class Root; /// \brief Initialize the SDF interface from the embedded root spec file @@ -334,6 +336,17 @@ namespace sdf bool convertFile(const std::string &_filename, const std::string &_version, const ParserConfig &_config, SDFPtr _sdf); + /// \brief Convert an SDF file to a specific SDF version. + /// \param[out] _sdf Pointer to the converted SDF document. + /// \param[in] _filename Name of the SDF file to convert. + /// \param[in] _version Version to convert _filename to. + /// \param[in] _config Custom parser configuration + /// \return Vector of errors, successful when vector is empty. + SDFORMAT_VISIBLE + sdf::Errors convertFile( + SDFPtr _sdf, const std::string &_filename, const std::string &_version, + const ParserConfig &_config = ParserConfig::GlobalConfig()); + /// \brief Convert an SDF string to a specific SDF version. /// \param[in] _sdfString The SDF string to convert. /// \param[in] _version Version to convert _filename to. @@ -353,6 +366,17 @@ namespace sdf bool convertString(const std::string &_sdfString, const std::string &_version, const ParserConfig &_config, SDFPtr _sdf); + /// \brief Convert an SDF string to a specific SDF version. + /// \param[out] _sdf Pointer to the converted SDF document. + /// \param[in] _sdfString The SDF string to convert. + /// \param[in] _version Version to convert _filename to. + /// \param[in] _config Custom parser configuration + /// \return Vector of errors, successful when vector is empty. + SDFORMAT_VISIBLE + sdf::Errors convertString( + SDFPtr _sdf, const std::string &_sdfString, const std::string &_version, + const ParserConfig &_config = ParserConfig::GlobalConfig()); + /// \brief Check that for each model, the canonical_link attribute value /// matches the name of a link in the model if the attribute is set and /// not empty. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 66affa10a..feab8e716 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -57,6 +57,7 @@ if (BUILD_TESTING) target_sources(UNIT_Converter_TEST PRIVATE Converter.cc EmbeddedSdf.cc + Utils.cc XmlUtils.cc) endif() diff --git a/src/Converter.cc b/src/Converter.cc index 451f4b696..92dd54ed1 100644 --- a/src/Converter.cc +++ b/src/Converter.cc @@ -30,6 +30,7 @@ #include "Converter.hh" #include "EmbeddedSdf.hh" +#include "Utils.hh" #include "XmlUtils.hh" using namespace sdf; @@ -53,44 +54,62 @@ bool IsNotFlattenedElement(const std::string &_elemName) // used to update //pose/@relative_to in FindNewModelElements() void UpdatePose(tinyxml2::XMLElement *_elem, const size_t &_childNameIdx, - const std::string &_modelName) + const std::string &_modelName, + sdf::Errors &_errors) { tinyxml2::XMLElement *pose = _elem->FirstChildElement("pose"); if (pose && pose->Attribute("relative_to")) { std::string poseRelTo = pose->Attribute("relative_to"); - SDF_ASSERT(poseRelTo.compare(0, _modelName.size(), _modelName) == 0, - "Error: Pose attribute 'relative_to' does not start with " + _modelName); + if (poseRelTo.compare(0, _modelName.size(), _modelName) != 0) + { + std::stringstream ss; + ss << "Error: Pose attribute 'relative_to' does not start with " + << _modelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return; + } poseRelTo = poseRelTo.substr(_childNameIdx); pose->SetAttribute("relative_to", poseRelTo.c_str()); } if (_elem->FirstChildElement("camera")) - UpdatePose(_elem->FirstChildElement("camera"), _childNameIdx, _modelName); + { + UpdatePose(_elem->FirstChildElement("camera"), _childNameIdx, + _modelName, _errors); + } } } ///////////////////////////////////////////////// -bool Converter::Convert(tinyxml2::XMLDocument *_doc, +bool Converter::Convert(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, const std::string &_toVersion, + const ParserConfig &_config, bool _quiet) { - SDF_ASSERT(_doc != nullptr, "SDF XML doc is NULL"); + if (_doc == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF XML doc is NULL"}); + return false; + } tinyxml2::XMLElement *elem = _doc->FirstChildElement("sdf"); // Check that the element exists if (!elem) { - sdferr << " element does not exist.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + " element does not exist."}); return false; } if (!elem->Attribute("version")) { - sdferr << " Unable to determine original SDF version\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Unable to determine original SDF version"}); return false; } @@ -143,18 +162,24 @@ bool Converter::Convert(tinyxml2::XMLDocument *_doc, xmlDoc.Parse(convertXml); if (xmlDoc.Error()) { - sdferr << "Error parsing XML from string: " - << xmlDoc.ErrorStr() << '\n'; + std::stringstream ss; + ss << "Error parsing XML from string: " + << xmlDoc.ErrorStr(); + _errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); return false; } - ConvertImpl(elem, xmlDoc.FirstChildElement("convert")); + ConvertImpl(elem, xmlDoc.FirstChildElement("convert"), _config, _errors); } // Check that we actually converted to the desired final version. if (curVersion != _toVersion) { - sdferr << "Unable to convert from SDF version " << origVersion - << " to " << _toVersion << "\n"; + std::stringstream ss; + ss << "Unable to convert from SDF version " + << origVersion + << " to " + << _toVersion; + _errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); return false; } @@ -162,18 +187,31 @@ bool Converter::Convert(tinyxml2::XMLDocument *_doc, } ///////////////////////////////////////////////// -void Converter::Convert(tinyxml2::XMLDocument *_doc, - tinyxml2::XMLDocument *_convertDoc) +void Converter::Convert(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + tinyxml2::XMLDocument *_convertDoc, + const ParserConfig &_config) { - SDF_ASSERT(_doc != NULL, "SDF XML doc is NULL"); - SDF_ASSERT(_convertDoc != NULL, "Convert XML doc is NULL"); + if (_doc == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF XML doc is NULL"}); + return; + } + if (_convertDoc == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Convert XML doc is NULL"}); + return; + } - ConvertImpl(_doc->FirstChildElement(), _convertDoc->FirstChildElement()); + ConvertImpl(_doc->FirstChildElement(), _convertDoc->FirstChildElement(), + _config, _errors); } ///////////////////////////////////////////////// void Converter::ConvertDescendantsImpl(tinyxml2::XMLElement *_e, - tinyxml2::XMLElement *_c) + tinyxml2::XMLElement *_c, + const ParserConfig &_config, + sdf::Errors &_errors) { if (!_c->Attribute("descendant_name")) { @@ -195,21 +233,31 @@ void Converter::ConvertDescendantsImpl(tinyxml2::XMLElement *_e, { if (strcmp(e->Name(), _c->Attribute("descendant_name")) == 0) { - ConvertImpl(e, _c); + ConvertImpl(e, _c, _config, _errors); } - ConvertDescendantsImpl(e, _c); + ConvertDescendantsImpl(e, _c, _config, _errors); e = e->NextSiblingElement(); } } ///////////////////////////////////////////////// void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_convert) + tinyxml2::XMLElement *_convert, + const ParserConfig &_config, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_convert != NULL, "Convert element is NULL"); + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Convert element is NULL"}); + return; + } - CheckDeprecation(_elem, _convert); + CheckDeprecation(_elem, _convert, _config, _errors); for (auto *convertElem = _convert->FirstChildElement("convert"); convertElem; convertElem = convertElem->NextSiblingElement("convert")) @@ -220,13 +268,13 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, convertElem->Attribute("name")); while (elem) { - ConvertImpl(elem, convertElem); + ConvertImpl(elem, convertElem, _config, _errors); elem = elem->NextSiblingElement(convertElem->Attribute("name")); } } if (convertElem->Attribute("descendant_name")) { - ConvertDescendantsImpl(_elem, convertElem); + ConvertDescendantsImpl(_elem, convertElem, _config, _errors); } } @@ -237,47 +285,55 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, if (name == "rename") { - Rename(_elem, childElem); + Rename(_elem, childElem, _errors); } else if (name == "copy") { - Move(_elem, childElem, true); + Move(_elem, childElem, true, _errors); } else if (name == "map") { - Map(_elem, childElem); + Map(_elem, childElem, _errors); } else if (name == "move") { - Move(_elem, childElem, false); + Move(_elem, childElem, false, _errors); } else if (name == "add") { - Add(_elem, childElem); + Add(_elem, childElem, _errors); } else if (name == "remove") { - Remove(_elem, childElem); + Remove(_errors, _elem, childElem); } else if (name == "remove_empty") { - Remove(_elem, childElem, true); + Remove(_errors, _elem, childElem, true); } else if (name == "unflatten") { - Unflatten(_elem); + Unflatten(_elem, _errors); } else if (name != "convert") { - sdferr << "Unknown convert element[" << name << "]\n"; + std::stringstream ss; + ss << "Unknown convert element[" + << name + << "]"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); } } } ///////////////////////////////////////////////// -void Converter::Unflatten(tinyxml2::XMLElement *_elem) +void Converter::Unflatten(tinyxml2::XMLElement *_elem, sdf::Errors &_errors) { - SDF_ASSERT(_elem != nullptr, "SDF element is nullptr"); + if (_elem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is nullptr"}); + return; + } tinyxml2::XMLDocument *doc = _elem->GetDocument(); @@ -304,7 +360,7 @@ void Converter::Unflatten(tinyxml2::XMLElement *_elem) // recursive unflatten if (elemName == "model") { - Unflatten(elem); + Unflatten(elem, _errors); break; } @@ -315,9 +371,9 @@ void Converter::Unflatten(tinyxml2::XMLElement *_elem) tinyxml2::XMLElement *newModel = doc->NewElement("model"); newModel->SetAttribute("name", newModelName.c_str()); - if (FindNewModelElements(_elem, newModel, found + 2)) + if (FindNewModelElements(_elem, newModel, found + 2, _errors)) { - Unflatten(newModel); + Unflatten(newModel, _errors); _elem->InsertEndChild(newModel); // since newModel is inserted at the end, point back to the top element @@ -332,7 +388,8 @@ void Converter::Unflatten(tinyxml2::XMLElement *_elem) ///////////////////////////////////////////////// bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_newModel, - const size_t &_childNameIdx) + const size_t &_childNameIdx, + sdf::Errors &_errors) { bool unflattenedNewModel = false; std::string newModelName = _newModel->Attribute("name"); @@ -393,9 +450,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, { attachedTo = elem->Attribute("attached_to"); - SDF_ASSERT(attachedTo.compare(0, newModelNameSize, newModelName) == 0, - "Error: Frame attribute 'attached_to' does not start with " + - newModelName); + if (attachedTo.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: Frame attribute 'attached_to' does not start with " + << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } // strip new model prefix from attached_to attachedTo = attachedTo.substr(_childNameIdx); @@ -420,7 +482,7 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, e; e = e->NextSiblingElement()) { - UpdatePose(e, _childNameIdx, newModelName); + UpdatePose(e, _childNameIdx, newModelName, _errors); } } // link @@ -434,8 +496,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, { eText = e->GetText(); - SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0, - "Error: Joint's value does not start with " + newModelName); + if (eText.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: Joint's value does not start with " + << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } e->SetText(eText.substr(_childNameIdx).c_str()); } @@ -446,8 +514,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, { eText = e->GetText(); - SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0, - "Error: Joint's value does not start with " + newModelName); + if (eText.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: Joint's value does not start with " + << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } e->SetText(eText.substr(_childNameIdx).c_str()); } @@ -465,10 +539,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, std::string expressIn = axisElem->FirstChildElement("xyz")->Attribute("expressed_in"); - SDF_ASSERT( - expressIn.compare(0, newModelNameSize, newModelName) == 0, - "Error: 's attribute 'expressed_in' does not start with " + - newModelName); + if (expressIn.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: 's attribute 'expressed_in' does not start " + << "with " << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } expressIn = expressIn.substr(_childNameIdx); @@ -487,7 +565,7 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, e; e = e->NextSiblingElement("sensor")) { - UpdatePose(e, _childNameIdx, newModelName); + UpdatePose(e, _childNameIdx, newModelName, _errors); } } // joint @@ -530,9 +608,14 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, { eText = e->GetText(); - SDF_ASSERT(eText.compare(0, newModelNameSize, newModelName) == 0, - "Error: Gripper's value does not start with " - + newModelName); + if (eText.compare(0, newModelNameSize, newModelName) != 0) + { + std::stringstream ss; + ss << "Error: Gripper's value does not start with " + << newModelName; + _errors.push_back({ErrorCode::FATAL_ERROR, ss.str()}); + return false; + } e->SetText(eText.substr(_childNameIdx).c_str()); } @@ -549,10 +632,19 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem, ///////////////////////////////////////////////// void Converter::Rename(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_renameElem) + tinyxml2::XMLElement *_renameElem, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_renameElem != NULL, "Rename element is NULL"); + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_renameElem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Rename element is NULL"}); + return; + } auto *fromConvertElem = _renameElem->FirstChildElement("from"); auto *toConvertElem = _renameElem->FirstChildElement("to"); @@ -571,7 +663,8 @@ void Converter::Rename(tinyxml2::XMLElement *_elem, if (!toElemName) { - sdferr << "No 'to' element name specified\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "No 'to' element name specified"}); return; } @@ -601,10 +694,20 @@ void Converter::Rename(tinyxml2::XMLElement *_elem, } ///////////////////////////////////////////////// -void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) +void Converter::Add(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_addElem, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_addElem != NULL, "Add element is NULL"); + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_addElem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Add element is NULL"}); + return; + } const char *attributeName = _addElem->Attribute("attribute"); const char *elementName = _addElem->Attribute("element"); @@ -612,8 +715,8 @@ void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) if (!((attributeName == nullptr) ^ (elementName == nullptr))) { - sdferr << "Exactly one 'element' or 'attribute'" - << " must be specified in \n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Exactly one 'element' or 'attribute' must be specified in "}); return; } @@ -625,7 +728,8 @@ void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) } else { - sdferr << "No 'value' specified in \n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "No 'value' specified in "}); return; } } @@ -643,20 +747,29 @@ void Converter::Add(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_addElem) } ///////////////////////////////////////////////// -void Converter::Remove(tinyxml2::XMLElement *_elem, +void Converter::Remove(sdf::Errors &_errors, + tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_removeElem, bool _removeOnlyEmpty) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_removeElem != NULL, "remove element is NULL"); + if (_elem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_removeElem == NULL) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "remove element is NULL"}); + return; + } const char *attributeName = _removeElem->Attribute("attribute"); const char *elementName = _removeElem->Attribute("element"); if (!((attributeName == nullptr) ^ (elementName == nullptr))) { - sdferr << "Exactly one 'element' or 'attribute'" - << " must be specified in \n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Exactly one 'element' or 'attribute' must be specified in "}); return; } @@ -686,22 +799,34 @@ void Converter::Remove(tinyxml2::XMLElement *_elem, } ///////////////////////////////////////////////// -void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) +void Converter::Map(tinyxml2::XMLElement *_elem, + tinyxml2::XMLElement *_mapElem, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != nullptr, "SDF element is nullptr"); - SDF_ASSERT(_mapElem != nullptr, "Map element is nullptr"); + if (_elem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is nullptr"}); + return; + } + if (_mapElem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Map element is nullptr"}); + return; + } tinyxml2::XMLElement *fromConvertElem = _mapElem->FirstChildElement("from"); tinyxml2::XMLElement *toConvertElem = _mapElem->FirstChildElement("to"); if (!fromConvertElem) { - sdferr << " element requires a child element.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + " element requires a child element."}); return; } if (!toConvertElem) { - sdferr << " element requires a child element.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + " element requires a child element."}); return; } @@ -710,12 +835,14 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) if (!fromNameStr || strlen(fromNameStr) == 0) { - sdferr << "Map: element requires a non-empty name attribute.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: element requires a non-empty name attribute."}); return; } if (!toNameStr || strlen(toNameStr) == 0) { - sdferr << "Map: element requires a non-empty name attribute.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: element requires a non-empty name attribute."}); return; } @@ -725,22 +852,26 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) auto *toValueElem = toConvertElem->FirstChildElement("value"); if (!fromValueElem) { - sdferr << "Map: element requires at least one element.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: element requires at least one element"}); return; } if (!toValueElem) { - sdferr << "Map: element requires at least one element.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: element requires at least one element."}); return; } if (!fromValueElem->GetText()) { - sdferr << "Map: from value must not be empty.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: from value must not be empty."}); return; } if (!toValueElem->GetText()) { - sdferr << "Map: to value must not be empty.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: to value must not be empty."}); return; } valueMap[fromValueElem->GetText()] = toValueElem->GetText(); @@ -753,12 +884,14 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) } if (!fromValueElem->GetText()) { - sdferr << "Map: from value must not be empty.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: from value must not be empty."}); return; } if (!toValueElem->GetText()) { - sdferr << "Map: to value must not be empty.\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: to value must not be empty."}); return; } valueMap[fromValueElem->GetText()] = toValueElem->GetText(); @@ -791,7 +924,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) if (fromLeaf[0] == '\0' || (fromLeaf[0] == '@' && fromLeaf[1] == '\0')) { - sdferr << "Map: has invalid name attribute\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: has invalid name attribute"}); return; } const char *fromValue = nullptr; @@ -834,7 +968,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) if (toLeaf[0] == '\0' || (toLeaf[0] == '@' && toLeaf[1] == '\0')) { - sdferr << "Map: has invalid name attribute\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: has invalid name attribute"}); return; } bool toAttribute = toLeaf[0] == '@'; @@ -850,7 +985,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) { if (toTokens[newDirIndex].empty()) { - sdferr << "Map: has invalid name attribute\n"; + _errors.push_back({ErrorCode::CONVERSION_ERROR, + "Map: has invalid name attribute"}); return; } @@ -875,10 +1011,19 @@ void Converter::Map(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_mapElem) ///////////////////////////////////////////////// void Converter::Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy) + const bool _copy, + sdf::Errors &_errors) { - SDF_ASSERT(_elem != NULL, "SDF element is NULL"); - SDF_ASSERT(_moveElem != NULL, "Move element is NULL"); + if (_elem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "SDF element is NULL"}); + return; + } + if (_moveElem == nullptr) + { + _errors.push_back({ErrorCode::FATAL_ERROR, "Move element is NULL"}); + return; + } tinyxml2::XMLElement *fromConvertElem = _moveElem->FirstChildElement("from"); tinyxml2::XMLElement *toConvertElem = _moveElem->FirstChildElement("to"); @@ -1061,7 +1206,9 @@ const char *Converter::GetValue(const char *_valueElem, const char *_valueAttr, ///////////////////////////////////////////////// void Converter::CheckDeprecation(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_convert) + tinyxml2::XMLElement *_convert, + const ParserConfig &_config, + sdf::Errors &_errors) { // Process deprecated elements for (auto *deprecatedElem = _convert->FirstChildElement("deprecated"); @@ -1097,7 +1244,11 @@ void Converter::CheckDeprecation(tinyxml2::XMLElement *_elem, } } - sdfwarn << "Deprecated SDF Values in original file:\n" - << stream.str() << "\n\n"; + std::stringstream ss; + ss << "Deprecated SDF Values in original file: \n" + << stream.str() << "\n"; + Error err(ErrorCode::WARNING, ss.str()); + sdf::enforceConfigurablePolicyCondition(_config.WarningsPolicy(), + err, _errors); } } diff --git a/src/Converter.hh b/src/Converter.hh index 5c3badb17..cfba5413f 100644 --- a/src/Converter.hh +++ b/src/Converter.hh @@ -23,6 +23,7 @@ #include #include +#include #include "sdf/system_util.hh" namespace sdf @@ -35,51 +36,71 @@ namespace sdf class Converter { /// \brief Convert SDF to the specified version. + /// \param[out] _errors Vector of errors. /// \param[in] _doc SDF xml doc - /// \param[in] _toVersion Version number in string format - /// \param[in] _quiet False to be more verbose - public: static bool Convert(tinyxml2::XMLDocument *_doc, + /// \param[in] _toVersion Version number in string format. + /// \param[in] _config Parser configuration. + /// \param[in] _quiet False to be more verbose. + public: static bool Convert(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, const std::string &_toVersion, + const ParserConfig &_config, bool _quiet = false); /// \cond /// This is an internal function. /// \brief Generic convert function that converts the SDF based on the /// given Convert file. + /// \param[out] _errors Vector of errors. /// \param[in] _doc SDF xml doc /// \param[in] _convertDoc Convert xml doc - public: static void Convert(tinyxml2::XMLDocument *_doc, - tinyxml2::XMLDocument *_convertDoc); + /// \param[in] _config Parser configuration. + public: static void Convert(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + tinyxml2::XMLDocument *_convertDoc, + const ParserConfig &_config); /// \endcond /// \brief Implementation of Convert functionality. /// \param[in] _elem SDF xml element tree to convert. /// \param[in] _convert Convert xml element tree. + /// \param[in] _config Parser configuration. + /// \param[out] _errors Vector of errors. private: static void ConvertImpl(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_convert); + tinyxml2::XMLElement *_convert, + const ParserConfig &_config, + sdf::Errors &_errors); /// \brief Recursive helper function for ConvertImpl that converts /// elements named by the descendant_name attribute. /// \param[in] _e SDF xml element tree to convert. /// \param[in] _c Convert xml element tree. + /// \param[in] _config Parser configuration. + /// \param[out] _errors Vector of errors. private: static void ConvertDescendantsImpl(tinyxml2::XMLElement *_e, - tinyxml2::XMLElement *_c); + tinyxml2::XMLElement *_c, + const ParserConfig &_config, + sdf::Errors &_errors); /// \brief Rename an element or attribute. /// \param[in] _elem The element to be renamed, or the element which /// has the attribute to be renamed. - /// \param[in] _renameElem A 'convert' element that describes the rename + /// \param[in] _renameElem A 'convert' element that describes the rename. + /// \param[out] _errors Vector of errors. /// operation. private: static void Rename(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_renameElem); + tinyxml2::XMLElement *_renameElem, + sdf::Errors &_errors); /// \brief Map values from one element or attribute to another. /// \param[in] _elem Ancestor element of the element or attribute to /// be mapped. /// \param[in] _mapElem A 'convert' element that describes the map + /// \param[out] _errors Vector of errors. /// operation. private: static void Map(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_mapElem); + tinyxml2::XMLElement *_mapElem, + sdf::Errors &_errors); /// \brief Move an element or attribute within a common ancestor element. /// \param[in] _elem Ancestor element of the element or attribute to @@ -87,47 +108,59 @@ namespace sdf /// \param[in] _moveElem A 'convert' element that describes the move /// operation. /// \param[in] _copy True to copy the element + /// \param[out] _errors Vector of errors. private: static void Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy); + const bool _copy, + sdf::Errors &_errors); /// \brief Add an element or attribute to an element. /// \param[in] _elem The element to receive the value. /// \param[in] _addElem A 'convert' element that describes the add + /// \param[out] _errors Vector of Errors. /// operation. private: static void Add(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_addElem); + tinyxml2::XMLElement *_addElem, + sdf::Errors &_errors); /// \brief Remove an attribute or elements. + /// \param[out] _errors Vector of Errors. /// \param[in] _elem The element from which data may be removed. /// \param[in] _removeElem The metadata about what to remove. /// \param[in] _removeOnlyEmpty If true, only remove an attribute /// containing an empty string or elements that contain neither value nor /// child elements nor attributes. - private: static void Remove(tinyxml2::XMLElement *_elem, + private: static void Remove(sdf::Errors &_errors, + tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_removeElem, bool _removeOnlyEmpty = false); /// \brief Unflatten an element (conversion from SDFormat <= 1.7 to 1.8) /// \param[in] _elem The element to unflatten - private: static void Unflatten(tinyxml2::XMLElement *_elem); + /// \param[out] _errors Vector of errors + private: static void Unflatten(tinyxml2::XMLElement *_elem, + sdf::Errors &_errors); /// \brief Finds all elements related to the unflattened model /// \param[in] _elem The element to unflatten /// \param[in] _newModel The new unflattened model element /// \param[in] _childNameIdx The beginning index of child element names + /// \param[out] _errors Vector of errors /// (e.g., in newModelName::childName then _childNameIdx = 14) /// \return True if unflattened new model elements private: static bool FindNewModelElements(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_newModel, - const size_t &_childNameIdx); + const size_t &_childNameIdx, + sdf::Errors &_errors); private: static const char *GetValue(const char *_valueElem, const char *_valueAttr, tinyxml2::XMLElement *_elem); private: static void CheckDeprecation(tinyxml2::XMLElement *_elem, - tinyxml2::XMLElement *_convert); + tinyxml2::XMLElement *_convert, + const ParserConfig &_config, + sdf::Errors &_errors); }; } } diff --git a/src/Converter_TEST.cc b/src/Converter_TEST.cc index 73cb54b05..679e31d2e 100644 --- a/src/Converter_TEST.cc +++ b/src/Converter_TEST.cc @@ -20,8 +20,10 @@ #include #include "sdf/Exception.hh" #include "sdf/Filesystem.hh" +#include "sdf/ParserConfig.hh" #include "Converter.hh" +#include "test_utils.hh" #include "XmlUtils.hh" #include "test_config.hh" @@ -98,7 +100,27 @@ TEST(Converter, MoveElemElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -136,7 +158,27 @@ TEST(Converter, MoveElemAttr) << ""; tinyxml2::XMLDocument convertXmlDoc2; convertXmlDoc2.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc2, &convertXmlDoc2); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc2, &convertXmlDoc2, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc2.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -174,7 +216,27 @@ TEST(Converter, MoveAttrAttr) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -215,7 +277,27 @@ TEST(Converter, MoveAttrElem) << ""; tinyxml2::XMLDocument convertXmlDoc4; convertXmlDoc4.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc4, &convertXmlDoc4); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc4, &convertXmlDoc4, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc4.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -254,7 +336,27 @@ TEST(Converter, MoveElemElemMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc5; convertXmlDoc5.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc5, &convertXmlDoc5); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc5, &convertXmlDoc5, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc5.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -289,7 +391,27 @@ TEST(Converter, MoveAttrAttrMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc6; convertXmlDoc6.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc6, &convertXmlDoc6); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc6, &convertXmlDoc6, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc6.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -327,7 +449,27 @@ TEST(Converter, MoveElemAttrMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc7; convertXmlDoc7.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc7, &convertXmlDoc7); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc7, &convertXmlDoc7, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc7.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -362,7 +504,27 @@ TEST(Converter, MoveAttrElemMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc8; convertXmlDoc8.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc8, &convertXmlDoc8); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc8, &convertXmlDoc8, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc8.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -420,7 +582,27 @@ TEST(Converter, Add) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -463,7 +645,30 @@ TEST(Converter, AddNoElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Exactly one 'element' or 'attribute' must be specified in ")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Verify the xml tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); @@ -505,7 +710,30 @@ TEST(Converter, AddNoValue) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("No 'value' specified in ")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); @@ -557,7 +785,27 @@ TEST(Converter, RemoveElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -606,7 +854,27 @@ TEST(Converter, RemoveDescendantElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -658,7 +926,27 @@ TEST(Converter, RemoveEmptyElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -722,7 +1010,27 @@ TEST(Converter, RemoveEmptyDescendantElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -787,7 +1095,28 @@ TEST(Converter, RemoveDescendantNestedElement) tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertString.c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); std::string expectedString = R"( @@ -846,7 +1175,27 @@ TEST(Converter, DescendantIgnorePluginOrNamespacedElements) tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertString.c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Only the elemB directly under the first elemA should be removed std::string expectedString = R"( @@ -909,7 +1258,27 @@ TEST(Converter, RemoveElementSubElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -955,7 +1324,27 @@ TEST(Converter, RemoveAttr) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1007,7 +1396,27 @@ TEST(Converter, RemoveEmptyAttr) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1045,7 +1454,30 @@ TEST(Converter, RemoveNoElement) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Exactly one 'element' or 'attribute' must be specified in ")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *childElem = xmlDoc.FirstChildElement(); ASSERT_NE(nullptr, childElem); @@ -1097,7 +1529,27 @@ TEST(Converter, MoveInvalid) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // In this case, we had an invalid elemC:: in the conversion, which // means that the conversion quietly failed. Make sure the new @@ -1153,7 +1605,27 @@ TEST(Converter, MoveInvalidPrefix) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // In this case, we had an invalid ::elemC in the conversion, which // means that the conversion quietly failed. Make sure the new @@ -1210,7 +1682,27 @@ TEST(Converter, CopyElemElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1385,7 +1877,75 @@ TEST(Converter, MapInvalid) tinyxml2::XMLPrinter printerBefore; xmlDoc.Print(&printerBefore); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 16u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[1].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[2].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[2].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[3].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[4].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[4].Message().find(" has invalid name attribute")); + EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[5].Message().find( + " element requires a child element.")); + EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[6].Message().find( + " element requires a child element.")); + EXPECT_EQ(errors[7].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[7].Message().find( + " element requires a non-empty name attribute.")); + EXPECT_EQ(errors[8].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[8].Message().find( + " element requires a non-empty name attribute.")); + EXPECT_EQ(errors[9].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[9].Message().find( + " element requires a non-empty name attribute.")); + EXPECT_EQ(errors[10].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[10].Message().find( + " element requires at least one element")); + EXPECT_EQ(errors[11].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[11].Message().find( + " element requires at least one element")); + EXPECT_EQ(errors[12].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[12].Message().find("from value must not be empty.")); + EXPECT_EQ(errors[13].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[13].Message().find("to value must not be empty.")); + EXPECT_EQ(errors[14].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[14].Message().find("from value must not be empty.")); + EXPECT_EQ(errors[15].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[15].Message().find("to value must not be empty.")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); + // Only invalid conversion statements. // Make sure the new document is the same as the original. @@ -1495,7 +2055,27 @@ TEST(Converter, MapElemElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1546,7 +2126,27 @@ TEST(Converter, MapElemAttr) << ""; tinyxml2::XMLDocument convertXmlDoc2; convertXmlDoc2.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc2, &convertXmlDoc2); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc2, &convertXmlDoc2, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc2.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1589,7 +2189,27 @@ TEST(Converter, MapAttrAttr) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1639,7 +2259,27 @@ TEST(Converter, MapAttrElem) << ""; tinyxml2::XMLDocument convertXmlDoc4; convertXmlDoc4.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc4, &convertXmlDoc4); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc4, &convertXmlDoc4, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc4.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1686,7 +2326,27 @@ TEST(Converter, MapElemElemMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc5; convertXmlDoc5.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc5, &convertXmlDoc5); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc5, &convertXmlDoc5, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc5.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1730,7 +2390,27 @@ TEST(Converter, MapAttrAttrMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc6; convertXmlDoc6.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc6, &convertXmlDoc6); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc6, &convertXmlDoc6, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc6.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -1777,7 +2457,27 @@ TEST(Converter, MapElemAttrMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc7; convertXmlDoc7.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc7, &convertXmlDoc7); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc7, &convertXmlDoc7, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc7.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -1821,7 +2521,27 @@ TEST(Converter, MapAttrElemMultipleLevels) << ""; tinyxml2::XMLDocument convertXmlDoc8; convertXmlDoc8.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc8, &convertXmlDoc8); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc8, &convertXmlDoc8, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc8.FirstChildElement(); ASSERT_NE(nullptr, convertedElem); @@ -1882,7 +2602,27 @@ TEST(Converter, RenameElemElem) << ""; tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1920,7 +2660,27 @@ TEST(Converter, RenameAttrAttr) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1957,7 +2717,27 @@ TEST(Converter, RenameNoFrom) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + EXPECT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -1994,7 +2774,30 @@ TEST(Converter, RenameNoTo) << ""; tinyxml2::XMLDocument convertXmlDoc3; convertXmlDoc3.Parse(convertStream.str().c_str()); - sdf::Converter::Convert(&xmlDoc3, &convertXmlDoc3); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc3, &convertXmlDoc3, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("No 'to' element name specified")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLElement *convertedElem = xmlDoc3.FirstChildElement(); EXPECT_STREQ(convertedElem->Name(), "elemA"); @@ -2019,7 +2822,28 @@ TEST(Converter, GazeboToSDF) tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - EXPECT_FALSE(sdf::Converter::Convert(&xmlDoc, "1.3")); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + EXPECT_FALSE(sdf::Converter::Convert(errors, &xmlDoc, "1.3", parserConfig)); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find(" element does not exist.")); } //////////////////////////////////////////////////// @@ -2028,12 +2852,44 @@ TEST(Converter, NullDoc) tinyxml2::XMLDocument xmlDoc; tinyxml2::XMLDocument convertXmlDoc; - ASSERT_THROW(sdf::Converter::Convert(nullptr, &convertXmlDoc), - sdf::AssertionInternalError); - ASSERT_THROW(sdf::Converter::Convert(&xmlDoc, nullptr), - sdf::AssertionInternalError); - ASSERT_THROW(sdf::Converter::Convert(nullptr, "1.4"), - sdf::AssertionInternalError); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + sdf::Converter::Convert(errors, nullptr, &convertXmlDoc, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FATAL_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("SDF XML doc is NULL")); + errors.clear(); + + sdf::Converter::Convert(errors, &xmlDoc, nullptr, parserConfig); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FATAL_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("Convert XML doc is NULL")); + errors.clear(); + + sdf::Converter::Convert(errors, nullptr, "1.4", parserConfig); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FATAL_ERROR); + EXPECT_NE(std::string::npos, + errors[0].Message().find("SDF XML doc is NULL")); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } //////////////////////////////////////////////////// @@ -2044,7 +2900,30 @@ TEST(Converter, NoVersion) tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - ASSERT_FALSE(sdf::Converter::Convert(&xmlDoc, "1.3")); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + ASSERT_FALSE(sdf::Converter::Convert(errors, &xmlDoc, "1.3", parserConfig)); + ASSERT_EQ(errors.size(), 1u); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::CONVERSION_ERROR); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Unable to determine original SDF version")); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } //////////////////////////////////////////////////// @@ -2058,7 +2937,27 @@ TEST(Converter, SameVersion) tinyxml2::XMLPrinter printerBefore; xmlDoc.Print(&printerBefore); - ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.3")); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + ASSERT_TRUE(sdf::Converter::Convert(errors, &xmlDoc, "1.3", parserConfig)); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); tinyxml2::XMLPrinter printerAfter; xmlDoc.Print(&printerAfter); @@ -2075,7 +2974,27 @@ TEST(Converter, NewerVersion) tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.6")); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + ASSERT_TRUE(sdf::Converter::Convert(errors, &xmlDoc, "1.6", parserConfig)); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } //////////////////////////////////////////////////// @@ -2086,7 +3005,27 @@ TEST(Converter, MuchNewerVersion) tinyxml2::XMLDocument xmlDoc; xmlDoc.Parse(xmlString.c_str()); - ASSERT_TRUE(sdf::Converter::Convert(&xmlDoc, "1.6")); + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + ASSERT_TRUE(sdf::Converter::Convert(errors, &xmlDoc, "1.6", parserConfig)); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } static std::string ConvertDoc_15_16() @@ -2139,7 +3078,27 @@ TEST(Converter, IMU_15_to_16) // Convert tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.LoadFile(ConvertDoc_15_16().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -2233,7 +3192,27 @@ TEST(Converter, World_15_to_16) // Convert tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.LoadFile(ConvertDoc_15_16().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -2289,7 +3268,27 @@ TEST(Converter, Pose_16_to_17) // Convert tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.LoadFile(ConvertDoc_16_17().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -2374,7 +3373,26 @@ TEST(Converter, World_17_to_18) // Convert tinyxml2::XMLDocument convertXmlDoc; convertXmlDoc.LoadFile(ConvertDoc_17_18().c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::ParserConfig parserConfig; + parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + + sdf::Errors errors; + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); // Compare converted xml with expected std::string convertedXmlStr = ElementToString(xmlDoc.RootElement()); @@ -2512,7 +3530,8 @@ TEST(Converter, World_17_to_18) xmlDoc.Clear(); xmlDoc.Parse(xmlString.c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); // Compare converted xml with expected convertedXmlStr = ElementToString(xmlDoc.RootElement()); @@ -2614,7 +3633,12 @@ TEST(Converter, World_17_to_18) xmlDoc.Clear(); xmlDoc.Parse(xmlString.c_str()); - sdf::Converter::Convert(&xmlDoc, &convertXmlDoc); + + sdf::Converter::Convert(errors, &xmlDoc, &convertXmlDoc, parserConfig); + ASSERT_TRUE(errors.empty()); + + // Check nothing has been printed during Convert calls + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Compare converted xml with expected convertedXmlStr = ElementToString(xmlDoc.RootElement()); diff --git a/src/parser.cc b/src/parser.cc index a82e37e12..844721a94 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -953,7 +953,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, SDFPtr _sdf, && strcmp(sdfNode->Attribute("version"), SDF::Version().c_str()) != 0) { sdfdbg << "Converting a deprecated source[" << _source << "].\n"; - Converter::Convert(_xmlDoc, SDF::Version()); + Converter::Convert(_errors, _xmlDoc, SDF::Version(), _config); } auto *elemXml = _xmlDoc->FirstChildElement(_sdf->Root()->GetName().c_str()); @@ -1043,7 +1043,7 @@ bool readDoc(tinyxml2::XMLDocument *_xmlDoc, ElementPtr _sdf, { sdfdbg << "Converting a deprecated SDF source[" << _source << "].\n"; - Converter::Convert(_xmlDoc, SDF::Version()); + Converter::Convert(_errors, _xmlDoc, SDF::Version(), _config); } tinyxml2::XMLElement *elemXml = sdfNode; @@ -1945,18 +1945,34 @@ bool convertFile(const std::string &_filename, const std::string &_version, bool convertFile(const std::string &_filename, const std::string &_version, const ParserConfig &_config, SDFPtr _sdf) { + sdf::Errors errors = convertFile(_sdf, _filename, _version, _config); + throwOrPrintErrors(errors); + return errors.empty(); +} + +///////////////////////////////////////////////// +sdf::Errors convertFile(SDFPtr _sdf, const std::string &_filename, + const std::string &_version, + const ParserConfig &_config) +{ + sdf::Errors errors; std::string filename = sdf::findFile(_filename, true, false, _config); if (filename.empty()) { - sdferr << "Error finding file [" << _filename << "].\n"; - return false; + std::stringstream ss; + ss << "Error finding file [" + << _filename + << "]."; + errors.push_back({ErrorCode::FILE_READ, ss.str()}); + return errors; } if (nullptr == _sdf || nullptr == _sdf->Root()) { - sdferr << "SDF pointer or its Root is null.\n"; - return false; + errors.push_back({ErrorCode::CONVERSION_ERROR, + "SDF pointer or its Root is null."}); + return errors; } auto xmlDoc = makeSdfDoc(); @@ -1974,25 +1990,26 @@ bool convertFile(const std::string &_filename, const std::string &_version, _sdf->SetOriginalVersion(originalVersion); - if (sdf::Converter::Convert(&xmlDoc, _version, true)) + if (sdf::Converter::Convert(errors, &xmlDoc, _version, _config, true)) { - Errors errors; bool result = sdf::readDoc(&xmlDoc, _sdf, filename, false, _config, errors); - - // Output errors - for (auto const &e : errors) - std::cerr << e << std::endl; - - return result; + if (!result) + { + std::stringstream ss; + ss << "Error in sdf::readDoc when parsing file[" << filename << "]"; + errors.push_back({ErrorCode::PARSING_ERROR, ss.str()}); + } } } else { - sdferr << "Error parsing file[" << filename << "]\n"; + std::stringstream ss; + ss << "Error parsing file[" << filename << "]"; + errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); } - return false; + return errors; } ///////////////////////////////////////////////// @@ -2007,10 +2024,22 @@ bool convertString(const std::string &_sdfString, const std::string &_version, bool convertString(const std::string &_sdfString, const std::string &_version, const ParserConfig &_config, SDFPtr _sdf) { + sdf::Errors errors = convertString(_sdf, _sdfString, _version, _config); + throwOrPrintErrors(errors); + return errors.empty(); +} + +///////////////////////////////////////////////// +sdf::Errors convertString(SDFPtr _sdf, const std::string &_sdfString, + const std::string &_version, + const ParserConfig &_config) +{ + sdf::Errors errors; + if (_sdfString.empty()) { - sdferr << "SDF string is empty.\n"; - return false; + errors.push_back({ErrorCode::CONVERSION_ERROR, "SDF string is empty."}); + return errors; } tinyxml2::XMLDocument xmlDoc; @@ -2030,25 +2059,30 @@ bool convertString(const std::string &_sdfString, const std::string &_version, _sdf->SetOriginalVersion(originalVersion); - if (sdf::Converter::Convert(&xmlDoc, _version, true)) + if (sdf::Converter::Convert(errors, &xmlDoc, _version, _config, true)) { - Errors errors; bool result = sdf::readDoc(&xmlDoc, _sdf, std::string(kSdfStringSource), false, _config, errors); - - // Output errors - for (auto const &e : errors) - std::cerr << e << std::endl; - - return result; + if (!result) + { + std::stringstream ss; + ss << "Error in sdf::readDoc when parsing XML from string[" + << _sdfString + << "]"; + errors.push_back({ErrorCode::PARSING_ERROR, ss.str()}); + } } } else { - sdferr << "Error parsing XML from string[" << _sdfString << "]\n"; + std::stringstream ss; + ss << "Error parsing XML from string[" + << _sdfString + << "]"; + errors.push_back({ErrorCode::CONVERSION_ERROR, ss.str()}); } - return false; + return errors; } ////////////////////////////////////////////////// From 8d765cd9ade689a192c2948bd22dd60d08f9094e Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 30 Nov 2022 11:32:33 -0800 Subject: [PATCH 25/31] 9.10.0~pre2 release (#1207) 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 04bda9d4f..4682a9c24 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,7 @@ set (SDF_MINOR_VERSION 10) 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}~pre1) +set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}~pre2) string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) string(REGEX REPLACE "[0-9]+" "" PROJECT_NAME_NO_VERSION ${PROJECT_NAME}) From 17953de68f42b5b57dcf94fa884d15e1462430c3 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 30 Nov 2022 13:56:56 -0800 Subject: [PATCH 26/31] 9.10.0 release (#1208) Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- Changelog.md | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4682a9c24..a47d03551 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,7 @@ set (SDF_MINOR_VERSION 10) 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}~pre2) +set (SDF_VERSION_FULL ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION}.${SDF_PATCH_VERSION}) 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 d7d670d4f..987e159d1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,12 @@ ## libsdformat 9.X -### libsdformat 9.10.0 (2022-11-17) +### libsdformat 9.10.0 (2022-11-30) + +1. Ign to gz header migration. + * [Pull request #1118](https://github.com/gazebosim/sdformat/pull/1118) + +1. Added HasLensProjection. + * [Pull request #1203](https://github.com/gazebosim/sdformat/pull/1203) 1. Added camera info topic to Camera * [Pull request #1198](https://github.com/gazebosim/sdformat/pull/1198) From 74b5785df2b3327771886f0479db62c3137a040d Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 8 Dec 2022 08:21:35 +0900 Subject: [PATCH 27/31] Warn child joint that resolves to world (#1211) * check for when joint child is resolved to world Signed-off-by: Liam Han --- src/parser.cc | 8 ++++++++ test/integration/joint_dom.cc | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/parser.cc b/src/parser.cc index 844721a94..ec36af87a 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -2614,6 +2614,14 @@ void checkScopedJointParentChildNames( "] that both resolve to [" + resolvedChildName + "], but they should resolve to different values."}); } + // childName == "world" case is handled above + if (childName != "world" && resolvedChildName == "world") + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " has a child with name[" + joint->ChildName() + + "] that resolves to world which is invalid."}); + } } } diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 552d91ad0..7ca33c506 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -646,7 +646,7 @@ TEST(DOMJoint, WorldJointInvalidChildWorld) auto errors = root.Load(testFile); for (auto e : errors) std::cout << e << std::endl; - ASSERT_EQ(3u, errors.size()); + ASSERT_EQ(4u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( From cd08bd4322b638297664b0cf27d6b7d4b82abcde Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 9 Dec 2022 10:48:29 +0800 Subject: [PATCH 28/31] Sensor: add sdf::Errors output to API methods (#1138) Signed-off-by: Marco A. Gutierrez Co-authored-by: Addisu Z. Taddese --- include/sdf/Sensor.hh | 8 ++++++++ src/Sensor.cc | 21 ++++++++++++++++----- test/integration/error_output.cc | 23 +++++++++++++++++++++++ 3 files changed, 47 insertions(+), 5 deletions(-) diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index b8733d2d4..b658917d6 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -214,6 +214,14 @@ namespace sdf /// \return SDF element pointer with updated sensor values. public: sdf::ElementPtr ToElement() const; + /// \brief Create and return an SDF element filled with data from this + /// sensor. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[in] _errors Vector of errors. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const; + /// \brief Get the sensor type. /// \return The sensor type. public: SensorType Type() const; diff --git a/src/Sensor.cc b/src/Sensor.cc index fc146beff..d1b0a2f20 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -691,13 +691,22 @@ Imu *Sensor::ImuSensor() ///////////////////////////////////////////////// sdf::ElementPtr Sensor::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("sensor.sdf", elem); - elem->GetAttribute("type")->Set(this->TypeStr()); - elem->GetAttribute("name")->Set(this->Name()); - sdf::ElementPtr poseElem = elem->GetElement("pose"); + elem->GetAttribute("type")->Set(this->TypeStr(), _errors); + elem->GetAttribute("name")->Set(this->Name(), _errors); + sdf::ElementPtr poseElem = elem->GetElement("pose", _errors); if (!this->dataPtr->poseRelativeTo.empty()) { poseElem->GetAttribute("relative_to")->Set( @@ -760,8 +769,10 @@ sdf::ElementPtr Sensor::ToElement() const } else { - std::cout << "Conversion of sensor type: [" << this->TypeStr() << "] from " - << "SDF DOM to Element is not supported yet." << std::endl; + std::stringstream ss; + ss << "Conversion of sensor type: [" << this->TypeStr() << "] from SDF " + << "DOM to Element is not supported yet." << this->Name(); + _errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); } // Add in the plugins diff --git a/test/integration/error_output.cc b/test/integration/error_output.cc index a4d53e2d2..c4607fc1d 100644 --- a/test/integration/error_output.cc +++ b/test/integration/error_output.cc @@ -24,6 +24,7 @@ #include "sdf/Error.hh" #include "sdf/Model.hh" #include "sdf/Param.hh" +#include "sdf/Sensor.hh" #include "sdf/parser.hh" #include "sdf/Types.hh" #include "sdf/World.hh" @@ -335,6 +336,28 @@ TEST(ErrorOutput, WorldErrorOutput) EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } +//////////////////////////////////////// +// Test Sensor class for sdf::Errors outputs +TEST(ErrorOutput, SensorErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + sdf::Errors errors; + sdf::Sensor sensor; + sensor.ToElement(errors); + ASSERT_EQ(errors.size(), 2u); + EXPECT_NE(std::string::npos, errors[0].Message().find( + "Empty string used when setting a required parameter. Key[name]")); + EXPECT_NE(std::string::npos, errors[1].Message().find( + "Conversion of sensor type: [none] from SDF DOM to Element is not" + " supported yet.")); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} + //////////////////////////////////////// // Test Model class for sdf::Errors outputs TEST(ErrorOutput, ModelErrorOutput) From c8782e7f8fc4cd32aab9d87423f86714c178958c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 14 Dec 2022 10:44:25 -0800 Subject: [PATCH 29/31] Prepare for 13.3.0~pre1 prerelease (#1212) Signed-off-by: Steve Peters --- CMakeLists.txt | 4 ++-- Changelog.md | 30 ++++++++++++++++++++++++++++++ 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f4e6ee7b3..840df5737 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.2.0) +project (sdformat13 VERSION 13.3.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software @@ -43,7 +43,7 @@ if (BUILD_SDF) gz_configure_project( NO_PROJECT_PREFIX REPLACE_INCLUDE_PATH sdf - VERSION_SUFFIX) + VERSION_SUFFIX pre1) ################################################# # Find tinyxml2. diff --git a/Changelog.md b/Changelog.md index 41cfadfc1..1f0077f2f 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,35 @@ ## libsdformat 13.X +### libsdformat 13.3.0 (2022-12-12) + +1. Sensor: add sdf::Errors output to API methods + * [Pull request #1138](https://github.com/gazebosim/sdformat/pull/1138) + +1. Warn child joint that resolves to world + * [Pull request #1211](https://github.com/gazebosim/sdformat/pull/1211) + +1. Converter: add sdf::Errors output to API methods + * [Pull request #1123](https://github.com/gazebosim/sdformat/pull/1123) + +1. ParamPassing: sdfwarns to sdf::Errors when warnings policy set to sdf::EnforcementPolicy::ERR + * [Pull request #1135](https://github.com/gazebosim/sdformat/pull/1135) + +1. Camera: added HasLensProjection + * [Pull request #1203](https://github.com/gazebosim/sdformat/pull/1203) + +1. Change default `camera_info_topic` value to `__default__` + * [Pull request #1201](https://github.com/gazebosim/sdformat/pull/1201) + +1. Check JointAxis expressed-in values during Load + * [Pull request #1195](https://github.com/gazebosim/sdformat/pull/1195) + +1. Added camera info topic to Camera + * [Pull request #1198](https://github.com/gazebosim/sdformat/pull/1198) + * [Pull request #1200](https://github.com/gazebosim/sdformat/pull/1200) + +1. Fix static URDF models with fixed joints + * [Pull request #1193](https://github.com/gazebosim/sdformat/pull/1193) + ### libsdformat 13.2.0 (2022-10-20) 1. sdf/1.10/joint.sdf: add `screw_thread_pitch` From fd1935145ebe97cacc6238d520e1e8c5708cce71 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 15 Dec 2022 19:56:23 +0800 Subject: [PATCH 30/31] update Param calls to use error vectors parameters (#1140) Signed-off-by: Marco A. Gutierrez --- src/Param.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Param.cc b/src/Param.cc index 8d0ce5a30..2336f4296 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -1473,7 +1473,7 @@ bool Param::ValidateValue(sdf::Errors &_errors) const std::ostringstream oss; oss << "The value [" << _val << "] is less than the minimum allowed value of [" - << *this->GetMinValueAsString() << "] for key [" + << *this->GetMinValueAsString(_errors) << "] for key [" << this->GetKey() << "]"; _errors.push_back({ErrorCode::PARAMETER_ERROR, oss.str()}); return false; @@ -1486,7 +1486,7 @@ bool Param::ValidateValue(sdf::Errors &_errors) const std::ostringstream oss; oss << "The value [" << _val << "] is greater than the maximum allowed value of [" - << *this->GetMaxValueAsString() << "] for key [" + << *this->GetMaxValueAsString(_errors) << "] for key [" << this->GetKey() << "]"; _errors.push_back({ErrorCode::PARAMETER_ERROR, oss.str()}); return false; From 8775d0b1dd11151cee6e6c665e767bab4aa764a1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 5 Jan 2023 18:20:14 -0800 Subject: [PATCH 31/31] macos workflow: don't upgrade existing packages (#1217) Set HOMEBREW_NO_INSTALLED_DEPENDENTS_CHECK environment variable to prevent upgrading preinstalled packages. Apply workaround for python issue. Signed-off-by: Steve Peters --- .github/workflows/macos.yml | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index 300d7470a..f32e4291c 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -15,7 +15,17 @@ jobs: uses: Homebrew/actions/setup-homebrew@master - run: brew config + # Workaround for https://github.com/actions/setup-python/issues/577 + - name: Clean up python binaries + run: | + rm /usr/local/bin/2to3; + rm /usr/local/bin/idle3; + rm /usr/local/bin/pydoc3; + rm /usr/local/bin/python3; + rm /usr/local/bin/python3-config; - name: Install base dependencies + env: + HOMEBREW_NO_INSTALLED_DEPENDENTS_CHECK: 1 run: | brew tap osrf/simulation; brew install --only-dependencies ${PACKAGE};