Unflatten flattened models

sdf/1.8/1_7.convert

@@ -1,3 +1,9 @@
 <convert name="sdf">
 
+  <convert name="world">
+    <convert name="model">
+      <unnest/>
+    </convert>
+  </convert>
+
 </convert> <!-- End SDF -->

src/Converter.cc

@@ -172,6 +172,17 @@ void Converter::ConvertDescendantsImpl(tinyxml2::XMLElement *_e,
   }
 }
 
+
+/////////////////////////////////////////////////
+// TODO(jenn) delete, used for debugging
+std::string PrintElement(const tinyxml2::XMLElement *_elem)
+{
+  tinyxml2::XMLPrinter printer;
+  _elem->Accept(&printer);
+
+  return std::string(printer.CStr());
+}
+
 /////////////////////////////////////////////////
 void Converter::ConvertImpl(tinyxml2::XMLElement *_elem,
                             tinyxml2::XMLElement *_convert)
@@ -229,13 +240,304 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem,
     {
       Remove(_elem, childElem);
     }
+    else if (name == "unnest")
+    {
+      Unnest(_elem);
+      // TODO(jenn) delete debug statements
+      // std::cout << "\n\n_elem after unnest:\n"
+      //             << PrintElement(_elem) << std::endl;
+    }
     else if (name != "convert")
     {
       sdferr << "Unknown convert element[" << name << "]\n";
     }
   }
 }
 
+/////////////////////////////////////////////////
+void Converter::Unnest(tinyxml2::XMLElement *_elem)
+{
+  SDF_ASSERT(_elem != NULL, "SDF element is NULL");
+
+  tinyxml2::XMLDocument *doc = _elem->GetDocument();
+
+  tinyxml2::XMLElement *nextElem = nullptr, *firstUnnestedModel = nullptr;
+  for (tinyxml2::XMLElement *elem = _elem->FirstChildElement();
+      elem;
+      elem = nextElem)
+  {
+    // break loop if reached the first unnested model
+    if (firstUnnestedModel == elem) break;
+
+    nextElem = elem->NextSiblingElement();
+    std::string elemName = elem->Name();
+
+    // skip element if not one of the following or if missing name attribute
+    if ((elemName != "frame" && elemName != "joint"
+        && elemName != "link" && elemName != "model")
+        || !elem->Attribute("name"))
+    {
+      continue;
+    }
+
+    std::string attrName = elem->Attribute("name");
+
+    size_t found = attrName.find("::");
+    if (found == std::string::npos)
+    {
+      // recursive unnest
+      if (elemName == "model")
+      {
+        // std::cout << "before recursive:\n"
+        //           << PrintElement(elem) << std::endl;
+        Unnest(elem);
+
+        // std::cout << "unnested model:\n" << PrintElement(elem) << std::endl;
+        break;
+      }
+
+      continue;
+    }
+
+    std::string newModelName = attrName.substr(0, found);
+    tinyxml2::XMLElement *newModel = doc->NewElement("model");
+    newModel->SetAttribute("name", newModelName.c_str());
+
+    if (FindNewModelElements(_elem, newModel, found + 2))
+    {
+      Unnest(newModel);
+      _elem->InsertEndChild(newModel);
+
+      // since newModel is inserted at the end, point back to the top element
+      nextElem = _elem->FirstChildElement();
+
+      if (!firstUnnestedModel)
+        firstUnnestedModel = newModel;
+      // std::cout << "newModel:\n" << PrintElement(newModel) << std::endl;
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,
+                                    tinyxml2::XMLElement *_newModel,
+                                    const size_t &_newNameIdx)
+{
+  bool unnestedNewModel = false;
+  std::string newModelName = _newModel->Attribute("name");
+
+  tinyxml2::XMLElement *elem = _elem->FirstChildElement(), *nextElem = nullptr;
+  while (elem)
+  {
+    nextElem = elem->NextSiblingElement();
+
+    std::string elemName = elem->Name();
+    std::string elemAttrName;
+
+    if (elem->Attribute("name"))
+      elemAttrName = elem->Attribute("name");
+
+    if (elemAttrName.empty() ||
+        elemAttrName.compare(0, _newNameIdx - 2, newModelName) != 0 ||
+        (elemName != "frame" && elemName != "joint" &&
+        elemName != "link" && elemName != "model"))
+    {
+      if (elemName != "gripper")
+      {
+        elem = nextElem;
+        continue;
+      }
+    }
+
+    // Child attribute name w/ newModelName prefix stripped except for //gripper
+    std::string childAttrName;
+    if (elemName != "gripper")
+    {
+      childAttrName = elemAttrName.substr(_newNameIdx);
+      elem->SetAttribute("name", childAttrName.c_str());
+    }
+
+    // strip new model prefix from //pose/@relative_to
+    tinyxml2::XMLElement *poseElem = elem->FirstChildElement("pose");
+    if (poseElem != nullptr && poseElem->Attribute("relative_to"))
+    {
+      std::string poseRelTo = poseElem->Attribute("relative_to");
+
+      if (poseRelTo.compare(0, _newNameIdx - 2, newModelName) == 0)
+      {
+        poseRelTo = poseRelTo.substr(_newNameIdx);
+        poseElem->SetAttribute("relative_to", poseRelTo.c_str());
+      }
+    }
+
+    if (elemName == "frame")
+    {
+      std::string attachedTo;
+
+      if (elem->Attribute("attached_to"))
+      {
+        attachedTo = elem->Attribute("attached_to");
+
+        SDF_ASSERT(attachedTo.compare(0, _newNameIdx - 2, newModelName) == 0,
+          "Error: Frame attribute 'attached_to' does not start with " +
+          newModelName);
+
+        // strip new model prefix from attached_to
+        attachedTo = attachedTo.substr(_newNameIdx);
+        elem->SetAttribute("attached_to", attachedTo.c_str());
+      }
+
+      // remove frame if childAttrName == __model__
+      if (childAttrName == "__model__" && elem->Attribute("attached_to"))
+      {
+        _newModel->SetAttribute("canonical_link", attachedTo.c_str());
+        _newModel->InsertFirstChild(poseElem);
+
+        _elem->DeleteChild(elem);
+        elem = poseElem;
+      }
+    }  // frame
+
+    else if (elemName == "link")
+    {
+      // find & strip new model prefix of all //link/<element>/pose/@relative_to
+      for (tinyxml2::XMLElement *e = elem->FirstChildElement();
+          e;
+          e = e->NextSiblingElement())
+      {
+        poseElem = e->FirstChildElement("pose");
+        if (poseElem != nullptr)
+        {
+          std::string poseRelTo = poseElem->Attribute("relative_to");
+
+          SDF_ASSERT(poseRelTo.compare(0, _newNameIdx - 2, newModelName) == 0,
+            "Error: Pose attribute 'relative_to' does not start with " +
+            newModelName);
+
+          poseRelTo = poseRelTo.substr(_newNameIdx);
+          poseElem->SetAttribute("relative_to", poseRelTo.c_str());
+        }
+      }
+    }  // link
+
+    else if (elemName == "joint")
+    {
+      // strip new model prefix from //joint/parent
+      tinyxml2::XMLElement *e = elem->FirstChildElement("parent");
+      std::string eText = e->GetText();
+
+      SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
+        "Error: Joint's <parent> value does not start with " + newModelName);
+
+      e->SetText(eText.substr(_newNameIdx).c_str());
+
+      // strip new model prefix from //joint/child
+      e = elem->FirstChildElement("child");
+      eText = e->GetText();
+
+      SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
+        "Error: Joint's <child> value does not start with " + newModelName);
+
+      e->SetText(std::string(e->GetText()).substr(_newNameIdx).c_str());
+
+      // strip new model prefix from //xyz/@expressed_in
+      std::string axisStr = "axis";
+      while (true)
+      {
+        tinyxml2::XMLElement *axisElem =
+            elem->FirstChildElement(axisStr.c_str());
+        if (axisElem != nullptr)
+        {
+          if (axisElem->FirstChildElement("xyz")->Attribute("expressed_in"))
+          {
+            std::string expressIn =
+                axisElem->FirstChildElement("xyz")->Attribute("expressed_in");
+
+            SDF_ASSERT(expressIn.compare(0, _newNameIdx - 2, newModelName) == 0,
+              "Error: <xyz>'s attribute 'expressed_in' does not start with " +
+              newModelName);
+
+            expressIn = expressIn.substr(_newNameIdx);
+
+            axisElem->FirstChildElement("xyz")
+                    ->SetAttribute("expressed_in", expressIn.c_str());
+          }
+        }
+
+        if (axisStr == "axis2") break;
+
+        axisStr += "2";
+      }
+
+      // strip new model prefix from all //joint/sensor/pose/@relative_to
+      for (e = elem->FirstChildElement("sensor");
+          e;
+          e = e->NextSiblingElement("sensor"))
+      {
+        poseElem = e->FirstChildElement("pose");
+        if (poseElem != nullptr)
+        {
+          std::string poseRelTo = poseElem->Attribute("relative_to");
+
+          SDF_ASSERT(poseRelTo.compare(0, _newNameIdx - 2, newModelName) == 0,
+            "Error: Pose attribute 'relative_to' does not start with " +
+            newModelName);
+
+          poseRelTo = poseRelTo.substr(_newNameIdx);
+          poseElem->SetAttribute("relative_to", poseRelTo.c_str());
+        }
+      }
+    }  // joint
+
+    else if (elemName == "gripper")
+    {
+      bool hasPrefix = true;
+
+      // strip prefix from all /model/gripper/gripper_link
+      tinyxml2::XMLElement *e = elem->FirstChildElement("gripper_link");
+      std::string eText;
+      while (e)
+      {
+        eText = e->GetText();
+
+        if (eText.compare(0, _newNameIdx - 2, newModelName) != 0)
+        {
+          hasPrefix = false;
+          break;
+        }
+
+        e->SetText(eText.substr(_newNameIdx).c_str());
+        e = e->NextSiblingElement("gripper_link");
+      }
+
+      // if //model/gripper/gripper_link does not have new model prefix
+      // don't add to new model
+      if (!hasPrefix)
+      {
+        elem = nextElem;
+        continue;
+      }
+
+      // strip prefix from //model/gripper/palm_link
+      e = elem->FirstChildElement("palm_link");
+      eText = e->GetText();
+
+      SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
+        "Error: Gripper's <palm_link> value does not start with "
+        + newModelName);
+
+      e->SetText(eText.substr(_newNameIdx).c_str());
+    }  // gripper
+
+    unnestedNewModel = true;
+    _newModel->InsertEndChild(elem);
+
+    elem = nextElem;
+  }
+
+  return unnestedNewModel;
+}
+
 /////////////////////////////////////////////////
 void Converter::Rename(tinyxml2::XMLElement *_elem,
                        tinyxml2::XMLElement *_renameElem)

src/Converter.hh

@@ -20,6 +20,8 @@
 #include <tinyxml2.h>
 
 #include <string>
+#include <vector>
+#include <tuple>
 
 #include <sdf/sdf_config.h>
 #include "sdf/system_util.hh"
@@ -103,6 +105,20 @@ namespace sdf
     private: static void Remove(tinyxml2::XMLElement *_elem,
                                 tinyxml2::XMLElement *_removeElem);
 
+    /// \brief Unnest an element (conversion from SDFormat <= 1.7 to 1.8)
+    /// \param[in] _elem The element to unnest
+    private: static void Unnest(tinyxml2::XMLElement *_elem);
+
+    /// \brief Finds all elements related to the unnested model
+    /// \param[in] _elem The element to unnest
+    /// \param[in] _newModel The new unnested model element
+    /// \param[in] _newNameIdx The index of the new name for child elements
+    /// \return True if unnested new model elements
+    private:
+      static bool FindNewModelElements(tinyxml2::XMLElement *_elem,
+                                      tinyxml2::XMLElement *_newModel,
+                                      const size_t &_newNameIdx);
+
     private: static const char *GetValue(const char *_valueElem,
                                          const char *_valueAttr,
                                          tinyxml2::XMLElement *_elem);