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

Merge sdf9 ➡️ sdf12 #1119

Merged
merged 2 commits into from
Aug 27, 2022
+210 −277
Merged

Merge sdf9 ➡️ sdf12 #1119

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2e891c2
urdf: fix sensor/light pose for links lumped by fixed joints (#1114)
scpeters Aug 24, 2022
d095680
Merge branch 'sdf9' into scpeters/merge_9_12
scpeters Aug 26, 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
9 changes: 9 additions & 0 deletions Migration.md
View file
Original file line number Diff line number Diff line change
@@ -283,6 +283,15 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1.
+ std::optional<std::string> GetMaxValueAsString() const;
+ bool ValidateValue() const;

## 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
200 changes: 41 additions & 159 deletions src/parser_urdf.cc
View file
Original file line number Diff line number Diff line change
@@ -91,17 +91,12 @@ void InsertSDFExtensionJoint(tinyxml2::XMLElement *_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<XMLDocumentPtr>::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<XMLDocumentPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform);
const ignition::math::Pose3d &_reductionTransform,
const std::string &_elementName);


/// reduced fixed joints: apply transform reduction to extensions
@@ -409,8 +404,8 @@ void ReduceVisualToParent(urdf::LinkSharedPtr _parentLink,
// collision elements of the child link into the parent link
void ReduceFixedJoints(tinyxml2::XMLElement *_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))
@@ -2479,8 +2474,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));
@@ -2568,11 +2562,12 @@ void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge)
for (auto 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");
}
}

@@ -3364,12 +3359,13 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt)
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionSensorTransformReduction(
void ReduceSDFExtensionElementTransformReduction(
std::vector<XMLDocumentPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform)
const ignition::math::Pose3d &_reductionTransform,
const std::string &_elementName)
{
auto sensorElement = (*_blobIt)->FirstChildElement();
if ( strcmp(sensorElement->Name(), "sensor") == 0)
auto element = (*_blobIt)->FirstChildElement();
if ( strcmp(element->Name(), _elementName.c_str()) == 0)
{
// parse it and add/replace the reduction transform
// find first instance of xyz and rpy, replace with reduction transform
@@ -3383,166 +3379,51 @@ void ReduceSDFExtensionSensorTransformReduction(
// sdfdbg << " " << streamIn.CStr() << "\n";
// }

auto sensorPose {ignition::math::Pose3d::Zero};
auto pose {ignition::math::Pose3d::Zero};
{
auto sensorPosText = "0 0 0 0 0 0";
const auto& oldPoseKey = sensorElement->FirstChildElement("pose");
std::string poseText = "0 0 0 0 0 0";

const auto& oldPoseKey = element->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 <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(),
_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;

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

poseKey->LinkEndChild(poseTxt);

sensorElement->LinkEndChild(poseKey);
}
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionProjectorTransformReduction(
std::vector<XMLDocumentPtr>::iterator _blobIt,
ignition::math::Pose3d _reductionTransform)
{
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
//
// for (tinyxml2::XMLNode* elIt = (*_blobIt)->FirstChildElement();
// elIt; elIt = elIt->NextSibling())
// {
// std::ostringstream streamIn;
// streamIn << *elIt;
// sdfdbg << " " << streamIn << "\n";
// }

auto projectorPose {ignition::math::Pose3d::Zero};
{
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
if (poseElemXml->GetText())
{
sdferr << "Unexpected case in projector pose computation\n";
return;
poseText = poseElemXml->GetText();
}

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

// parse the 6-tuple text into math::Pose3d
std::stringstream ss;
std::stringstream ss;
ss.imbue(std::locale::classic());
ss << projectorPosText;
ss >> projectorPose;
ss << poseText;
ss >> pose;
if (ss.fail())
{
sdferr << "Could not parse <projector><pose>: ["
<< projectorPosText << "]\n";
sdferr << "Could not parse <" << _elementName << "><pose>: ["
<< poseText << "]\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;
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);
@@ -3555,10 +3436,11 @@ void ReduceSDFExtensionProjectorTransformReduction(

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

poseKey->LinkEndChild(poseTxt);

projectorElement->LinkEndChild(poseKey);
element->LinkEndChild(poseKey);
}
}

241 changes: 124 additions & 117 deletions test/integration/urdf_gazebo_extensions.cc
View file
Original file line number Diff line number Diff line change
@@ -142,149 +142,156 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
}
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"))
auto checkElementPoses = [&](const std::string &_elementName) -> void
{
const auto& sensorName = sensor->Get<std::string>("name");
if (sensorName == "sensorNoPose")
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))
{
foundSensorNoPose = true;
ASSERT_TRUE(sensor->HasElement("pose"));
const auto poseElem = sensor->GetElement("pose");
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& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;
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_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");
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& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;
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_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");
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& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;
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_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");
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& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;
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_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");
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& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;
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_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");
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& posePair = poseElem->Get<ignition::math::Pose3d>(
"", ignition::math::Pose3d::Zero);
ASSERT_TRUE(posePair.second);

const auto& pose = posePair.first;
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_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_FALSE(poseElem->GetNextElement("pose"));
}
}
}

EXPECT_TRUE(foundSensorNoPose);
EXPECT_TRUE(foundSensorPose);
EXPECT_TRUE(foundSensorPoseRelative);
EXPECT_TRUE(foundSensorPoseTwoLevel);
EXPECT_TRUE(foundIssue378Sensor);
EXPECT_TRUE(foundIssue67Sensor);
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");
}
37 changes: 36 additions & 1 deletion test/integration/urdf_gazebo_extensions.urdf
View file
Original file line number Diff line number Diff line change
@@ -165,6 +165,8 @@
<child link="linkSensorNoPose"/>
</joint>
<gazebo reference="linkSensorNoPose">
<light name="lightNoPose" type="point"/>
<projector name="projectorNoPose"/>
<sensor name="sensorNoPose" type="camera">
<update_rate>6.0</update_rate>
<camera name="cam">
@@ -195,6 +197,12 @@
<child link="linkSensorPose"/>
</joint>
<gazebo reference="linkSensorPose">
<light name="lightPose" type="point">
<pose>111 0 0 0 0 -1</pose>
</light>
<projector name="projectorPose">
<pose>111 0 0 0 0 -1</pose>
</projector>
<sensor name="sensorPose" type="camera">
<pose>111 0 0 0 0 -1</pose>
<update_rate>6.0</update_rate>
@@ -226,6 +234,12 @@
<child link="linkSensorPoseRelative"/>
</joint>
<gazebo reference="linkSensorPoseRelative">
<light name="lightPoseRelative" type="point">
<pose relative_to="link0">111 0 0 0 0 -1</pose>
</light>
<projector name="projectorPoseRelative">
<pose relative_to="link0">111 0 0 0 0 -1</pose>
</projector>
<sensor name="sensorPoseRelative" type="camera">
<pose relative_to="link0">111 0 0 0 0 -1</pose>
<update_rate>6.0</update_rate>
@@ -269,8 +283,17 @@
<child link="linkSensorPoseTwoLevel2"/>
</joint>
<gazebo reference="linkSensorPoseTwoLevel2">
<light name="lightPoseTwoLevel" type="point">
<!-- TwoLevel to link0 -->
<pose>111 0 0 0 0 -1</pose>
</light>
<projector name="projectorPoseTwoLevel">
<!-- TwoLevel to link0 -->
<pose>111 0 0 0 0 -1</pose>
</projector>
<sensor name="sensorPoseTwoLevel" type="camera">
<pose twoLevel_to="link0">111 0 0 0 0 -1</pose>
<!-- TwoLevel to link0 -->
<pose>111 0 0 0 0 -1</pose>
<update_rate>6.0</update_rate>
<camera name="cam">
<horizontal_fov>1.36869112579</horizontal_fov>
@@ -289,6 +312,12 @@

<!-- Issue 378 setting -->
<gazebo reference="issue378_link">
<light name="issue378_light" type="point">
<pose>1.0 2.0 3.0 0.1 0.2 0.3</pose>
</light>
<projector name="issue378_projector">
<pose>1.0 2.0 3.0 0.1 0.2 0.3</pose>
</projector>
<sensor name="issue378_sensor" type="imu">
<always_on>1</always_on>
<update_rate>400</update_rate>
@@ -320,6 +349,12 @@
<axis xyz="0 0 0"/>
</joint>
<gazebo reference="Camera">
<light name="issue67_light" type="point">
<pose>1 1 1 1.570796 1.570796 1.570796</pose>
</light>
<projector name="issue67_projector">
<pose>1 1 1 1.570796 1.570796 1.570796</pose>
</projector>
<sensor name="issue67_sensor" type="camera">
<visualize>true</visualize>
<pose>1 1 1 1.570796 1.570796 1.570796</pose>