Merge branch 'sdf12' into pr_optical_frame

Author: ahcorde

Migration.md

@@ -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

src/parser_urdf.cc

@@ -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);
   }
 }