Skip to content

Fix remaining ReduceSDFExtension*FrameReplace tinyxml2 bugs #1691

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
trippyogi wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Fix remaining ReduceSDFExtension*FrameReplace tinyxml2 bugs #1691

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4ace0b5
Fix remaining ReduceSDFExtension*FrameReplace tinyxml2 bugs
trippyogi Aug 3, 2026
1213db9
Merge branch 'main' into fix/746-reducesdf-frame-replace-leftovers
trippyogi Aug 12, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 30 additions & 25 deletions src/parser_urdf.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<XMLDocumentPtr>::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<XMLDocumentPtr>::iterator _blobIt,
tinyxml2::XMLElement *_blob,
urdf::LinkSharedPtr _link);

/// reduced fixed joints: apply appropriate frame updates in plugins
Expand All @@ -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<XMLDocumentPtr>::iterator _blobIt,
tinyxml2::XMLElement *_blob,
urdf::LinkSharedPtr _link);

/// \brief reduced fixed joints: apply appropriate updates to urdf
Expand Down Expand Up @@ -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);
}
}
Expand Down Expand Up @@ -3568,16 +3571,16 @@ void ReduceSDFExtensionElementTransformReduction(

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionContactSensorFrameReplace(
std::vector<XMLDocumentPtr>::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");
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -3707,7 +3711,7 @@ void ReduceSDFExtensionPluginFrameReplace(

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionProjectorFrameReplace(
std::vector<XMLDocumentPtr>::iterator _blobIt,
tinyxml2::XMLElement *_blob,
urdf::LinkSharedPtr _link)
{
std::string linkName = _link->name;
Expand All @@ -3717,7 +3721,7 @@ void ReduceSDFExtensionProjectorFrameReplace(
// projector plugins
// update from <projector>MyLinkName/MyProjectorName</projector>
// to <projector>NewLinkName/MyProjectorName</projector>
tinyxml2::XMLNode *projectorElem = (*_blobIt)->FirstChildElement("projector");
tinyxml2::XMLNode *projectorElem = _blob->FirstChildElement("projector");
{
if (projectorElem)
{
Expand All @@ -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);
}
}
}
Expand All @@ -3757,46 +3762,46 @@ void ReduceSDFExtensionProjectorFrameReplace(

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionGripperFrameReplace(
std::vector<XMLDocumentPtr>::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;
bodyNameStream << parentLinkName;
tinyxml2::XMLText *bodyNameTxt =
doc->NewText(bodyNameStream.str().c_str());
bodyNameKey->LinkEndChild(bodyNameTxt);
(*_blobIt)->LinkEndChild(bodyNameKey);
_blob->LinkEndChild(bodyNameKey);
}
}
}
Expand Down
93 changes: 93 additions & 0 deletions test/integration/fixed_joint_reduction.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -794,3 +800,90 @@ TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest)
bool correctedOffset = plugin->Get<bool>("gz::corrected_offsets");
EXPECT_TRUE(correctedOffset);
}

/////////////////////////////////////////////////
// This test uses a urdf that has chained fixed joints with a contact
// sensor whose <collision> 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<std::string>("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<std::string>("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<std::string>("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 <gazebo> 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<std::string>("name"));

auto gripperLink = gripper->Get<std::string>("gripper_link");
auto palmLink = gripper->Get<std::string>("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 <projector>link/name</projector> 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<std::string>("projector");
EXPECT_EQ("base_link/my_projector", projector);
}
69 changes: 69 additions & 0 deletions test/integration/fixed_joint_reduction_contact_sensor_frame_extension.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,69 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="chained_fixed_joint_links">
<!-- Base Link -->
<link name="base_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Link 2 -->
<link name="link2">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Joint 2 -->
<joint name="joint2" type="fixed">
<parent link="base_link"/>
<child link="link2"/>
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>

<gazebo reference="link2">
<sensor name="link2_contact_sensor" type="contact">
<update_rate>100.0</update_rate>
<always_on>1</always_on>
<contact>
<collision>link2_collision</collision>
</contact>
</sensor>
</gazebo>
</robot>
Loading