USD -> SDF: Added lights attached to the world (#875)

Signed-off-by: ahcorde <[email protected]>
Co-authored-by: Ashton Larkin <[email protected]>

test/usd/upAxisZ.usda

@@ -13,6 +13,32 @@ def "shapes"
         float physics:gravityMagnitude = 9.8
     }
 
+    def DistantLight "defaultLight" (
+        prepend apiSchemas = ["ShapingAPI"]
+        kind = "model"
+    )
+    {
+        float angle = 1
+        float inputs:diffuse = 0.5
+        float inputs:intensity = 5000
+        float shaping:cone:angle = 180
+        float3 xformOp:rotateZYX = (0, 45, 0)
+        float3 xformOp:translate = (0, 0, 0)
+        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
+    }
+
+    def DiskLight "diskLight" (
+        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"]
+    }
+
     def Xform "ground_plane"
     {
         float3 xformOp:rotateXYZ = (0, 0, 0)

usd/src/CMakeLists.txt

@@ -13,6 +13,7 @@ set(sources
   usd_parser/Parser.cc
   usd_parser/USD2SDF.cc
   usd_parser/USDData.cc
+  usd_parser/USDLights.cc
   usd_parser/USDMaterial.cc
   usd_parser/USDPhysics.cc
   usd_parser/USDStage.cc
@@ -47,6 +48,7 @@ set(gtest_sources
   sdf_parser/World_Sdf2Usd_TEST.cc
   usd_parser/USDData_TEST.cc
   usd_parser/USDPhysics_TEST.cc
+  usd_parser/USDLight_TEST.cc
   usd_parser/USDStage_TEST.cc
   usd_parser/USDTransforms_TEST.cc
   Conversions_TEST.cc
@@ -59,14 +61,19 @@ ign_build_tests(
   TYPE UNIT
   SOURCES ${gtest_sources}
   LIB_DEPS ${usd_target} ignition-cmake${IGN_CMAKE_VER}::utilities
-  INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test
+  INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test usd_parser
 )
 
 if (TARGET UNIT_USDPhysics_TEST)
   target_sources(UNIT_USDPhysics_TEST PRIVATE
   usd_parser/USDPhysics.cc)
 endif()
 
+if (TARGET UNIT_USDTransport_TEST)
+  target_sources(UNIT_USDTransport_TEST PRIVATE
+  usd_parser/USDTransforms.cc)
+endif()
+
 if (TARGET UNIT_Conversions_TEST)
   target_sources(UNIT_Conversions_TEST PRIVATE
     Conversions.cc)

usd/src/usd_parser/USDLight_TEST.cc

@@ -0,0 +1,91 @@
+/*
+ * 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 <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+// TODO(ahcorde) This is to remove deprecated "warnings" in usd, these warnings
+// are reported using #pragma message so normal diagnostic flags cannot remove
+// them. This workaround requires this block to be used whenever usd is
+// included.
+#pragma push_macro ("__DEPRECATED")
+#undef __DEPRECATED
+#include <pxr/usd/usd/prim.h>
+#include <pxr/usd/usd/stage.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include <sdf/usd/usd_parser/USDData.hh>
+#include <sdf/usd/UsdError.hh>
+
+#include <ignition/math/Color.hh>
+#include <ignition/math/Pose3.hh>
+
+#include "test_config.h"
+#include "test_utils.hh"
+
+#include "sdf/Light.hh"
+
+#include "USDLights.hh"
+
+/////////////////////////////////////////////////
+TEST(USDLightsTest, DistanceLight)
+{
+  std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda");
+  sdf::usd::USDData usdData(filename);
+  EXPECT_EQ(0u, usdData.Init().size());
+
+  auto stage = pxr::UsdStage::Open(filename);
+  ASSERT_TRUE(stage);
+
+  pxr::UsdPrim prim = stage->GetPrimAtPath(
+    pxr::SdfPath("/shapes/defaultLight"));
+  ASSERT_TRUE(prim);
+
+  auto light = sdf::usd::ParseUSDLights(
+    prim, usdData, "/shapes");
+  ASSERT_TRUE(light);
+
+  EXPECT_EQ("defaultLight", light->Name());
+  EXPECT_EQ(sdf::LightType::DIRECTIONAL, light->Type());
+  EXPECT_TRUE(light->CastShadows());
+  EXPECT_EQ(ignition::math::Color(0.5, 0.5, 0.5, 1), light->Diffuse());
+  EXPECT_EQ(ignition::math::Color(1, 1, 1, 1), light->Specular());
+  EXPECT_FLOAT_EQ(0.5, light->Intensity());
+  EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, IGN_DTOR(45), 0),
+      light->RawPose());
+
+  prim = stage->GetPrimAtPath(pxr::SdfPath("/shapes/diskLight"));
+  ASSERT_TRUE(prim);
+
+  auto diskLight = sdf::usd::ParseUSDLights(
+    prim, usdData, "/shapes");
+  ASSERT_TRUE(diskLight);
+
+  EXPECT_EQ("diskLight", diskLight->Name());
+  EXPECT_EQ(sdf::LightType::SPOT, diskLight->Type());
+  EXPECT_TRUE(diskLight->CastShadows());
+  EXPECT_EQ(ignition::math::Color(1, 1, 1, 1), diskLight->Diffuse());
+  EXPECT_EQ(ignition::math::Color(0.5, 0.5, 0.5, 1), diskLight->Specular());
+  EXPECT_FLOAT_EQ(0.3, diskLight->Intensity());
+  EXPECT_EQ(
+    ignition::math::Pose3d(0, 0, 10, 0, 0, IGN_DTOR(45)), diskLight->RawPose());
+  EXPECT_DOUBLE_EQ(0.1, diskLight->SpotInnerAngle().Radian());
+  EXPECT_DOUBLE_EQ(0.5, diskLight->SpotOuterAngle().Radian());
+  EXPECT_DOUBLE_EQ(0.8, diskLight->SpotFalloff());
+}

usd/src/usd_parser/USDLights.cc

@@ -0,0 +1,102 @@
+/*
+ * 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 "USDLights.hh"
+
+#include <memory>
+
+// TODO(ahcorde) This is to remove deprecated "warnings" in usd, these warnings
+// are reported using #pragma message so normal diagnostic flags cannot remove
+// them. This workaround requires this block to be used whenever usd is
+// included.
+#pragma push_macro ("__DEPRECATED")
+#undef __DEPRECATED
+#include "pxr/usd/usdLux/lightAPI.h"
+#include "pxr/usd/usdLux/boundableLightBase.h"
+#include "pxr/usd/usdLux/distantLight.h"
+#include "pxr/usd/usdLux/diskLight.h"
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/usd/usd_parser/USDData.hh"
+#include "sdf/usd/usd_parser/USDTransforms.hh"
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+//
+namespace usd
+{
+  std::optional<sdf::Light> ParseUSDLights(
+    const pxr::UsdPrim &_prim,
+    const USDData &_usdData,
+    const std::string &_linkName)
+  {
+    sdf::Light lightSdf;
+    std::optional<sdf::Light> light = lightSdf;
+
+    auto variantLight = pxr::UsdLuxBoundableLightBase(_prim);
+
+    ignition::math::Pose3d pose;
+    ignition::math::Vector3d scale(1, 1, 1);
+    GetTransform(_prim, _usdData, pose, scale, _linkName);
+
+    light->SetName(_prim.GetPath().GetName());
+    float intensity;
+    variantLight.GetIntensityAttr().Get(&intensity);
+    // This value was found trying to find a similar light intensity
+    // between isaac sim and gazebo. Might be wrong
+    // TODO(ahcorde): Convert the light intensity with an equation or unit
+    // conversions
+    light->SetIntensity(intensity / 10000);
+    float diffuse;
+    variantLight.GetDiffuseAttr().Get(&diffuse);
+    light->SetDiffuse(ignition::math::Color(diffuse, diffuse, diffuse, 1));
+    float specular;
+    variantLight.GetSpecularAttr().Get(&specular);
+    light->SetSpecular(ignition::math::Color(specular, specular, specular, 1));
+    light->SetCastShadows(true);
+
+    if (_prim.IsA<pxr::UsdLuxDistantLight>())
+    {
+      light->SetType(sdf::LightType::DIRECTIONAL);
+
+      // DistantLight in USD does not define height. Added some height to the
+      // light. The default sun light in ign-gazebo sdf world is 10.
+      pose += ignition::math::Pose3d(0, 0, 10, 0, 0, 0);
+      // Light emitted from a distant source along the -Z axis
+      // The pose should set the direction
+      light->SetDirection(ignition::math::Vector3d(0, 0, -1));
+    }
+    else if (_prim.IsA<pxr::UsdLuxDiskLight>())
+    {
+      light->SetType(sdf::LightType::SPOT);
+      // These parameters are not defined in USD. Added some generic values.
+      light->SetSpotInnerAngle(0.1);
+      light->SetSpotOuterAngle(0.5);
+      light->SetSpotFalloff(0.8);
+    }
+    else
+    {
+      return std::nullopt;
+    }
+    light->SetRawPose(pose);
+    return light;
+  }
+}
+}
+}

usd/src/usd_parser/USDLights.hh

@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2021 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_USD_PARSER_LIGHTS_HH
+#define USD_USD_PARSER_LIGHTS_HH
+
+#include <memory>
+#include <optional>
+#include <string>
+
+#pragma push_macro ("__DEPRECATED")
+#undef __DEPRECATED
+#include <pxr/usd/usd/prim.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/Light.hh"
+
+#include "sdf/config.hh"
+#include "sdf/usd/Export.hh"
+
+#include "sdf/usd/usd_parser/USDData.hh"
+
+namespace sdf
+{
+  // Inline bracket to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief Parse lights
+    /// Supported lights:
+    /// - UsdLuxDistantLight
+    /// - UsdLuxDiskLight
+    /// \param[in] _prim Prim to extract the light data
+    /// \param[in] _usdData Object to get transform data
+    /// \param[in] _linkName Name of the link to find the transform
+    /// \return Shared point with the sdf Light object or std::nullopt if the
+    /// light is not supported.
+    std::optional<sdf::Light> IGNITION_SDFORMAT_USD_VISIBLE ParseUSDLights(
+      const pxr::UsdPrim &_prim,
+      const USDData &_usdData,
+      const std::string &_linkName);
+  }
+}
+}
+
+#endif

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(23u, stage.USDPaths().size());
+    EXPECT_EQ(25u, stage.USDPaths().size());
   }
 
   // Check up Axis equal to Y and metersPerUnit