sdf -> usd: Added sensor

test/sdf/usd_sensors.sdf

@@ -0,0 +1,144 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="sensors">
+    <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>
+        <inertial>
+          <mass>0.1</mass>
+          <inertia>
+            <ixx>0.000166667</ixx>
+            <iyy>0.000166667</iyy>
+            <izz>0.000166667</izz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name="imu" type="imu">
+          <always_on>1</always_on>
+          <update_rate>100</update_rate>
+          <visualize>true</visualize>
+          <topic>imu</topic>
+          <enable_metrics>true</enable_metrics>
+        </sensor>
+      </link>
+    </model>
+
+    <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>
+        <inertial>
+          <mass>0.1</mass>
+          <inertia>
+            <ixx>0.000166667</ixx>
+            <iyy>0.000166667</iyy>
+            <izz>0.000166667</izz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name="camera" type="camera">
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <visualize>true</visualize>
+          <topic>camera</topic>
+        </sensor>
+      </link>
+    </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>
+        <inertial>
+          <mass>0.1</mass>
+          <inertia>
+            <ixx>0.000166667</ixx>
+            <iyy>0.000166667</iyy>
+            <izz>0.000166667</izz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+
+        <sensor name='gpu_lidar' type='gpu_lidar'>
+          <topic>lidar</topic>
+          <update_rate>10</update_rate>
+          <lidar>
+            <scan>
+              <horizontal>
+                <samples>640</samples>
+                <resolution>1</resolution>
+                <min_angle>-1.396263</min_angle>
+                <max_angle>1.396263</max_angle>
+              </horizontal>
+              <vertical>
+                <samples>16</samples>
+                <resolution>1</resolution>
+                <min_angle>-0.261799</min_angle>
+                <max_angle>0.261799</max_angle>
+              </vertical>
+            </scan>
+            <range>
+              <min>0.08</min>
+              <max>10.0</max>
+              <resolution>0.01</resolution>
+            </range>
+          </lidar>
+          <alwaysOn>1</alwaysOn>
+          <visualize>true</visualize>
+        </sensor>
+      </link>
+    </model>
+  </world>
+</sdf>

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

@@ -0,0 +1,60 @@
+/*
+ * 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_SENSOR_HH_
+#define SDF_USD_SDF_PARSER_SENSOR_HH_
+
+#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/stage.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/Sensor.hh"
+#include "sdf/usd/Export.hh"
+#include "sdf/usd/UsdError.hh"
+#include "sdf/sdf_config.h"
+
+namespace sdf
+{
+  // 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 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);
+  }
+  }
+}
+
+#endif

usd/src/CMakeLists.txt

@@ -5,6 +5,7 @@ set(sources
   sdf_parser/Light.cc
   sdf_parser/Link.cc
   sdf_parser/Model.cc
+  sdf_parser/Sensor.cc
   sdf_parser/Visual.cc
   sdf_parser/World.cc
 )
@@ -31,6 +32,7 @@ set(gtest_sources
   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
   UsdError_TEST.cc

usd/src/sdf_parser/Link.cc

@@ -37,6 +37,7 @@
 
 #include "sdf/Link.hh"
 #include "sdf/usd/sdf_parser/Light.hh"
+#include "sdf/usd/sdf_parser/Sensor.hh"
 #include "sdf/usd/sdf_parser/Visual.hh"
 #include "../UsdUtils.hh"
 
@@ -140,6 +141,22 @@ namespace usd
       }
     }
 
+    // convert the link's sensors
+    for (uint64_t i = 0; i < _link.SensorCount(); ++i)
+    {
+      const auto sensor = *(_link.SensorByIndex(i));
+      const auto sensorPath = std::string(_path + "/" + sensor.Name());
+      UsdErrors errorsSensor = ParseSdfSensor(sensor, _stage, sensorPath);
+      if (!errorsSensor.empty())
+      {
+        errors.push_back(
+            UsdError(sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR,
+              "Error parsing sensor [" + sensor.Name() + "]"));
+        errors.insert(errors.end(), errorsSensor.begin(), errorsSensor.end());
+        return errors;
+      }
+    }
+
     // links can have lights attached to them
     for (uint64_t i = 0; i < _link.LightCount(); ++i)
     {