diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 88295b555..0d9a025dc 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -171,13 +171,13 @@ void ReduceSDFExtensionJointFrameReplace( /// reduced fixed joints: apply appropriate frame updates in gripper /// inside urdf extensions when doing fixed joint reduction void ReduceSDFExtensionGripperFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link); /// reduced fixed joints: apply appropriate frame updates in projector /// inside urdf extensions when doing fixed joint reduction void ReduceSDFExtensionProjectorFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link); /// reduced fixed joints: apply appropriate frame updates in plugins @@ -191,7 +191,7 @@ void ReduceSDFExtensionPluginFrameReplace( /// reduced fixed joints: apply appropriate frame updates in urdf /// extensions when doing fixed joint reduction void ReduceSDFExtensionContactSensorFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link); /// \brief reduced fixed joints: apply appropriate updates to urdf @@ -2583,15 +2583,18 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge, << linkName << "]-->[" << parentLinkName << "]: [" << debugStreamIn.CStr() << "]\n"; - ReduceSDFExtensionContactSensorFrameReplace(blobIt, _link); + ReduceSDFExtensionContactSensorFrameReplace( + (*blobIt)->FirstChildElement(), _link); ReduceSDFExtensionPluginFrameReplace( (*blobIt)->FirstChildElement(), _link, "plugin", "bodyName", _ge->reductionTransform); ReduceSDFExtensionPluginFrameReplace( (*blobIt)->FirstChildElement(), _link, "plugin", "frameName", _ge->reductionTransform); - ReduceSDFExtensionProjectorFrameReplace(blobIt, _link); - ReduceSDFExtensionGripperFrameReplace(blobIt, _link); + ReduceSDFExtensionProjectorFrameReplace( + (*blobIt)->FirstChildElement(), _link); + ReduceSDFExtensionGripperFrameReplace( + (*blobIt)->FirstChildElement(), _link); ReduceSDFExtensionJointFrameReplace((*blobIt)->FirstChildElement(), _link); } } @@ -3568,16 +3571,16 @@ void ReduceSDFExtensionElementTransformReduction( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionContactSensorFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; - if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "sensor") == 0) + if (strcmp(_blob->Name(), "sensor") == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform - tinyxml2::XMLNode *contact = (*_blobIt)->FirstChildElement("contact"); + tinyxml2::XMLNode *contact = _blob->FirstChildElement("contact"); if (contact) { tinyxml2::XMLNode *collision = contact->FirstChildElement("collision"); @@ -3625,8 +3628,9 @@ void ReduceSDFExtensionPluginFrameReplace( { if (GetKeyValueAsString(elementNode->ToElement()) == linkName) { + // Capture the document before DeleteChild — TinyXML2 frees the node. + auto* doc = _blob->GetDocument(); _blob->DeleteChild(elementNode); - auto* doc = elementNode->GetDocument(); tinyxml2::XMLElement *bodyNameKey = doc->NewElement(_elementName.c_str()); std::ostringstream bodyNameStream; @@ -3707,7 +3711,7 @@ void ReduceSDFExtensionPluginFrameReplace( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionProjectorFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link) { std::string linkName = _link->name; @@ -3717,7 +3721,7 @@ void ReduceSDFExtensionProjectorFrameReplace( // projector plugins // update from MyLinkName/MyProjectorName // to NewLinkName/MyProjectorName - tinyxml2::XMLNode *projectorElem = (*_blobIt)->FirstChildElement("projector"); + tinyxml2::XMLNode *projectorElem = _blob->FirstChildElement("projector"); { if (projectorElem) { @@ -3740,15 +3744,16 @@ void ReduceSDFExtensionProjectorFrameReplace( projectorName = parentLinkName + "/" + projectorName.substr(pos+1, projectorName.size()); - (*_blobIt)->DeleteChild(projectorElem); - auto* doc = projectorElem->GetDocument(); + // Capture the document before DeleteChild — TinyXML2 frees the node. + auto* doc = _blob->GetDocument(); + _blob->DeleteChild(projectorElem); tinyxml2::XMLElement *bodyNameKey = doc->NewElement("projector"); std::ostringstream bodyNameStream; bodyNameStream << projectorName; tinyxml2::XMLText *bodyNameTxt = doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); - (*_blobIt)->LinkEndChild(bodyNameKey); + _blob->LinkEndChild(bodyNameKey); } } } @@ -3757,38 +3762,38 @@ void ReduceSDFExtensionProjectorFrameReplace( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionGripperFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; - if (strcmp((*_blobIt)->FirstChildElement()->Name(), "gripper") == 0) + if (strcmp(_blob->Name(), "gripper") == 0) { tinyxml2::XMLNode *gripperLink = - (*_blobIt)->FirstChildElement("gripper_link"); + _blob->FirstChildElement("gripper_link"); if (gripperLink) { if (GetKeyValueAsString(gripperLink->ToElement()) == linkName) { - (*_blobIt)->DeleteChild(gripperLink); - auto* doc = (*_blobIt)->GetDocument(); + _blob->DeleteChild(gripperLink); + auto* doc = _blob->GetDocument(); tinyxml2::XMLElement *bodyNameKey = doc->NewElement("gripper_link"); std::ostringstream bodyNameStream; bodyNameStream << parentLinkName; tinyxml2::XMLText *bodyNameTxt = doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); - (*_blobIt)->LinkEndChild(bodyNameKey); + _blob->LinkEndChild(bodyNameKey); } } - tinyxml2::XMLNode *palmLink = (*_blobIt)->FirstChildElement("palm_link"); + tinyxml2::XMLNode *palmLink = _blob->FirstChildElement("palm_link"); if (palmLink) { if (GetKeyValueAsString(palmLink->ToElement()) == linkName) { - (*_blobIt)->DeleteChild(palmLink); - auto* doc = (*_blobIt)->GetDocument(); + _blob->DeleteChild(palmLink); + auto* doc = _blob->GetDocument(); tinyxml2::XMLElement *bodyNameKey = doc->NewElement("palm_link"); std::ostringstream bodyNameStream; @@ -3796,7 +3801,7 @@ void ReduceSDFExtensionGripperFrameReplace( tinyxml2::XMLText *bodyNameTxt = doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); - (*_blobIt)->LinkEndChild(bodyNameKey); + _blob->LinkEndChild(bodyNameKey); } } } diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index c9f97937d..5f9b69cdf 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -43,6 +43,12 @@ const char SDF_TEST_FILE_JOINT_FRAME_EXTENSION[] = "fixed_joint_reduction_joint_frame_extension.urdf"; const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] = "fixed_joint_reduction_plugin_frame_extension.urdf"; +const char SDF_TEST_FILE_CONTACT_SENSOR_FRAME_EXTENSION[] = + "fixed_joint_reduction_contact_sensor_frame_extension.urdf"; +const char SDF_TEST_FILE_GRIPPER_FRAME_EXTENSION[] = + "fixed_joint_reduction_gripper_frame_extension.urdf"; +const char SDF_TEST_FILE_PROJECTOR_FRAME_EXTENSION[] = + "fixed_joint_reduction_projector_frame_extension.urdf"; static std::string GetFullTestFilePath(const char *_input) { @@ -794,3 +800,90 @@ TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest) bool correctedOffset = plugin->Get("gz::corrected_offsets"); EXPECT_TRUE(correctedOffset); } + +///////////////////////////////////////////////// +// This test uses a urdf that has chained fixed joints with a contact +// sensor whose names a reduced child link. +// Test to make sure that the collision name is updated during reduction. +TEST(SDFParser, FixedJointReductionContactSensorFrameExtensionTest) +{ + sdf::SDFPtr robot(new sdf::SDF()); + sdf::init(robot); + ASSERT_TRUE(sdf::readFile( + GetFullTestFilePath(SDF_TEST_FILE_CONTACT_SENSOR_FRAME_EXTENSION), + robot)); + + sdf::ElementPtr model = robot->Root()->GetElement("model"); + ASSERT_NE(nullptr, model); + + // After reduction the contact sensor should live on base_link. + sdf::ElementPtr link = model->GetElement("link"); + sdf::ElementPtr baseLink; + while (link) + { + if (link->Get("name") == "base_link") + { + baseLink = link; + break; + } + link = link->GetNextElement("link"); + } + ASSERT_NE(nullptr, baseLink); + + sdf::ElementPtr sensor = baseLink->GetElement("sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ("link2_contact_sensor", sensor->Get("name")); + + sdf::ElementPtr contact = sensor->GetElement("contact"); + ASSERT_NE(nullptr, contact); + // link2_collision should be rewritten to base_link_collision_link2 when + // link2 is reduced into base_link. + auto collisionName = contact->Get("collision"); + EXPECT_EQ("base_link_collision_link2", collisionName); +} + +///////////////////////////////////////////////// +// This test uses a urdf that has chained fixed joints with an SDFormat +// gripper embedded in a tag whose gripper_link and palm_link +// name a reduced child link. +// Test to make sure that those link names are updated during reduction. +TEST(SDFParser, FixedJointReductionGripperFrameExtensionTest) +{ + sdf::SDFPtr robot(new sdf::SDF()); + sdf::init(robot); + ASSERT_TRUE(sdf::readFile( + GetFullTestFilePath(SDF_TEST_FILE_GRIPPER_FRAME_EXTENSION), robot)); + + sdf::ElementPtr model = robot->Root()->GetElement("model"); + ASSERT_NE(nullptr, model); + + sdf::ElementPtr gripper = model->GetElement("gripper"); + ASSERT_NE(nullptr, gripper); + EXPECT_EQ("test_gripper", gripper->Get("name")); + + auto gripperLink = gripper->Get("gripper_link"); + auto palmLink = gripper->Get("palm_link"); + EXPECT_EQ("base_link", gripperLink); + EXPECT_EQ("base_link", palmLink); +} + +///////////////////////////////////////////////// +// This test uses a urdf that has chained fixed joints with a plugin that +// contains a link/name reference to a reduced link. +// Test to make sure that the projector link name is updated during reduction. +TEST(SDFParser, FixedJointReductionProjectorFrameExtensionTest) +{ + sdf::SDFPtr robot(new sdf::SDF()); + sdf::init(robot); + ASSERT_TRUE(sdf::readFile( + GetFullTestFilePath(SDF_TEST_FILE_PROJECTOR_FRAME_EXTENSION), robot)); + + sdf::ElementPtr model = robot->Root()->GetElement("model"); + ASSERT_NE(nullptr, model); + + sdf::ElementPtr plugin = model->GetElement("plugin"); + ASSERT_NE(nullptr, plugin); + + auto projector = plugin->Get("projector"); + EXPECT_EQ("base_link/my_projector", projector); +} diff --git a/test/integration/fixed_joint_reduction_contact_sensor_frame_extension.urdf b/test/integration/fixed_joint_reduction_contact_sensor_frame_extension.urdf new file mode 100644 index 000000000..56eecdd24 --- /dev/null +++ b/test/integration/fixed_joint_reduction_contact_sensor_frame_extension.urdf @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100.0 + 1 + + link2_collision + + + + diff --git a/test/integration/fixed_joint_reduction_gripper_frame_extension.urdf b/test/integration/fixed_joint_reduction_gripper_frame_extension.urdf new file mode 100644 index 000000000..a631d8ee5 --- /dev/null +++ b/test/integration/fixed_joint_reduction_gripper_frame_extension.urdf @@ -0,0 +1,98 @@ + + + + + link2 + link2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/integration/fixed_joint_reduction_projector_frame_extension.urdf b/test/integration/fixed_joint_reduction_projector_frame_extension.urdf new file mode 100644 index 000000000..48705f944 --- /dev/null +++ b/test/integration/fixed_joint_reduction_projector_frame_extension.urdf @@ -0,0 +1,97 @@ + + + + + link2/my_projector + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +