diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 3187c568f..b1d3ecf91 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -183,7 +183,7 @@ void ReduceSDFExtensionProjectorFrameReplace( /// reduced fixed joints: apply appropriate frame updates in plugins /// inside urdf extensions when doing fixed joint reduction void ReduceSDFExtensionPluginFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link, const std::string &_pluginName, const std::string &_elementName, ignition::math::Pose3d _reductionTransform); @@ -2579,12 +2579,12 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge, << debugStreamIn.CStr() << "]\n"; ReduceSDFExtensionContactSensorFrameReplace(blobIt, _link); - ReduceSDFExtensionPluginFrameReplace(blobIt, _link, - "plugin", "bodyName", - _ge->reductionTransform); - ReduceSDFExtensionPluginFrameReplace(blobIt, _link, - "plugin", "frameName", - _ge->reductionTransform); + ReduceSDFExtensionPluginFrameReplace( + (*blobIt)->FirstChildElement(), _link, "plugin", "bodyName", + _ge->reductionTransform); + ReduceSDFExtensionPluginFrameReplace( + (*blobIt)->FirstChildElement(), _link, "plugin", "frameName", + _ge->reductionTransform); ReduceSDFExtensionProjectorFrameReplace(blobIt, _link); ReduceSDFExtensionGripperFrameReplace(blobIt, _link); ReduceSDFExtensionJointFrameReplace(blobIt, _link); @@ -3581,24 +3581,24 @@ void ReduceSDFExtensionContactSensorFrameReplace( //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionPluginFrameReplace( - std::vector::iterator _blobIt, + tinyxml2::XMLElement *_blob, urdf::LinkSharedPtr _link, const std::string &_pluginName, const std::string &_elementName, ignition::math::Pose3d _reductionTransform) { std::string linkName = _link->name; std::string parentLinkName = _link->getParent()->name; - if ((*_blobIt)->FirstChildElement()->Name() == _pluginName) + if (_blob->Name() == _pluginName) { // replace element containing _link names to parent link names // find first instance of xyz and rpy, replace with reduction transform tinyxml2::XMLNode *elementNode = - (*_blobIt)->FirstChildElement(_elementName.c_str()); + _blob->FirstChildElement(_elementName.c_str()); if (elementNode) { if (GetKeyValueAsString(elementNode->ToElement()) == linkName) { - (*_blobIt)->DeleteChild(elementNode); + _blob->DeleteChild(elementNode); auto* doc = elementNode->GetDocument(); tinyxml2::XMLElement *bodyNameKey = doc->NewElement(_elementName.c_str()); @@ -3607,27 +3607,27 @@ void ReduceSDFExtensionPluginFrameReplace( tinyxml2::XMLText *bodyNameTxt = doc->NewText(bodyNameStream.str().c_str()); bodyNameKey->LinkEndChild(bodyNameTxt); - (*_blobIt)->LinkEndChild(bodyNameKey); + _blob->LinkEndChild(bodyNameKey); /// @todo update transforms for this sdf plugin too // look for offset transforms, add reduction transform - tinyxml2::XMLNode *xyzKey = (*_blobIt)->FirstChildElement("xyzOffset"); + tinyxml2::XMLNode *xyzKey = _blob->FirstChildElement("xyzOffset"); if (xyzKey) { urdf::Vector3 v1 = ParseVector3(xyzKey); _reductionTransform.Pos() = ignition::math::Vector3d(v1.x, v1.y, v1.z); // remove xyzOffset and rpyOffset - (*_blobIt)->DeleteChild(xyzKey); + _blob->DeleteChild(xyzKey); } - tinyxml2::XMLNode *rpyKey = (*_blobIt)->FirstChildElement("rpyOffset"); + tinyxml2::XMLNode *rpyKey = _blob->FirstChildElement("rpyOffset"); if (rpyKey) { urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0); _reductionTransform.Rot() = ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z); // remove xyzOffset and rpyOffset - (*_blobIt)->DeleteChild(rpyKey); + _blob->DeleteChild(rpyKey); } // pass through the parent transform from fixed joint reduction @@ -3661,8 +3661,8 @@ void ReduceSDFExtensionPluginFrameReplace( xyzKey->LinkEndChild(xyzTxt); rpyKey->LinkEndChild(rpyTxt); - (*_blobIt)->LinkEndChild(xyzKey); - (*_blobIt)->LinkEndChild(rpyKey); + _blob->LinkEndChild(xyzKey); + _blob->LinkEndChild(rpyKey); } } } diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc index 0475db78b..d1780b0d9 100644 --- a/test/integration/fixed_joint_reduction.cc +++ b/test/integration/fixed_joint_reduction.cc @@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] = "fixed_joint_reduction_collision_visual_empty_root.urdf"; const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] = "fixed_joint_reduction_collision_visual_empty_root.sdf"; +const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] = + "fixed_joint_reduction_plugin_frame_extension.urdf"; static std::string GetFullTestFilePath(const char *_input) { @@ -734,3 +736,21 @@ TEST(SDFParser, FixedJointReductionSimple) EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance); } } + +///////////////////////////////////////////////// +// This test uses a urdf that has chained fixed joints with plugin that +// contains bodyName, xyzOffset and rpyOffset. +// Test to make sure that the bodyName is updated to the new base link. +TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest) +{ + sdf::SDFPtr robot(new sdf::SDF()); + sdf::init(robot); + ASSERT_TRUE(sdf::readFile( + GetFullTestFilePath(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION), robot)); + + sdf::ElementPtr model = robot->Root()->GetElement("model"); + sdf::ElementPtr plugin = model->GetElement("plugin"); + + auto bodyName = plugin->Get("bodyName"); + EXPECT_EQ("base_link", bodyName); +} diff --git a/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf new file mode 100644 index 000000000..4c2293670 --- /dev/null +++ b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf @@ -0,0 +1,103 @@ + + + + + /test/plugin/service + /test/plugin/topic + link2 + 100 + 0 0 0 + 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +