Skip to content

Commit

Permalink
urdf: fix sensor/light pose for links lumped by fixed joints (#1114)
Browse files Browse the repository at this point in the history 
When converting a fixed joint in a URDF file to SDFormat,
by default the contents of the child link are merged into
the parent link, but poses in gazebo extensions such as
//gazebo/sensor/pose are not properly transformed (#378).
This issue has already been fixed in newer branches by #525,
so the test and that fix have been backported and refactored to
reduce code duplication (about 30 lines removed from parser_urdf.cc)
and also fix the treatment of //gazebo/light/pose.

Fixes #378.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@scpeters
scpeters authored Aug 24, 2022
1 parent 3e76d92 commit 2e891c2
Show file tree
Hide file tree
Showing 4 changed files with 446 additions and 90 deletions.
9 changes: 9 additions & 0 deletions Migration.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,15 @@ forward programmatically.
This document aims to contain similar information to those files
but with improved human-readability..

## libsdformat 9.8.0 to 9.8.1

### Modifications

1. URDF parser now properly transforms poses for lights, projectors and sensors
from gazebo extension tags that are moved to a new link during fixed joint
reduction.
+ [pull request 1114](https://github.com/osrf/sdformat/pull/1114)

## libsdformat 9.4 to 9.5

### Additions
Expand Down
148 changes: 58 additions & 90 deletions src/parser_urdf.cc
View file
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
Loading

0 comments on commit 2e891c2

Please sign in to comment.