Skip to content

Commit

Permalink
Fixed application of <sensor><pose> tags in lumped linkes during URDF…
Browse files Browse the repository at this point in the history 
… conversion (#525)

* Fixed application of <sensor><pose> tags in lumped linkes during URDF conversion.

Fixed <projector>, too.

Signed-off-by: Martin Pecka <[email protected]>

* Changelog

Signed-off-by: Martin Pecka <[email protected]>

* Fixed float equality in test.

Signed-off-by: Martin Pecka <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>
  • Loading branch information
@peci1 @nkoenig
peci1 and nkoenig authored Apr 6, 2021
1 parent dc3d100 commit 87a4992
Show file tree
Hide file tree
Showing 4 changed files with 445 additions and 18 deletions.
3 changes: 3 additions & 0 deletions Migration.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@ but with improved human-readability..

1. Fixed Atmosphere DOM class's temperature default value. Changed from -0.065 to -0.0065.
* [Pull request 482](https:/osrf/sdformat/pull/482)

1. Fixed parsing of `<sensor><pose>` tags on lumped links when converting from URDF.
* [Pull request 525](https:/osrf/sdformat/pull/525)

## SDFormat 9.x to 10.0

Expand Down
123 changes: 105 additions & 18 deletions src/parser_urdf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2125,6 +2125,8 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
for (auto blobIt = (*ge)->blobs.begin();
blobIt != (*ge)->blobs.end(); ++blobIt)
{
// Be sure to always copy only the first element; code in
// ReduceSDFExtensionSensorTransformReduction depends in this behavior
CopyBlob((*blobIt)->FirstChildElement(), _elem);
}
}
Expand Down Expand Up @@ -3354,8 +3356,8 @@ void ReduceSDFExtensionSensorTransformReduction(
std::vector<XMLDocumentPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform)
{
// overwrite <xyz> and <rpy> if they exist
if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "sensor") == 0)
auto sensorElement = (*_blobIt)->FirstChildElement();
if ( strcmp(sensorElement->Name(), "sensor") == 0)
{
// parse it and add/replace the reduction transform
// find first instance of xyz and rpy, replace with reduction transform
Expand All @@ -3369,16 +3371,58 @@ void ReduceSDFExtensionSensorTransformReduction(
// sdfdbg << " " << streamIn.CStr() << "\n";
// }

auto sensorPose {ignition::math::Pose3d::Zero};
{
tinyxml2::XMLNode *oldPoseKey = (*_blobIt)->FirstChildElement("pose");
/// @todo: FIXME: we should read xyz, rpy and aggregate it to
/// reductionTransform instead of just throwing the info away.
auto sensorPosText = "0 0 0 0 0 0";
const auto& oldPoseKey = sensorElement->FirstChildElement("pose");
if (oldPoseKey)
{
(*_blobIt)->DeleteChild(oldPoseKey);
const auto& poseElemXml = oldPoseKey->ToElement();
if (poseElemXml->Attribute("relative_to"))
return;

// see below for explanation; if a sibling element exists, it stores the
// original <sensor><pose> tag content
const auto& sibling = sensorElement->NextSibling();
if (poseElemXml->GetText() && !sibling)
sensorPosText = poseElemXml->GetText();
else if (sibling && sibling->ToElement()->GetText())
sensorPosText = sibling->ToElement()->GetText();
else
{
sdferr << "Unexpected case in sensor pose computation\n";
return;
}

// delete the <pose> tag, we'll add a new one at the end
sensorElement->DeleteChild(oldPoseKey);
}

// parse the 6-tuple text into math::Pose3d
std::stringstream ss;
ss.imbue(std::locale::classic());
ss << sensorPosText;
ss >> sensorPose;
if (ss.fail())
{
sdferr << "Could not parse <sensor><pose>: [" << sensorPosText << "]\n";
return;
}

// critical part: we store the original <pose> tag from <sensor> actually
// as a sibling of <sensor>... only first element of the blob is processed
// further, so its siblings can be used as temporary storage; we store the
// original <pose> tag there so that we can use the <sensor><pose> tag
// for storing the reduced position
auto doc = (*_blobIt)->GetDocument();
const auto& poseTxt = doc->NewText(sensorPosText);
auto poseKey = doc->NewElement("pose");
poseKey->LinkEndChild(poseTxt);
(*_blobIt)->LinkEndChild(poseKey);
}

_reductionTransform = _reductionTransform * sensorPose;

// convert reductionTransform to values
urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
_reductionTransform.Pos().Y(),
Expand All @@ -3403,7 +3447,7 @@ void ReduceSDFExtensionSensorTransformReduction(

poseKey->LinkEndChild(poseTxt);

(*_blobIt)->LinkEndChild(poseKey);
sensorElement->LinkEndChild(poseKey);
}
}

Expand All @@ -3412,8 +3456,8 @@ void ReduceSDFExtensionProjectorTransformReduction(
std::vector<XMLDocumentPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform)
{
// overwrite <pose> (xyz/rpy) if it exists
if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "projector") == 0)
auto projectorElement = (*_blobIt)->FirstChildElement();
if ( strcmp(projectorElement->Name(), "projector") == 0)
{
// parse it and add/replace the reduction transform
// find first instance of xyz and rpy, replace with reduction transform
Expand All @@ -3426,16 +3470,59 @@ void ReduceSDFExtensionProjectorTransformReduction(
// sdfdbg << " " << streamIn << "\n";
// }

// should read <pose>...</pose> and agregate reductionTransform
tinyxml2::XMLNode *poseKey = (*_blobIt)->FirstChildElement("pose");
// read pose and save it

// remove the tag for now
if (poseKey)
auto projectorPose {ignition::math::Pose3d::Zero};
{
(*_blobIt)->DeleteChild(poseKey);
auto projectorPosText = "0 0 0 0 0 0";
const auto& oldPoseKey = projectorElement->FirstChildElement("pose");
if (oldPoseKey)
{
const auto& poseElemXml = oldPoseKey->ToElement();
if (poseElemXml->Attribute("relative_to"))
return;

// see below for explanation; if a sibling element exists, it stores the
// original <projector><pose> tag content
const auto& sibling = projectorElement->NextSibling();
if (poseElemXml->GetText() && !sibling)
projectorPosText = poseElemXml->GetText();
else if (sibling && sibling->ToElement()->GetText())
projectorPosText = sibling->ToElement()->GetText();
else
{
sdferr << "Unexpected case in projector pose computation\n";
return;
}

// delete the <pose> tag, we'll add a new one at the end
projectorElement->DeleteChild(oldPoseKey);
}

// parse the 6-tuple text into math::Pose3d
std::stringstream ss;
ss.imbue(std::locale::classic());
ss << projectorPosText;
ss >> projectorPose;
if (ss.fail())
{
sdferr << "Could not parse <projector><pose>: ["
<< projectorPosText << "]\n";
return;
}

// critical part: we store the original <pose> tag from <projector>
// actually as a sibling of <projector>... only first element of the blob
// is processed further, so its siblings can be used as temporary storage;
// we store the original <pose> tag there so that we can use the
// <projector><pose> tag for storing the reduced position
auto doc = (*_blobIt)->GetDocument();
const auto& poseTxt = doc->NewText(projectorPosText);
auto poseKey = doc->NewElement("pose");
poseKey->LinkEndChild(poseTxt);
(*_blobIt)->LinkEndChild(poseKey);
}

_reductionTransform = _reductionTransform * projectorPose;

// convert reductionTransform to values
urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
_reductionTransform.Pos().Y(),
Expand All @@ -3456,10 +3543,10 @@ void ReduceSDFExtensionProjectorTransformReduction(

auto* doc = (*_blobIt)->GetDocument();
tinyxml2::XMLText *poseTxt = doc->NewText(poseStream.str().c_str());
poseKey = doc->NewElement("pose");
auto poseKey = doc->NewElement("pose");
poseKey->LinkEndChild(poseTxt);

(*_blobIt)->LinkEndChild(poseKey);
projectorElement->LinkEndChild(poseKey);
}
}

Expand Down
159 changes: 159 additions & 0 deletions test/integration/urdf_gazebo_extensions.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,4 +99,163 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
EXPECT_TRUE(physics->Get<bool>("implicit_spring_damper"));
}
}

sdf::ElementPtr link0;
for (sdf::ElementPtr link = model->GetElement("link"); link;
link = link->GetNextElement("link"))
{
const auto& linkName = link->Get<std::string>("name");
if (linkName == "link0")
{
link0 = link;
break;
}
}
ASSERT_TRUE(link0);

bool foundSensorNoPose {false};
bool foundSensorPose {false};
bool foundSensorPoseRelative {false};
bool foundSensorPoseTwoLevel {false};
bool foundIssue378Sensor {false};
bool foundIssue67Sensor {false};

for (sdf::ElementPtr sensor = link0->GetElement("sensor"); sensor;
sensor = sensor->GetNextElement("sensor"))
{
const auto& sensorName = sensor->Get<std::string>("name");
if (sensorName == "sensorNoPose")
{
foundSensorNoPose = true;
ASSERT_TRUE(sensor->HasElement("pose"));
const auto poseElem = sensor->GetElement("pose");

const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;

EXPECT_DOUBLE_EQ(pose.X(), 333.0);
EXPECT_DOUBLE_EQ(pose.Y(), 0.0);
EXPECT_DOUBLE_EQ(pose.Z(), 0.0);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.0);
EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0);
EXPECT_NEAR(pose.Yaw(), IGN_PI_2, 1e-5);

EXPECT_FALSE(poseElem->GetNextElement("pose"));
}
else if (sensorName == "sensorPose")
{
foundSensorPose = true;
ASSERT_TRUE(sensor->HasElement("pose"));
const auto poseElem = sensor->GetElement("pose");

const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;

EXPECT_DOUBLE_EQ(pose.X(), 333.0);
EXPECT_DOUBLE_EQ(pose.Y(), 111.0);
EXPECT_DOUBLE_EQ(pose.Z(), 0.0);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.0);
EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0);
EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5);

EXPECT_FALSE(poseElem->GetNextElement("pose"));
}
else if (sensorName == "sensorPoseRelative")
{
foundSensorPoseRelative = true;
ASSERT_TRUE(sensor->HasElement("pose"));
const auto poseElem = sensor->GetElement("pose");

const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;

EXPECT_DOUBLE_EQ(pose.X(), 111.0);
EXPECT_DOUBLE_EQ(pose.Y(), 0.0);
EXPECT_DOUBLE_EQ(pose.Z(), 0.0);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.0);
EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0);
EXPECT_NEAR(pose.Yaw(), -1, 1e-5);

EXPECT_FALSE(poseElem->GetNextElement("pose"));
}
else if (sensorName == "sensorPoseTwoLevel")
{
foundSensorPoseTwoLevel = true;
ASSERT_TRUE(sensor->HasElement("pose"));
const auto poseElem = sensor->GetElement("pose");

const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;

EXPECT_DOUBLE_EQ(pose.X(), 333.0);
EXPECT_DOUBLE_EQ(pose.Y(), 111.0);
EXPECT_DOUBLE_EQ(pose.Z(), 222.0);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.0);
EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0);
EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5);

EXPECT_FALSE(poseElem->GetNextElement("pose"));
}
else if (sensorName == "issue378_sensor")
{
foundIssue378Sensor = true;
ASSERT_TRUE(sensor->HasElement("pose"));
const auto poseElem = sensor->GetElement("pose");

const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;

EXPECT_DOUBLE_EQ(pose.X(), 1);
EXPECT_DOUBLE_EQ(pose.Y(), 2);
EXPECT_DOUBLE_EQ(pose.Z(), 3);
EXPECT_DOUBLE_EQ(pose.Roll(), 0.1);
EXPECT_DOUBLE_EQ(pose.Pitch(), 0.2);
EXPECT_DOUBLE_EQ(pose.Yaw(), 0.3);

EXPECT_FALSE(poseElem->GetNextElement("pose"));
}
else if (sensorName == "issue67_sensor")
{
foundIssue67Sensor = true;
ASSERT_TRUE(sensor->HasElement("pose"));
const auto poseElem = sensor->GetElement("pose");

const auto& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;

EXPECT_GT(std::abs(pose.X() - (-0.20115)), 0.1);
EXPECT_GT(std::abs(pose.Y() - 0.42488), 0.1);
EXPECT_GT(std::abs(pose.Z() - 0.30943), 0.1);
EXPECT_GT(std::abs(pose.Roll() - 1.5708), 0.1);
EXPECT_GT(std::abs(pose.Pitch() - (-0.89012)), 0.1);
EXPECT_GT(std::abs(pose.Yaw() - 1.5708), 0.1);

EXPECT_FALSE(poseElem->GetNextElement("pose"));
}
}

EXPECT_TRUE(foundSensorNoPose);
EXPECT_TRUE(foundSensorPose);
EXPECT_TRUE(foundSensorPoseRelative);
EXPECT_TRUE(foundSensorPoseTwoLevel);
EXPECT_TRUE(foundIssue378Sensor);
EXPECT_TRUE(foundIssue67Sensor);
}
Loading

0 comments on commit 87a4992

Please sign in to comment.