Skip to content
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

urdf: fix sensor/light pose for links lumped by fixed joints #1114

Merged
merged 7 commits into from
Aug 24, 2022
Merged

urdf: fix sensor/light pose for links lumped by fixed joints #1114

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
16ff849
Backport test for URDF sensor poses
scpeters Aug 19, 2022
400f3fa
Refactor duplicate functions into shared function
scpeters Aug 20, 2022
4fb726e
Fix comment
scpeters Aug 22, 2022
2986a27
Add element variable instead of (*blobIt)
scpeters Aug 22, 2022
aa0a15c
Apply pose transform if specified
scpeters Aug 22, 2022
bb8be15
Fix //light/pose as well
scpeters Aug 23, 2022
7657bbe
Update Migration guide
scpeters Aug 24, 2022
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
148 changes: 58 additions & 90 deletions src/parser_urdf.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,17 +87,12 @@ void InsertSDFExtensionJoint(TiXmlElement *_elem,
/// option is set
bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt);

/// reduced fixed joints: apply transform reduction for ray sensors
/// reduced fixed joints: apply transform reduction for named elements
/// in extensions when doing fixed joint reduction
void ReduceSDFExtensionSensorTransformReduction(
void ReduceSDFExtensionElementTransformReduction(
std::vector<TiXmlElementPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform);

/// reduced fixed joints: apply transform reduction for projectors in
/// extensions when doing fixed joint reduction
void ReduceSDFExtensionProjectorTransformReduction(
std::vector<TiXmlElementPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform);
const ignition::math::Pose3d &_reductionTransform,
const std::string &_elementName);


/// reduced fixed joints: apply transform reduction to extensions
Expand Down Expand Up @@ -400,8 +395,8 @@ void ReduceVisualToParent(urdf::LinkSharedPtr _parentLink,
// collision elements of the child link into the parent link
void ReduceFixedJoints(TiXmlElement *_root, urdf::LinkSharedPtr _link)
{
// if child is attached to self by fixed _link first go up the tree,
// check it's children recursively
// if child is attached to self by fixed joint first go up the tree,
// check its children recursively
for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
{
if (FixedJointShouldBeReduced(_link->child_links[i]->parent_joint))
Expand Down Expand Up @@ -2453,8 +2448,7 @@ void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link)
for (std::vector<SDFExtensionPtr>::iterator ge = ext->second.begin();
ge != ext->second.end(); ++ge)
{
(*ge)->reductionTransform = TransformToParentFrame(
(*ge)->reductionTransform,
(*ge)->reductionTransform = CopyPose(
_link->parent_joint->parent_to_joint_origin_transform);
// for sensor and projector blocks only
ReduceSDFExtensionsTransform((*ge));
Expand Down Expand Up @@ -2545,11 +2539,12 @@ void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge)
for (std::vector<TiXmlElementPtr>::iterator blobIt = _ge->blobs.begin();
blobIt != _ge->blobs.end(); ++blobIt)
{
/// @todo make sure we are not missing any additional transform reductions
ReduceSDFExtensionSensorTransformReduction(blobIt,
_ge->reductionTransform);
ReduceSDFExtensionProjectorTransformReduction(blobIt,
_ge->reductionTransform);
ReduceSDFExtensionElementTransformReduction(
blobIt, _ge->reductionTransform, "light");
ReduceSDFExtensionElementTransformReduction(
blobIt, _ge->reductionTransform, "projector");
ReduceSDFExtensionElementTransformReduction(
blobIt, _ge->reductionTransform, "sensor");
}
}

Expand Down Expand Up @@ -3282,12 +3277,13 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt)
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionSensorTransformReduction(
void ReduceSDFExtensionElementTransformReduction(
std::vector<TiXmlElementPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform)
const ignition::math::Pose3d &_reductionTransform,
const std::string &_elementName)
{
// overwrite <xyz> and <rpy> if they exist
if ((*_blobIt)->ValueStr() == "sensor")
auto element = *_blobIt;
if (element->ValueStr() == _elementName)
{
// parse it and add/replace the reduction transform
// find first instance of xyz and rpy, replace with reduction transform
Expand All @@ -3298,82 +3294,54 @@ void ReduceSDFExtensionSensorTransformReduction(
// {
// std::ostringstream streamIn;
// streamIn << *elIt;
// sdfdbg << " " << streamIn << "\n";
// sdfdbg << " " << streamIn.str() << "\n";
// }

auto pose {ignition::math::Pose3d::Zero};
{
TiXmlNode* oldPoseKey = (*_blobIt)->FirstChild("pose");
/// @todo: FIXME: we should read xyz, rpy and aggregate it to
/// reductionTransform instead of just throwing the info away.
std::string poseText = "0 0 0 0 0 0";

TiXmlNode* oldPoseKey = element->FirstChild("pose");
if (oldPoseKey)
{
(*_blobIt)->RemoveChild(oldPoseKey);
}
}

// convert reductionTransform to values
urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
_reductionTransform.Pos().Y(),
_reductionTransform.Pos().Z());
urdf::Rotation reductionQ(_reductionTransform.Rot().X(),
_reductionTransform.Rot().Y(),
_reductionTransform.Rot().Z(),
_reductionTransform.Rot().W());

urdf::Vector3 reductionRpy;
reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z);

// output updated pose to text
std::ostringstream poseStream;
poseStream << reductionXyz.x << " " << reductionXyz.y
<< " " << reductionXyz.z << " " << reductionRpy.x
<< " " << reductionRpy.y << " " << reductionRpy.z;
TiXmlText* poseTxt = new TiXmlText(poseStream.str());

TiXmlElement* poseKey = new TiXmlElement("pose");
poseKey->LinkEndChild(poseTxt);

(*_blobIt)->LinkEndChild(poseKey);
}
}
const auto& poseElemXml = oldPoseKey->ToElement();
if (poseElemXml->Attribute("relative_to"))
{
return;
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionProjectorTransformReduction(
std::vector<TiXmlElementPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform)
{
// overwrite <pose> (xyz/rpy) if it exists
if ((*_blobIt)->ValueStr() == "projector")
{
// parse it and add/replace the reduction transform
// find first instance of xyz and rpy, replace with reduction transform
//
// for (TiXmlNode* elIt = (*_blobIt)->FirstChild();
// elIt; elIt = elIt->NextSibling())
// {
// std::ostringstream streamIn;
// streamIn << *elIt;
// sdfdbg << " " << streamIn << "\n";
// }
if (poseElemXml->GetText())
{
poseText = poseElemXml->GetText();
}

// should read <pose>...</pose> and agregate reductionTransform
TiXmlNode* poseKey = (*_blobIt)->FirstChild("pose");
// read pose and save it
// delete the <pose> tag, we'll add a new one at the end
element->RemoveChild(oldPoseKey);
}

// remove the tag for now
if (poseKey)
{
(*_blobIt)->RemoveChild(poseKey);
// parse the 6-tuple text into math::Pose3d
std::stringstream ss;
ss.imbue(std::locale::classic());
ss << poseText;
ss >> pose;
if (ss.fail())
{
sdferr << "Could not parse <" << _elementName << "><pose>: ["
<< poseText << "]\n";
return;
}
}

pose = _reductionTransform * pose;

// convert reductionTransform to values
urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
_reductionTransform.Pos().Y(),
_reductionTransform.Pos().Z());
urdf::Rotation reductionQ(_reductionTransform.Rot().X(),
_reductionTransform.Rot().Y(),
_reductionTransform.Rot().Z(),
_reductionTransform.Rot().W());
urdf::Vector3 reductionXyz(pose.Pos().X(),
pose.Pos().Y(),
pose.Pos().Z());
urdf::Rotation reductionQ(pose.Rot().X(),
pose.Rot().Y(),
pose.Rot().Z(),
pose.Rot().W());

urdf::Vector3 reductionRpy;
reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z);
Expand All @@ -3383,12 +3351,12 @@ void ReduceSDFExtensionProjectorTransformReduction(
poseStream << reductionXyz.x << " " << reductionXyz.y
<< " " << reductionXyz.z << " " << reductionRpy.x
<< " " << reductionRpy.y << " " << reductionRpy.z;
TiXmlText* poseTxt = new TiXmlText(poseStream.str());
TiXmlText* poseTxt = new TiXmlText(poseStream.str().c_str());

poseKey = new TiXmlElement("pose");
TiXmlElement* poseKey = new TiXmlElement("pose");
poseKey->LinkEndChild(poseTxt);

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

Expand Down
166 changes: 166 additions & 0 deletions test/integration/urdf_gazebo_extensions.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -135,4 +135,170 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
EXPECT_FALSE(link->HasElement("velocity_decay"));
}
}

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);

auto checkElementPoses = [&](const std::string &_elementName) -> void
{
bool foundElementNoPose {false};
bool foundElementPose {false};
bool foundElementPoseRelative {false};
bool foundElementPoseTwoLevel {false};
bool foundIssue378Element {false};
bool foundIssue67Element {false};

for (sdf::ElementPtr element = link0->GetElement(_elementName); element;
element = element->GetNextElement(_elementName))
{
const auto& elementName = element->Get<std::string>("name");
if (elementName == _elementName + "NoPose")
{
foundElementNoPose = true;
EXPECT_TRUE(element->HasElement("pose"));
const auto poseElem = element->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 (elementName == _elementName + "Pose")
{
foundElementPose = true;
EXPECT_TRUE(element->HasElement("pose"));
const auto poseElem = element->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 (elementName == _elementName + "PoseRelative")
{
foundElementPoseRelative = true;
EXPECT_TRUE(element->HasElement("pose"));
const auto poseElem = element->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 (elementName == _elementName + "PoseTwoLevel")
{
foundElementPoseTwoLevel = true;
EXPECT_TRUE(element->HasElement("pose"));
const auto poseElem = element->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 (elementName == "issue378_" + _elementName)
{
foundIssue378Element = true;
EXPECT_TRUE(element->HasElement("pose"));
const auto poseElem = element->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 (elementName == "issue67_" + _elementName)
{
foundIssue67Element = true;
EXPECT_TRUE(element->HasElement("pose"));
const auto poseElem = element->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(foundElementNoPose) << _elementName;
EXPECT_TRUE(foundElementPose) << _elementName;
EXPECT_TRUE(foundElementPoseRelative) << _elementName;
EXPECT_TRUE(foundElementPoseTwoLevel) << _elementName;
EXPECT_TRUE(foundIssue378Element) << _elementName;
EXPECT_TRUE(foundIssue67Element) << _elementName;
};

checkElementPoses("light");
checkElementPoses("projector");
checkElementPoses("sensor");
}
Loading