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 diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 2f9501bd3..4e6772b72 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 @@ -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)) @@ -2453,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)); @@ -2545,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 - ReduceSDFExtensionSensorTransformReduction(blobIt, - _ge->reductionTransform); - ReduceSDFExtensionProjectorTransformReduction(blobIt, - _ge->reductionTransform); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "light"); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "projector"); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "sensor"); } } @@ -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") + 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,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::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"; - // } + if (poseElemXml->GetText()) + { + poseText = poseElemXml->GetText(); + } - // should read ... and agregate reductionTransform - TiXmlNode* poseKey = (*_blobIt)->FirstChild("pose"); - // read pose and save it + // delete the 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 << ">: [" + << 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); @@ -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); } } diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 36d0d8603..6b9fbcf37 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -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("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("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& 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::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::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::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::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::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"); } diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index fc7216a16..a0a92c6c7 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -150,4 +150,217 @@ + + + + + + + + + + + + + + + + + + + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 111 0 0 0 0 -1 + + + + 111 0 0 0 0 -1 + + + + 111 0 0 0 0 -1 + 6.0 + + 1.36869112579 + + 1232 + 1616 + R8G8B8 + + + 0.02 + 300 + + + + + + + + + 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 + 1.0 2.0 3.0 0.1 0.2 0.3 + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + +