diff --git a/test/sdf/usd_sensors.sdf b/test/sdf/usd_sensors.sdf new file mode 100644 index 000000000..eb25d97fc --- /dev/null +++ b/test/sdf/usd_sensors.sdf @@ -0,0 +1,144 @@ + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 100 + true + imu + true + + + + + + 4 0 1.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + + + + true + 4 0 0.5 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + lidar + 10 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 16 + 1 + -0.261799 + 0.261799 + + + + 0.08 + 10.0 + 0.01 + + + 1 + true + + + + + diff --git a/usd/include/sdf/usd/sdf_parser/Sensor.hh b/usd/include/sdf/usd/sdf_parser/Sensor.hh new file mode 100644 index 000000000..97529c173 --- /dev/null +++ b/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 + +// 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 +#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 diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 0623359d4..72db29aad 100644 --- a/usd/src/CMakeLists.txt +++ b/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 diff --git a/usd/src/sdf_parser/Link.cc b/usd/src/sdf_parser/Link.cc index fbd02dbf4..973cd5bf1 100644 --- a/usd/src/sdf_parser/Link.cc +++ b/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) { diff --git a/usd/src/sdf_parser/Sensor.cc b/usd/src/sdf_parser/Sensor.cc new file mode 100644 index 000000000..728b503d4 --- /dev/null +++ b/usd/src/sdf_parser/Sensor.cc @@ -0,0 +1,246 @@ +/* + * 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/Sensor.hh" + +#include + +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Camera.hh" +#include "sdf/Lidar.hh" +#include "sdf/Sensor.hh" +#include "../UsdUtils.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + /// \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; + } + + const auto sdfCamera = _sensor.CameraSensor(); + + // 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(sdfCamera->LensFocalLength())); + } + else + { + // The default value in USD is 50, but something more + // similar to ignition Gazebo is 40. + usdCamera.CreateFocalLengthAttr().Set( + static_cast(40.0f)); + } + usdCamera.CreateClippingRangeAttr().Set(pxr::GfVec2f( + static_cast(sdfCamera->NearClip()), + static_cast(sdfCamera->FarClip()))); + + usdCamera.CreateHorizontalApertureAttr().Set( + static_cast(sdfCamera->HorizontalFov().Degree())); + return errors; + } + + /// \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) + { + UsdErrors errors; + + 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"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(sdfLidar->RangeMin())); + lidarPrim.CreateAttribute(pxr::TfToken("maxRange"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(sdfLidar->RangeMax())); + const auto horizontalFov = sdfLidar->HorizontalScanMaxAngle() - + sdfLidar->HorizontalScanMinAngle(); + lidarPrim.CreateAttribute(pxr::TfToken("horizontalFov"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(horizontalFov.Degree())); + const auto verticalFov = sdfLidar->VerticalScanMaxAngle() - + sdfLidar->VerticalScanMinAngle(); + lidarPrim.CreateAttribute(pxr::TfToken("verticalFov"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(verticalFov.Degree())); + lidarPrim.CreateAttribute(pxr::TfToken("horizontalResolution"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(sdfLidar->HorizontalScanResolution())); + lidarPrim.CreateAttribute(pxr::TfToken("verticalResolution"), + pxr::SdfValueTypeNames->Float, false).Set( + static_cast(sdfLidar->VerticalScanResolution())); + + // TODO(adlarkin) incorporate SDF lidar's horizontal/samples and + // vertical/samples values somehow? There is a "rotationRate" + // attribute for the USD sensor, which might be related + + return errors; + } + + /// \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. + // 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 errors; + } + + UsdErrors ParseSdfSensor(const sdf::Sensor &_sensor, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + pxr::SdfPath sdfSensorPath(_path); + + switch (_sensor.Type()) + { + case sdf::SensorType::CAMERA: + errors = ParseSdfCameraSensor(_sensor, _stage, sdfSensorPath); + break; + case sdf::SensorType::LIDAR: + case sdf::SensorType::GPU_LIDAR: + errors = ParseSdfLidarSensor(_sensor, _stage, sdfSensorPath); + break; + case sdf::SensorType::IMU: + errors = ParseSdfImuSensor(_stage, sdfSensorPath); + break; + case sdf::SensorType::CONTACT: + // TODO(adlarkin) figure out how to convert contact sensor. I found the + // following docs, but they seem to require isaac sim specific packages: + // https://docs.omniverse.nvidia.com/py/isaacsim/source/extensions/omni.isaac.contact_sensor/docs/index.html + default: + errors.push_back( + UsdError(sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "This type of sensor [" + _sensor.TypeStr() + + "] is not supported")); + } + + if (errors.empty()) + { + ignition::math::Pose3d pose; + auto poseErrors = sdf::usd::PoseWrtParent(_sensor, pose); + if (!poseErrors.empty()) + { + errors.insert(errors.end(), poseErrors.begin(), poseErrors.end()); + return errors; + } + + if (_sensor.Type() == sdf::SensorType::CAMERA) + { + // Camera sensors are upAxis equal to "Y", we need to rotate the camera + // properly. + const ignition::math::Pose3d poseCamera( + 0, 0, 0, IGN_PI_2, 0, -IGN_PI_2); + usd::SetPose( + pose * poseCamera, _stage, sdfSensorPath); + } + else + { + usd::SetPose(pose, _stage, sdfSensorPath); + } + } + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Sensors_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Sensors_Sdf2Usd_TEST.cc new file mode 100644 index 000000000..4eefdf32e --- /dev/null +++ b/usd/src/sdf_parser/Sensors_Sdf2Usd_TEST.cc @@ -0,0 +1,128 @@ +/* + * Copyright 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 + +#include + +// 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 +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include + +#include "sdf/usd/sdf_parser/World.hh" +#include "sdf/Root.hh" +#include "test_config.h" +#include "test_utils.hh" +#include "../UsdTestUtils.hh" + +///////////////////////////////////////////////// +// Fixture that creates a USD stage for each test case. +class UsdStageFixture : public::testing::Test +{ + public: UsdStageFixture() = default; + + protected: void SetUp() override + { + this->stage = pxr::UsdStage::CreateInMemory(); + ASSERT_TRUE(this->stage); + } + + public: pxr::UsdStageRefPtr stage; +}; + +///////////////////////////////////////////////// +TEST_F(UsdStageFixture, Sensors) +{ + sdf::setFindCallback(sdf::usd::testing::findFileCb); + ignition::common::addFindFileURICallback( + std::bind(&sdf::usd::testing::FindResourceUri, std::placeholders::_1)); + + const auto path = sdf::testing::TestFile("sdf", "usd_sensors.sdf"); + sdf::Root root; + + ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root)); + ASSERT_EQ(1u, root.WorldCount()); + auto world = root.WorldByIndex(0u); + + const auto worldPath = std::string("/" + world->Name()); + auto usdErrors = sdf::usd::ParseSdfWorld(*world, stage, worldPath); + EXPECT_TRUE(usdErrors.empty()); + + auto worldPrim = this->stage->GetPrimAtPath(pxr::SdfPath(worldPath)); + ASSERT_TRUE(worldPrim); + + const std::string cameraPath = worldPath + "/model_with_camera"; + const std::string cameraLinkPath = cameraPath + "/link"; + const pxr::SdfPath cameraSensorPath(cameraLinkPath + "/camera"); + const auto usdCamera = pxr::UsdGeomCamera::Get(this->stage, cameraSensorPath); + ASSERT_TRUE(usdCamera); + + float focalLength; + pxr::GfVec2f clippingRange; + float horizontalAperture; + usdCamera.GetFocalLengthAttr().Get(&focalLength); + usdCamera.GetClippingRangeAttr().Get(&clippingRange); + usdCamera.GetHorizontalApertureAttr().Get(&horizontalAperture); + EXPECT_FLOAT_EQ(40.0f, focalLength); + EXPECT_EQ(pxr::GfVec2f(0.1, 100), clippingRange); + EXPECT_FLOAT_EQ(59.98868, horizontalAperture); + + const std::string lidarPath = worldPath + "/model_with_lidar"; + const std::string lidarLinkPath = lidarPath + "/link"; + const std::string lidarSensorPath = lidarLinkPath + "/gpu_lidar"; + const auto lidarSensor = this->stage->GetPrimAtPath( + pxr::SdfPath(lidarSensorPath)); + ASSERT_TRUE(lidarSensor); + float hFOV; + float hResolution; + float vFOV; + float vResolution; + float minRange; + float maxRange; + lidarSensor.GetAttribute(pxr::TfToken("minRange")).Get(&minRange); + lidarSensor.GetAttribute(pxr::TfToken("maxRange")).Get(&maxRange); + lidarSensor.GetAttribute(pxr::TfToken("horizontalFov")).Get(&hFOV); + lidarSensor.GetAttribute(pxr::TfToken("horizontalResolution")).Get(&hResolution); + lidarSensor.GetAttribute(pxr::TfToken("verticalFov")).Get(&vFOV); + lidarSensor.GetAttribute(pxr::TfToken("verticalResolution")).Get(&vResolution); + EXPECT_FLOAT_EQ(10.0f, maxRange); + EXPECT_FLOAT_EQ(0.08f, minRange); + EXPECT_FLOAT_EQ(159.99995f, hFOV); + EXPECT_FLOAT_EQ(1.0f, hResolution); + EXPECT_FLOAT_EQ(29.999956f, vFOV); + EXPECT_FLOAT_EQ(1.0f, vResolution); + + const std::string imuPath = worldPath + "/model_with_imu"; + const std::string imuLinkPath = imuPath + "/link"; + const pxr::SdfPath imuSensorPath(imuLinkPath + "/imu"); + const auto usdIMUCube = pxr::UsdGeomCube::Get(this->stage, imuSensorPath); + ASSERT_TRUE(usdIMUCube); + double imuSize; + usdIMUCube.GetSizeAttr().Get(&imuSize); + EXPECT_DOUBLE_EQ(0.001, imuSize); +}