12 ➡️ 13 (main) (#1009)

* USD to SDF: Added Lights attached to models (#903)

Signed-off-by: ahcorde <ahcorde@gmail.com>
Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>

* USD to SDF: Added diff drive plugin (#904)

Signed-off-by: ahcorde <ahcorde@gmail.com>
Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>

* USD to SDF: some fixes (#991)

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Added equality operators to Plugin (#912)

* Added convenience constructor to plugin

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added equality operators to Plugin

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Remove common dependency

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Fixed test

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Co-authored-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>
Co-authored-by: Nate Koenig <nkoenig@users.noreply.github.com>
Co-authored-by: Nate Koenig <nate@openrobotics.org>

Author: 5 people

include/sdf/Plugin.hh

@@ -144,6 +144,18 @@ namespace sdf
     /// \return A reference to this plugin
     public: Plugin &operator=(Plugin &&_plugin) noexcept;
 
+    /// \brief Plugin equality operator.
+    /// \param[in] _plugin Plugin to compare against.
+    /// \return True if this plugin matches the provided plugin. The name,
+    /// filename, and contents must all match to return true.
+    public: bool operator==(const Plugin &_plugin) const;
+
+    /// \brief Plugin inequality operator.
+    /// \param[in] _plugin Plugin to compare against.
+    /// \return True if this plugin does not match the provided plugin.
+    /// The name, filename, or contents must be different to return false.
+    public: bool operator!=(const Plugin &_plugin) const;
+
     /// \brief Output stream operator for a Plugin.
     /// \param[in] _out The output stream
     /// \param[in] _plugin Plugin to output

src/Plugin.cc

@@ -259,3 +259,16 @@ Plugin &Plugin::operator=(Plugin &&_plugin) noexcept
   this->dataPtr = std::move(_plugin.dataPtr);
   return *this;
 }
+
+/////////////////////////////////////////////////
+bool Plugin::operator==(const Plugin &_plugin) const
+{
+  // Simplest thing to do is compare the string form of each plugin
+  return _plugin.ToElement()->ToString("") == this->ToElement()->ToString("");
+}
+
+/////////////////////////////////////////////////
+bool Plugin::operator!=(const Plugin &_plugin) const
+{
+  return !(*this == _plugin);
+}

src/Plugin_TEST.cc

@@ -385,3 +385,34 @@ TEST(DOMPlugin, InsertStringContent)
   EXPECT_EQ("name", plugin5.Name());
   EXPECT_TRUE(plugin5.Contents().empty());
 }
+
+/////////////////////////////////////////////////
+TEST(DOMPlugin, EqualityOperators)
+{
+  sdf::Plugin plugin("my-filename", "my-name",
+      "<render_engine>ogre2</render_engine>");
+  sdf::Plugin plugin2(plugin);
+  sdf::Plugin plugin3;
+
+  EXPECT_EQ(plugin, plugin2);
+  EXPECT_NE(plugin, plugin3);
+  EXPECT_NE(plugin2, plugin3);
+
+  // Test contents
+  plugin2.ClearContents();
+  EXPECT_NE(plugin, plugin2);
+  plugin.ClearContents();
+  EXPECT_EQ(plugin, plugin2);
+
+  // test name
+  plugin2.SetName("new-name");
+  EXPECT_NE(plugin, plugin2);
+  plugin.SetName("new-name");
+  EXPECT_EQ(plugin, plugin2);
+
+  // test filename
+  plugin2.SetFilename("new-filename");
+  EXPECT_NE(plugin, plugin2);
+  plugin.SetFilename("new-filename");
+  EXPECT_EQ(plugin, plugin2);
+}

test/usd/upAxisZ.usda

@@ -102,7 +102,9 @@ def Xform "box" (
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
 
-            def Cube "geometry"
+            def Cube "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)]
                 rel material:binding = </Looks/Material_1>
@@ -121,6 +123,18 @@ def Xform "box" (
                 uniform token[] xformOpOrder = ["xformOp:scale"]
             }
         }
+
+        def DiskLight "boxModelLight" (
+            prepend apiSchemas = ["ShapingAPI"]
+            kind = "model"
+        )
+        {
+            float inputs:intensity = 3000
+            float inputs:specular = 0.5
+            float3 xformOp:rotateZYX = (0, 0, 45)
+            float3 xformOp:translate = (0, 0, 1000)
+            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
+        }
     }
 }
 
@@ -147,7 +161,9 @@ def Xform "cylinder"
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
 
-            def Cylinder "geometry"
+            def Cylinder "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)]
                 double height = 1
@@ -190,7 +206,9 @@ def Xform "sphere"
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
 
-            def Sphere "geometry"
+            def Sphere "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)]
                 rel material:binding = </Looks/Material_3>
@@ -231,7 +249,9 @@ def Xform "capsule"
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
 
-            def Capsule "geometry"
+            def Capsule "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.2, -0.2, -0.5), (0.2, 0.2, 0.5)]
                 double height = 0.6
@@ -274,7 +294,9 @@ def Xform "ellipsoid"
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
 
-            def Sphere "geometry"
+            def Sphere "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)]
                 rel material:binding = </Looks/Material_5>

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

@@ -34,12 +34,15 @@ namespace sdf
     /// \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
+    /// \param[in] _useGazeboPlugins Whether _outputFilenameSdf should have
+    /// gazebo plugins in it (true) or not (false)
     /// \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);
+      const std::string &_outputFilenameSdf,
+      bool _useGazeboPlugins = true);
   }
   }
 }

usd/src/cmd/usd2sdf.cc

@@ -43,12 +43,17 @@ struct Options
 
   /// \brief output filename
   std::string outputFilename{"output.sdf"};
+
+  /// \brief Whether gazebo plugins should be used in the parsed sdf file
+  /// (true) or not (false)
+  bool useGazeboPlugins{true};
 };
 
 void runCommand(const Options &_opt)
 {
   const auto errors =
-    sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename);
+    sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename,
+        _opt.useGazeboPlugins);
   if (!errors.empty())
   {
     std::cerr << "Errors occurred when generating [" << _opt.outputFilename
@@ -70,6 +75,11 @@ void addFlags(CLI::App &_app)
     opt->outputFilename,
     "Output filename. Defaults to output.sdf unless otherwise specified.");
 
+  _app.add_option("use-gazebo-plugins",
+    opt->useGazeboPlugins,
+    "Whether gazebo plugins should be added to the output sdf file or not. "
+    "Defaults to true.");
+
   _app.callback([&_app, opt](){
     runCommand(*opt);
   });

usd/src/usd_parser/Parser.cc

@@ -27,12 +27,13 @@ namespace usd
 {
   UsdErrors parseUSDFile(
       const std::string &_inputFilenameUsd,
-      const std::string &_outputFilenameSdf)
+      const std::string &_outputFilenameSdf,
+      bool _useGazeboPlugins)
   {
     UsdErrors errors;
     USD2SDF usd2sdf;
     sdf::Root root;
-    errors = usd2sdf.Read(_inputFilenameUsd, root);
+    errors = usd2sdf.Read(_inputFilenameUsd, _useGazeboPlugins, root);
     if (!errors.empty())
     {
       return errors;

usd/src/usd_parser/USD2SDF.cc

@@ -31,14 +31,14 @@ namespace sdf {
 inline namespace SDF_VERSION_NAMESPACE {
 namespace usd {
 ////////////////////////////////////////////////
-UsdErrors USD2SDF::Read(const std::string &_fileName,
+UsdErrors USD2SDF::Read(const std::string &_fileName, bool _useGazeboPlugins,
   sdf::Root &_root)
 {
   UsdErrors errors;
 
   sdf::World sdfWorld;
 
-  errors = parseUSDWorld(_fileName, sdfWorld);
+  errors = parseUSDWorld(_fileName, _useGazeboPlugins, sdfWorld);
   if (!errors.empty())
   {
     errors.emplace_back(UsdError(

usd/src/usd_parser/USD2SDF.hh

@@ -40,13 +40,16 @@ inline namespace SDF_VERSION_NAMESPACE {
 
       /// \brief convert USD file to a sdf::Root object
       /// \param[in] _fileName string containing USD filename.
+      /// \param[in] _useGazeboPlugins Whether _root should have gazebo plugins
+      /// in it (true) or not (false)
       /// \param[out] _root Root element to populate with the equivalent sdf
       /// information from _fileName.
       /// \return UsdErrors, which is a list of UsdError objects. An empty list
       /// means no errors occurred when populating _root with the contents
       /// of _fileName
       public: UsdErrors Read(
         const std::string &_fileName,
+        bool _useGazeboPlugins,
         sdf::Root &_root);
     };
   }

usd/src/usd_parser/USDLinks.cc

@@ -233,7 +233,7 @@ int ParseMeshSubGeom(const pxr::UsdPrim &_prim,
 
       ignition::common::SubMesh subMeshSubset;
       subMeshSubset.SetPrimitiveType(ignition::common::SubMesh::TRISTRIPS);
-      subMeshSubset.SetName("subgeommesh");
+      subMeshSubset.SetName("subgeommesh_" + std::to_string(numSubMeshes));
 
       if (it != _usdData.Materials().end())
       {
@@ -288,12 +288,18 @@ int ParseMeshSubGeom(const pxr::UsdPrim &_prim,
       _meshGeom.SetUri(directoryMesh + ".dae");
 
       geomSubset.SetMeshShape(_meshGeom);
-      visSubset.SetName("mesh_subset");
+      visSubset.SetName("mesh_subset_" + std::to_string(numSubMeshes));
       visSubset.SetGeom(geomSubset);
 
       ignition::math::Pose3d pose;
       ignition::math::Vector3d scale(1, 1, 1);
-      GetTransform(child, _usdData, pose, scale, _link->Name());
+      std::string linkName = pxr::TfStringify(_prim.GetPath());
+      auto found = linkName.find(_link->Name());
+      if (found != std::string::npos)
+      {
+        linkName = linkName.substr(0, found + _link->Name().size());
+      }
+      GetTransform(child, _usdData, pose, scale, linkName);
       _scale *= scale;
       visSubset.SetRawPose(pose);
       _link->AddVisual(visSubset);
@@ -386,15 +392,21 @@ UsdErrors ParseMesh(
 
   ignition::math::Pose3d pose;
   ignition::math::Vector3d scale(1, 1, 1);
-  std::string linkName = _link->Name();
+  std::string linkName = pxr::TfStringify(_prim.GetPath());
+  auto found = linkName.find(_link->Name());
+  if (found != std::string::npos)
+  {
+    linkName = linkName.substr(0, found + _link->Name().size());
+  }
+
   size_t nSlash = std::count(linkName.begin(), linkName.end(), '/');
   if (nSlash == 1)
   {
     GetTransform(_prim, _usdData, pose, scale, "/");
   }
   else
   {
-    GetTransform(_prim, _usdData, pose, scale, _link->Name());
+    GetTransform(_prim, _usdData, pose, scale, linkName);
   }
 
   _pose = pose;
@@ -522,7 +534,7 @@ void ParseCylinder(
   _geom.SetType(sdf::GeometryType::CYLINDER);
 
   c.SetRadius(radius * _metersPerUnit * _scale.X());
-  c.SetLength(height * _metersPerUnit * _scale.Y());
+  c.SetLength(height * _metersPerUnit * _scale.Z());
 
   _geom.SetCylinderShape(c);
 }
@@ -632,9 +644,9 @@ UsdErrors ParseUSDLinks(
       pxr::UsdModelAPI(parent).GetKind(&kindOfSchema);
     }
 
-    if (_prim.HasAPI<pxr::UsdPhysicsRigidBodyAPI>()
-      || pxr::KindRegistry::IsA(kindOfSchema, pxr::KindTokens->model)
-      || !collisionEnabled)
+    if ((_prim.HasAPI<pxr::UsdPhysicsRigidBodyAPI>()
+      || pxr::KindRegistry::IsA(kindOfSchema, pxr::KindTokens->model))
+      && (!collisionEnabled || _prim.HasAPI<pxr::UsdPhysicsMassAPI>()))
     {
       double metersPerUnit = data.second->MetersPerUnit();
 
@@ -696,7 +708,15 @@ UsdErrors ParseUSDLinks(
 
       ignition::math::Pose3d poseCol;
       ignition::math::Vector3d scaleCol(1, 1, 1);
-      GetTransform(_prim, _usdData, poseCol, scaleCol, _link->Name());
+      std::string linkName = pxr::TfStringify(_prim.GetPath());
+      auto found = linkName.find(_link->Name());
+      if (found != std::string::npos)
+      {
+        linkName = linkName.substr(0, found + _link->Name().size());
+      }
+      GetTransform(_prim, _usdData, poseCol, scaleCol, linkName);
+
+      scaleCol *= _scale;
 
       double metersPerUnit = data.second->MetersPerUnit();
 

usd/src/usd_parser/USDStage_TEST.cc

@@ -39,7 +39,7 @@ TEST(USDStage, Constructor)
 
     EXPECT_EQ("Z", stage.UpAxis());
     EXPECT_DOUBLE_EQ(0.01, stage.MetersPerUnit());
-    EXPECT_EQ(25u, stage.USDPaths().size());
+    EXPECT_EQ(26u, stage.USDPaths().size());
   }
 
   // Check up Axis equal to Y and metersPerUnit