Skip to content

Commit 1637dda

Browse files
ahcordeadlarkin
ahcorde
and
adlarkin
authored
USD to SDF: Added Sensors (#900)
Signed-off-by: Alejandro Hernández <ahcorde@gmail.com> Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>
1 parent b45fcd0 commit 1637dda

File tree

6 files changed

+342
-1
lines changed

6 files changed

+342
-1
lines changed

‎test/usd/sensors.usda

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#usda 1.0
2+
(
3+
metersPerUnit = 0.01
4+
upAxis = "Z"
5+
)
6+
7+
def Lidar "lidar"
8+
{
9+
bool highLod = 0
10+
float horizontalFov = 360
11+
float horizontalResolution = 0.4
12+
float maxRange = 100
13+
float minRange = 0.4
14+
float rotationRate = 0
15+
float verticalFov = 30
16+
float verticalResolution = 4
17+
float3 xformOp:rotateZYX = (0, 0, 0)
18+
float3 xformOp:scale = (1, 1, 1)
19+
double3 xformOp:translate = (-6, 0, 44)
20+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX", "xformOp:scale"]
21+
float yawOffset = 0
22+
}
23+
24+
def Camera "camera" (
25+
kind = "model"
26+
)
27+
{
28+
float focalLength = 24
29+
float focusDistance = 400
30+
float3 xformOp:rotateZYX = (360, 285.3, 270)
31+
float3 xformOp:scale = (1, 1.0000027, 1.0000008)
32+
float3 xformOp:translate = (-183.61661, 0, 77.35324)
33+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX", "xformOp:scale"]
34+
}

‎usd/src/CMakeLists.txt

+7
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ set(sources
2020
usd_parser/USDLinks.cc
2121
usd_parser/USDMaterial.cc
2222
usd_parser/USDPhysics.cc
23+
usd_parser/USDSensors.cc
2324
usd_parser/USDStage.cc
2425
usd_parser/USDTransforms.cc
2526
usd_parser/USDWorld.cc
@@ -55,6 +56,7 @@ set(gtest_sources
5556
usd_parser/USDJoints_TEST.cc
5657
usd_parser/USDLinks_TEST.cc
5758
usd_parser/USDLight_TEST.cc
59+
usd_parser/USDSensors_TEST.cc
5860
usd_parser/USDStage_TEST.cc
5961
usd_parser/USDTransforms_TEST.cc
6062
Conversions_TEST.cc
@@ -77,6 +79,11 @@ if (TARGET UNIT_USDLinks_TEST)
7779
usd_parser/polygon_helper.cc)
7880
endif()
7981

82+
if (TARGET UNIT_USDSensors_TEST)
83+
target_sources(UNIT_USDSensors_TEST PRIVATE
84+
usd_parser/USDSensors.cc)
85+
endif()
86+
8087
if (TARGET UNIT_USDJoints_TEST)
8188
target_sources(UNIT_USDJoints_TEST PRIVATE
8289
usd_parser/USDJoints.cc)

‎usd/src/usd_parser/USDSensors.cc

+132
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,132 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include "USDSensors.hh"
19+
20+
#include <string>
21+
22+
#pragma push_macro ("__DEPRECATED")
23+
#undef __DEPRECATED
24+
#include "pxr/usd/usdGeom/camera.h"
25+
#include "pxr/usd/usdGeom/gprim.h"
26+
#pragma pop_macro ("__DEPRECATED")
27+
28+
#include "sdf/usd/usd_parser/USDTransforms.hh"
29+
30+
#include "sdf/Camera.hh"
31+
#include "sdf/Lidar.hh"
32+
33+
namespace sdf
34+
{
35+
// Inline bracket to help doxygen filtering.
36+
inline namespace SDF_VERSION_NAMESPACE {
37+
//
38+
namespace usd
39+
{
40+
sdf::Sensor ParseSensors(
41+
const pxr::UsdPrim &_prim,
42+
const USDData &_usdData)
43+
{
44+
sdf::Sensor sensor;
45+
46+
const std::string primType =
47+
_prim.GetPrimTypeInfo().GetTypeName().GetText();
48+
49+
ignition::math::Pose3d pose;
50+
ignition::math::Vector3d scale(1, 1, 1);
51+
GetTransform(
52+
_prim,
53+
_usdData,
54+
pose,
55+
scale,
56+
std::string(_prim.GetParent().GetPath().GetText()));
57+
58+
if (_prim.IsA<pxr::UsdGeomCamera>())
59+
{
60+
sensor.SetType(sdf::SensorType::CAMERA);
61+
62+
sdf::Camera camera;
63+
64+
auto variantCamera = pxr::UsdGeomCamera(_prim);
65+
float focalLength;
66+
pxr::GfVec2f clippingRange;
67+
variantCamera.GetFocalLengthAttr().Get(&focalLength);
68+
variantCamera.GetClippingRangeAttr().Get(&clippingRange);
69+
70+
sensor.SetName(_prim.GetPath().GetName());
71+
camera.SetName(_prim.GetPath().GetName());
72+
camera.SetHorizontalFov(20.955);
73+
camera.SetLensFocalLength(focalLength);
74+
// Camera is Y up axis, rotate the camera to match with Ignition
75+
ignition::math::Pose3d poseCamera(0, 0, 0, IGN_PI_2, 0, -IGN_PI_2);
76+
sensor.SetRawPose(pose * -poseCamera);
77+
camera.SetNearClip(clippingRange[0]);
78+
camera.SetFarClip(clippingRange[1]);
79+
camera.SetImageWidth(640);
80+
camera.SetImageHeight(480);
81+
camera.SetPixelFormat(sdf::PixelFormatType::RGB_INT8);
82+
sensor.SetCameraSensor(camera);
83+
}
84+
else if (primType == "Lidar")
85+
{
86+
sensor.SetType(sdf::SensorType::GPU_LIDAR);
87+
88+
sdf::Lidar lidar;
89+
sensor.SetName(_prim.GetPath().GetName());
90+
sensor.SetRawPose(pose);
91+
92+
float hFOV;
93+
float hResolution;
94+
float vFOV;
95+
float vResolution;
96+
_prim.GetAttribute(pxr::TfToken("horizontalFov")).Get(&hFOV);
97+
_prim.GetAttribute(
98+
pxr::TfToken("horizontalResolution")).Get(&hResolution);
99+
_prim.GetAttribute(pxr::TfToken("verticalFov")).Get(&vFOV);
100+
_prim.GetAttribute(pxr::TfToken("verticalResolution")).Get(&vResolution);
101+
hResolution = IGN_DTOR(hResolution);
102+
vResolution = IGN_DTOR(vResolution);
103+
hFOV = IGN_DTOR(hFOV);
104+
vFOV = IGN_DTOR(vFOV);
105+
106+
lidar.SetHorizontalScanMinAngle(ignition::math::Angle(-hFOV / 2));
107+
lidar.SetHorizontalScanMaxAngle(ignition::math::Angle(hFOV / 2));
108+
lidar.SetHorizontalScanResolution(1);
109+
lidar.SetHorizontalScanSamples(hFOV / hResolution);
110+
111+
lidar.SetVerticalScanMinAngle(ignition::math::Angle(-vFOV / 2));
112+
lidar.SetVerticalScanMaxAngle(ignition::math::Angle(vFOV / 2));
113+
lidar.SetVerticalScanResolution(1);
114+
lidar.SetVerticalScanSamples(vFOV / vResolution);
115+
116+
float minRange;
117+
float maxRange;
118+
_prim.GetAttribute(pxr::TfToken("minRange")).Get(&minRange);
119+
_prim.GetAttribute(pxr::TfToken("maxRange")).Get(&maxRange);
120+
121+
lidar.SetRangeMin(minRange);
122+
lidar.SetRangeMax(maxRange);
123+
lidar.SetRangeResolution(0.1);
124+
125+
sensor.SetLidarSensor(lidar);
126+
sensor.SetUpdateRate(20.0);
127+
}
128+
return sensor;
129+
}
130+
}
131+
}
132+
}

‎usd/src/usd_parser/USDSensors.hh

+51
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#ifndef USD_PARSER_SENSORS_HH
19+
#define USD_PARSER_SENSORS_HH
20+
21+
#pragma push_macro ("__DEPRECATED")
22+
#undef __DEPRECATED
23+
#include <pxr/usd/usdGeom/gprim.h>
24+
#pragma pop_macro ("__DEPRECATED")
25+
26+
#include "sdf/Sensor.hh"
27+
28+
#include "sdf/usd/usd_parser/USDData.hh"
29+
30+
#include "sdf/config.hh"
31+
32+
namespace sdf
33+
{
34+
// Inline bracket to help doxygen filtering.
35+
inline namespace SDF_VERSION_NAMESPACE {
36+
//
37+
namespace usd
38+
{
39+
/// \brief Parse a USD sensor to its SDF representation.
40+
/// Currently, camera and lidar sensors are the only types supported.
41+
/// \param[in] _prim The USD sensor prim
42+
/// \param[in] _usdData Object that holds data about the USD stage
43+
/// \return The sdf::Sensor representation of the USD sensor (_prim)
44+
sdf::Sensor ParseSensors(
45+
const pxr::UsdPrim &_prim,
46+
const USDData &_usdData);
47+
}
48+
}
49+
}
50+
51+
#endif

‎usd/src/usd_parser/USDSensors_TEST.cc

+102
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include <gtest/gtest.h>
19+
20+
#include <string>
21+
22+
// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings
23+
// are reported using #pragma message so normal diagnostic flags cannot remove
24+
// them. This workaround requires this block to be used whenever usd is
25+
// included.
26+
#pragma push_macro ("__DEPRECATED")
27+
#undef __DEPRECATED
28+
#include <pxr/usd/usd/stage.h>
29+
#pragma pop_macro ("__DEPRECATED")
30+
31+
#include <ignition/math/Pose3.hh>
32+
33+
#include "test_config.h"
34+
#include "test_utils.hh"
35+
36+
#include "USDSensors.hh"
37+
38+
#include "sdf/Sensor.hh"
39+
#include "sdf/Camera.hh"
40+
#include "sdf/Lidar.hh"
41+
#include "sdf/usd/usd_parser/USDData.hh"
42+
43+
/////////////////////////////////////////////////
44+
TEST(USDJointTest, JointTest)
45+
{
46+
const std::string filename =
47+
sdf::testing::TestFile("usd", "sensors.usda");
48+
const auto stage = pxr::UsdStage::Open(filename);
49+
ASSERT_TRUE(stage);
50+
51+
sdf::usd::USDData usdData(filename);
52+
usdData.Init();
53+
54+
const auto cameraPrim = stage->GetPrimAtPath(pxr::SdfPath(
55+
"/camera"));
56+
ASSERT_TRUE(cameraPrim);
57+
58+
sdf::Sensor sensorCamera = sdf::usd::ParseSensors(
59+
cameraPrim, usdData);
60+
61+
EXPECT_EQ(sdf::SensorType::CAMERA, sensorCamera.Type());
62+
EXPECT_EQ(ignition::math::Pose3d(-1.83617, 0, 0.773532, 0, 0.267035, -0),
63+
sensorCamera.RawPose());
64+
auto cameraSDF = sensorCamera.CameraSensor();
65+
ASSERT_TRUE(cameraSDF);
66+
EXPECT_EQ(640u, cameraSDF->ImageWidth());
67+
EXPECT_EQ(480u, cameraSDF->ImageHeight());
68+
EXPECT_EQ(sdf::PixelFormatType::RGB_INT8, cameraSDF->PixelFormat());
69+
EXPECT_DOUBLE_EQ(1000000, cameraSDF->FarClip());
70+
EXPECT_DOUBLE_EQ(1, cameraSDF->NearClip());
71+
EXPECT_DOUBLE_EQ(20.955, cameraSDF->HorizontalFov().Radian());
72+
EXPECT_DOUBLE_EQ(24.0, cameraSDF->LensFocalLength());
73+
74+
const auto lidarPrim = stage->GetPrimAtPath(pxr::SdfPath(
75+
"/lidar"));
76+
ASSERT_TRUE(lidarPrim);
77+
78+
sdf::Sensor sensorLidar = sdf::usd::ParseSensors(
79+
lidarPrim, usdData);
80+
81+
EXPECT_EQ(sdf::SensorType::GPU_LIDAR, sensorLidar.Type());
82+
auto lidarSDF = sensorLidar.LidarSensor();
83+
ASSERT_TRUE(lidarSDF);
84+
EXPECT_DOUBLE_EQ(-180.00000500895632,
85+
lidarSDF->HorizontalScanMinAngle().Degree());
86+
EXPECT_DOUBLE_EQ(180.00000500895632,
87+
lidarSDF->HorizontalScanMaxAngle().Degree());
88+
EXPECT_DOUBLE_EQ(1, lidarSDF->HorizontalScanResolution());
89+
EXPECT_DOUBLE_EQ(900, lidarSDF->HorizontalScanSamples());
90+
91+
EXPECT_DOUBLE_EQ(-15.000000417413029,
92+
lidarSDF->VerticalScanMinAngle().Degree());
93+
EXPECT_DOUBLE_EQ(15.000000417413029,
94+
lidarSDF->VerticalScanMaxAngle().Degree());
95+
EXPECT_DOUBLE_EQ(1, lidarSDF->VerticalScanResolution());
96+
EXPECT_DOUBLE_EQ(7, lidarSDF->VerticalScanSamples());
97+
98+
EXPECT_DOUBLE_EQ(0.40000000596046448, lidarSDF->RangeMin());
99+
EXPECT_DOUBLE_EQ(100, lidarSDF->RangeMax());
100+
EXPECT_DOUBLE_EQ(0.1, lidarSDF->RangeResolution());
101+
EXPECT_DOUBLE_EQ(20, sensorLidar.UpdateRate());
102+
}

‎usd/src/usd_parser/USDWorld.cc

+16-1
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@
4646
#include "USDJoints.hh"
4747
#include "USDLights.hh"
4848
#include "USDPhysics.hh"
49+
#include "USDSensors.hh"
4950
#include "USDLinks.hh"
5051

5152
#include "sdf/Collision.hh"
@@ -241,7 +242,21 @@ namespace usd
241242
continue;
242243
}
243244

244-
if (!prim.IsA<pxr::UsdGeomGprim>() && (primType != "Plane"))
245+
if (prim.IsA<pxr::UsdGeomCamera>() || primType == "Lidar")
246+
{
247+
if (!linkName.empty())
248+
{
249+
auto sensor = ParseSensors(prim, usdData);
250+
auto link = modelPtr->LinkByName(linkName);
251+
if (link != nullptr)
252+
{
253+
link->AddSensor(sensor);
254+
}
255+
}
256+
continue;
257+
}
258+
259+
if (!prim.IsA<pxr::UsdGeomGprim>() && !(primType == "Plane"))
245260
{
246261
continue;
247262
}

0 commit comments

Comments
 (0)