USD -> SDF: Parse Physics and generated SDF file

test/usd/upAxisY.usda

@@ -6,12 +6,6 @@
 
 def "shapes"
 {
-    def PhysicsScene "physics"
-    {
-        vector3f physics:gravityDirection = (0, 0, -1)
-        float physics:gravityMagnitude = 9.8
-    }
-
     def Xform "ground_plane"
     {
         float3 xformOp:rotateXYZ = (0, 0, 0)

usd/include/sdf/usd/usd_parser/Parser.hh

@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef SDF_USD_USD_PARSER_PARSER_HH
+#define SDF_USD_USD_PARSER_PARSER_HH
+
+#include <string>
+
+#include "sdf/config.hh"
+#include "sdf/usd/Export.hh"
+#include "sdf/usd/UsdError.hh"
+
+namespace sdf
+{
+  // Inline bracket to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief Parse a USD file and convert it to a SDF file
+    /// \param[in] _inputFilenameUsd Path of the USD file to parse
+    /// \param[in] _outputFilenameSdf Path where the SDF file will be located
+    /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError
+    /// includes an error code and message. An empty vector indicates no error
+    /// occurred when parsing the USD file to its SDF representation.
+    UsdErrors IGNITION_SDFORMAT_USD_VISIBLE parseUSDFile(
+      const std::string &_inputFilenameUsd,
+      const std::string &_outputFilenameSdf);
+  }
+  }
+}
+#endif

usd/src/CMakeLists.txt

@@ -10,9 +10,13 @@ set(sources
   sdf_parser/Sensor.cc
   sdf_parser/Visual.cc
   sdf_parser/World.cc
+  usd_parser/Parser.cc
+  usd_parser/USD2SDF.cc
   usd_parser/USDData.cc
   usd_parser/USDMaterial.cc
+  usd_parser/USDPhysics.cc
   usd_parser/USDStage.cc
+  usd_parser/USDWorld.cc
 )
 
 ign_add_component(usd SOURCES ${sources} GET_TARGET_NAME usd_target)
@@ -41,6 +45,7 @@ set(gtest_sources
   sdf_parser/Visual_Sdf2Usd_TEST.cc
   sdf_parser/World_Sdf2Usd_TEST.cc
   usd_parser/USDData_TEST.cc
+  usd_parser/USDPhysics_TEST.cc
   usd_parser/USDStage_TEST.cc
   Conversions_TEST.cc
   UsdError_TEST.cc

usd/src/cmd/usd2sdf.cc

@@ -15,11 +15,11 @@
  *
  */
 
-#include <iostream>
 #include <string>
 
 #include <ignition/utils/cli/CLI.hpp>
 
+#include "sdf/usd/usd_parser/Parser.hh"
 #include "sdf/config.hh"
 
 //////////////////////////////////////////////////
@@ -43,10 +43,17 @@ struct Options
   std::string outputFilename{"output.sdf"};
 };
 
-void runCommand(const Options &/*_opt*/)
+void runCommand(const Options &_opt)
 {
-  // TODO(ahcorde): Call here the USD to SDF converter code
-  std::cerr << "USD to SDF conversion is not implemented yet.\n";
+  const auto errors =
+    sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename);
+  if (!errors.empty())
+  {
+    std::cerr << "Errors occurred when generating [" << _opt.outputFilename
+              << "] from [" << _opt.inputFilename << "]:\n";
+    for (const auto &e : errors)
+      std::cerr << "\t" << e << "\n";
+  }
 }
 
 void addFlags(CLI::App &_app)

usd/src/usd_parser/Parser.cc

@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "sdf/usd/usd_parser/Parser.hh"
+#include "USD2SDF.hh"
+
+namespace sdf
+{
+inline namespace SDF_VERSION_NAMESPACE {
+namespace usd
+{
+  UsdErrors parseUSDFile(
+      const std::string &_inputFilenameUsd,
+      const std::string &_outputFilenameSdf)
+  {
+    UsdErrors errors;
+    USD2SDF usd2sdf;
+    auto doc = tinyxml2::XMLDocument(true, tinyxml2::COLLAPSE_WHITESPACE);
+    auto readErrors = usd2sdf.Read(_inputFilenameUsd, &doc);
+    if (!readErrors.empty())
+    {
+      errors.insert(errors.end(), readErrors.begin(), readErrors.end());
+      return errors;
+    }
+    doc.SaveFile(_outputFilenameSdf.c_str());
+    return errors;
+  }
+}
+}
+}

usd/src/usd_parser/USD2SDF.cc

@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "USD2SDF.hh"
+
+#include "sdf/Console.hh"
+#include "sdf/Types.hh"
+#include "usd_model/WorldInterface.hh"
+#include "USDWorld.hh"
+
+namespace sdf {
+inline namespace SDF_VERSION_NAMESPACE {
+namespace usd {
+/////////////////////////////////////////////////
+USD2SDF::USD2SDF()
+{
+}
+
+/////////////////////////////////////////////////
+USD2SDF::~USD2SDF()
+{
+}
+
+/////////////////////////////////////////////////
+UsdErrors USD2SDF::Read(const std::string &_fileName,
+  tinyxml2::XMLDocument *_sdfXmlOut)
+{
+  UsdErrors errors;
+
+  std::shared_ptr<WorldInterface> worldInterface =
+    std::make_shared<WorldInterface>();
+
+  const auto errorsParseUSD = parseUSDWorld(_fileName, worldInterface);
+  if (!errorsParseUSD.empty())
+  {
+    errors.emplace_back(UsdError(
+      UsdErrorCode::SDF_TO_USD_PARSING_ERROR,
+      "Error parsing usd file [" + _fileName + "]"));
+    return errors;
+  }
+
+  tinyxml2::XMLElement *sdf = nullptr;
+  sdf = _sdfXmlOut->NewElement("sdf");
+  sdf->SetAttribute("version", "1.7");
+
+  tinyxml2::XMLElement *world = nullptr;
+  world = _sdfXmlOut->NewElement("world");
+  std::string worldName = worldInterface->worldName;
+  if (worldName.empty())
+  {
+    worldName = "world_name";
+  }
+  world->SetAttribute("name", (worldName + "_world").c_str());
+
+  this->AddKeyValue(world, "gravity", Vector32Str(
+    worldInterface->gravity * worldInterface->magnitude));
+
+  sdf->LinkEndChild(world);
+  _sdfXmlOut->LinkEndChild(sdf);
+  return errors;
+}
+
+/////////////////////////////////////////////////
+std::string USD2SDF::GetKeyValueAsString(
+    const tinyxml2::XMLElement *_elem) const
+{
+  std::string valueStr;
+  if (_elem->Attribute("value"))
+  {
+    valueStr = _elem->Attribute("value");
+  }
+  else if (_elem->FirstChild())
+  {
+    // Check that this node is a XMLText
+    if (_elem->FirstChild()->ToText())
+    {
+      valueStr = _elem->FirstChild()->Value();
+    }
+    else
+    {
+      sdfwarn << "Attribute value string not set\n";
+    }
+  }
+  return trim(valueStr);
+}
+
+/////////////////////////////////////////////////
+void USD2SDF::AddKeyValue(
+  tinyxml2::XMLElement *_elem,
+  const std::string &_key,
+  const std::string &_value)
+{
+  tinyxml2::XMLElement *childElem = _elem->FirstChildElement(_key.c_str());
+  if (childElem)
+  {
+    std::string oldValue = this->GetKeyValueAsString(childElem);
+    if (oldValue != _value)
+    {
+      sdferr << "multiple inconsistent <" << _key
+              << "> exists due to fixed joint reduction"
+              << " overwriting previous value [" << oldValue
+              << "] with [" << _value << "].\n";
+    }
+    else
+    {
+       sdferr << "multiple consistent <" << _key
+              << "> exists with [" << _value
+              << "] due to fixed joint reduction.\n";
+    }
+    // remove old _elem
+    _elem->DeleteChild(childElem);
+  }
+
+  auto *doc = _elem->GetDocument();
+  tinyxml2::XMLElement *ekey = doc->NewElement(_key.c_str());
+  tinyxml2::XMLText *textEkey = doc->NewText(_value.c_str());
+  ekey->LinkEndChild(textEkey);
+  _elem->LinkEndChild(ekey);
+}
+
+/////////////////////////////////////////////////
+std::string USD2SDF::Vector32Str(const ignition::math::Vector3d &_vector) const
+{
+  std::stringstream ss;
+  ss << _vector;
+  return ss.str();
+}
+}
+}
+}

usd/src/usd_parser/USD2SDF.hh

@@ -0,0 +1,80 @@
+/*
+ * Copyright 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef USD_PARSER_USD2SDF_HH_
+#define USD_PARSER_USD2SDF_HH_
+
+#include <string>
+
+#include <ignition/math/Vector3.hh>
+#include <tinyxml2.h>
+
+#include "sdf/sdf_config.h"
+#include "sdf/usd/UsdError.hh"
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+  namespace usd
+  {
+    /// \brief USD to SDF converter
+    /// This class helps generate the SDF file
+    class USD2SDF
+    {
+      /// \brief constructor
+      public: USD2SDF();
+
+      /// \brief destructor
+      public: ~USD2SDF();
+
+      /// \brief convert USD file to sdf xml document
+      /// \param[in] _fileMame string containing USD filename.
+      /// \param[out] _sdfXmlDoc Document to populate with the sdf model.
+      /// \return UsdErrors, which is a list of UsdError objects. An empty list
+      /// means no errors occurred when populating _sdfXmlOut with the contents
+      /// of _fileName
+      public: UsdErrors Read(
+        const std::string &_fileName,
+        tinyxml2::XMLDocument *_sdfXmlOut);
+
+      /// \brief get value from <key value="..."/> pair and return it as string
+      /// \param[in] _elem pointer to xml element
+      /// return a string with the key
+      private: std::string GetKeyValueAsString(
+                   const tinyxml2::XMLElement *_elem) const;
+
+      /// \brief append key value pair to the end of the xml element
+      /// \param[in] _elem pointer to xml element
+      /// \param[in] _key string containing key to add to xml element
+      /// \param[in] _value string containing value for the key added
+      private: void AddKeyValue(
+        tinyxml2::XMLElement *_elem,
+        const std::string &_key,
+        const std::string &_value);
+
+      /// \brief convert Vector3 to string
+      /// \param[in] _vector a ignition::math::Vector3d
+      /// \return string representation of Vector3
+      private: std::string Vector32Str(
+                   const ignition::math::Vector3d &_vector) const;
+    };
+  }
+}
+}
+
+#endif