Add sdf -> usd light functionality to usd component

usd/include/sdf/usd/sdf_parser/Light.hh

@@ -0,0 +1,57 @@
+/*
+ * 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_SDF_PARSER_LIGHT_HH_
+#define SDF_USD_SDF_PARSER_LIGHT_HH_
+
+#include <string>
+
+// TODO(adlarkin):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/stage.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/config.hh"
+#include "sdf/system_util.hh"
+#include "sdf/Light.hh"
+
+namespace sdf
+{
+  // Inline bracke to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief Parse an SDF light into a USD stage.
+    /// \param[in] _light The SDF light to parse.
+    /// \param[in] _stage The stage that should contain the USD representation
+    /// of _light.
+    /// \param[in] _path The USD path of the parsed light in _stage, which must
+    /// be a valid USD path.
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error.
+    sdf::Errors SDFORMAT_VISIBLE ParseSdfLight(const sdf::Light &_light,
+        pxr::UsdStageRefPtr &_stage, const std::string &_path);
+  }
+  }
+}
+
+#endif

usd/include/sdf/usd/sdf_parser/Utils.hh

@@ -0,0 +1,94 @@
+/*
+ * 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_SDF_PARSER_UTILS_HH_
+#define SDF_USD_SDF_PARSER_UTILS_HH_
+
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Quaternion.hh>
+
+// TODO(adlarkin):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/base/gf/vec3d.h>
+#include <pxr/usd/usd/stage.h>
+#include <pxr/usd/usdGeom/xformCommonAPI.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/SemanticPose.hh"
+#include "sdf/config.hh"
+#include "sdf/system_util.hh"
+
+namespace sdf
+{
+  // Inline bracke to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief Get an object's pose w.r.t. its parent.
+    /// \param[in] _obj The object whose pose should be computed/retrieved.
+    /// \tparam T An object that has the following method signatures:
+    ///   sdf::SemanticPose SemanticPose();
+    /// \return _obj's pose w.r.t. its parent. If there was an error computing
+    /// this pose, the pose's position will be NaNs.
+    template <typename T>
+    inline ignition::math::Pose3d SDFORMAT_VISIBLE PoseWrtParent(const T &_obj)
+    {
+      ignition::math::Pose3d pose(ignition::math::Vector3d::NaN,
+          ignition::math::Quaterniond::Identity);
+      auto errors = _obj.SemanticPose().Resolve(pose, "");
+      if (!errors.empty())
+      {
+        std::cerr << "Errors occurred when resolving the pose of ["
+                  << _obj.Name() << "] w.r.t its parent:\n\t" << errors;
+      }
+      return pose;
+    }
+
+    /// \brief Set the pose of a USD prim.
+    /// \param[in] _pose The pose to set.
+    /// \param[in] _stage The stage that contains the USD prim at path _usdPath.
+    /// \param[in] _usdPath The path to the USD prim that should have its
+    /// pose modified to match _pose.
+    inline void SDFORMAT_VISIBLE SetPose(const ignition::math::Pose3d &_pose,
+        pxr::UsdStageRefPtr &_stage, const pxr::SdfPath &_usdPath)
+    {
+      pxr::UsdGeomXformCommonAPI geomXformAPI(_stage->GetPrimAtPath(_usdPath));
+
+      const auto &position = _pose.Pos();
+      geomXformAPI.SetTranslate(pxr::GfVec3d(
+            position.X(), position.Y(), position.Z()));
+
+      const auto &rotation = _pose.Rot();
+      // roll/pitch/yaw from ignition::math::Pose3d are in radians, but this API
+      // call expects degrees
+      geomXformAPI.SetRotate(pxr::GfVec3f(
+            ignition::math::Angle(rotation.Roll()).Degree(),
+            ignition::math::Angle(rotation.Pitch()).Degree(),
+            ignition::math::Angle(rotation.Yaw()).Degree()));
+    }
+  }
+  }
+}
+
+#endif

usd/src/CMakeLists.txt

@@ -1,4 +1,5 @@
 set(sources
+  sdf_parser/Light.cc
   sdf_parser/World.cc
 )
 
@@ -17,6 +18,7 @@ target_link_libraries(${usd_target}
 
 set(gtest_sources
   sdf_parser/sdf2usd_TEST.cc
+  sdf_parser/Light_Sdf2Usd_TEST.cc
   sdf_parser/World_Sdf2Usd_TEST.cc
 )
 

usd/src/sdf_parser/Light.cc

@@ -0,0 +1,100 @@
+/*
+ * 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/sdf_parser/Light.hh"
+
+#include <string>
+
+#include <pxr/base/tf/token.h>
+#include <pxr/usd/sdf/path.h>
+#include <pxr/usd/sdf/types.h>
+#include <pxr/usd/usdLux/diskLight.h>
+#include <pxr/usd/usdLux/distantLight.h>
+#include <pxr/usd/usdLux/lightAPI.h>
+#include <pxr/usd/usdLux/sphereLight.h>
+#include <pxr/usd/usd/prim.h>
+#pragma push_macro ("__DEPRECATED")
+#undef __DEPRECATED
+#include <pxr/usd/usd/stage.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/Light.hh"
+#include "sdf/usd/sdf_parser/Utils.hh"
+
+namespace sdf
+{
+// Inline bracke to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+//
+namespace usd
+{
+  sdf::Errors ParseSdfLight(const sdf::Light &_light,
+      pxr::UsdStageRefPtr &_stage, const std::string &_path)
+  {
+    const pxr::SdfPath sdfLightPath(_path);
+    sdf::Errors errors;
+    switch (_light.Type())
+    {
+      case sdf::LightType::POINT:
+        {
+          auto pointLight =
+            pxr::UsdLuxSphereLight::Define(_stage, sdfLightPath);
+          pointLight.CreateTreatAsPointAttr().Set(true);
+        }
+        break;
+      case sdf::LightType::SPOT:
+        pxr::UsdLuxDiskLight::Define(_stage, sdfLightPath);
+        break;
+      case sdf::LightType::DIRECTIONAL:
+        pxr::UsdLuxDistantLight::Define(_stage, sdfLightPath);
+        break;
+      case sdf::LightType::INVALID:
+      default:
+        errors.push_back(sdf::Error(sdf::ErrorCode::ATTRIBUTE_INCORRECT_TYPE,
+              "The light type that was given cannot be parsed to USD."));
+        return errors;
+    }
+
+    // TODO(adlarkin) incorporate sdf::Light's <direction> somehow? According
+    // to the USD API, things like UsdLuxDistantLight and UsdLuxDiskLight emit
+    // light along the -Z axis, so I'm not sure if this can be changed.
+    sdf::usd::SetPose(sdf::usd::PoseWrtParent(_light), _stage, sdfLightPath);
+
+    // This is a workaround to set the light's intensity attribute. Using the
+    // UsdLuxLightAPI sets the light's "inputs:intensity" attribute, but isaac
+    // sim reads the light's "intensity" attribute. Both inputs:intensity and
+    // intensity are set to provide flexibility with other USD renderers
+    const float usdLightIntensity =
+      static_cast<float>(_light.Intensity()) * 100.0f;
+    auto lightPrim = _stage->GetPrimAtPath(sdfLightPath);
+    lightPrim.CreateAttribute(pxr::TfToken("intensity"),
+        pxr::SdfValueTypeNames->Float, false).Set(usdLightIntensity);
+    auto lightAPI = pxr::UsdLuxLightAPI(lightPrim);
+    lightAPI.CreateIntensityAttr().Set(usdLightIntensity);
+
+    // TODO(adlarkin) Other things to look at (there may be more):
+    // * exposure - I don't think SDF has this, but USD does. See the
+    //    UsdLightAPI::GetExposureAttr method
+    // * diffuse, specular - USD takes it as a scalar multiplier,
+    //    SDF takes it as a RGB color vector. Perhaps this can be handled by
+    //    applying a material to a light
+
+    return errors;
+  }
+}
+}
+}