From 45f5e42830f69914454930a7b3d4d21c110215dc Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 30 Mar 2021 07:48:53 +0200 Subject: [PATCH 1/3] Fixed application of tags in lumped linkes during URDF conversion. Fixed , too. Signed-off-by: Martin Pecka --- src/parser_urdf.cc | 123 +++++++++++-- test/integration/urdf_gazebo_extensions.cc | 159 +++++++++++++++++ test/integration/urdf_gazebo_extensions.urdf | 178 +++++++++++++++++++ 3 files changed, 442 insertions(+), 18 deletions(-) diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 85eebbaa9..a266ddb5f 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -2125,6 +2125,8 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem, for (auto blobIt = (*ge)->blobs.begin(); blobIt != (*ge)->blobs.end(); ++blobIt) { + // Be sure to always copy only the first element; code in + // ReduceSDFExtensionSensorTransformReduction depends in this behavior CopyBlob((*blobIt)->FirstChildElement(), _elem); } } @@ -3354,8 +3356,8 @@ void ReduceSDFExtensionSensorTransformReduction( std::vector::iterator _blobIt, ignition::math::Pose3d _reductionTransform) { - // overwrite and if they exist - if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "sensor") == 0) + auto sensorElement = (*_blobIt)->FirstChildElement(); + if ( strcmp(sensorElement->Name(), "sensor") == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform @@ -3369,16 +3371,58 @@ void ReduceSDFExtensionSensorTransformReduction( // sdfdbg << " " << streamIn.CStr() << "\n"; // } + auto sensorPose {ignition::math::Pose3d::Zero}; { - tinyxml2::XMLNode *oldPoseKey = (*_blobIt)->FirstChildElement("pose"); - /// @todo: FIXME: we should read xyz, rpy and aggregate it to - /// reductionTransform instead of just throwing the info away. + auto sensorPosText = "0 0 0 0 0 0"; + const auto& oldPoseKey = sensorElement->FirstChildElement("pose"); if (oldPoseKey) { - (*_blobIt)->DeleteChild(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 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 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 : [" << sensorPosText << "]\n"; + return; } + + // critical part: we store the original tag from actually + // as a sibling of ... only first element of the blob is processed + // further, so its siblings can be used as temporary storage; we store the + // original tag there so that we can use the 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(), @@ -3403,7 +3447,7 @@ void ReduceSDFExtensionSensorTransformReduction( poseKey->LinkEndChild(poseTxt); - (*_blobIt)->LinkEndChild(poseKey); + sensorElement->LinkEndChild(poseKey); } } @@ -3412,8 +3456,8 @@ void ReduceSDFExtensionProjectorTransformReduction( std::vector::iterator _blobIt, ignition::math::Pose3d _reductionTransform) { - // overwrite (xyz/rpy) if it exists - if ( strcmp((*_blobIt)->FirstChildElement()->Name(), "projector") == 0) + 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 @@ -3426,16 +3470,59 @@ void ReduceSDFExtensionProjectorTransformReduction( // sdfdbg << " " << streamIn << "\n"; // } - // should read ... and agregate reductionTransform - tinyxml2::XMLNode *poseKey = (*_blobIt)->FirstChildElement("pose"); - // read pose and save it - - // remove the tag for now - if (poseKey) + auto projectorPose {ignition::math::Pose3d::Zero}; { - (*_blobIt)->DeleteChild(poseKey); + 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 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 + { + sdferr << "Unexpected case in projector pose computation\n"; + return; + } + + // delete the tag, we'll add a new one at the end + projectorElement->DeleteChild(oldPoseKey); + } + + // parse the 6-tuple text into math::Pose3d + std::stringstream ss; + ss.imbue(std::locale::classic()); + ss << projectorPosText; + ss >> projectorPose; + if (ss.fail()) + { + sdferr << "Could not parse : [" + << projectorPosText << "]\n"; + return; + } + + // critical part: we store the original tag from + // actually as a sibling of ... only first element of the blob + // is processed further, so its siblings can be used as temporary storage; + // we store the original tag there so that we can use the + // 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; + // convert reductionTransform to values urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), _reductionTransform.Pos().Y(), @@ -3456,10 +3543,10 @@ void ReduceSDFExtensionProjectorTransformReduction( auto* doc = (*_blobIt)->GetDocument(); tinyxml2::XMLText *poseTxt = doc->NewText(poseStream.str().c_str()); - poseKey = doc->NewElement("pose"); + auto poseKey = doc->NewElement("pose"); poseKey->LinkEndChild(poseTxt); - (*_blobIt)->LinkEndChild(poseKey); + projectorElement->LinkEndChild(poseKey); } } diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 6c51d6b69..387b7500a 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -99,4 +99,163 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) EXPECT_TRUE(physics->Get("implicit_spring_damper")); } } + + 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; + ASSERT_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_FLOAT_EQ(pose.X(), 333.0); + EXPECT_FLOAT_EQ(pose.Y(), 0.0); + EXPECT_FLOAT_EQ(pose.Z(), 0.0); + EXPECT_FLOAT_EQ(pose.Roll(), 0.0); + EXPECT_FLOAT_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"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_FLOAT_EQ(pose.X(), 333.0); + EXPECT_FLOAT_EQ(pose.Y(), 111.0); + EXPECT_FLOAT_EQ(pose.Z(), 0.0); + EXPECT_FLOAT_EQ(pose.Roll(), 0.0); + EXPECT_FLOAT_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"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_FLOAT_EQ(pose.X(), 111.0); + EXPECT_FLOAT_EQ(pose.Y(), 0.0); + EXPECT_FLOAT_EQ(pose.Z(), 0.0); + EXPECT_FLOAT_EQ(pose.Roll(), 0.0); + EXPECT_FLOAT_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"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_FLOAT_EQ(pose.X(), 333.0); + EXPECT_FLOAT_EQ(pose.Y(), 111.0); + EXPECT_FLOAT_EQ(pose.Z(), 222.0); + EXPECT_FLOAT_EQ(pose.Roll(), 0.0); + EXPECT_FLOAT_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"); + + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); + + const auto& pose = posePair.first; + + EXPECT_FLOAT_EQ(pose.X(), 1); + EXPECT_FLOAT_EQ(pose.Y(), 2); + EXPECT_FLOAT_EQ(pose.Z(), 3); + EXPECT_FLOAT_EQ(pose.Roll(), 0.1); + EXPECT_FLOAT_EQ(pose.Pitch(), 0.2); + EXPECT_FLOAT_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"); + + 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 4703907db..558ca9357 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -137,4 +137,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 d8f92deac3aad51ae734cb924c67421cc10df22c Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 30 Mar 2021 08:18:01 +0200 Subject: [PATCH 2/3] Changelog Signed-off-by: Martin Pecka --- Migration.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Migration.md b/Migration.md index 5d92c43ee..ab4fbbe8d 100644 --- a/Migration.md +++ b/Migration.md @@ -18,6 +18,9 @@ but with improved human-readability.. 1. Fixed Atmosphere DOM class's temperature default value. Changed from -0.065 to -0.0065. * [Pull request 482](https://github.com/osrf/sdformat/pull/482) + +1. Fixed parsing of `` tags on lumped links when converting from URDF. + * [Pull request 525](https://github.com/osrf/sdformat/pull/525) ## SDFormat 9.x to 10.0 From d8490a231c0bf6c094992548ccb9e7b2795bf451 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 6 Apr 2021 21:15:17 +0200 Subject: [PATCH 3/3] Fixed float equality in test. Signed-off-by: Martin Pecka --- test/integration/urdf_gazebo_extensions.cc | 52 +++++++++++----------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index 387b7500a..45ba29698 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -136,11 +136,11 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) const auto& pose = posePair.first; - EXPECT_FLOAT_EQ(pose.X(), 333.0); - EXPECT_FLOAT_EQ(pose.Y(), 0.0); - EXPECT_FLOAT_EQ(pose.Z(), 0.0); - EXPECT_FLOAT_EQ(pose.Roll(), 0.0); - EXPECT_FLOAT_EQ(pose.Pitch(), 0.0); + 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")); @@ -157,11 +157,11 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) const auto& pose = posePair.first; - EXPECT_FLOAT_EQ(pose.X(), 333.0); - EXPECT_FLOAT_EQ(pose.Y(), 111.0); - EXPECT_FLOAT_EQ(pose.Z(), 0.0); - EXPECT_FLOAT_EQ(pose.Roll(), 0.0); - EXPECT_FLOAT_EQ(pose.Pitch(), 0.0); + 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")); @@ -178,11 +178,11 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) const auto& pose = posePair.first; - EXPECT_FLOAT_EQ(pose.X(), 111.0); - EXPECT_FLOAT_EQ(pose.Y(), 0.0); - EXPECT_FLOAT_EQ(pose.Z(), 0.0); - EXPECT_FLOAT_EQ(pose.Roll(), 0.0); - EXPECT_FLOAT_EQ(pose.Pitch(), 0.0); + 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")); @@ -199,11 +199,11 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) const auto& pose = posePair.first; - EXPECT_FLOAT_EQ(pose.X(), 333.0); - EXPECT_FLOAT_EQ(pose.Y(), 111.0); - EXPECT_FLOAT_EQ(pose.Z(), 222.0); - EXPECT_FLOAT_EQ(pose.Roll(), 0.0); - EXPECT_FLOAT_EQ(pose.Pitch(), 0.0); + 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")); @@ -220,12 +220,12 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) const auto& pose = posePair.first; - EXPECT_FLOAT_EQ(pose.X(), 1); - EXPECT_FLOAT_EQ(pose.Y(), 2); - EXPECT_FLOAT_EQ(pose.Z(), 3); - EXPECT_FLOAT_EQ(pose.Roll(), 0.1); - EXPECT_FLOAT_EQ(pose.Pitch(), 0.2); - EXPECT_FLOAT_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")); }