From 16ff8495982630ee90eb00f9c5914603e1427b82 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 19 Aug 2022 16:43:53 -0700 Subject: [PATCH 1/7] Backport test for URDF sensor poses Backported from #525. Signed-off-by: Steve Peters --- test/integration/urdf_gazebo_extensions.cc | 159 +++++++++++++++++ test/integration/urdf_gazebo_extensions.urdf | 178 +++++++++++++++++++ 2 files changed, 337 insertions(+) diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 36d0d8603..1a33e4667 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -135,4 +135,163 @@ 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("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("name"); + if (sensorName == "sensorNoPose") + { + foundSensorNoPose = true; + EXPECT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", 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; + EXPECT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", 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; + EXPECT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", 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; + EXPECT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", 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; + EXPECT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", 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; + EXPECT_TRUE(sensor->HasElement("pose")); + const auto poseElem = sensor->GetElement("pose"); + + const auto& posePair = poseElem->Get( + "", 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); } diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index fc7216a16..1daf5a8cd 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -150,4 +150,182 @@ + + + + + + + + + + + + + + + + + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + 1 + 400 + 1.0 2.0 3.0 0.1 0.2 0.3 + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 1 1 1 1.570796 1.570796 1.570796 + + + + + From 400f3faa7ab71ea873f996b256703e00f70ee1c4 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 19 Aug 2022 22:43:38 -0700 Subject: [PATCH 2/7] Refactor duplicate functions into shared function Add ReduceSDFExtensionElementTransformReduction with an extra std::string argument to replace ReduceSDFExtensionSensorTransformReduction and ReduceSDFExtensionProjectorTransformReduction. Signed-off-by: Steve Peters --- src/parser_urdf.cc | 85 +++++++--------------------------------------- 1 file changed, 13 insertions(+), 72 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 2f9501bd3..8b2aeb7bc 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -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::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::iterator _blobIt, - ignition::math::Pose3d _reductionTransform); + const ignition::math::Pose3d &_reductionTransform, + const std::string &_elementName); /// reduced fixed joints: apply transform reduction to extensions @@ -2546,10 +2541,10 @@ void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge) 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, "sensor"); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "projector"); } } @@ -3282,12 +3277,13 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt) } //////////////////////////////////////////////////////////////////////////////// -void ReduceSDFExtensionSensorTransformReduction( +void ReduceSDFExtensionElementTransformReduction( std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform) + const ignition::math::Pose3d &_reductionTransform, + const std::string &_elementName) { // overwrite and if they exist - if ((*_blobIt)->ValueStr() == "sensor") + if ((*_blobIt)->ValueStr() == _elementName) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform @@ -3298,7 +3294,7 @@ void ReduceSDFExtensionSensorTransformReduction( // { // std::ostringstream streamIn; // streamIn << *elIt; - // sdfdbg << " " << streamIn << "\n"; + // sdfdbg << " " << streamIn.str() << "\n"; // } { @@ -3337,61 +3333,6 @@ void ReduceSDFExtensionSensorTransformReduction( } } -//////////////////////////////////////////////////////////////////////////////// -void ReduceSDFExtensionProjectorTransformReduction( - std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform) -{ - // overwrite (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"; - // } - - // should read ... and agregate reductionTransform - TiXmlNode* poseKey = (*_blobIt)->FirstChild("pose"); - // read pose and save it - - // remove the tag for now - if (poseKey) - { - (*_blobIt)->RemoveChild(poseKey); - } - - // 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()); - - poseKey = new TiXmlElement("pose"); - poseKey->LinkEndChild(poseTxt); - - (*_blobIt)->LinkEndChild(poseKey); - } -} - //////////////////////////////////////////////////////////////////////////////// void ReduceSDFExtensionContactSensorFrameReplace( std::vector::iterator _blobIt, From 4fb726ea98f8567c418f77e2336a1f5c06c26ce5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 21 Aug 2022 18:11:29 -0700 Subject: [PATCH 3/7] Fix comment Signed-off-by: Steve Peters --- src/parser_urdf.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 8b2aeb7bc..4097d7da8 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -395,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)) From 2986a275da2130dc386c30aaf7be8bee7ae34c0c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 21 Aug 2022 18:28:42 -0700 Subject: [PATCH 4/7] Add element variable instead of (*blobIt) Signed-off-by: Steve Peters --- src/parser_urdf.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 4097d7da8..ab0358f03 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3282,8 +3282,8 @@ void ReduceSDFExtensionElementTransformReduction( const ignition::math::Pose3d &_reductionTransform, const std::string &_elementName) { - // overwrite and if they exist - if ((*_blobIt)->ValueStr() == _elementName) + 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 @@ -3298,12 +3298,12 @@ void ReduceSDFExtensionElementTransformReduction( // } { - TiXmlNode* oldPoseKey = (*_blobIt)->FirstChild("pose"); + TiXmlNode* oldPoseKey = element->FirstChild("pose"); /// @todo: FIXME: we should read xyz, rpy and aggregate it to /// reductionTransform instead of just throwing the info away. if (oldPoseKey) { - (*_blobIt)->RemoveChild(oldPoseKey); + element->RemoveChild(oldPoseKey); } } @@ -3329,7 +3329,7 @@ void ReduceSDFExtensionElementTransformReduction( TiXmlElement* poseKey = new TiXmlElement("pose"); poseKey->LinkEndChild(poseTxt); - (*_blobIt)->LinkEndChild(poseKey); + element->LinkEndChild(poseKey); } } From aa0a15c52f1dfe1223cdddb62ef36832709d767a Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 21 Aug 2022 18:30:04 -0700 Subject: [PATCH 5/7] Apply pose transform if specified Unless //pose/@relative_to is specified, in that case do nothing. Signed-off-by: Steve Peters --- src/parser_urdf.cc | 50 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 38 insertions(+), 12 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index ab0358f03..e68fa39fe 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -2448,8 +2448,7 @@ void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link) for (std::vector::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)); @@ -3297,24 +3296,51 @@ void ReduceSDFExtensionElementTransformReduction( // sdfdbg << " " << streamIn.str() << "\n"; // } + auto pose {ignition::math::Pose3d::Zero}; { + std::string poseText = "0 0 0 0 0 0"; + TiXmlNode* oldPoseKey = element->FirstChild("pose"); - /// @todo: FIXME: we should read xyz, rpy and aggregate it to - /// reductionTransform instead of just throwing the info away. if (oldPoseKey) { + const auto& poseElemXml = oldPoseKey->ToElement(); + if (poseElemXml->Attribute("relative_to")) + { + return; + } + + if (poseElemXml->GetText()) + { + poseText = poseElemXml->GetText(); + } + + // delete the tag, we'll add a new one at the end element->RemoveChild(oldPoseKey); } + + // 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 << ">: [" + << 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); @@ -3324,7 +3350,7 @@ void ReduceSDFExtensionElementTransformReduction( 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()); TiXmlElement* poseKey = new TiXmlElement("pose"); poseKey->LinkEndChild(poseTxt); From bb8be15acfcafc3ebafc9b69b9bee8fd971dbec1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 22 Aug 2022 18:45:58 -0700 Subject: [PATCH 6/7] Fix //light/pose as well Generalize the sensor test to apply to projector and light as well. Signed-off-by: Steve Peters --- src/parser_urdf.cc | 5 +- test/integration/urdf_gazebo_extensions.cc | 241 ++++++++++--------- test/integration/urdf_gazebo_extensions.urdf | 37 ++- 3 files changed, 163 insertions(+), 120 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index e68fa39fe..4e6772b72 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -2539,11 +2539,12 @@ void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge) for (std::vector::iterator blobIt = _ge->blobs.begin(); blobIt != _ge->blobs.end(); ++blobIt) { - /// @todo make sure we are not missing any additional transform reductions ReduceSDFExtensionElementTransformReduction( - blobIt, _ge->reductionTransform, "sensor"); + blobIt, _ge->reductionTransform, "light"); ReduceSDFExtensionElementTransformReduction( blobIt, _ge->reductionTransform, "projector"); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "sensor"); } } diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 1a33e4667..6b9fbcf37 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -149,149 +149,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("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; - EXPECT_TRUE(sensor->HasElement("pose")); - const auto poseElem = sensor->GetElement("pose"); + const auto& elementName = element->Get("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::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", 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; - EXPECT_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::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", 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; - EXPECT_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::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", 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; - EXPECT_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::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", 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; - EXPECT_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::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", 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; - EXPECT_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::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", 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"); } diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index 1daf5a8cd..a0a92c6c7 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -165,6 +165,8 @@ + + 6.0 @@ -195,6 +197,12 @@ + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + 111 0 0 0 0 -1 6.0 @@ -226,6 +234,12 @@ + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + 111 0 0 0 0 -1 6.0 @@ -269,8 +283,17 @@ + + + 111 0 0 0 0 -1 + + + + 111 0 0 0 0 -1 + - 111 0 0 0 0 -1 + + 111 0 0 0 0 -1 6.0 1.36869112579 @@ -289,6 +312,12 @@ + + 1.0 2.0 3.0 0.1 0.2 0.3 + + + 1.0 2.0 3.0 0.1 0.2 0.3 + 1 400 @@ -320,6 +349,12 @@ + + 1 1 1 1.570796 1.570796 1.570796 + + + 1 1 1 1.570796 1.570796 1.570796 + true 1 1 1 1.570796 1.570796 1.570796 From 7657bbe4a23fda0683f745ff7ad5abf2c9bf72e5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 24 Aug 2022 14:27:52 -0700 Subject: [PATCH 7/7] Update Migration guide Signed-off-by: Steve Peters --- Migration.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Migration.md b/Migration.md index b9b0b967c..ae3bc26e1 100644 --- a/Migration.md +++ b/Migration.md @@ -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