From 9d9693b38a806a43f066bc6329e81eed6ceba33e Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 10 May 2023 17:44:45 +0000 Subject: [PATCH 01/17] URDF->SDF handle links with no inertia or small mass (#1238) When converting from URDF to SDFormat, some links without an block or a small mass may be dropped with only debug messages in a log file, which are easily missed. This makes the following changes: * When a massless link in the middle of a kinematic chain is successfully merged to its parent by fixed joint reduction, fix bug that was dropping the massless link's child links and joints. * Print no warnings or debug messages when a massless link is successfully merged to a parent by fixed joint reduction. * Promote debug messages to warnings / errors when links are dropped to improve visibility. Improve message clarity and suggest fixes to the user. * Change massless threshold test to `> 0` instead of being within a 1e-6 tolerance of 0. * Add unit and integration tests. Related to #199 and #1007. (cherry picked from commit 6ffe66975f5b5260929eecb2f480d4f1d4a5638f) Signed-off-by: Aaron Chong Co-authored-by: Steve Peters --- src/parser_urdf.cc | 140 ++- src/parser_urdf_TEST.cc | 986 ++++++++++++++++++ test/integration/CMakeLists.txt | 1 + ...orce_torque_sensor_lumped_and_reduced.urdf | 46 + ...rce_torque_sensor_massless_child_link.urdf | 49 + test/integration/urdf_to_sdf.cc | 763 ++++++++++++++ test/test_utils.hh | 12 + 7 files changed, 1969 insertions(+), 28 deletions(-) create mode 100644 test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf create mode 100644 test/integration/invalid_force_torque_sensor_massless_child_link.urdf create mode 100644 test/integration/urdf_to_sdf.cc diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 854165f35..1b320fbf9 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -32,7 +32,9 @@ #include #include "sdf/parser_urdf.hh" +#include "sdf/Error.hh" #include "sdf/sdf.hh" +#include "sdf/Types.hh" #include "SDFExtension.hh" @@ -57,7 +59,8 @@ bool g_initialRobotPoseValid = false; std::set g_fixedJointsTransformedInRevoluteJoints; std::set g_fixedJointsTransformedInFixedJoints; const int g_outputDecimalPrecision = 16; - +const char kSdformatUrdfExtensionUrl[] = + "http://sdformat.org/tutorials?tut=sdformat_urdf_extensions"; /// \brief parser xml string into urdf::Vector3 /// \param[in] _key XML key where vector3 value might be @@ -2645,48 +2648,129 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference) void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link) { - // must have an block and cannot have zero mass. - // allow det(I) == zero, in the case of point mass geoms. + // Links without an block will be considered to have zero mass. + const bool linkHasZeroMass = !_link->inertial || _link->inertial->mass <= 0; + + // link must have an block and cannot have zero mass, if its parent + // joint and none of its child joints are reduced. + // 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) || gz::math::equal(_link->inertial->mass, 0.0))) + if (_link->name != "world" && linkHasZeroMass) { - if (!_link->child_links.empty()) + Errors nonJointReductionErrors; + std::stringstream errorStream; + errorStream << "urdf2sdf: link[" << _link->name << "] has "; + if (!_link->inertial) { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, [" - << static_cast(_link->child_links.size()) - << "] children links ignored.\n"; + errorStream << "no block defined. "; } - - if (!_link->child_joints.empty()) + else { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, [" - << static_cast(_link->child_links.size()) - << "] children joints ignored.\n"; + errorStream << "a mass value of less than or equal to zero. "; } + errorStream << "Please ensure this link has a valid mass to prevent any " + << "missing links or joints in the resulting SDF model."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + + // if the parent joint is reduced, which resolves the massless issue of this + // link, no warnings will be emitted + bool parentJointReduced = _link->parent_joint && + FixedJointShouldBeReduced(_link->parent_joint) && + g_reduceFixedJoints; + + if (!parentJointReduced) + { + // check if joint lumping was turned off for the parent joint + if (_link->parent_joint) + { + if (_link->parent_joint->type == urdf::Joint::FIXED) + { + errorStream << "urdf2sdf: allowing joint lumping by removing any " + << " or " + << "gazebo tag on fixed parent joint[" + << _link->parent_joint->name + << "], as well as ensuring that " + << "ParserConfig::URDFPreserveFixedJoint is false, could " + << "help resolve this warning. See " + << std::string(kSdformatUrdfExtensionUrl) + << " for more information about this behavior."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } - if (_link->parent_joint) - { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, " - << "parent joint [" << _link->parent_joint->name - << "] ignored.\n"; - } + errorStream << "urdf2sdf: parent joint[" + << _link->parent_joint->name + << "] ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, not modeled in sdf\n"; - return; + // check if joint lumping was turned off for child joints + if (!_link->child_joints.empty()) + { + for (const auto &cj : _link->child_joints) + { + // Lumping child joints occur before CreateSDF, so if it does occur + // this link would already contain the lumped mass from the child + // link. If a child joint is still fixed at this point, it means joint + // lumping has been disabled. + if (cj->type == urdf::Joint::FIXED) + { + errorStream << "urdf2sdf: allowing joint lumping by removing any " + << " or " + << "gazebo tag on fixed child joint[" + << cj->name + << "], as well as ensuring that " + << "ParserConfig::URDFPreserveFixedJoint is false, " + << "could help resolve this warning. See " + << std::string(kSdformatUrdfExtensionUrl) + << " for more information about this behavior."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } + } + + errorStream << "urdf2sdf: [" + << static_cast(_link->child_joints.size()) + << "] child joints ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + errorStream << "urdf2sdf: [" + << static_cast(_link->child_links.size()) + << "] child links ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } + + // Emit all accumulated warnings and return + for (const auto &e : nonJointReductionErrors) + { + sdfwarn << e << '\n'; + } + errorStream << "urdf2sdf: link[" << _link->name + << "] is not modeled in sdf."; + sdfwarn << Error(ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + return; + } } - // create block for non fixed joint attached bodies + // create block for non fixed joint attached bodies that have mass if ((_link->getParent() && _link->getParent()->name == "world") || !g_reduceFixedJoints || (!_link->parent_joint || !FixedJointShouldBeReduced(_link->parent_joint))) { - CreateLink(_root, _link, gz::math::Pose3d::Zero); + if (!linkHasZeroMass) + { + CreateLink(_root, _link, gz::math::Pose3d::Zero); + } } // recurse into children diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index 06e27c111..e3cfa0af5 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -21,6 +21,7 @@ #include "sdf/sdf.hh" #include "sdf/parser_urdf.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// std::string getMinimalUrdfTxt() @@ -844,6 +845,991 @@ TEST(URDFParser, OutputPrecision) EXPECT_EQ("0", poseValues[5]); } +///////////////////////////////////////////////// +TEST(URDFParser, LinksWithMassIssuesIdentified) +{ + // Redirect sdfwarn output + 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 + + // explicitly zero mass + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has a mass value of less than or equal to zero. Please " + "ensure this link has a valid mass to prevent any missing links or " + "joints in the resulting SDF model"); + } + + // no block defined + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + } + + // negative mass + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has a mass value of less than or equal to zero. Please " + "ensure this link has a valid mass to prevent any missing links or " + "joints in the resulting SDF model"); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithNoFixedJoints) +{ + // Redirect sdfwarn output + 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 + + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // conversion fails + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + TiXmlElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithFixedParentJoint) +{ + // Redirect sdfwarn output + 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 + + // joint lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 visual lumps into link1 + TiXmlElement *lumpedVisual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, lumpedVisual); + ASSERT_NE(nullptr, lumpedVisual->Attribute("name")); + EXPECT_EQ("link1_fixed_joint_lump__link2_visual", + std::string(lumpedVisual->Attribute("name"))); + + // link2 collision lumps into link1 + TiXmlElement *lumpedCollision = + link->FirstChildElement("collision"); + ASSERT_NE(nullptr, lumpedCollision); + ASSERT_NE(nullptr, lumpedCollision->Attribute("name")); + EXPECT_EQ("link1_fixed_joint_lump__link2_collision", + std::string(lumpedCollision->Attribute("name"))); + + // joint1_2 converted into frame, attached to link1 + TiXmlElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link1", std::string(frame->Attribute("attached_to"))); + + // link2 converted into frame, attached to joint1_2 + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link2", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint1_2", std::string(frame->Attribute("attached_to"))); + + // joint2_3 will change to be relative to link1 + TiXmlElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(joint->Attribute("name"))); + TiXmlElement *jointPose = joint->FirstChildElement("pose"); + ASSERT_NE(nullptr, jointPose); + ASSERT_NE(nullptr, jointPose->Attribute("relative_to")); + EXPECT_EQ("link1", std::string(jointPose->Attribute("relative_to"))); + + // link3 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link3", std::string(link->Attribute("name"))); + TiXmlElement *linkPose = link->FirstChildElement("pose"); + ASSERT_NE(nullptr, linkPose); + ASSERT_NE(nullptr, linkPose->Attribute("relative_to")); + EXPECT_EQ("joint2_3", std::string(linkPose->Attribute("relative_to"))); + } + + // Disabling lumping using gazebo tags, fails with warnings suggesting to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // conversion fails + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + TiXmlElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithFixedChildJoint) +{ + // Redirect sdfwarn output + 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 + + // lumping and reduction occurs, mass of link3 lumped into link2 + // link3 and joint2_3 converted into frames + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // mass of link3 lumped into link2, link2 not converted to frame + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // link2 visual and collision remain + TiXmlElement *visual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, visual); + ASSERT_NE(nullptr, visual->Attribute("name")); + EXPECT_EQ("link2_visual", std::string(visual->Attribute("name"))); + TiXmlElement *collision = link->FirstChildElement("collision"); + ASSERT_NE(nullptr, collision); + ASSERT_NE(nullptr, collision->Attribute("name")); + EXPECT_EQ("link2_collision", std::string(collision->Attribute("name"))); + + // joint1_2 + TiXmlElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // joint2_3 converted into a frame + TiXmlElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link2", std::string(frame->Attribute("attached_to"))); + + // link3 converted into a frame + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("attached_to"))); + } + + // turn off lumping with gazebo tag, conversion fails with warnings to suggest + // removing gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + TiXmlElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithAllFixedJointsButLumpingOff) +{ + // Redirect sdfwarn output + 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 + + // turn off all lumping with gazebo tags, conversion fails with suggestions to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + true + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + TiXmlElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassLeafLink) +{ + // Redirect sdfwarn output + 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 + + // lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // joint1_2 + TiXmlElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // link3 visual and collision lumped into link2 + TiXmlElement *visual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, visual); + ASSERT_NE(nullptr, visual->Attribute("name")); + EXPECT_EQ("link2_fixed_joint_lump__link3_visual", + std::string(visual->Attribute("name"))); + TiXmlElement *collision = link->FirstChildElement("collision"); + ASSERT_NE(nullptr, collision); + ASSERT_NE(nullptr, collision->Attribute("name")); + EXPECT_EQ("link2_fixed_joint_lump__link3_collision", + std::string(collision->Attribute("name"))); + + // joint2_3 converted into a frame + TiXmlElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link2", std::string(frame->Attribute("attached_to"))); + + // link3 converted into a frame + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("attached_to"))); + } + + // lumping and reduction turned off with gazebo tag, expect link3 and joint2_3 + // to be ignored and warnings suggesting to remove gazebo tag + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // joint2_3 and link3 will be ignored, and warnings suggesting to remove + // gazebo tags + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint2_3] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // joint1_2 + TiXmlElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // link3 visual and collision not lumped into link2 + TiXmlElement *visual = link->FirstChildElement("visual"); + EXPECT_EQ(nullptr, visual); + TiXmlElement *collision = link->FirstChildElement("collision"); + EXPECT_EQ(nullptr, collision); + + // joint2_3 ignored + joint = joint->NextSiblingElement("joint"); + EXPECT_EQ(nullptr, joint); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + } +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index f0d7cce9c..762c76a56 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -41,6 +41,7 @@ set(tests unknown.cc urdf_gazebo_extensions.cc urdf_joint_parameters.cc + urdf_to_sdf.cc visual_dom.cc world_dom.cc ) diff --git a/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf b/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf new file mode 100644 index 000000000..3c30afbc2 --- /dev/null +++ b/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + 1 + 100.0 + 1 + + child + + + + + + diff --git a/test/integration/invalid_force_torque_sensor_massless_child_link.urdf b/test/integration/invalid_force_torque_sensor_massless_child_link.urdf new file mode 100644 index 000000000..6e4a8093e --- /dev/null +++ b/test/integration/invalid_force_torque_sensor_massless_child_link.urdf @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + 1 + 100.0 + 1 + + child + + + + + + diff --git a/test/integration/urdf_to_sdf.cc b/test/integration/urdf_to_sdf.cc new file mode 100644 index 000000000..377eb14a1 --- /dev/null +++ b/test/integration/urdf_to_sdf.cc @@ -0,0 +1,763 @@ +/* + * Copyright 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include "sdf/sdf.hh" +#include "sdf/parser.hh" +#include "test_config.h" +#include "test_utils.hh" + +////////////////////////////////////////////////// +TEST(URDF2SDF, ValidBasicURDF) +{ + std::string urdfXml = R"( + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + ASSERT_TRUE(errors.empty()); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ValidContinuousJointedURDF) +{ + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + ASSERT_TRUE(errors.empty()); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + EXPECT_TRUE(model->LinkNameExists("link2")); + EXPECT_TRUE(model->JointNameExists("joint1_2")); +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ZeroMassIntermediateLinkWithFixedParentJoint) +{ + // Redirect sdfwarn output + 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 + + // joint lumping and reduction occurs + { + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + ASSERT_TRUE(errors.empty()); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + const sdf::Link *link = model->LinkByName("link1"); + ASSERT_NE(nullptr, link); + + // link2 visual and collision lumps into link1 + EXPECT_TRUE(link->VisualNameExists("link1_fixed_joint_lump__link2_visual")); + EXPECT_TRUE( + link->CollisionNameExists("link1_fixed_joint_lump__link2_collision")); + + // joint1_2 converted into frame, attached to link1 + EXPECT_TRUE(model->FrameNameExists("joint1_2")); + const sdf::Frame *frame = model->FrameByName("joint1_2"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link1"), frame->AttachedTo()); + + // link2 converted into frame, attached to joint1_2 + EXPECT_TRUE(model->FrameNameExists("link2")); + frame = model->FrameByName("link2"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint1_2"), frame->AttachedTo()); + + // joint2_3 will change to be relative to link1 + EXPECT_TRUE(model->JointNameExists("joint2_3")); + const sdf::Joint *joint = model->JointByName("joint2_3"); + ASSERT_NE(nullptr, joint); + EXPECT_EQ(std::string("link1"), joint->ParentLinkName()); + EXPECT_EQ(std::string("link3"), joint->ChildLinkName()); + + // link3 + EXPECT_TRUE(model->LinkNameExists("link3")); + } + + // Disabling lumping using gazebo tags, fails with warnings suggesting to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // no sdf errors, however we expect warnings and ignored joints and links + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning."); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + const sdf::Link *link = model->LinkByName("link1"); + ASSERT_NE(nullptr, link); + + // expect everything below joint1_2 to be ignored + EXPECT_FALSE(model->LinkNameExists("link2")); + EXPECT_FALSE(model->JointNameExists("joint1_2")); + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + + // expect no visual or collision from link2 lumped into link1 + EXPECT_FALSE( + link->VisualNameExists("link1_fixed_joint_lump__link2_visual")); + EXPECT_FALSE( + link->CollisionNameExists("link1_fixed_joint_lump__link2_collision")); + } +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ZeroMassIntermediateLinkWithFixedChildJoint) +{ + // Redirect sdfwarn output + 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 + + // lumping and reduction occurs, mass of link3 lumped into link2 + // link3 and joint2_3 converted into frames + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // lumping and reduction occurs, no warnings or errors should be present + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // mass of link3 lumped into link2, link2 not converted to frame + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link2 visual and collision remain + EXPECT_TRUE(link->VisualNameExists("link2_visual")); + EXPECT_TRUE(link->CollisionNameExists("link2_collision")); + + // joint2_3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("joint2_3")); + const sdf::Frame *frame = model->FrameByName("joint2_3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link2"), frame->AttachedTo()); + + // link3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("link3")); + frame = model->FrameByName("link3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint2_3"), frame->AttachedTo()); + } + + // turn off lumping with gazebo tag, conversion fails with dropped links and + // joints, and warnings to remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // Everything beneath joint1_2 is ignored, no sdf errors, but warnings + // expected with suggestion to remove gazebo tag + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning."); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + EXPECT_FALSE(model->LinkNameExists("link2")); + EXPECT_FALSE(model->FrameNameExists("link2")); + EXPECT_FALSE(model->JointNameExists("joint1_2")); + EXPECT_FALSE(model->FrameNameExists("joint1_2")); + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->FrameNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + EXPECT_FALSE(model->FrameNameExists("joint2_3")); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassLeafLink) +{ + // Redirect sdfwarn output + 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 + + // lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // lumping and reduction occurs, no warnings should be present + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // link2 + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link3 visual and collision lumped into link2 + EXPECT_TRUE( + link->VisualNameExists("link2_fixed_joint_lump__link3_visual")); + EXPECT_TRUE( + link->CollisionNameExists("link2_fixed_joint_lump__link3_collision")); + + // joint2_3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("joint2_3")); + const sdf::Frame *frame = model->FrameByName("joint2_3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link2"), frame->AttachedTo()); + + // link3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("link3")); + frame = model->FrameByName("link3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint2_3"), frame->AttachedTo()); + } + + // lumping and reduction turned off with gazebo tag, expect link3 and joint2_3 + // to be ignored and warnings suggesting to remove gazebo tag + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // joint2_3 and link3 will be ignored, and warnings suggesting to remove + // gazebo tags + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint2_3] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // link2 + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link3 visual and collision not lumped into link2 + EXPECT_FALSE( + link->VisualNameExists("link2_fixed_joint_lump__link3_visual")); + EXPECT_FALSE( + link->CollisionNameExists("link2_fixed_joint_lump__link3_collision")); + + // joint2_3 and link3 ignored + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->FrameNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + EXPECT_FALSE(model->FrameNameExists("joint2_3")); + } +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, URDFConvertForceTorqueSensorModels) +{ + // Redirect sdfwarn output + 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 + + // valid force torque sensor + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile("integration", "force_torque_sensor.urdf"); + sdf::Root root; + sdf::Errors errors = root.Load(sdfTestFile); + EXPECT_TRUE(errors.empty()); + } + + // invalid force torque sensor, with massless child link, fixed parent joint, + // lumping and reduction occurs, no errors or warning will be emitted, but + // force torque sensor won't be able to attach to joint-converted frame, this + // is the case where users need to be wary about + // TODO(aaronchongth): To provide warning when joint is dropped/converted + // during lumping and reduction + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile( + "integration", + "invalid_force_torque_sensor_lumped_and_reduced.urdf"); + sdf::Root root; + sdf::Errors errors = root.Load(sdfTestFile); + + // lumping and reduction occurs, we expect no warnings or errors + EXPECT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link_1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link_1] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("base_link")); + EXPECT_TRUE(model->FrameNameExists("link_1")); + EXPECT_TRUE(model->FrameNameExists("joint_1")); + EXPECT_TRUE(model->LinkNameExists("link_2")); + EXPECT_TRUE(model->JointNameExists("joint_2")); + } + + // invalid force torque sensor, with massless child link, revolute parent + // joint, there will be warnings with joint_1 and link_1 ignored + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile( + "integration", + "invalid_force_torque_sensor_massless_child_link.urdf"); + sdf::Root root; + sdf::Errors errors = root.Load(sdfTestFile); + + // lumping and reduction does not occur, joint_1 and link_1 ignored + EXPECT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link_1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint_1] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link_1] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("base_link")); + EXPECT_TRUE(model->LinkNameExists("link_2")); + EXPECT_TRUE(model->JointNameExists("joint_2")); + EXPECT_FALSE(model->FrameNameExists("link_1")); + EXPECT_FALSE(model->LinkNameExists("link_1")); + EXPECT_FALSE(model->FrameNameExists("joint_1")); + EXPECT_FALSE(model->JointNameExists("joint_1")); + } +} diff --git a/test/test_utils.hh b/test/test_utils.hh index 150f70b60..44451d980 100644 --- a/test/test_utils.hh +++ b/test/test_utils.hh @@ -104,6 +104,18 @@ class RedirectConsoleStream private: sdf::Console::ConsoleStream oldStream; }; +/// \brief Checks that string _a contains string _b +inline bool contains(const std::string &_a, const std::string &_b) +{ + return _a.find(_b) != std::string::npos; +} + +/// \brief Check that string _a does not contain string _b +inline bool notContains(const std::string &_a, const std::string &_b) +{ + return !contains(_a, _b);; +} + } // namespace testing } // namespace sdf From 1a484c04521e52db49169ec18f7b9477db07482b Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 1 Sep 2023 15:47:18 -0500 Subject: [PATCH 02/17] Prepare for 12.7.2 release (#1320) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 24 ++++++++++++++++++++++++ 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f092cfb52..502d8281f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0004 NEW) endif(COMMAND CMAKE_POLICY) -project (sdformat12 VERSION 12.7.1) +project (sdformat12 VERSION 12.7.2) # 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 71b20f678..9ff0b33ff 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,29 @@ ## libsdformat 12.X +### libsdformat 12.7.2 (2023-09-01) + +1. Fixed 1.9/light.sdf + * [Pull request #1281](https://github.com/gazebosim/sdformat/pull/1281) + +1. URDF->SDF handle links with no inertia or small mass + * [Pull request #1238](https://github.com/gazebosim/sdformat/pull/1238) + +1. Fix Element::Set method return value + * [Pull request #1256](https://github.com/gazebosim/sdformat/pull/1256) + +1. Add missing values in Surace ToElement method + * [Pull request #1263](https://github.com/gazebosim/sdformat/pull/1263) + +1. Rename COPYING to LICENSE + * [Pull request #1252](https://github.com/gazebosim/sdformat/pull/1252) + +1. Infrastructure + * [Pull request #1245](https://github.com/gazebosim/sdformat/pull/1245) + * [Pull request #1271](https://github.com/gazebosim/sdformat/pull/1271) + +1. Allow relative paths in URDF + * [Pull request #1213](https://github.com/gazebosim/sdformat/pull/1213) + ### libsdformat 12.7.1 (2023-02-28) 1. Fix camera info topic default value From c63dbd7ab7839b3a48556a6e0dd01c0613634fc7 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Mon, 16 Oct 2023 15:46:56 -0500 Subject: [PATCH 03/17] Enables ruby commands on Windows Signed-off-by: Voldivh --- conf/CMakeLists.txt | 8 ++++++-- src/CMakeLists.txt | 4 +--- src/cmd/CMakeLists.txt | 10 ++++++---- test/CMakeLists.txt | 7 ++++++- test/test_config.hh.in | 2 +- 5 files changed, 20 insertions(+), 11 deletions(-) diff --git a/conf/CMakeLists.txt b/conf/CMakeLists.txt index e42216a9f..7ac7c9e15 100644 --- a/conf/CMakeLists.txt +++ b/conf/CMakeLists.txt @@ -1,12 +1,16 @@ # Used only for internal testing. -set(gz_library_path "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${PROJECT_NAME}") +set(gz_library_path "${CMAKE_BINARY_DIR}/test/lib/$/ruby/gz/cmd${PROJECT_NAME}") # Generate a configuration file for internal testing. # Note that the major version of the library is included in the name. # Ex: sdformat0.yaml configure_file( "${PROJECT_NAME_NO_VERSION_LOWER}.yaml.in" - "${CMAKE_BINARY_DIR}/test/conf/${PROJECT_NAME}.yaml" @ONLY) + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.yaml.configured" @ONLY) + +file(GENERATE + OUTPUT "${CMAKE_BINARY_DIR}/test/conf/$/${PROJECT_NAME}.yaml" + INPUT "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.yaml.configured") # Used for the installed version. set(gz_library_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/gz/cmd${PROJECT_NAME}") diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index feab8e716..ad3eb9682 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -126,6 +126,4 @@ target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} ${CMAKE_CURRENT_SOURCE_DIR} ) -if(NOT WIN32) - add_subdirectory(cmd) -endif() +add_subdirectory(cmd) diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index 0c4c1a2f0..a869d5fca 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -2,8 +2,10 @@ # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. # Ex: cmdsdformat0.rb -set(cmd_script_generated_test "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${PROJECT_NAME}.rb") -set(cmd_script_configured_test "${cmd_script_generated_test}.configured") +set(cmd_script_generated_test + "${CMAKE_BINARY_DIR}/test/lib/$/ruby/gz/cmd${PROJECT_NAME}.rb") +set(cmd_script_configured_test + "${CMAKE_CURRENT_BINARY_DIR}/test_cmd${PROJECT_NAME}.rb.configured") # Set the library_location variable to the full path of the library file within # the build directory. @@ -24,8 +26,8 @@ file(GENERATE # Generate the ruby script that gets installed. # Note that the major version of the library is included in the name. # Ex: cmdsdformat0.rb -set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmd${PROJECT_NAME}.rb") -set(cmd_script_configured "${cmd_script_generated}.configured") +set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/$/cmd${PROJECT_NAME}.rb") +set(cmd_script_configured "${CMAKE_CURRENT_BINARY_DIR}/cmd${PROJECT_NAME}.rb.configured") # Set the library_location variable to the relative path to the library file # within the install directory structure. diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a93dcf814..1afd56f23 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,4 +1,9 @@ -configure_file(test_config.hh.in ${PROJECT_BINARY_DIR}/include/test_config.hh) +configure_file(test_config.hh.in ${PROJECT_BINARY_DIR}/include/test_config.hh.configured) + +file(GENERATE +OUTPUT ${PROJECT_BINARY_DIR}/include/test_config.hh +INPUT ${PROJECT_BINARY_DIR}/include/test_config.hh.configured) + include_directories( ${PROJECT_BINARY_DIR}/include ) diff --git a/test/test_config.hh.in b/test/test_config.hh.in index a84d1cddd..89a908f4b 100644 --- a/test/test_config.hh.in +++ b/test/test_config.hh.in @@ -18,7 +18,7 @@ #ifndef SDF_TEST_CONFIG_HH_ #define SDF_TEST_CONFIG_HH_ -#define GZ_CONFIG_PATH "@CMAKE_BINARY_DIR@/test/conf" +#define GZ_CONFIG_PATH "@CMAKE_BINARY_DIR@/test/conf/$" #define GZ_PATH "@GZ_PROGRAM@" #define GZ_TEST_LIBRARY_PATH "${PROJECT_BINARY_DIR}/src:"\ "@GZ-MSGS_LIBRARY_DIRS@:" From 0d34a61fb88666a511448fdf2a1747968afa1f98 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 17 Oct 2023 10:50:58 -0500 Subject: [PATCH 04/17] Use a compile definition for setting `GZ_CONFIG_PATH` Signed-off-by: Addisu Z. Taddese --- src/CMakeLists.txt | 4 ++++ src/gz_TEST.cc | 3 +++ test/CMakeLists.txt | 7 +------ test/test_config.hh.in | 1 - 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ad3eb9682..0757017d2 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -61,6 +61,10 @@ if (BUILD_TESTING) XmlUtils.cc) endif() + if(TARGET UNIT_gz_TEST) + target_compile_definitions(UNIT_gz_TEST PUBLIC "-DDETAIL_GZ_CONFIG_PATH=\"${CMAKE_BINARY_DIR}/test/conf/$\"") + endif() + if (TARGET UNIT_FrameSemantics_TEST) target_sources(UNIT_FrameSemantics_TEST PRIVATE FrameSemantics.cc Utils.cc) target_link_libraries(UNIT_FrameSemantics_TEST TINYXML2::TINYXML2) diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index a7c5cacdd..72ff7e5cb 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -36,6 +36,9 @@ #define pclose _pclose #endif +// DETAIL_GZ_CONFIG_PATH is compiler definition set in CMake. +#define GZ_CONFIG_PATH DETAIL_GZ_CONFIG_PATH + static std::string SdfVersion() { return " --force-version " + std::string(SDF_VERSION_FULL); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 1afd56f23..a93dcf814 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,9 +1,4 @@ -configure_file(test_config.hh.in ${PROJECT_BINARY_DIR}/include/test_config.hh.configured) - -file(GENERATE -OUTPUT ${PROJECT_BINARY_DIR}/include/test_config.hh -INPUT ${PROJECT_BINARY_DIR}/include/test_config.hh.configured) - +configure_file(test_config.hh.in ${PROJECT_BINARY_DIR}/include/test_config.hh) include_directories( ${PROJECT_BINARY_DIR}/include ) diff --git a/test/test_config.hh.in b/test/test_config.hh.in index 89a908f4b..8e025d922 100644 --- a/test/test_config.hh.in +++ b/test/test_config.hh.in @@ -18,7 +18,6 @@ #ifndef SDF_TEST_CONFIG_HH_ #define SDF_TEST_CONFIG_HH_ -#define GZ_CONFIG_PATH "@CMAKE_BINARY_DIR@/test/conf/$" #define GZ_PATH "@GZ_PROGRAM@" #define GZ_TEST_LIBRARY_PATH "${PROJECT_BINARY_DIR}/src:"\ "@GZ-MSGS_LIBRARY_DIRS@:" From fa910435eb98e59871ca28a5ab1742da1c930df4 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Tue, 17 Oct 2023 14:34:48 -0500 Subject: [PATCH 05/17] Modifies path for sdf_description generation Signed-off-by: Voldivh --- sdf/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt index 983e7804b..d243b028b 100644 --- a/sdf/CMakeLists.txt +++ b/sdf/CMakeLists.txt @@ -50,7 +50,7 @@ if (GZ_PROGRAM) add_custom_command( OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf COMMAND - ${CMAKE_COMMAND} -E env GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf + ${CMAKE_COMMAND} -E env GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf/$ ${GZ_PROGRAM} ARGS sdf -d ${desc_ver} > ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf COMMENT "Generating full description for spec ${desc_ver}" From a88ac4dfb50caa53bc99eb0fd973ab4ad35bb15f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 13 Nov 2023 10:13:58 -0600 Subject: [PATCH 06/17] Update github action workflows (#1345) * Use on `push` only on stable branches to avoid duplicate runs * Update project automation Signed-off-by: Addisu Z. Taddese --- .github/workflows/ci.yml | 11 ++++++++--- .github/workflows/triage.yml | 9 +++------ 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d03658836..34f0ee1fc 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,11 @@ name: Ubuntu -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'sdf[0-9]?' + - 'main' jobs: bionic-ci: @@ -8,7 +13,7 @@ jobs: name: Ubuntu Bionic CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@bionic @@ -19,7 +24,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@focal diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml index 736670e0e..2332244bf 100644 --- a/.github/workflows/triage.yml +++ b/.github/workflows/triage.yml @@ -10,10 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Add ticket to inbox - uses: technote-space/create-project-card-action@v1 + uses: actions/add-to-project@v0.5.0 with: - PROJECT: Core development - COLUMN: Inbox - GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} - CHECK_ORG_PROJECT: true - + project-url: https://github.com/orgs/gazebosim/projects/7 + github-token: ${{ secrets.TRIAGE_TOKEN }} From ffd7e0dc92fda96f0d66407aced3ea187e5f8cc2 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 27 Nov 2023 14:05:09 -0600 Subject: [PATCH 07/17] Fix static builds and optimize test compilation (#1343) Static builds of sdformat failbecause we are creating the using_parser_urdf target and linking it against the core library target, which is then exported. When the core library is built as a static library, CMake requires that other targets linked against it are exported as well (see https://stackoverflow.com/a/71080574/283225 case (2)). The solution in this PR is to compile the URDF sources directly into the core library instead of creating an extra target. While working on this, I realized we could create a static library that includes all the extra sources needed for tests instead of building each test with its own extra sources. I believe this will marginally improve compilation time, but more importantly, I think it cleans up the CMake file and will make adding tests easier. --------- Signed-off-by: Addisu Z. Taddese --- src/CMakeLists.txt | 148 ++++++++++++++++++++------------------------- 1 file changed, 64 insertions(+), 84 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a16582d63..522ae477d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -5,10 +5,9 @@ ign_get_libsources_and_unittests(sources gtest_sources) # Add the source file auto-generated into the build folder from sdf/CMakeLists.txt list(APPEND sources EmbeddedSdf.cc) -# Use interface library to deduplicate cmake logic for URDF linking -add_library(using_parser_urdf INTERFACE) +# When using the internal URDF parser, we build its sources with the core library if (USE_INTERNAL_URDF) - set(sources ${sources} + set(urdf_internal_sources urdf/urdf_parser/model.cpp urdf/urdf_parser/link.cpp urdf/urdf_parser/joint.cpp @@ -17,59 +16,48 @@ if (USE_INTERNAL_URDF) urdf/urdf_parser/urdf_model_state.cpp urdf/urdf_parser/urdf_sensor.cpp urdf/urdf_parser/world.cpp) - target_include_directories(using_parser_urdf INTERFACE - ${CMAKE_CURRENT_SOURCE_DIR}/urdf) - if (WIN32) - target_compile_definitions(using_parser_urdf INTERFACE -D_USE_MATH_DEFINES) - endif() -else() - target_link_libraries(using_parser_urdf INTERFACE - IgnURDFDOM::IgnURDFDOM) + set(sources ${sources} ${urdf_internal_sources}) endif() -if (BUILD_TESTING) - # Build this test file only if Ignition Tools is installed. - if (NOT IGN_PROGRAM) - list(REMOVE_ITEM gtest_sources ign_TEST.cc) - endif() +ign_create_core_library(SOURCES ${sources} + CXX_STANDARD 17 + LEGACY_PROJECT_PREFIX SDFormat + ) - # Skip tests that don't work on Windows - if (WIN32) - list(REMOVE_ITEM gtest_sources Converter_TEST.cc) - list(REMOVE_ITEM gtest_sources FrameSemantics_TEST.cc) - list(REMOVE_ITEM gtest_sources ParamPassing_TEST.cc) - list(REMOVE_ITEM gtest_sources Utils_TEST.cc) - list(REMOVE_ITEM gtest_sources XmlUtils_TEST.cc) - list(REMOVE_ITEM gtest_sources parser_urdf_TEST.cc) +target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} + PUBLIC + ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER} + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} + PRIVATE + TINYXML2::TINYXML2) + + if (USE_INTERNAL_URDF) + target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/urdf) + if (WIN32) + target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE -D_USE_MATH_DEFINES) + endif() + else() + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE + IgnURDFDOM::IgnURDFDOM) endif() - ign_build_tests( - TYPE UNIT - SOURCES ${gtest_sources} - INCLUDE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR} - ${PROJECT_SOURCE_DIR}/test - ) +if (WIN32 AND USE_INTERNAL_URDF) + target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE URDFDOM_STATIC) +endif() - if (TARGET UNIT_Converter_TEST) - target_link_libraries(UNIT_Converter_TEST - TINYXML2::TINYXML2) - target_sources(UNIT_Converter_TEST PRIVATE - Converter.cc - EmbeddedSdf.cc - XmlUtils.cc) - endif() +target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) - if (TARGET UNIT_FrameSemantics_TEST) - target_sources(UNIT_FrameSemantics_TEST PRIVATE FrameSemantics.cc Utils.cc) - target_link_libraries(UNIT_FrameSemantics_TEST TINYXML2::TINYXML2) +if (BUILD_TESTING) + # Build this test file only if Ignition Tools is installed. + if (NOT IGN_PROGRAM) + list(REMOVE_ITEM gtest_sources ign_TEST.cc) endif() - if (TARGET UNIT_ParamPassing_TEST) - target_link_libraries(UNIT_ParamPassing_TEST - TINYXML2::TINYXML2 - using_parser_urdf) - target_sources(UNIT_ParamPassing_TEST PRIVATE + add_library(library_for_tests OBJECT Converter.cc EmbeddedSdf.cc FrameSemantics.cc @@ -78,53 +66,45 @@ if (BUILD_TESTING) Utils.cc XmlUtils.cc parser.cc - parser_urdf.cc) - endif() + parser_urdf.cc + ) - if (TARGET UNIT_Utils_TEST) - target_sources(UNIT_Utils_TEST PRIVATE Utils.cc) - target_link_libraries(UNIT_Utils_TEST TINYXML2::TINYXML2) + if (USE_INTERNAL_URDF) + target_sources(library_for_tests PRIVATE ${urdf_internal_sources}) endif() - if (TARGET UNIT_XmlUtils_TEST) - target_link_libraries(UNIT_XmlUtils_TEST - TINYXML2::TINYXML2) - target_sources(UNIT_XmlUtils_TEST PRIVATE XmlUtils.cc) - endif() + # Link against the publically and privately linked libraries of the core library + target_link_libraries(library_for_tests + $ + ) - if (TARGET UNIT_parser_urdf_TEST) - target_link_libraries(UNIT_parser_urdf_TEST - TINYXML2::TINYXML2 - using_parser_urdf) - target_sources(UNIT_parser_urdf_TEST PRIVATE - SDFExtension.cc - XmlUtils.cc - parser_urdf.cc) - endif() -endif() + # Use the include flags from the core library + target_include_directories(library_for_tests PUBLIC + $ + ) -ign_create_core_library(SOURCES ${sources} - CXX_STANDARD 17 - LEGACY_PROJECT_PREFIX SDFormat + # Use the private compile flags from the core library. Also set IGNITION_SDFORMAT_STATIC_DEFINE to avoid + # inconsistent linkage issues on windows. Setting the define will cause the SDFORMAT_VISIBLE/SDFORMAT_HIDDEN macros + # to expand to nothing when building a static library + target_compile_definitions(library_for_tests PUBLIC + $ + -DIGNITION_SDFORMAT_STATIC_DEFINE ) -target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} - PUBLIC - ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER} - ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} - PRIVATE - TINYXML2::TINYXML2 - using_parser_urdf) + ign_build_tests( + TYPE UNIT + SOURCES ${gtest_sources} + INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR} + ${PROJECT_SOURCE_DIR}/test + LIB_DEPS + library_for_tests + TEST_LIST + test_targets + ) -if (WIN32) - target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE URDFDOM_STATIC) endif() -target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) - if(NOT WIN32) add_subdirectory(cmd) endif() From eee0af8d01d52c9a3cd59f5f7af19112ed2afeb2 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 27 Nov 2023 15:11:31 -0600 Subject: [PATCH 08/17] Address review feedback from PR 1343 (#1347) Signed-off-by: Addisu Z. Taddese --- src/CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 522ae477d..8a796c9ad 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -73,7 +73,7 @@ if (BUILD_TESTING) target_sources(library_for_tests PRIVATE ${urdf_internal_sources}) endif() - # Link against the publically and privately linked libraries of the core library + # Link against the publicly and privately linked libraries of the core library target_link_libraries(library_for_tests $ ) @@ -99,8 +99,6 @@ if (BUILD_TESTING) ${PROJECT_SOURCE_DIR}/test LIB_DEPS library_for_tests - TEST_LIST - test_targets ) endif() From a20cf95d943589692664cc2561d637fef5424346 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 13 Dec 2023 08:59:09 -0600 Subject: [PATCH 09/17] Bazel updates for Garden build (#1239) * Cherry-pick the python3 embedSdf script and tests (#884) These won't be used as part of the cmake build on this branch, but are useful for generating the same file from bazel. Co-authored-by: Bi0T1N Co-authored-by: Michael Carroll Signed-off-by: Michael Carroll * Improvements to embedSdf script Signed-off-by: Michael Carroll * Update sdf/CMakeLists.txt Signed-off-by: Michael Carroll Co-authored-by: Addisu Z. Taddese * Update bazel files Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Add utils back to parser Signed-off-by: Michael Carroll * Fix gz_TEST Signed-off-by: Michael Carroll * Ignore pycache folders Signed-off-by: Michael Carroll * Fix parser_TEST Signed-off-by: Michael Carroll * Bazel nits Signed-off-by: Michael Carroll * Fix path logic Signed-off-by: Michael Carroll * Fix visibility Signed-off-by: Michael Carroll * Fix strings Signed-off-by: Michael Carroll * Use standard vars Signed-off-by: Michael Carroll * Fix string conversions Signed-off-by: Michael Carroll * Fix string conversions Signed-off-by: Michael Carroll * Fix embedded sdf Signed-off-by: Michael Carroll * Fix converter test Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Update bazel files Signed-off-by: Michael Carroll * Add detail prefix Signed-off-by: Michael Carroll * Fix test path Signed-off-by: Michael Carroll * Set homepath on Windows Signed-off-by: Michael Carroll --------- Signed-off-by: Michael Carroll Signed-off-by: Michael Carroll Co-authored-by: Bi0T1N <28802083+Bi0T1N@users.noreply.github.com> Co-authored-by: Bi0T1N Co-authored-by: Addisu Z. Taddese --- .gitignore | 2 + BUILD.bazel | 200 ++++++++++++ include/sdf/Param.hh | 5 + include/sdf/config.hh.in | 7 +- sdf/embedSdf.py | 4 +- src/CMakeLists.txt | 7 +- src/Capsule_TEST.cc | 2 +- src/Cylinder_TEST.cc | 2 +- src/Plane_TEST.cc | 2 +- src/SDF.cc | 18 +- src/SDF_TEST.cc | 44 +-- src/gz_TEST.cc | 302 ++++++++++++------ src/parser_TEST.cc | 22 +- test/BUILD.bazel | 72 +++++ test/integration/CMakeLists.txt | 19 +- test/integration/converter.cc | 2 +- test/integration/default_elements.cc | 16 +- test/integration/frame.cc | 3 +- test/integration/nested_model.cc | 4 +- .../nested_multiple_elements_error.cc | 8 +- test/integration/schema_test.cc | 4 +- test/integration/toml_parser.hh | 4 +- test/test_config.hh.in | 38 ++- 23 files changed, 607 insertions(+), 180 deletions(-) create mode 100644 BUILD.bazel create mode 100644 test/BUILD.bazel diff --git a/.gitignore b/.gitignore index 08e5eba3e..50e689128 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,5 @@ build build_* *.*.sw? .vscode + +__pycache__ diff --git a/BUILD.bazel b/BUILD.bazel new file mode 100644 index 000000000..0e26e2c84 --- /dev/null +++ b/BUILD.bazel @@ -0,0 +1,200 @@ +load( + "@gz//bazel/skylark:build_defs.bzl", + "GZ_FEATURES", + "GZ_ROOT", + "GZ_VISIBILITY", + "add_lint_tests", + "gz_configure_file", + "gz_configure_header", + "gz_export_header", + "gz_include_header", + "gz_py_binary", +) + +package( + default_visibility = GZ_VISIBILITY, + features = GZ_FEATURES, +) + +licenses(["notice"]) + +exports_files(["LICENSE"]) + +gz_configure_header( + name = "config", + src = "include/sdf/config.hh.in", + cmakelists = ["CMakeLists.txt"], + defines = { + "CMAKE_INSTALL_FULL_DATAROOTDIR": "unused", + }, + package = "sdformat", +) + +gz_py_binary( + name = "embed_sdf", + srcs = ["sdf/embedSdf.py"], + main = "sdf/embedSdf.py", +) + +genrule( + name = "embed_sdf_genrule", + srcs = glob([ + "sdf/**/*.sdf", + "sdf/**/*.convert", + ]), + outs = ["EmbeddedSdf.cc"], + cmd = "$(execpath :embed_sdf) --output-file $@ --sdf-root sdformat/sdf/ --input-files $(SRCS)", # noqa + tools = [":embed_sdf"], +) + +public_headers_no_gen = glob([ + "include/sdf/*.h", + "include/sdf/*.hh", +]) + +private_headers = glob(["src/*.hh"]) + +sources = glob( + ["src/*.cc"], + exclude = [ + "src/*_TEST.cc", + "src/gz.cc", + ], +) + +gz_export_header( + name = "include/sdf/Export.hh", + export_base = "GZ_SDFORMAT", + lib_name = "sdf", + visibility = ["//visibility:private"], +) + +gz_include_header( + name = "sdformat_hh_genrule", + out = "include/sdformat.hh", + hdrs = public_headers_no_gen + [ + "include/sdf/config.hh", + "include/sdf/Export.hh", + ], +) + +public_headers = public_headers_no_gen + [ + "include/sdf/Export.hh", + "include/sdf/config.hh", + "include/sdformat.hh", +] + +cc_library( + name = "urdf", + srcs = [ + "src/urdf/urdf_parser/joint.cpp", + "src/urdf/urdf_parser/link.cpp", + "src/urdf/urdf_parser/model.cpp", + "src/urdf/urdf_parser/pose.cpp", + "src/urdf/urdf_parser/twist.cpp", + "src/urdf/urdf_parser/urdf_model_state.cpp", + "src/urdf/urdf_parser/urdf_sensor.cpp", + "src/urdf/urdf_parser/world.cpp", + ], + hdrs = glob( + ["src/urdf/**/*.h"], + ), + copts = ["-Wno-unknown-pragmas"], + includes = ["src/urdf"], + deps = [ + "@tinyxml2", + ], +) + +cc_library( + name = "sdformat", + srcs = sources + private_headers + ["EmbeddedSdf.cc"], + hdrs = public_headers, + defines = [ + 'SDF_SHARE_PATH=\\".\\"', + 'SDF_VERSION_PATH=\\"sdformat\\"', + ], + includes = [ + "include", + "src", + ], + deps = [ + ":urdf", + GZ_ROOT + "math", + GZ_ROOT + "utils", + "@tinyxml2", + ], +) + +cc_library( + name = "sdformat_internal", + srcs = [ + "src/gz.cc", + "src/gz.hh", + ], + visibility = ["//visibility:private"], + deps = [":sdformat"], +) + +test_sources = glob( + ["src/*_TEST.cc"], + exclude = ["src/gz_TEST.cc"], +) + +[cc_test( + name = src.replace("/", "_").replace(".cc", "").replace("src_", ""), + srcs = [src], + data = [ + "sdf", + GZ_ROOT + "sdformat/test:integration", + GZ_ROOT + "sdformat/test:sdf", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "sdformat", + }, + deps = [ + ":sdformat", + GZ_ROOT + "sdformat/test:test_utils", + "@gtest", + "@gtest//:gtest_main", + ], +) for src in test_sources] + +gz_configure_file( + name = "sdformat.rb", + src = "src/cmd/cmdsdformat.rb.in", + out = "cmdsdformat.rb", + cmakelists = ["CMakeLists.txt"], + defines = [ + "library_location=libgz-sdformat.so", + ], + package = "sdformat", + visibility = [GZ_ROOT + "tools:__pkg__"], +) + +gz_configure_file( + name = "sdformat_yaml", + src = "conf/sdformat.yaml.in", + out = "sdformat.yaml", + cmakelists = ["CMakeLists.txt"], + defines = [ + "gz_library_path=gz/sdformat/cmdsdformat.rb", + ], + package = "sdformat", + visibility = [GZ_ROOT + "tools:__pkg__"], +) + +cc_binary( + name = "libgz-sdformat.so", + srcs = [":sdformat_internal"], + linkshared = True, + visibility = [GZ_ROOT + "tools:__pkg__"], + deps = [ + ":sdformat", + ], +) + +exports_files(["sdf"]) + +add_lint_tests() diff --git a/include/sdf/Param.hh b/include/sdf/Param.hh index 29c5b90e5..dea9aa07a 100644 --- a/include/sdf/Param.hh +++ b/include/sdf/Param.hh @@ -85,6 +85,11 @@ namespace sdf : val(_val), precision(_precision) {} }; + // Template deduction guide for ParamVariant + template + ParamStreamer(const ParamVariant &_val, int _precision) + -> ParamStreamer; + template std::ostream& operator<<(std::ostream &os, ParamStreamer s) { diff --git a/include/sdf/config.hh.in b/include/sdf/config.hh.in index d1b3db532..6a32cb8bc 100644 --- a/include/sdf/config.hh.in +++ b/include/sdf/config.hh.in @@ -47,7 +47,12 @@ #cmakedefine SDFORMAT_DISABLE_CONSOLE_LOGFILE 1 +#ifndef SDF_SHARE_PATH #define SDF_SHARE_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" -#define SDF_VERSION_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${SDF_MAJOR_VERSION}/${SDF_PKG_VERSION}" +#endif + +#ifndef SDF_VERSION_PATH +#define SDF_VERSION_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/${PROJECT_VERSION}" +#endif #endif // #ifndef SDF_CONFIG_HH_ diff --git a/sdf/embedSdf.py b/sdf/embedSdf.py index 292958712..8e81054d2 100644 --- a/sdf/embedSdf.py +++ b/sdf/embedSdf.py @@ -160,10 +160,10 @@ def generate_map_content(paths: List[Path], relative_to: Optional[str] = None) - for path in paths: with open(path, "r", encoding="utf8") as input_sdf: file_content = input_sdf.read() - # Strip relative path if requested if relative_to is not None: - path = path.relative_to(relative_to) + _, relative_path = str(path).split(relative_to) + path = relative_path content.append(embed_sdf_content(str(path), file_content)) return ",".join(content) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0757017d2..408add0c5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -61,8 +61,11 @@ if (BUILD_TESTING) XmlUtils.cc) endif() - if(TARGET UNIT_gz_TEST) - target_compile_definitions(UNIT_gz_TEST PUBLIC "-DDETAIL_GZ_CONFIG_PATH=\"${CMAKE_BINARY_DIR}/test/conf/$\"") + if (TARGET UNIT_gz_TEST) + target_compile_definitions(UNIT_gz_TEST PRIVATE + -DGZ_PATH="${GZ_PROGRAM}" + -DDETAIL_GZ_CONFIG_PATH="${CMAKE_BINARY_DIR}/test/conf/$" + -DGZ_TEST_LIBRARY_PATH="${PROJECT_BINARY_DIR}/src") endif() if (TARGET UNIT_FrameSemantics_TEST) diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index b24495966..f2a2993f2 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -154,7 +154,7 @@ TEST(DOMCapsule, Load) // Add a radius element sdf::ElementPtr radiusDesc(new sdf::Element()); radiusDesc->SetName("radius"); - radiusDesc->AddValue("double", "1.0", "1", "radius"); + radiusDesc->AddValue("double", "1.0", true, "radius"); sdf->AddElementDescription(radiusDesc); sdf::ElementPtr radiusElem = sdf->AddElement("radius"); radiusElem->Set(2.0); diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index efc7f302b..b1edb35a5 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -150,7 +150,7 @@ TEST(DOMCylinder, Load) // Add a radius element sdf::ElementPtr radiusDesc(new sdf::Element()); radiusDesc->SetName("radius"); - radiusDesc->AddValue("double", "1.0", "1", "radius"); + radiusDesc->AddValue("double", "1.0", true, "radius"); sdf->AddElementDescription(radiusDesc); sdf::ElementPtr radiusElem = sdf->AddElement("radius"); radiusElem->Set(2.0); diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index ea93941a0..81439ffed 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -145,7 +145,7 @@ TEST(DOMPlane, Load) // Add a normal element sdf::ElementPtr normalDesc(new sdf::Element()); normalDesc->SetName("normal"); - normalDesc->AddValue("vector3", "0 0 1", "1", "normal"); + normalDesc->AddValue("vector3", "0 0 1", true, "normal"); sdf->AddElementDescription(normalDesc); sdf::ElementPtr normalElem = sdf->AddElement("normal"); normalElem->Set({1, 0, 0}); diff --git a/src/SDF.cc b/src/SDF.cc index 07056d792..7a43d0f9a 100644 --- a/src/SDF.cc +++ b/src/SDF.cc @@ -43,6 +43,15 @@ inline namespace SDF_VERSION_NAMESPACE // returns the version string when possible. std::string SDF::version = SDF_VERSION; // NOLINT(runtime/string) +std::string sdfSharePath() +{ +#ifdef SDF_SHARE_PATH + if (std::string(SDF_SHARE_PATH) != "/") + return SDF_SHARE_PATH; +#endif + return ""; +} + ///////////////////////////////////////////////// void setFindCallback(std::function _cb) { @@ -109,16 +118,17 @@ std::string findFile(const std::string &_filename, bool _searchLocalPath, } // Next check the install path. - std::string path = sdf::filesystem::append(SDF_SHARE_PATH, filename); + std::string path = sdf::filesystem::append(sdfSharePath(), filename); if (sdf::filesystem::exists(path)) { return path; } // Next check the versioned install path. - path = sdf::filesystem::append(SDF_SHARE_PATH, - "sdformat" SDF_MAJOR_VERSION_STR, - sdf::SDF::Version(), filename); + path = sdf::filesystem::append(sdfSharePath(), + "sdformat" + std::string(SDF_MAJOR_VERSION_STR), + sdf::SDF::Version(), filename); + if (sdf::filesystem::exists(path)) { return path; diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc index 43d2e4732..44bb9f284 100644 --- a/src/SDF_TEST.cc +++ b/src/SDF_TEST.cc @@ -16,7 +16,10 @@ */ #include + #include +#include + #include #include #include @@ -284,7 +287,7 @@ TEST(SDF, SetRoot) sdf::ElementPtr elem; elem.reset(new sdf::Element()); - elem->AddValue("bool", "true", "0", "description"); + elem->AddValue("bool", "true", false, "description"); s.SetRoot(elem); EXPECT_EQ(elem, s.Root()); @@ -306,56 +309,56 @@ TEST(SDF, EmptyValues) elem.reset(new sdf::Element()); EXPECT_FALSE(elem->Get(emptyString)); - elem->AddValue("bool", "true", "0", "description"); + elem->AddValue("bool", "true", false, "description"); EXPECT_TRUE(elem->Get(emptyString)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), 0); - elem->AddValue("int", "12", "0", "description"); + elem->AddValue("int", "12", false, "description"); EXPECT_EQ(elem->Get(emptyString), 12); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), static_cast(0)); - elem->AddValue("unsigned int", "123", "0", "description"); + elem->AddValue("unsigned int", "123", false, "description"); EXPECT_EQ(elem->Get(emptyString), static_cast(123)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), '\0'); - elem->AddValue("char", "a", "0", "description"); + elem->AddValue("char", "a", false, "description"); EXPECT_EQ(elem->Get(emptyString), 'a'); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), ""); - elem->AddValue("string", "hello", "0", "description"); + elem->AddValue("string", "hello", false, "description"); EXPECT_EQ(elem->Get(emptyString), "hello"); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Vector2d()); - elem->AddValue("vector2d", "1 2", "0", "description"); + elem->AddValue("vector2d", "1 2", false, "description"); EXPECT_EQ(elem->Get(emptyString), gz::math::Vector2d(1, 2)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Vector3d()); - elem->AddValue("vector3", "1 2 3", "0", "description"); + elem->AddValue("vector3", "1 2 3", false, "description"); EXPECT_EQ(elem->Get(emptyString), gz::math::Vector3d(1, 2, 3)); elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Quaterniond()); - elem->AddValue("quaternion", "1 2 3", "0", "description"); + elem->AddValue("quaternion", "1 2 3", false, "description"); EXPECT_EQ(elem->Get(emptyString), gz::math::Quaterniond(-2.14159, 1.14159, -0.141593)); elem.reset(new sdf::Element()); 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"); + elem->AddValue("pose", "1.0 2.0 3.0 4.0 5.0 6.0", false, "description"); EXPECT_EQ(elem->Get(emptyString).Pos(), gz::math::Pose3d(1, 2, 3, 4, 5, 6).Pos()); EXPECT_EQ(elem->Get(emptyString).Rot().Euler(), @@ -364,23 +367,23 @@ TEST(SDF, EmptyValues) elem.reset(new sdf::Element()); EXPECT_EQ(elem->Get(emptyString), gz::math::Color()); - elem->AddValue("color", ".1 .2 .3 1.0", "0", "description"); + elem->AddValue("color", ".1 .2 .3 1.0", false, "description"); 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()); - elem->AddValue("time", "1 2", "0", "description"); + elem->AddValue("time", "1 2", false, "description"); EXPECT_EQ(elem->Get(emptyString), sdf::Time(1, 2)); elem.reset(new sdf::Element()); EXPECT_NEAR(elem->Get(emptyString), 0.0, 1e-6); - elem->AddValue("float", "12.34", "0", "description"); + elem->AddValue("float", "12.34", false, "description"); EXPECT_NEAR(elem->Get(emptyString), 12.34, 1e-6); elem.reset(new sdf::Element()); EXPECT_NEAR(elem->Get(emptyString), 0.0, 1e-6); - elem->AddValue("double", "12.34", "0", "description"); + elem->AddValue("double", "12.34", false, "description"); EXPECT_NEAR(elem->Get(emptyString), 12.34, 1e-6); } @@ -788,17 +791,17 @@ TEST(SDF, WriteURIPath) ASSERT_EQ(std::remove(tempFile.c_str()), 0); ASSERT_EQ(rmdir(tempDir.c_str()), 0); } - ///////////////////////////////////////////////// TEST(SDF, FindFileModelSDFCurrDir) { - std::string currDir; - - // Get current directory path from $PWD env variable - currDir = sdf::filesystem::current_path(); + // Change to a temporary directory before running test + auto prevPath = std::filesystem::current_path(); + std::string tmpDir; + ASSERT_TRUE(sdf::testing::TestTmpPath(tmpDir)); + std::filesystem::current_path(tmpDir); // A file named model.sdf in current directory - auto tempFile = currDir + "/model.sdf"; + auto tempFile = tmpDir + "/model.sdf"; sdf::SDF tempSDF; tempSDF.Write(tempFile); @@ -815,5 +818,6 @@ TEST(SDF, FindFileModelSDFCurrDir) // Cleanup ASSERT_EQ(std::remove(tempFile.c_str()), 0); + std::filesystem::current_path(prevPath); } #endif // _WIN32 diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 72ff7e5cb..3d145b911 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -22,6 +22,7 @@ #include #include +#include #include "sdf/Error.hh" #include "sdf/Filesystem.hh" @@ -74,12 +75,10 @@ std::string custom_exec_str(std::string _cmd) ///////////////////////////////////////////////// TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - // Check an SDFormat file with unrecognized elements { - std::string path = pathBase +"/unrecognized_elements.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "unrecognized_elements.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -104,7 +103,8 @@ TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDFormat file with unrecognized elements with XML namespaces { - std::string path = pathBase +"/unrecognized_elements_with_namespace.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "unrecognized_elements_with_namespace.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -122,12 +122,10 @@ TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ///////////////////////////////////////////////// TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - // Check a good SDF file { - std::string path = pathBase +"/box_plane_low_friction_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_plane_low_friction_test.world"); // Check box_plane_low_friction_test.world std::string output = @@ -137,7 +135,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a bad SDF file { - std::string path = pathBase +"/box_bad_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_bad_test.world"); // Check box_bad_test.world std::string output = @@ -149,7 +148,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (world) // that have duplicate names. { - std::string path = pathBase +"/world_duplicate.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "world_duplicate.sdf"); // Check world_duplicate.sdf std::string output = @@ -161,7 +161,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of different types (model, light) // that have duplicate names. { - std::string path = pathBase +"/world_sibling_same_names.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "world_sibling_same_names.sdf"); // Check world_sibling_same_names.sdf std::string output = @@ -171,7 +172,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } // Check an SDF world file is allowed to have duplicate plugin names { - std::string path = pathBase +"/world_duplicate_plugins.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "world_duplicate_plugins.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -181,7 +183,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (link) // that have duplicate names. { - std::string path = pathBase +"/model_duplicate_links.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_duplicate_links.sdf"); // Check model_duplicate_links.sdf std::string output = @@ -193,7 +196,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (joint) // that have duplicate names. { - std::string path = pathBase +"/model_duplicate_joints.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_duplicate_joints.sdf"); // Check model_duplicate_joints.sdf std::string output = @@ -205,7 +209,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of different types (link, joint) // that have duplicate names. { - std::string path = pathBase +"/model_link_joint_same_name.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_link_joint_same_name.sdf"); // Check model_link_joint_same_name.sdf std::string output = @@ -216,7 +221,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF model file is allowed to have duplicate plugin names { - std::string path = pathBase +"/model_duplicate_plugins.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_duplicate_plugins.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -226,7 +232,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (collision) // that have duplicate names. { - std::string path = pathBase +"/link_duplicate_sibling_collisions.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "link_duplicate_sibling_collisions.sdf"); // Check link_duplicate_sibling_collisions.sdf std::string output = @@ -239,7 +247,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with sibling elements of the same type (visual) // that have duplicate names. { - std::string path = pathBase +"/link_duplicate_sibling_visuals.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "link_duplicate_sibling_visuals.sdf"); // Check link_duplicate_sibling_visuals.sdf std::string output = @@ -251,7 +261,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with cousin elements of the same type (collision) // that have duplicate names. This is a valid file. { - std::string path = pathBase +"/link_duplicate_cousin_collisions.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "link_duplicate_cousin_collisions.sdf"); // Check link_duplicate_cousin_collisions.sdf std::string output = @@ -262,7 +274,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with cousin elements of the same type (visual) // that have duplicate names. This is a valid file. { - std::string path = pathBase +"/link_duplicate_cousin_visuals.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "link_duplicate_cousin_visuals.sdf"); // Check link_duplicate_cousin_visuals.sdf std::string output = @@ -272,7 +286,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint with an invalid child link. { - std::string path = pathBase +"/joint_invalid_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_invalid_child.sdf"); // Check joint_invalid_child.sdf std::string output = @@ -285,7 +300,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint with an invalid parent link. { - std::string path = pathBase +"/joint_invalid_parent.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_invalid_parent.sdf"); // Check joint_invalid_parent.sdf std::string output = @@ -298,7 +314,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint that names itself as the child frame. { - std::string path = pathBase +"/joint_invalid_self_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_invalid_self_child.sdf"); // Check joint_invalid_self_child.sdf std::string output = @@ -310,7 +327,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint that names itself as the parent frame. { - std::string path = pathBase +"/joint_invalid_self_parent.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_invalid_self_parent.sdf"); // Check joint_invalid_self_parent.sdf std::string output = @@ -323,7 +341,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint with identical parent and child. { - std::string path = pathBase +"/joint_invalid_parent_same_as_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "joint_invalid_parent_same_as_child.sdf"); // Check joint_invalid_parent_same_as_child.sdf std::string output = @@ -337,8 +357,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a joint with parent parent frame that resolves // to the same value as the child. { - std::string path = - pathBase + "/joint_invalid_resolved_parent_same_as_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "joint_invalid_resolved_parent_same_as_child.sdf"); // Check joint_invalid_resolved_parent_same_as_child.sdf std::string output = @@ -351,7 +372,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with the world specified as a child link. { - std::string path = pathBase +"/joint_child_world.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_child_world.sdf"); // Check joint_child_world.sdf std::string output = @@ -367,7 +389,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with the world specified as a parent link. // This is a valid file. { - std::string path = pathBase +"/joint_parent_world.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_parent_world.sdf"); // Check joint_parent_world.sdf std::string output = @@ -378,7 +401,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a frame specified as the joint child. // This is a valid file. { - std::string path = pathBase +"/joint_child_frame.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_child_frame.sdf"); // Check joint_child_frame.sdf std::string output = @@ -389,7 +413,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a frame specified as the joint parent. // This is a valid file. { - std::string path = pathBase +"/joint_parent_frame.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_parent_frame.sdf"); // Check joint_parent_frame.sdf std::string output = @@ -400,7 +425,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with the infinite values for joint axis limits. // This is a valid file. { - std::string path = pathBase +"/joint_axis_infinite_limits.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "joint_axis_infinite_limits.sdf"); // Check joint_axis_infinite_limits.sdf std::string output = @@ -411,7 +437,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with the second link specified as the canonical link. // This is a valid file. { - std::string path = pathBase +"/model_canonical_link.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_canonical_link.sdf"); // Check model_canonical_link.sdf std::string output = @@ -421,7 +448,8 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with an invalid link specified as the canonical link. { - std::string path = pathBase +"/model_invalid_canonical_link.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_invalid_canonical_link.sdf"); // Check model_invalid_canonical_link.sdf std::string output = @@ -433,7 +461,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with an invalid model without links. { - std::string path = pathBase +"/model_without_links.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_without_links.sdf"); // Check model_without_links.sdf std::string output = @@ -444,7 +474,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a nested model. { - std::string path = pathBase +"/nested_model.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_model.sdf"); // Check nested_model.sdf std::string output = @@ -454,7 +486,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a model that has a nested canonical link. { - std::string path = pathBase +"/nested_canonical_link.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_canonical_link.sdf"); // Check nested_canonical_link.sdf std::string output = @@ -466,7 +500,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // that is explicitly specified by //model/@canonical_link using :: // syntax. { - std::string path = pathBase +"/nested_explicit_canonical_link.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_explicit_canonical_link.sdf"); // Check nested_explicit_canonical_link.sdf std::string output = @@ -476,7 +512,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a model that a nested model without a link. { - std::string path = pathBase +"/nested_without_links_invalid.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_without_links_invalid.sdf"); // Check nested_without_links_invalid.sdf std::string output = @@ -487,7 +525,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an invalid SDF file that uses reserved names. { - std::string path = pathBase +"/model_invalid_reserved_names.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_reserved_names.sdf"); // Check model_invalid_reserved_names.sdf std::string output = @@ -513,7 +553,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check that validity checks are disabled inside elements { - std::string path = pathBase +"/ignore_sdf_in_plugin.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "ignore_sdf_in_plugin.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -522,7 +564,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check that validity checks are disabled inside namespaced elements { - std::string path = pathBase +"/ignore_sdf_in_namespaced_elements.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "ignore_sdf_in_namespaced_elements.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -532,7 +576,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames using the attached_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_frame_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_attached_to.sdf"); // Check model_frame_attached_to.sdf std::string output = @@ -543,7 +589,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames attached_to joints. // This is a valid file. { - std::string path = pathBase +"/model_frame_attached_to_joint.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_attached_to_joint.sdf"); // Check model_frame_attached_to_joint.sdf std::string output = @@ -554,7 +602,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames attached_to a nested model. // This is a valid file. { - std::string path = pathBase +"/model_frame_attached_to_nested_model.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_attached_to_nested_model.sdf"); // Check model_frame_attached_to_nested_model.sdf std::string output = @@ -564,7 +614,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames with invalid attached_to attributes. { - std::string path = pathBase +"/model_frame_invalid_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_invalid_attached_to.sdf"); // Check model_frame_invalid_attached_to.sdf std::string output = @@ -582,7 +634,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a cycle in its FrameAttachedTo graph { - std::string path = pathBase +"/model_frame_invalid_attached_to_cycle.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_invalid_attached_to_cycle.sdf"); // Check model_frame_invalid_attached_to_cycle.sdf std::string output = @@ -600,7 +654,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames using the attached_to attribute. // This is a valid file. { - std::string path = pathBase +"/world_frame_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_frame_attached_to.sdf"); // Check world_frame_attached_to.sdf std::string output = @@ -610,7 +666,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with world frames with invalid attached_to attributes. { - std::string path = pathBase +"/world_frame_invalid_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_frame_invalid_attached_to.sdf"); // Check world_frame_invalid_attached_to.sdf std::string output = @@ -630,7 +688,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with links using the relative_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_link_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_link_relative_to.sdf"); // Check model_link_relative_to.sdf std::string output = @@ -640,7 +700,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model links with invalid relative_to attributes. { - std::string path = pathBase +"/model_invalid_link_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_link_relative_to.sdf"); // Check model_invalid_link_relative_to.sdf std::string output = @@ -659,7 +721,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with nested_models using the relative_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_nested_model_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_nested_model_relative_to.sdf"); // Check model_nested_model_relative_to.sdf std::string output = @@ -671,7 +735,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // parent or child frames. // This is a valid file. { - std::string path = pathBase +"/joint_nested_parent_child.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "joint_nested_parent_child.sdf"); // Check model_nested_model_relative_to.sdf std::string output = @@ -682,7 +748,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with joints using the relative_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_joint_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_joint_relative_to.sdf"); // Check model_joint_relative_to.sdf std::string output = @@ -692,7 +760,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model joints with invalid relative_to attributes. { - std::string path = pathBase +"/model_invalid_joint_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_joint_relative_to.sdf"); // Check model_invalid_joint_relative_to.sdf std::string output = @@ -711,7 +781,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames using the relative_to attribute. // This is a valid file. { - std::string path = pathBase +"/model_frame_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_relative_to.sdf"); // Check model_frame_relative_to.sdf std::string output = @@ -722,7 +794,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames relative_to joints. // This is a valid file. { - std::string path = pathBase +"/model_frame_relative_to_joint.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_frame_relative_to_joint.sdf"); // Check model_frame_relative_to_joint.sdf std::string output = @@ -732,7 +806,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames with invalid relative_to attributes. { - std::string path = pathBase +"/model_invalid_frame_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_frame_relative_to.sdf"); // Check model_invalid_frame_relative_to.sdf std::string output = @@ -750,7 +826,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with a cycle in its PoseRelativeTo graph { - std::string path = pathBase +"/model_invalid_frame_relative_to_cycle.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_frame_relative_to_cycle.sdf"); // Check model_invalid_frame_relative_to_cycle.sdf std::string output = @@ -768,7 +846,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with model frames using the attached_to attribute. // This is a valid file. { - std::string path = pathBase +"/world_frame_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_frame_relative_to.sdf"); // Check world_frame_relative_to.sdf std::string output = @@ -778,7 +858,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with world frames with invalid relative_to attributes. { - std::string path = pathBase +"/world_frame_invalid_relative_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_frame_invalid_relative_to.sdf"); // Check world_frame_invalid_relative_to.sdf std::string output = @@ -807,7 +889,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with an invalid frame specified as the placement_frame // attribute. { - std::string path = pathBase + "/model_invalid_placement_frame.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_placement_frame.sdf"); // Check model_invalid_placement_frame.sdf std::string output = @@ -820,7 +904,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF file with an valid nested model cross references { - std::string path = pathBase + "/nested_model_cross_references.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "nested_model_cross_references.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -829,7 +915,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an SDF model file with an invalid usage of __root__ { - std::string path = pathBase + "/model_invalid_root_reference.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_root_reference.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -848,8 +936,11 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { // Set SDF_PATH so that included models can be found gz::utils::setenv( - "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); - std::string path = pathBase + "/world_invalid_root_reference.sdf"; + "SDF_PATH", sdf::testing::TestFile("integration", "model")); + + const auto path = + sdf::testing::TestFile("sdf", + "world_invalid_root_reference.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -896,7 +987,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } // Check an SDF world file with an valid usage of __root__ { - std::string path = pathBase + "/world_valid_root_reference.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "world_valid_root_reference.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -904,7 +997,9 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } // Check an SDF with an invalid relative frame at the top level model { - std::string path = pathBase + "/model_invalid_top_level_frame.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "model_invalid_top_level_frame.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -918,11 +1013,10 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ///////////////////////////////////////////////// TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - { - std::string path = pathBase +"/shapes.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "shapes.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -930,7 +1024,9 @@ TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } { - std::string path = pathBase +"/shapes_world.sdf"; + const auto path = + sdf::testing::TestFile("sdf", + "shapes_world.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -941,12 +1037,11 @@ TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ///////////////////////////////////////////////// TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/integration/model/box"; - // Check a good SDF file by passing the absolute path { - std::string path = pathBase +"/model.sdf"; + const auto path = + sdf::testing::TestFile("integration", + "model", "box", "model.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf -k " + path + SdfVersion()); @@ -955,7 +1050,9 @@ TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a good SDF file from the same folder by passing a relative path { - std::string path = "model.sdf"; + const auto pathBase = sdf::testing::TestFile("integration", + "model", "box"); + const auto path = "model.sdf"; std::string output = custom_exec_str("cd " + pathBase + " && " + @@ -979,12 +1076,10 @@ TEST(describe, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ///////////////////////////////////////////////// TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - // Check a good SDF file { - std::string path = pathBase +"/box_plane_low_friction_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_plane_low_friction_test.world"); sdf::SDFPtr sdf(new sdf::SDF()); EXPECT_TRUE(sdf::init(sdf)); EXPECT_TRUE(sdf::readFile(path, sdf)); @@ -997,7 +1092,8 @@ TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a bad SDF file { - std::string path = pathBase +"/box_bad_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_bad_test.world"); // Check box_bad_test.world std::string output = @@ -1359,7 +1455,7 @@ TEST(print_includes_rotations_in_quaternions, { // Set SDF_PATH so that included models can be found gz::utils::setenv( - "SDF_PATH", sdf::testing::SourceFile("test", "integration", "model")); + "SDF_PATH", sdf::testing::TestFile("integration", "model")); const auto path = sdf::testing::TestFile( "sdf", "includes_rotations_in_quaternions.sdf"); @@ -1637,11 +1733,9 @@ TEST(print_snap_to_degrees_tolerance_too_high, ///////////////////////////////////////////////// TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) { - const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; - // world pose relative_to graph const std::string path = - pathBase + "/world_relative_to_nested_reference.sdf"; + sdf::testing::TestFile("sdf", "world_relative_to_nested_reference.sdf"); const std::string output = custom_exec_str(GzCommand() + " sdf -g pose " + path + SdfVersion()); @@ -1688,8 +1782,9 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) ///////////////////////////////////////////////// TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) { - const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; - const std::string path = pathBase + "/model_relative_to_nested_reference.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_relative_to_nested_reference.sdf"); + const std::string output = custom_exec_str(GzCommand() + " sdf -g pose " + path + SdfVersion()); @@ -1764,8 +1859,8 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) ///////////////////////////////////////////////// TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) { - const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; - const std::string path = pathBase + "/world_nested_frame_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "world_nested_frame_attached_to.sdf"); const std::string output = custom_exec_str(GzCommand() + " sdf -g frame " + path + SdfVersion()); @@ -1810,8 +1905,9 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) ///////////////////////////////////////////////// TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) { - const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; - const std::string path = pathBase + "/model_nested_frame_attached_to.sdf"; + const auto path = + sdf::testing::TestFile("sdf", "model_nested_frame_attached_to.sdf"); + const std::string output = custom_exec_str(GzCommand() + " sdf -g frame " + path + SdfVersion()); @@ -1861,9 +1957,6 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) ///////////////////////////////////////////////// TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - std::string pathBase = PROJECT_SOURCE_PATH; - pathBase += "/test/sdf"; - std::string expectedOutput = "Inertial statistics for model: test_model\n" "---\n" @@ -1882,7 +1975,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a good SDF file by passing the absolute path { - std::string path = pathBase +"/inertial_stats.sdf"; + const auto path = sdf::testing::TestFile("sdf", "inertial_stats.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf --inertial-stats " + @@ -1892,7 +1985,8 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check a good SDF file from the same folder by passing a relative path { - std::string path = "inertial_stats.sdf"; + const auto path = "inertial_stats.sdf"; + const auto pathBase = sdf::testing::TestFile("sdf"); std::string output = custom_exec_str("cd " + pathBase + " && " + @@ -1923,7 +2017,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // Check an invalid SDF file by passing the absolute path { - std::string path = pathBase +"/inertial_invalid.sdf"; + const auto path = sdf::testing::TestFile("sdf", "inertial_invalid.sdf"); std::string output = custom_exec_str(GzCommand() + " sdf --inertial-stats " + @@ -1935,7 +2029,8 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) "Error: Expected a model file but received a world file.\n"; // Check a valid world file. { - std::string path = pathBase +"/box_plane_low_friction_test.world"; + const auto path = + sdf::testing::TestFile("sdf", "box_plane_low_friction_test.world"); std::string output = custom_exec_str(GzCommand() + " sdf --inertial-stats " + @@ -1954,9 +2049,8 @@ TEST(HelpVsCompletionFlags, SDF) std::string helpOutput = custom_exec_str(GzCommand() + " sdf --help"); // Call the output function in the bash completion script - std::string scriptPath = PROJECT_SOURCE_PATH; - scriptPath = sdf::filesystem::append(scriptPath, "src", "cmd", - "sdf.bash_completion.sh"); + const auto scriptPath = + sdf::testing::SourceFile("src", "cmd", "sdf.bash_completion.sh"); // Equivalent to: // sh -c "bash -c \". /path/to/sdf.bash_completion.sh; _gz_sdf_flags\"" diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index 7eb54d5e4..f8a614e34 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -23,11 +23,12 @@ #include "sdf/Element.hh" #include "sdf/Console.hh" #include "sdf/Filesystem.hh" -#include "test_config.hh" -#include "test_utils.hh" #include +#include "test_config.hh" +#include "test_utils.hh" + ///////////////////////////////////////////////// TEST(Parser, initStringTrim) { @@ -418,10 +419,18 @@ TEST(Parser, IncludesErrors) const auto path = sdf::testing::TestFile("sdf", "includes_missing_model.sdf"); + sdf::setFindCallback([&](const std::string &/*_file*/) + { + return ""; + }); + sdf::SDFPtr sdf(new sdf::SDF()); sdf::init(sdf); sdf::readFile(path, sdf); + + std::cout << buffer.str() << std::endl; + EXPECT_PRED2(sdf::testing::contains, buffer.str(), "Error Code " + std::to_string( @@ -937,7 +946,14 @@ int main(int argc, char **argv) { // temporarily set HOME std::string homeDir; - sdf::testing::TestSetHomePath(homeDir); + sdf::testing::TestTmpPath(homeDir); + +#ifdef _WIN32 + gz::utils::setenv("HOMEPATH", homeDir); +#else + gz::utils::setenv("HOME", homeDir); +#endif + sdf::Console::Clear(); ::testing::InitGoogleTest(&argc, argv); diff --git a/test/BUILD.bazel b/test/BUILD.bazel new file mode 100644 index 000000000..bb1baa410 --- /dev/null +++ b/test/BUILD.bazel @@ -0,0 +1,72 @@ +load( + "@gz//bazel/skylark:build_defs.bzl", + "GZ_FEATURES", + "GZ_ROOT", + "GZ_VISIBILITY", + "cmake_configure_file", +) + +package( + default_visibility = GZ_VISIBILITY, + features = GZ_FEATURES, +) + +licenses(["notice"]) + +cmake_configure_file( + name = "config", + src = "test_config.hh.in", + out = "test_config.hh", + cmakelists = ["CMakeLists.txt"], + defines = [], +) + +cc_library( + name = "test_utils", + hdrs = [ + "test_config.hh", + "test_utils.hh", + ], + includes = ["."], + deps = [ + GZ_ROOT + "utils:utils", + GZ_ROOT + "sdformat:sdformat", + ], +) + +integration_test_sources = glob( + ["integration/*.cc"], + exclude = [ + "integration/schema_test.cc", + "integration/element_memory_leak.cc", + ], +) + +[cc_test( + name = src.replace("/", "_").replace(".cc", "").replace("integration_", "INTEGRATION_"), + srcs = [ + src, + "integration/toml_parser.hh", + ], + data = [ + GZ_ROOT + "sdformat:sdf", + "integration", + "sdf", + ], + env = { + "GZ_BAZEL": "1", + "GZ_BAZEL_PATH": "sdformat", + }, + includes = ["integration"], + deps = [ + GZ_ROOT + "sdformat:sdformat", + ":test_utils", + "@gtest", + "@gtest//:gtest_main", + ], +) for src in integration_test_sources] + +exports_files([ + "sdf", + "integration", +]) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 9bad38ea4..b7d846e95 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -74,11 +74,22 @@ else() gz_build_warning("xmllint not found. schema_test won't be run") endif() -gz_build_tests(TYPE ${TEST_TYPE} SOURCES ${tests} INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test) +gz_build_tests(TYPE ${TEST_TYPE} + SOURCES ${tests} + INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test +) -if (EXISTS ${XMLLINT_EXE}) - # Need to run schema target (build .xsd files) before running schema_test - add_dependencies(${TEST_TYPE}_schema_test schema) + +if (TARGET ${TEST_TYPE}_schema_test) + target_compile_definitions(${TEST_TYPE}_schema_test + PRIVATE + -DSDF_ROOT_SCHEMA="${PROJECT_BINARY_DIR}/sdf/${SDF_PROTOCOL_VERSION}/root.xsd" + ) + + if (EXISTS ${XMLLINT_EXE}) + # Need to run schema target (build .xsd files) before running schema_test + add_dependencies(${TEST_TYPE}_schema_test schema) + endif() endif() # Test symbols having the right name on linux only diff --git a/test/integration/converter.cc b/test/integration/converter.cc index ec062d5ad..1509de722 100644 --- a/test/integration/converter.cc +++ b/test/integration/converter.cc @@ -156,7 +156,7 @@ void ParserStringConverter(const std::string &_version) TEST(ConverterIntegration, UnflattenConversion) { const std::string filename = - sdf::testing::SourceFile("test", "sdf", + sdf::testing::TestFile("sdf", "flattened_test_nested_model_with_frames.sdf"); sdf::SDFPtr sdf(new sdf::SDF()); diff --git a/test/integration/default_elements.cc b/test/integration/default_elements.cc index efad8d8ad..f07cee0c8 100644 --- a/test/integration/default_elements.cc +++ b/test/integration/default_elements.cc @@ -26,14 +26,14 @@ #include "sdf/Root.hh" #include "sdf/World.hh" #include "sdf/Filesystem.hh" + #include "test_config.hh" ////////////////////////////////////////////////// TEST(ExplicitlySetInFile, EmptyRoadSphCoords) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty_road_sph_coords.sdf"); + const auto testFile = + sdf::testing::TestFile("sdf", "empty_road_sph_coords.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -109,9 +109,8 @@ TEST(ExplicitlySetInFile, EmptyRoadSphCoords) ////////////////////////////////////////////////// TEST(ExplicitlySetInFile, EmptyAxis) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty_axis.sdf"); + const auto testFile = + sdf::testing::TestFile("sdf", "empty_axis.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); @@ -157,9 +156,8 @@ TEST(ExplicitlySetInFile, EmptyAxis) ////////////////////////////////////////////////// TEST(ExplicitlySetInFile, ToString) { - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "empty_road_sph_coords.sdf"); + const auto testFile = + sdf::testing::TestFile("sdf", "empty_road_sph_coords.sdf"); sdf::Root root; sdf::Errors errors = root.Load(testFile); diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 9e0438273..1ed67cd3a 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -1299,8 +1299,7 @@ TEST(DOMFrame, WorldIncludeModel) TEST(Frame, IncludeFrameWithSubmodel) { const std::string MODEL_PATH = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "model", "box_with_submodel"); + sdf::testing::TestFile("integration", "model", "box_with_submodel"); std::ostringstream stream; std::string version = SDF_VERSION; diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index ec9181c4e..0c63cbc43 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -623,7 +623,7 @@ TEST(NestedModel, NestedModelWithFramesDirectComparison) const std::string expectedSdfPath = sdf::testing::TestFile( "integration", "nested_model_with_frames_expected.sdf"); - std::fstream fs; + std::ifstream fs; fs.open(expectedSdfPath); EXPECT_TRUE(fs.is_open()); std::stringstream expected; @@ -779,7 +779,7 @@ TEST(NestedModel, TwoLevelNestedModelWithFramesDirectComparison) const std::string expectedSdfPath = sdf::testing::TestFile( "integration", "two_level_nested_model_with_frames_expected.sdf"); - std::fstream fs; + std::ifstream fs; fs.open(expectedSdfPath); EXPECT_TRUE(fs.is_open()); std::stringstream expected; diff --git a/test/integration/nested_multiple_elements_error.cc b/test/integration/nested_multiple_elements_error.cc index 46c0e975a..7831d2271 100644 --- a/test/integration/nested_multiple_elements_error.cc +++ b/test/integration/nested_multiple_elements_error.cc @@ -26,17 +26,17 @@ #include "sdf/Model.hh" #include "sdf/Root.hh" #include "sdf/World.hh" + #include "test_config.hh" const auto g_testPath = sdf::testing::TestFile(); -const auto g_modelsPath = - sdf::filesystem::append(g_testPath, "integration", "model"); -const auto g_sdfPath = sdf::filesystem::append(g_testPath, "sdf"); +const auto g_modelsPath = sdf::testing::TestFile("integration", "model"); +const auto g_sdfPath = sdf::testing::TestFile("sdf"); ///////////////////////////////////////////////// std::string findFileCb(const std::string &_input) { - return sdf::filesystem::append(g_testPath, "integration", "model", _input); + return sdf::filesystem::append(g_modelsPath, _input); } ////////////////////////////////////////////////// diff --git a/test/integration/schema_test.cc b/test/integration/schema_test.cc index 1fa5e5a8f..99a309fdb 100644 --- a/test/integration/schema_test.cc +++ b/test/integration/schema_test.cc @@ -31,9 +31,7 @@ class SDFSchemaGenerator : public testing::Test public: void runXMLlint(const std::string & model) { - const std::string sdfRootSchema = sdf::filesystem::append( - PROJECT_BINARY_DIR, "sdf", SDF_PROTOCOL_VERSION, "root.xsd"); - + const auto sdfRootSchema = sdf::filesystem::append(SDF_ROOT_SCHEMA); std::string xmllintCmd = "xmllint --noout --schema " + sdfRootSchema + " " + model; std::cout << "CMD[" << xmllintCmd << "]\n"; diff --git a/test/integration/toml_parser.hh b/test/integration/toml_parser.hh index ee3f68ba1..528563cab 100644 --- a/test/integration/toml_parser.hh +++ b/test/integration/toml_parser.hh @@ -135,8 +135,8 @@ struct Document: public Value Document parseToml(const std::string &_filePath, sdf::Errors &_errors) { - std::fstream fs; - fs.open(_filePath, std::fstream::in); + std::ifstream fs; + fs.open(_filePath); if (!fs.good()) { _errors.emplace_back(sdf::ErrorCode::FILE_READ, diff --git a/test/test_config.hh.in b/test/test_config.hh.in index 8e025d922..303eab375 100644 --- a/test/test_config.hh.in +++ b/test/test_config.hh.in @@ -18,17 +18,25 @@ #ifndef SDF_TEST_CONFIG_HH_ #define SDF_TEST_CONFIG_HH_ -#define GZ_PATH "@GZ_PROGRAM@" -#define GZ_TEST_LIBRARY_PATH "${PROJECT_BINARY_DIR}/src:"\ - "@GZ-MSGS_LIBRARY_DIRS@:" -#define PROJECT_SOURCE_PATH "${PROJECT_SOURCE_DIR}" -#define PROJECT_BINARY_DIR "${CMAKE_BINARY_DIR}" -#define SDF_PROTOCOL_VERSION "${SDF_PROTOCOL_VERSION}" -#define SDF_TMP_DIR "tmp-sdf/" - #include #include + +#cmakedefine PROJECT_SOURCE_DIR "@PROJECT_SOURCE_DIR@" +#cmakedefine PROJECT_BINARY_DIR "@PROJECT_BINARY_DIR@" + +#ifdef PROJECT_SOURCE_DIR +constexpr const char* kProjectSourceDir = PROJECT_SOURCE_DIR; +#else +constexpr const char* kProjectSourceDir = ""; +#endif + +#ifdef PROJECT_BINARY_DIR +constexpr const char* kProjectBinaryDir = PROJECT_BINARY_DIR; +#else +constexpr const char* kProjectBinaryDir= ""; +#endif + namespace sdf { namespace testing @@ -40,17 +48,18 @@ namespace sdf /// \return True if directory is set correctly, false otherwise bool ProjectSourcePath(std::string &_sourceDir) { + std::string bazel_path; // Bazel builds set TEST_SRCDIR - if(gz::utils::env("TEST_SRCDIR", _sourceDir)) + if(gz::utils::env("TEST_SRCDIR", _sourceDir) && + gz::utils::env("GZ_BAZEL_PATH", bazel_path)) { _sourceDir = sdf::filesystem::append( - _sourceDir, "__main__", "sdformat"); + _sourceDir, "gz", bazel_path); return true; } else { - // CMake builds set PROJECT_SOURCE_PATH - _sourceDir = PROJECT_SOURCE_PATH; + _sourceDir = kProjectSourceDir; return true; } @@ -70,7 +79,8 @@ namespace sdf } else { - _tmpDir = sdf::filesystem::append(PROJECT_BINARY_DIR, SDF_TMP_DIR); + _tmpDir = sdf::filesystem::append(kProjectBinaryDir, "sdf-tmp"); + sdf::filesystem::create_directory(_tmpDir); return true; } } @@ -92,7 +102,7 @@ namespace sdf } else { - _homeDir = PROJECT_BINARY_DIR; + _homeDir = kProjectBinaryDir; // Set both for linux and windows return gz::utils::setenv("HOME", _homeDir) && gz::utils::setenv("HOMEPATH", _homeDir); From 4726a531d2b99185c49297b3049d717011616311 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 2 Jan 2024 11:08:21 -0800 Subject: [PATCH 10/17] Update CI badges in README (#1352) * Update CI badges in README Signed-off-by: Ian Chen * point to release branch job Signed-off-by: Ian Chen * coverage Signed-off-by: Ian Chen --------- Signed-off-by: Ian Chen --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 78e8321b9..d8deb5a05 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,10 @@ Note: The branch name in the codecov URL & library version should be updated whe --> Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/gazebosim/sdformat/branch/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/sdformat/branch/main) -Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-focal-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-focal-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-homebrew-amd64) -Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-windows7-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-windows7-amd64) +Test coverage | [![codecov](https://codecov.io/gh/gazebosim/sdformat/tree/sdf14/graph/badge.svg)](https://codecov.io/gh/gazebosim/sdformat/tree/sdf14) +Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-sdf14-jammy-amd64)](https://build.osrfoundation.org/job/sdformat-ci-sdf14-jammy-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-sdf14-homebrew-amd64)](https://build.osrfoundation.org/job/sdformat-ci-sdf14-homebrew-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-sdf14-win)](https://build.osrfoundation.org/job/sdformat-sdf14-win) SDFormat is an XML file format that describes environments, objects, and robots From 1774642ee6ac2d46f9fb335470dd7e881d0e6cc5 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 5 Jan 2024 16:08:46 -0600 Subject: [PATCH 11/17] Prepare for 9.10.1 (#1354) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 21 +++++++++++++++++++++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a47d03551..ec2812b3f 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 10) -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 987e159d1..182f69d69 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,26 @@ ## libsdformat 9.X +### libsdformat 9.10.1 (2024-01-05) + +1. URDF->SDF handle links with no inertia or small mass + * [Pull request #1238](https://github.com/gazebosim/sdformat/pull/1238) + +1. Fix Element::Set method return value + * [Pull request #1256](https://github.com/gazebosim/sdformat/pull/1256) + +1. Allowing relative paths in URDF + * [Pull request #1213](https://github.com/gazebosim/sdformat/pull/1213) + +1. Use `File.exist?` for Ruby 3.2 compatibility + * [Pull request #1216](https://github.com/gazebosim/sdformat/pull/1216) + +1. Infrastructure + * [Pull request #1217](https://github.com/gazebosim/sdformat/pull/1217) + * [Pull request #1225](https://github.com/gazebosim/sdformat/pull/1225) + * [Pull request #1271](https://github.com/gazebosim/sdformat/pull/1271) + * [Pull request #1345](https://github.com/gazebosim/sdformat/pull/1345) + * [Pull request #1252](https://github.com/gazebosim/sdformat/pull/1252) + ### libsdformat 9.10.0 (2022-11-30) 1. Ign to gz header migration. From 191e58bc761330ed51dd3a30e64cb6db7401e623 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Tue, 30 Jan 2024 14:50:33 -0600 Subject: [PATCH 12/17] In parser config test, use a filename less likely to exist (#1362) * In parser config test, use a filename less likely to exist The file /usr/share/empty is not unique enough to guarantee that it does not exist, and the test seems to require that there be no file or directory with that name. On Fedora, the file /usr/share/empty is present on every single system. --------- Signed-off-by: Scott K Logan --- test/integration/parser_config.cc | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/test/integration/parser_config.cc b/test/integration/parser_config.cc index 577bbd01a..31a3e2a64 100644 --- a/test/integration/parser_config.cc +++ b/test/integration/parser_config.cc @@ -50,10 +50,10 @@ TEST(ParserConfig, GlobalConfig) ASSERT_TRUE(sdf::ParserConfig::GlobalConfig().FindFileCallback()); EXPECT_EQ("test/dir2", - sdf::ParserConfig::GlobalConfig().FindFileCallback()("empty")); + sdf::ParserConfig::GlobalConfig().FindFileCallback()("should_not_exist")); // sdf::findFile requires explicitly enabling callbacks - EXPECT_EQ("test/dir2", sdf::findFile("empty", false, true)); - EXPECT_EQ("test/dir2", sdf::findFile("empty", true, true)); + EXPECT_EQ("test/dir2", sdf::findFile("should_not_exist", false, true)); + EXPECT_EQ("test/dir2", sdf::findFile("should_not_exist", true, true)); } ///////////////////////////////////////////////// @@ -81,9 +81,10 @@ TEST(ParserConfig, NonGlobalConfig) EXPECT_EQ(it->second.front(), testDir); ASSERT_TRUE(config.FindFileCallback()); - EXPECT_EQ("test/dir2", config.FindFileCallback()("empty")); - EXPECT_EQ("test/dir2", sdf::findFile("empty", false, true, config)); - EXPECT_EQ("test/dir2", sdf::findFile("empty", true, true, config)); + EXPECT_EQ("test/dir2", config.FindFileCallback()("should_not_exist")); + EXPECT_EQ("test/dir2", + sdf::findFile("should_not_exist", false, true, config)); + EXPECT_EQ("test/dir2", sdf::findFile("should_not_exist", true, true, config)); EXPECT_TRUE(sdf::ParserConfig::GlobalConfig().URIPathMap().empty()); EXPECT_FALSE(sdf::ParserConfig::GlobalConfig().FindFileCallback()); From d40b96fa6612987e6ad3ed1cc51fd719d6658880 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Tue, 30 Jan 2024 16:07:42 -0600 Subject: [PATCH 13/17] Fix version variable in reference manual PDF filename (#1363) The variable SDF_VERSION_FULL is a C preprocessor macro which is set to the value of the CMake variable PROJECT_VERSION_FULL in config.hh.in. It is not defined as a CMake variable itself, and currently evaluates to an empty string resulting in refman.pdf being renamed to sdf-.pdf. Signed-off-by: Scott K Logan --- doc/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 019149959..66d4caa6a 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -15,7 +15,7 @@ if (DOXYGEN_FOUND) ${CMAKE_BINARY_DIR}/doxygen/html/search COMMAND make -C ${CMAKE_BINARY_DIR}/doxygen/latex COMMAND mv ${CMAKE_BINARY_DIR}/doxygen/latex/refman.pdf - ${CMAKE_BINARY_DIR}/doxygen/latex/sdf-${SDF_VERSION_FULL}.pdf + ${CMAKE_BINARY_DIR}/doxygen/latex/sdf-${PROJECT_VERSION_FULL}.pdf COMMENT "Generating API documentation with Doxygen" VERBATIM) endif() From 4ca24c9f893f2af8319090a9198fa88b9f03fedf Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Tue, 30 Jan 2024 17:04:23 -0600 Subject: [PATCH 14/17] Fix a little typo in the README.md (#1365) acutators -> actuators Signed-off-by: Scott K Logan Co-authored-by: Steve Peters --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index d8deb5a05..4a8a8cf60 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/ico SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications. SDFormat is capable of representing and describing different physic engines, lighting properties, terrain, static -or dynamic objects, and articulated robots with various sensors, and acutators. +or dynamic objects, and articulated robots with various sensors, and actuators. The format of SDFormat is also described by XML, which facilitates updates and allows conversion from previous versions. From d374404cdeb5fd181b0a2ab5dd9d6400087634a1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 31 Jan 2024 13:34:04 -0800 Subject: [PATCH 15/17] Fix find Python3 logic and macOS workflow (#1367) * Find Python3 with find_package instead of GzPython, adapting the approach from gz-sim. * macos workflow: use brew --prefix, not /usr/local * macos workflow: set Python3_EXECUTABLE cmake var Signed-off-by: Steve Peters --- .github/workflows/macos.yml | 14 +++++------ CMakeLists.txt | 49 +++++++++++++++++++------------------ 2 files changed, 32 insertions(+), 31 deletions(-) diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index ff4cb40c2..ab70886ae 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -19,11 +19,11 @@ jobs: # Workaround for https://github.com/actions/setup-python/issues/577 - name: Clean up python binaries run: | - rm -f /usr/local/bin/2to3*; - rm -f /usr/local/bin/idle3*; - rm -f /usr/local/bin/pydoc3*; - rm -f /usr/local/bin/python3*; - rm -f /usr/local/bin/python3*-config; + rm -f $(brew --prefix)/bin/2to3*; + rm -f $(brew --prefix)/bin/idle3*; + rm -f $(brew --prefix)/bin/pydoc3*; + rm -f $(brew --prefix)/bin/python3*; + rm -f $(brew --prefix)/bin/python3*-config; - name: Install base dependencies env: HOMEBREW_NO_INSTALLED_DEPENDENTS_CHECK: 1 @@ -50,8 +50,8 @@ jobs: - name: cmake working-directory: build run: | - export PYTHONPATH=$PYTHONPATH:/usr/local/lib/python - cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local/Cellar/${PACKAGE}/HEAD + export PYTHONPATH=$PYTHONPATH:$(brew --prefix)/lib/python + cmake .. -DCMAKE_INSTALL_PREFIX=$(brew --prefix)/Cellar/${PACKAGE}/HEAD -DPython3_EXECUTABLE=$(brew --prefix)/bin/python3 - run: make working-directory: build - run: make test diff --git a/CMakeLists.txt b/CMakeLists.txt index abed6df35..47baffc14 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,9 +78,33 @@ if (BUILD_SDF) # available during build time set(GZ_TOOLS_VER 2) + ################################################# + # Find python + if (SKIP_PYBIND11) + message(STATUS "SKIP_PYBIND11 set - disabling python bindings") + find_package(Python3 REQUIRED COMPONENTS Interpreter) + else() + find_package(Python3 REQUIRED + COMPONENTS Interpreter + OPTIONAL_COMPONENTS Development + ) + if (NOT Python3_Development_FOUND) + GZ_BUILD_WARNING("Python development libraries are missing: Python interfaces are disabled.") + else() + set(PYBIND11_PYTHON_VERSION 3) + find_package(pybind11 2.4 CONFIG QUIET) + + if (pybind11_FOUND) + message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + else() + GZ_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") + message (STATUS "Searching for pybind11 - not found.") + endif() + endif() + endif() + ################################################# # Copied from catkin/cmake/empy.cmake - include(GzPython) function(find_python_module module) # cribbed from http://www.cmake.org/pipermail/cmake/2011-January/041666.html string(TOUPPER ${module} module_upper) @@ -121,29 +145,6 @@ if (BUILD_SDF) gz_find_package(gz-utils2 REQUIRED COMPONENTS cli) set(GZ_UTILS_VER ${gz-utils2_VERSION_MAJOR}) - ######################################## - # Python interfaces - if (NOT PYTHON3_FOUND) - GZ_BUILD_ERROR("Python is missing - Needed to build/embed xml schemas") - else() - message (STATUS "Searching for Python - found version ${Python3_VERSION}.") - - if (SKIP_PYBIND11) - message(STATUS "SKIP_PYBIND11 set - disabling python bindings") - else() - set(PYBIND11_PYTHON_VERSION 3) - find_package(pybind11 2.4 QUIET) - - if (${pybind11_FOUND}) - find_package(Python3 ${GZ_PYTHON_VERSION} REQUIRED COMPONENTS Development) - message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") - else() - GZ_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") - message (STATUS "Searching for pybind11 - not found.") - endif() - endif() - endif() - gz_configure_build(HIDE_SYMBOLS_BY_DEFAULT QUIT_IF_BUILD_ERRORS) gz_create_packages() From 3e0d533ffeb52cabe9cd3fcd146beb4b724ff3b0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 1 Feb 2024 23:39:43 -0800 Subject: [PATCH 16/17] macos.yml: use $(brew --prefix python3) Signed-off-by: Steve Peters --- .github/workflows/macos.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index ab70886ae..72404dba4 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -51,7 +51,7 @@ jobs: working-directory: build run: | export PYTHONPATH=$PYTHONPATH:$(brew --prefix)/lib/python - cmake .. -DCMAKE_INSTALL_PREFIX=$(brew --prefix)/Cellar/${PACKAGE}/HEAD -DPython3_EXECUTABLE=$(brew --prefix)/bin/python3 + cmake .. -DCMAKE_INSTALL_PREFIX=$(brew --prefix)/Cellar/${PACKAGE}/HEAD -DPython3_EXECUTABLE=$(brew --prefix python3)/bin/python3 - run: make working-directory: build - run: make test From f3607761d3846dba0b660880484ff1e5eb1e9425 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 2 Feb 2024 14:10:50 -0800 Subject: [PATCH 17/17] Use //link/inertial/density for auto-inertials (#1335) The density precedence for Collisions is now: 1. Density explicitly set in //collision/density or by Collision::SetDensity. 2. Density explicitly set in //link/inertial/density or by Link::SetDensity. 3. Default density in Collision::DensityDefault(). * Fix density handling in Collision::ToElement * Add Link auto_inertia_params to API Pass //link/inertial/auto_inertia_params ElementPtr to Collision::CalculateInertial and use it if the Collision does not have auto_inertia_params of its own. Also add a test for precedence of AutoInertiaParams. * Mesh: don't warn if FilePath is not set The FilePath refers to the SDFormat file that the Mesh object was parsed from, not the Mesh file itself, so remove the unrelated warning. Signed-off-by: Steve Peters --- include/sdf/Collision.hh | 27 ++- include/sdf/Link.hh | 22 ++ src/Collision.cc | 76 ++++-- src/Collision_TEST.cc | 26 ++- src/Link.cc | 52 ++++- src/Link_TEST.cc | 386 +++++++++++++++++++++++++++++++ src/Mesh.cc | 8 - src/Mesh_TEST.cc | 3 - test/sdf/inertial_stats_auto.sdf | 5 +- 9 files changed, 570 insertions(+), 35 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index d6b8470c9..2ab4114ce 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -18,6 +18,7 @@ #define SDF_COLLISION_HH_ #include +#include #include #include #include @@ -78,6 +79,11 @@ namespace sdf /// \param[in] _name Name of the collision. public: void SetName(const std::string &_name); + /// \brief Get the default density of a collision if its density is not + /// specified. + /// \return Default density. + public: static double DensityDefault(); + /// \brief Get the density of the collision. /// \return Density of the collision. public: double Density() const; @@ -145,13 +151,32 @@ namespace sdf /// \brief Calculate and return the MassMatrix for the collision /// \param[out] _errors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors - /// \param[in] _config Custom parser configuration /// \param[out] _inertial An inertial object which will be set with the /// calculated inertial values + /// \param[in] _config Custom parser configuration public: void CalculateInertial(sdf::Errors &_errors, gz::math::Inertiald &_inertial, const ParserConfig &_config); + /// \brief Calculate and return the MassMatrix for the collision + /// \param[out] _errors A vector of Errors objects. Each errors contains an + /// Error code and a message. An empty errors vector indicates no errors + /// \param[out] _inertial An inertial object which will be set with the + /// calculated inertial values + /// \param[in] _config Custom parser configuration + /// \param[in] _density An optional density value to override the default + /// collision density. This value is used instead of DefaultDensity() + // if this collision's density has not been explicitly set. + /// \param[in] _autoInertiaParams An ElementPtr to the auto_inertia_params + /// element to be used if the auto_inertia_params have not been set in this + /// collision. + public: void CalculateInertial( + sdf::Errors &_errors, + gz::math::Inertiald &_inertial, + const ParserConfig &_config, + const std::optional &_density, + sdf::ElementPtr _autoInertiaParams); + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index c92d8d18d..3e5260e73 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -18,6 +18,7 @@ #define SDF_LINK_HH_ #include +#include #include #include #include @@ -79,6 +80,27 @@ namespace sdf /// \param[in] _name Name of the link. public: void SetName(const std::string &_name); + /// \brief Get the density of the inertial if it has been set. + /// \return Density of the inertial if it has been set, + /// otherwise std::nullopt. + public: std::optional Density() const; + + /// \brief Set the density of the inertial. + /// \param[in] _density Density of the inertial. + public: void SetDensity(double _density); + + /// \brief Get the ElementPtr to the element + /// This element can be used as a parent element to hold user-defined + /// params for the custom moment of inertia calculator. + /// \return ElementPtr object for the element. + public: sdf::ElementPtr AutoInertiaParams() const; + + /// \brief Function to set the auto inertia params using a + /// sdf ElementPtr object + /// \param[in] _autoInertiaParams ElementPtr to + /// element + public: void SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams); + /// \brief Get the number of visuals. /// \return Number of visuals contained in this Link object. public: uint64_t VisualCount() const; diff --git a/src/Collision.cc b/src/Collision.cc index c2b2a15ec..ca944712d 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -49,13 +49,10 @@ class sdf::Collision::Implementation /// \brief The collision's surface parameters. public: sdf::Surface surface; - /// \brief Density of the collision. Default is 1000.0 - public: double density{1000.0}; + /// \brief Density of the collision if it has been set. + public: std::optional density; - /// \brief True if density was set during load from sdf. - public: bool densitySetAtLoad = false; - - /// \brief SDF element pointer to tag + /// \brief SDF element pointer to tag public: sdf::ElementPtr autoInertiaParams{nullptr}; /// \brief The SDF element pointer used during load. @@ -130,7 +127,6 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) if (_sdf->HasElement("density")) { this->dataPtr->density = _sdf->Get("density"); - this->dataPtr->densitySetAtLoad = true; } // Load the auto_inertia_params element @@ -155,10 +151,20 @@ void Collision::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +double Collision::DensityDefault() +{ + return 1000.0; +} + ///////////////////////////////////////////////// double Collision::Density() const { - return this->dataPtr->density; + if (this->dataPtr->density) + { + return *this->dataPtr->density; + } + return DensityDefault(); } ///////////////////////////////////////////////// @@ -256,23 +262,59 @@ void Collision::CalculateInertial( gz::math::Inertiald &_inertial, const ParserConfig &_config) { - // Check if density was not set during load & send a warning - // about the default value being used - if (!this->dataPtr->densitySetAtLoad) + this->CalculateInertial( + _errors, _inertial, _config, std::nullopt, ElementPtr()); +} + +///////////////////////////////////////////////// +void Collision::CalculateInertial( + sdf::Errors &_errors, + gz::math::Inertiald &_inertial, + const ParserConfig &_config, + const std::optional &_density, + sdf::ElementPtr _autoInertiaParams) +{ + // Order of precedence for density: + double density; + // 1. Density explicitly set in this collision, either from the + // `//collision/density` element or from Collision::SetDensity. + if (this->dataPtr->density) + { + density = *this->dataPtr->density; + } + // 2. Density passed into this function, which likely comes from the + // `//link/inertial/density` element or from Link::SetDensity. + else if (_density) + { + density = *_density; + } + // 3. DensityDefault value. + else { + // If density was not explicitly set, send a warning + // about the default value being used + density = DensityDefault(); Error densityMissingErr( ErrorCode::ELEMENT_MISSING, "Collision is missing a child element. " - "Using a default density value of 1000.0 kg/m^3. " + "Using a default density value of " + + std::to_string(DensityDefault()) + " kg/m^3. " ); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), densityMissingErr, _errors ); } + // If this Collision's auto inertia params have not been set, then use the + // params passed into this function. + sdf::ElementPtr autoInertiaParams = this->dataPtr->autoInertiaParams; + if (!autoInertiaParams) + { + autoInertiaParams = _autoInertiaParams; + } auto geomInertial = this->dataPtr->geom.CalculateInertial(_errors, _config, - this->dataPtr->density, this->dataPtr->autoInertiaParams); + density, autoInertiaParams); if (!geomInertial) { @@ -309,6 +351,7 @@ sdf::ElementPtr Collision::Element() const return this->dataPtr->sdf; } +///////////////////////////////////////////////// sdf::ElementPtr Collision::ToElement() const { sdf::Errors errors; @@ -335,8 +378,11 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const poseElem->Set(_errors, this->RawPose()); // Set the density - sdf::ElementPtr densityElem = elem->GetElement("density", _errors); - densityElem->Set(this->Density()); + if (this->dataPtr->density.has_value()) + { + sdf::ElementPtr densityElem = elem->GetElement("density", _errors); + densityElem->Set(this->Density()); + } // Set the geometry elem->InsertElement(this->dataPtr->geom.ToElement(_errors), true); diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 07f894a58..1ab3d1b18 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -38,6 +38,7 @@ TEST(DOMcollision, Construction) EXPECT_EQ(nullptr, collision.Element()); EXPECT_TRUE(collision.Name().empty()); EXPECT_EQ(collision.Density(), 1000.0); + EXPECT_EQ(collision.DensityDefault(), 1000.0); collision.SetName("test_collison"); EXPECT_EQ(collision.Name(), "test_collison"); @@ -58,12 +59,12 @@ TEST(DOMcollision, Construction) EXPECT_DOUBLE_EQ(collision.Density(), 1240.0); EXPECT_EQ(collision.AutoInertiaParams(), nullptr); - sdf::ElementPtr autoInertialParamsElem(new sdf::Element()); - autoInertialParamsElem->SetName("auto_inertial_params"); - collision.SetAutoInertiaParams(autoInertialParamsElem); - EXPECT_EQ(collision.AutoInertiaParams(), autoInertialParamsElem); + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + autoInertiaParamsElem->SetName("auto_inertia_params"); + collision.SetAutoInertiaParams(autoInertiaParamsElem); + EXPECT_EQ(collision.AutoInertiaParams(), autoInertiaParamsElem); EXPECT_EQ(collision.AutoInertiaParams()->GetName(), - autoInertialParamsElem->GetName()); + autoInertiaParamsElem->GetName()); collision.SetRawPose({-10, -20, -30, GZ_PI, GZ_PI, GZ_PI}); EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, GZ_PI, GZ_PI, GZ_PI), @@ -420,6 +421,8 @@ TEST(DOMCollision, ToElement) sdf::ElementPtr elem = collision.ToElement(); ASSERT_NE(nullptr, elem); + // Expect no density element + EXPECT_FALSE(elem->HasElement("density")); sdf::Collision collision2; collision2.Load(elem); @@ -434,6 +437,19 @@ TEST(DOMCollision, ToElement) ASSERT_NE(nullptr, surface2->Friction()); ASSERT_NE(nullptr, surface2->Friction()->ODE()); EXPECT_DOUBLE_EQ(1.23, surface2->Friction()->ODE()->Mu()); + + // Now set density in collision + const double kDensity = 1234.5; + collision.SetDensity(kDensity); + sdf::ElementPtr elemWithDensity = collision.ToElement(); + ASSERT_NE(nullptr, elemWithDensity); + // Expect density element + ASSERT_TRUE(elemWithDensity->HasElement("density")); + EXPECT_DOUBLE_EQ(kDensity, elemWithDensity->Get("density")); + + sdf::Collision collision3; + collision3.Load(elem); + EXPECT_DOUBLE_EQ(kDensity, collision.Density()); } ///////////////////////////////////////////////// diff --git a/src/Link.cc b/src/Link.cc index 40a64012f..085feab93 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -69,6 +69,13 @@ class sdf::Link::Implementation /// \brief The projectors specified in this link. public: std::vector projectors; + /// \brief Density of the inertial which will be used for auto-inertial + /// calculations if the collision density has not been set. + public: std::optional density; + + /// \brief SDF element pointer to tag + public: sdf::ElementPtr autoInertiaParams{nullptr}; + /// \brief The inertial information for this link. public: gz::math::Inertiald inertial {{1.0, gz::math::Vector3d::One, gz::math::Vector3d::Zero}, @@ -180,6 +187,18 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { sdf::ElementPtr inertialElem = _sdf->GetElement("inertial"); + if (inertialElem->HasElement("density")) + { + this->dataPtr->density = inertialElem->Get("density"); + } + + // Load the auto_inertia_params element + if (inertialElem->HasElement("auto_inertia_params")) + { + this->dataPtr->autoInertiaParams = + inertialElem->GetElement("auto_inertia_params"); + } + if (inertialElem->Get("auto")) { this->dataPtr->autoInertia = true; @@ -309,6 +328,30 @@ void Link::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +std::optional Link::Density() const +{ + return this->dataPtr->density; +} + +///////////////////////////////////////////////// +void Link::SetDensity(double _density) +{ + this->dataPtr->density = _density; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Link::AutoInertiaParams() const +{ + return this->dataPtr->autoInertiaParams; +} + +///////////////////////////////////////////////// +void Link::SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams) +{ + this->dataPtr->autoInertiaParams = _autoInertiaParams; +} + ///////////////////////////////////////////////// uint64_t Link::VisualCount() const { @@ -620,7 +663,9 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors, for (sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; - collision.CalculateInertial(_errors, collisionInertia, _config); + collision.CalculateInertial(_errors, collisionInertia, _config, + this->dataPtr->density, + this->dataPtr->autoInertiaParams); totalInertia = totalInertia + collisionInertia; } @@ -942,6 +987,11 @@ sdf::ElementPtr Link::ToElement() const inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz()); inertiaElem->GetElement("izz")->Set(massMatrix.Izz()); + if (this->dataPtr->density.has_value()) + { + inertialElem->GetElement("density")->Set(*this->dataPtr->density); + } + if (this->dataPtr->inertial.FluidAddedMass().has_value()) { auto addedMass = this->dataPtr->inertial.FluidAddedMass().value(); diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 5e0968b75..2182ff2e3 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -125,6 +125,20 @@ TEST(DOMLink, Construction) EXPECT_EQ(inertial.BodyMatrix(), inertial.SpatialMatrix()); EXPECT_TRUE(inertial.MassMatrix().IsValid()); + EXPECT_FALSE(link.Density().has_value()); + const double density = 123.0; + link.SetDensity(density); + ASSERT_TRUE(link.Density().has_value()); + EXPECT_DOUBLE_EQ(density, *link.Density()); + + EXPECT_EQ(link.AutoInertiaParams(), nullptr); + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + autoInertiaParamsElem->SetName("auto_inertia_params"); + link.SetAutoInertiaParams(autoInertiaParamsElem); + EXPECT_EQ(link.AutoInertiaParams(), autoInertiaParamsElem); + EXPECT_EQ(link.AutoInertiaParams()->GetName(), + autoInertiaParamsElem->GetName()); + EXPECT_EQ(0u, link.CollisionCount()); EXPECT_EQ(nullptr, link.CollisionByIndex(0)); EXPECT_EQ(nullptr, link.CollisionByIndex(1)); @@ -302,6 +316,355 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink) EXPECT_NE(nullptr, root.Element()); } +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithDifferentDensity) +{ + const std::string sdfString = R"( + + + + 0 0 1.0 0 0 0 + + 0 1.0 0 0 0 0 + + 12.0 + + + 1.0 0 0 0 0 0 + + + 1 1 1 + + + + + + 0 2.0 0 0 0 0 + + + 1.0 0 0 0 0 0 + 24.0 + + + 1 1 1 + + + + + + 0 3.0 0 0 0 0 + + 12.0 + + + 1.0 0 0 0 0 0 + 24.0 + + + 1 1 1 + + + + + + 0 4.0 0 0 0 0 + + + 1.0 0 0 0 0 0 + + + 1 1 1 + + + + + + )"; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdfString, sdfParserConfig); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ(4u, model->LinkCount()); + + // ResolveAutoInertials should run by default during Root::Load. + + { + const sdf::Link *link = model->LinkByName("link_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + ASSERT_TRUE(linkDensity.has_value()); + EXPECT_DOUBLE_EQ(12.0, *linkDensity); + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(12.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(2, 2, 2), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } + + { + const sdf::Link *link = model->LinkByName("collision_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + EXPECT_FALSE(linkDensity.has_value()); + // assume Collision density is properly set to 24 + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } + + { + const sdf::Link *link = + model->LinkByName("collision_density_overrides_link_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + ASSERT_TRUE(linkDensity.has_value()); + EXPECT_DOUBLE_EQ(12.0, *linkDensity); + // assume Collision density is properly set to 24 and overrides the link + // density + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } + + { + const sdf::Link *link = + model->LinkByName("default_density"); + ASSERT_NE(nullptr, link); + auto linkDensity = link->Density(); + EXPECT_FALSE(linkDensity.has_value()); + // assume density is the default value of 1000.0 + // Expected inertial + auto inertial = link->Inertial(); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(1000.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d::One * 500.0 / 3.0, + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + } +} + +///////////////////////////////////////////////// +TEST(DOMLink, ResolveAutoInertialsWithDifferentAutoInertiaParams) +{ + const std::string sdfString = R"( + + + + 0 0 1.0 0 0 0 + + 0 1.0 0 0 0 0 + + + 12 + 1 1 1 + + + + 1.0 0 0 0 0 0 + + + uri + + + + + + 0 2.0 0 0 0 0 + + + 2.0 0 0 0 0 0 + + 24 + 1 1 1 + + + + uri + + + + + + 0 3.0 0 0 0 0 + + + 12 + 1 1 1 + + + + 3.0 0 0 0 0 0 + + 24 + 1 1 1 + + + + uri + + + + + + 0 4.0 0 0 0 0 + + + 4.0 0 0 0 0 0 + + + uri + + + + + + )"; + + // Lambda function for registering as custom inertia calculator + auto customMeshInertiaCalculator = []( + sdf::Errors &, + const sdf::CustomInertiaCalcProperties &_inertiaProps + ) -> std::optional + { + auto autoInertiaParams = _inertiaProps.AutoInertiaParams(); + if (!autoInertiaParams || + !autoInertiaParams->HasElement("gz:density") || + !autoInertiaParams->HasElement("gz:box_size")) + { + // return default inertial values + gz::math::Inertiald meshInertial; + + meshInertial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + + return meshInertial; + } + + gz::math::Inertiald meshInerial; + + double gzDensity = autoInertiaParams->Get("gz:density"); + gz::math::Vector3d gzBoxSize = + autoInertiaParams->Get("gz:box_size"); + gz::math::Material material = gz::math::Material(gzDensity); + + gz::math::MassMatrix3d massMatrix; + massMatrix.SetFromBox(gz::math::Material(gzDensity), gzBoxSize); + + meshInerial.SetMassMatrix(massMatrix); + + return meshInerial; + }; + + + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + sdfParserConfig.RegisterCustomInertiaCalc(customMeshInertiaCalculator); + sdf::Errors errors = root.LoadSdfString(sdfString, sdfParserConfig); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + ASSERT_NE(nullptr, model); + EXPECT_EQ(4u, model->LinkCount()); + + // ResolveAutoInertials should run by default during Root::Load. + + { + const sdf::Link *link = model->LinkByName("link_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //inertial/auto_inertia_params + // * gz:density == 12 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(12.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(2, 2, 2), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(1, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = model->LinkByName("collision_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //collision/auto_inertia_params + // * gz:density == 24 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(2, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = + model->LinkByName("collision_auto_inertia_params_overrides"); + ASSERT_NE(nullptr, link); + + // Verify inertial values computed from //collision/auto_inertia_params + // * gz:density == 24 + // * gz:box_size == (1 1 1) + auto inertial = link->Inertial(); + // box is 1 m^3, so expected mass == density + EXPECT_DOUBLE_EQ(24.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(4, 4, 4), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(3, 0, 0, 0, 0, 0), inertial.Pose()); + } + + { + const sdf::Link *link = + model->LinkByName("default_auto_inertia_params"); + ASSERT_NE(nullptr, link); + + // Verify default inertial values + auto inertial = link->Inertial(); + EXPECT_DOUBLE_EQ(1.0, inertial.MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(1, 1, 1), + inertial.MassMatrix().DiagonalMoments()); + EXPECT_EQ(gz::math::Vector3d::Zero, + inertial.MassMatrix().OffDiagonalMoments()); + EXPECT_EQ(gz::math::Pose3d(4, 0, 0, 0, 0, 0), inertial.Pose()); + } +} + ///////////////////////////////////////////////// TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions) { @@ -621,12 +984,18 @@ TEST(DOMLink, ToElement) sdf::ElementPtr elem = link.ToElement(); ASSERT_NE(nullptr, elem); + // Expect no density element + { + auto inertialElem = elem->FindElement("inertial"); + EXPECT_FALSE(inertialElem->HasElement("density")); + } sdf::Link link2; link2.Load(elem); EXPECT_EQ(link.Name(), link2.Name()); EXPECT_EQ(link.Inertial(), link2.Inertial()); + EXPECT_EQ(link.Density(), link2.Density()); EXPECT_EQ(link.RawPose(), link2.RawPose()); EXPECT_EQ(link.EnableWind(), link2.EnableWind()); EXPECT_EQ(link.CollisionCount(), link2.CollisionCount()); @@ -652,6 +1021,23 @@ TEST(DOMLink, ToElement) EXPECT_EQ(link.ProjectorCount(), link2.ProjectorCount()); for (uint64_t i = 0; i < link2.ProjectorCount(); ++i) EXPECT_NE(nullptr, link2.ProjectorByIndex(i)); + + // Now set density in link + const double kDensity = 1234.5; + link.SetDensity(kDensity); + sdf::ElementPtr elemWithDensity = link.ToElement(); + ASSERT_NE(nullptr, elemWithDensity); + // Expect density element + { + auto inertialElem = elemWithDensity->FindElement("inertial"); + ASSERT_TRUE(inertialElem->HasElement("density")); + EXPECT_DOUBLE_EQ(kDensity, inertialElem->Get("density")); + } + + sdf::Link link3; + link3.Load(elemWithDensity); + ASSERT_TRUE(link3.Density().has_value()); + EXPECT_DOUBLE_EQ(kDensity, *link3.Density()); } ///////////////////////////////////////////////// diff --git a/src/Mesh.cc b/src/Mesh.cc index a3d4d1274..fa180b2bb 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -199,14 +199,6 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, double _density, const sdf::ElementPtr _autoInertiaParams, const ParserConfig &_config) { - if (this->dataPtr->filePath.empty()) - { - _errors.push_back({ - sdf::ErrorCode::WARNING, - "File Path for the mesh was empty. Could not calculate inertia"}); - return std::nullopt; - } - const auto &customCalculator = _config.CustomInertiaCalc(); if (!customCalculator) diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 7a8c4c4ca..51c1751f4 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -201,9 +201,6 @@ TEST(DOMMesh, CalcualteInertial) double density = 0; sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); - // File Path needs to be considered as a valid mesh - mesh.SetFilePath("/some_path"); - // Test Lambda function for registering as custom inertia calculator auto customMeshInertiaCalculator = []( sdf::Errors &_errors, diff --git a/test/sdf/inertial_stats_auto.sdf b/test/sdf/inertial_stats_auto.sdf index 010313b5d..239b9a340 100644 --- a/test/sdf/inertial_stats_auto.sdf +++ b/test/sdf/inertial_stats_auto.sdf @@ -25,9 +25,10 @@ 5 0 0 0 0 0 - - + 6 + + 1 1 1