review feedback

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

test/sdf/camera_lidar_sensor.sdf → test/sdf/usd_sensors.sdf

@@ -1,17 +1,7 @@
 <?xml version="1.0" ?>
-<!--
-  Ignition Gazebo sensor demo for sensors that do not require rendering.
-
-  Listen to sensor readings:
-
-    Altimeter: ign topic -e -t /altimeter
-    Imu: ign topic -e -t /imu
-    Magnetometer: ign topic -e -t /magnetometer
-    Air pressure: ign topic -e -t /air_pressure
--->
 <sdf version="1.6">
   <world name="sensors">
-    <model name="sensors_box">
+    <model name="model_with_imu">
       <pose>4 0 3.0 0 0.0 3.14</pose>
       <link name="link">
         <pose>0.05 0.05 0.05 0 0 0</pose>
@@ -46,7 +36,8 @@
         </sensor>
       </link>
     </model>
-    <model name="camera">
+
+    <model name="model_with_camera">
       <pose>4 0 1.0 0 0.0 3.14</pose>
       <link name="link">
         <pose>0.05 0.05 0.05 0 0 0</pose>
@@ -93,6 +84,7 @@
     </model>
 
     <model name="model_with_lidar">
+      <static>true</static>
       <pose>4 0 0.5 0 0.0 3.14</pose>
       <link name="link">
         <pose>0.05 0.05 0.05 0 0 0</pose>
@@ -147,7 +139,6 @@
           <visualize>true</visualize>
         </sensor>
       </link>
-      <static>true</static>
     </model>
   </world>
 </sdf>

usd/include/sdf/usd/sdf_parser/Sensor.hh

@@ -15,12 +15,12 @@
  *
 */
 
-#ifndef SDF_PARSER_SENSOR_HH_
-#define SDF_PARSER_SENSOR_HH_
+#ifndef SDF_USD_SDF_PARSER_SENSOR_HH_
+#define SDF_USD_SDF_PARSER_SENSOR_HH_
 
 #include <string>
 
-// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings
+// 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.
@@ -36,23 +36,23 @@
 
 namespace sdf
 {
-  // Inline bracke to help doxygen filtering.
+  // Inline bracket to help doxygen filtering.
   inline namespace SDF_VERSION_NAMESPACE {
   //
   namespace usd
   {
-  /// \brief Parse an SDF sensor into a USD stage.
-  /// \param[in] _sensor The SDF sensor to parse.
-  /// \param[in] _stage The stage that should contain the USD representation
-  /// of _sensor.
-  /// \param[in] _path The USD path of the parsed sensor 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.
-  UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfSensor(
-    const sdf::Sensor &_sensor,
-    pxr::UsdStageRefPtr &_stage,
-    const std::string &_path);
+    /// \brief Parse an SDF sensor into a USD stage.
+    /// \param[in] _sensor The SDF sensor to parse.
+    /// \param[in] _stage The stage that should contain the USD representation
+    /// of _sensor.
+    /// \param[in] _path The USD path of the parsed sensor in _stage, which must
+    /// be a valid USD path.
+    /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError
+    /// includes an error code and message. An empty vector indicates no error.
+    UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfSensor(
+      const sdf::Sensor &_sensor,
+      pxr::UsdStageRefPtr &_stage,
+      const std::string &_path);
   }
   }
 }

usd/src/CMakeLists.txt

@@ -30,6 +30,8 @@ set(gtest_sources
   sdf_parser/Joint_Sdf2Usd_TEST.cc
   sdf_parser/Light_Sdf2Usd_TEST.cc
   sdf_parser/Link_Sdf2Usd_TEST.cc
+  # TODO(adlarkin) add a test for SDF -> USD models once model parsing
+  # functionality is complete
   sdf_parser/Sensors_Sdf2Usd_TEST.cc
   sdf_parser/Visual_Sdf2Usd_TEST.cc
   sdf_parser/World_Sdf2Usd_TEST.cc

usd/src/sdf_parser/Sensor.cc

@@ -19,7 +19,7 @@
 
 #include <string>
 
-// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings
+// 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.
@@ -43,30 +43,47 @@
 
 namespace sdf
 {
-// Inline bracke to help doxygen filtering.
+// Inline bracket to help doxygen filtering.
 inline namespace SDF_VERSION_NAMESPACE {
 //
 namespace usd
 {
-  bool ParseSdfCameraSensor(const sdf::Sensor &_sensor,
+  /// \brief Create a USD camera from a SDF camera object.
+  /// \param[in] _sensor The SDF camera object
+  /// \param[in] _stage The stage that contains the definition of the USD
+  /// camera
+  /// \param[in] _path The path where the USD representation of _sensor should
+  /// be defined in _stage
+  /// \return UsdErrors, which is a list of UsdError objects. An empty list
+  /// means there were no issues defining the USD representation of _sensor in
+  /// _stage
+  UsdErrors ParseSdfCameraSensor(const sdf::Sensor &_sensor,
     pxr::UsdStageRefPtr &_stage, const pxr::SdfPath &_path)
   {
+    UsdErrors errors;
+
     auto usdCamera = pxr::UsdGeomCamera::Define(_stage, _path);
+    if (!usdCamera)
+    {
+      errors.push_back(UsdError(
+          sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION,
+          "Unable to define a USD camera at path [" + _path.GetString() + "]"));
+      return errors;
+    }
 
-    auto sdfCamera = _sensor.CameraSensor();
+    const auto sdfCamera = _sensor.CameraSensor();
 
     // TODO(adlarkin) check units to make sure they match (no documented
     // units for SDF)
-    // When then focal length is not defined in the SDF the default value is 1,
-    // The following condition adapt this value to USD.
+    // When then focal length is not defined in SDF, the default value is 1
     if (!ignition::math::equal(sdfCamera->LensFocalLength(), 1.0))
     {
       usdCamera.CreateFocalLengthAttr().Set(
           static_cast<float>(sdfCamera->LensFocalLength()));
     }
     else
     {
-      // TODO(ahcorde): The default value in USD is 50, but something more
+      // The default value in USD is 50, but something more
       // similar to ignition Gazebo is 40.
       usdCamera.CreateFocalLengthAttr().Set(
           static_cast<float>(40.0f));
@@ -77,19 +94,34 @@ namespace usd
 
     usdCamera.CreateHorizontalApertureAttr().Set(
       static_cast<float>(sdfCamera->HorizontalFov().Degree()));
-    return true;
+    return errors;
   }
 
-  bool ParseSdfLidarSensor(const sdf::Sensor &_sensor,
+  /// \brief Create a USD lidar sensor from a SDF lidar sensor object.
+  /// \param[in] _sensor The SDF lidar sensor object
+  /// \param[in] _stage The stage that contains the definition of the USD
+  /// lidar sensor
+  /// \param[in] _path The path where the USD representation of _sensor should
+  /// be defined in _stage
+  /// \return UsdErrors, which is a list of UsdError objects. An empty list
+  /// means there were no issues defining the USD representation of _sensor in
+  /// _stage
+  UsdErrors ParseSdfLidarSensor(const sdf::Sensor &_sensor,
     pxr::UsdStageRefPtr &_stage, const pxr::SdfPath &_path)
   {
-    pxr::UsdGeomXform::Define(
-      _stage,
-      pxr::SdfPath(_path.GetString() + "/sensor"));
-    auto lidarPrim = _stage->GetPrimAtPath(
-      pxr::SdfPath(_path.GetString() + "/sensor"));
+    UsdErrors errors;
 
-    auto sdfLidar = _sensor.LidarSensor();
+    pxr::UsdGeomXform::Define(_stage, _path);
+    auto lidarPrim = _stage->GetPrimAtPath(_path);
+    if (!lidarPrim)
+    {
+      errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH,
+            "Unable to find a lidar sensor prim at path ["
+            + _path.GetString() + "]"));
+      return errors;
+    }
+
+    const auto sdfLidar = _sensor.LidarSensor();
 
     lidarPrim.SetTypeName(pxr::TfToken("Lidar"));
     lidarPrim.CreateAttribute(pxr::TfToken("minRange"),
@@ -100,7 +132,6 @@ namespace usd
           static_cast<float>(sdfLidar->RangeMax()));
     const auto horizontalFov = sdfLidar->HorizontalScanMaxAngle() -
       sdfLidar->HorizontalScanMinAngle();
-    // TODO(adlarkin) double check if these FOV calculations are correct
     lidarPrim.CreateAttribute(pxr::TfToken("horizontalFov"),
         pxr::SdfValueTypeNames->Float, false).Set(
           static_cast<float>(horizontalFov.Degree()));
@@ -118,28 +149,43 @@ namespace usd
 
     // TODO(adlarkin) incorporate SDF lidar's horizontal/samples and
     // vertical/samples values somehow? There is a "rotationRate"
-    // attribute for the USD sensor, I wonder if these are all related somehow
+    // attribute for the USD sensor, which might be related
 
-    return true;
+    return errors;
   }
 
-  bool ParseSdfImuSensor(const sdf::Sensor &/*_sensor*/,
-    pxr::UsdStageRefPtr &_stage, pxr::SdfPath &_path)
+  /// \brief Create a USD IMU sensor
+  /// \param[in] _stage The stage that contains the definition of the USD
+  /// IMU sensor
+  /// \param[in] _path The path where the USD IMU should be defined in _stage
+  /// \return UsdErrors, which is a list of UsdError objects. An empty list
+  /// means there were no issues defining the USD IMU sensor in _stage
+  UsdErrors ParseSdfImuSensor(pxr::UsdStageRefPtr &_stage, pxr::SdfPath &_path)
   {
+    UsdErrors errors;
+
     // for now, IMUs are defined as a cube geometry named "imu"
     // (there will be an IMU schema released upstream in the future).
     // It should be noted that the Carter robot example from isaac sim sample
     // assets has its IMU prim labeled with a "kind = model", but
     // https://graphics.pixar.com/usd/release/glossary.html#usdglossary-kind
-    // says, “'model' is considered an abstract type and should not be assigned as
-    // any prim’s kind." So, "kind = model" is not applied to the IMU prim here.
+    // says, “'model' is considered an abstract type and should not be assigned
+    // as any prim’s kind." So, "kind = model" is not applied to the IMU prim
+    // here.
     // TODO(adlarkin) update this code when an IMU schema is released
     _path = _path.ReplaceName(pxr::TfToken("imu"));
     auto usdCube = pxr::UsdGeomCube::Define(_stage, pxr::SdfPath(_path));
+    if (!usdCube)
+    {
+      errors.push_back(
+          UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION,
+            "Unable to define an IMU at path [" + _path.GetString() + "]"));
+      return errors;
+    }
     // Set the size of the box very small
     usdCube.CreateSizeAttr().Set(0.001);
 
-    return true;
+    return errors;
   }
 
   UsdErrors ParseSdfSensor(const sdf::Sensor &_sensor,
@@ -148,18 +194,17 @@ namespace usd
     UsdErrors errors;
     pxr::SdfPath sdfSensorPath(_path);
 
-    bool typeParsed = false;
     switch (_sensor.Type())
     {
       case sdf::SensorType::CAMERA:
-        typeParsed = ParseSdfCameraSensor(_sensor, _stage, sdfSensorPath);
+        errors = ParseSdfCameraSensor(_sensor, _stage, sdfSensorPath);
         break;
       case sdf::SensorType::LIDAR:
       case sdf::SensorType::GPU_LIDAR:
-        typeParsed = ParseSdfLidarSensor(_sensor, _stage, sdfSensorPath);
+        errors = ParseSdfLidarSensor(_sensor, _stage, sdfSensorPath);
         break;
       case sdf::SensorType::IMU:
-        typeParsed = ParseSdfImuSensor(_sensor, _stage, sdfSensorPath);
+        errors = ParseSdfImuSensor(_stage, sdfSensorPath);
         break;
       case sdf::SensorType::CONTACT:
         // TODO(adlarkin) figure out how to convert contact sensor. I found the
@@ -171,7 +216,7 @@ namespace usd
             "This type of sensor is not supported"));
     }
 
-    if (typeParsed)
+    if (errors.empty())
     {
       ignition::math::Pose3d pose;
       auto poseErrors = sdf::usd::PoseWrtParent(_sensor, pose);
@@ -185,7 +230,7 @@ namespace usd
       {
         // Camera sensors are upAxis equal to "Y", we need to rotate the camera
         // properly.
-        ignition::math::Pose3d poseCamera(0, 0, 0, 1.57, 0, -1.57);
+        const ignition::math::Pose3d poseCamera(0, 0, 0, 1.57, 0, -1.57);
         usd::SetPose(
           pose * poseCamera, _stage, sdfSensorPath);
       }