Skip to content

Commit 8955a74

Browse files
ahcordeahcorde
committed
Merge remote-tracking branch 'origin/ahcorde/usd_to_sdf_cmd' into ahcorde/usd_to_sdf_physics
2 parents 7252bc6 + 36ad5ff commit 8955a74

File tree

14 files changed

+671
-4
lines changed

14 files changed

+671
-4
lines changed

‎include/sdf/Camera.hh

+16
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,22 @@ namespace sdf
9595
/// \param[in] _name Name of the sensor.
9696
public: void SetName(const std::string &_name);
9797

98+
/// \brief Get whether the camera is triggered by a topic.
99+
/// \return True if the camera is triggered by a topic.
100+
public: bool Triggered() const;
101+
102+
/// \brief Set whether the camera should be triggered by a topic.
103+
/// \param[in] _triggered True if the camera should be triggered by a topic.
104+
public: void SetTriggered(bool _triggered);
105+
106+
/// \brief Get the topic that will trigger the camera.
107+
/// \return Topic for the camera trigger.
108+
public: std::string TriggerTopic() const;
109+
110+
/// \brief Set the topic that will trigger the camera.
111+
/// \param[in] _triggerTopic Topic for the camera trigger.
112+
public: void SetTriggerTopic(const std::string &_triggerTopic);
113+
98114
/// \brief Get the horizontal field of view in radians.
99115
/// \return The horizontal field of view in radians.
100116
public: ignition::math::Angle HorizontalFov() const;

‎sdf/1.9/camera.sdf

+8
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,14 @@
55
<description>An optional name for the camera.</description>
66
</attribute>
77

8+
<element name="triggered" type="bool" default="false" required="0">
9+
<description>If the camera will be triggered by a topic</description>
10+
</element> <!-- End Triggered -->
11+
12+
<element name="trigger_topic" type="string" default="__default__/trigger" required="0">
13+
<description>Name of the topic that will trigger the camera if enabled</description>
14+
</element> <!-- End Trigger_Topic -->
15+
816
<element name="horizontal_fov" type="double" default="1.047" min="0.1" max="6.283186" required="1">
917
<description>Horizontal field of view</description>
1018
</element> <!-- End Horizontal_FOV -->

‎src/Camera.cc

+38
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,12 @@ class sdf::Camera::Implementation
5555
/// \brief Name of the camera.
5656
public: std::string name = "";
5757

58+
/// \brief True if the camera is triggered by a topic.
59+
public: bool triggered{false};
60+
61+
/// \brief Camera trigger topic.
62+
public: std::string triggerTopic = "";
63+
5864
/// \brief Horizontal fied of view.
5965
public: ignition::math::Angle hfov{1.047};
6066

@@ -216,6 +222,12 @@ Errors Camera::Load(ElementPtr _sdf)
216222
// Read the camera's name
217223
loadName(_sdf, this->dataPtr->name);
218224

225+
this->dataPtr->triggered = _sdf->Get<bool>("triggered",
226+
this->dataPtr->triggered).first;
227+
228+
this->dataPtr->triggerTopic = _sdf->Get<std::string>("trigger_topic",
229+
this->dataPtr->triggerTopic).first;
230+
219231
this->dataPtr->hfov = _sdf->Get<ignition::math::Angle>("horizontal_fov",
220232
this->dataPtr->hfov).first;
221233

@@ -402,6 +414,32 @@ void Camera::SetName(const std::string &_name)
402414
this->dataPtr->name = _name;
403415
}
404416

417+
/////////////////////////////////////////////////
418+
bool Camera::Triggered() const
419+
{
420+
return this->dataPtr->triggered;
421+
}
422+
423+
424+
/////////////////////////////////////////////////
425+
void Camera::SetTriggered(bool _triggered)
426+
{
427+
this->dataPtr->triggered = _triggered;
428+
}
429+
430+
/////////////////////////////////////////////////
431+
std::string Camera::TriggerTopic() const
432+
{
433+
return this->dataPtr->triggerTopic;
434+
}
435+
436+
437+
/////////////////////////////////////////////////
438+
void Camera::SetTriggerTopic(const std::string &_triggerTopic)
439+
{
440+
this->dataPtr->triggerTopic = _triggerTopic;
441+
}
442+
405443
/////////////////////////////////////////////////
406444
ignition::math::Angle Camera::HorizontalFov() const
407445
{

‎src/Camera_TEST.cc

+8
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,14 @@ TEST(DOMCamera, Construction)
2626
cam.SetName("my_camera");
2727
EXPECT_EQ("my_camera", cam.Name());
2828

29+
EXPECT_FALSE(cam.Triggered());
30+
cam.SetTriggered(true);
31+
EXPECT_TRUE(cam.Triggered());
32+
33+
EXPECT_TRUE(cam.TriggerTopic().empty());
34+
cam.SetTriggerTopic("my_camera/trigger");
35+
EXPECT_EQ("my_camera/trigger", cam.TriggerTopic());
36+
2937
EXPECT_DOUBLE_EQ(1.047, cam.HorizontalFov().Radian());
3038
cam.SetHorizontalFov(1.45);
3139
EXPECT_DOUBLE_EQ(1.45, cam.HorizontalFov().Radian());

‎test/sdf/usd_sensors.sdf

+144
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,144 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.6">
3+
<world name="sensors">
4+
<model name="model_with_imu">
5+
<pose>4 0 3.0 0 0.0 3.14</pose>
6+
<link name="link">
7+
<pose>0.05 0.05 0.05 0 0 0</pose>
8+
<inertial>
9+
<mass>0.1</mass>
10+
<inertia>
11+
<ixx>0.000166667</ixx>
12+
<iyy>0.000166667</iyy>
13+
<izz>0.000166667</izz>
14+
</inertia>
15+
</inertial>
16+
<collision name="collision">
17+
<geometry>
18+
<box>
19+
<size>0.1 0.1 0.1</size>
20+
</box>
21+
</geometry>
22+
</collision>
23+
<visual name="visual">
24+
<geometry>
25+
<box>
26+
<size>0.1 0.1 0.1</size>
27+
</box>
28+
</geometry>
29+
</visual>
30+
<sensor name="imu" type="imu">
31+
<always_on>1</always_on>
32+
<update_rate>100</update_rate>
33+
<visualize>true</visualize>
34+
<topic>imu</topic>
35+
<enable_metrics>true</enable_metrics>
36+
</sensor>
37+
</link>
38+
</model>
39+
40+
<model name="model_with_camera">
41+
<pose>4 0 1.0 0 0.0 3.14</pose>
42+
<link name="link">
43+
<pose>0.05 0.05 0.05 0 0 0</pose>
44+
<inertial>
45+
<mass>0.1</mass>
46+
<inertia>
47+
<ixx>0.000166667</ixx>
48+
<iyy>0.000166667</iyy>
49+
<izz>0.000166667</izz>
50+
</inertia>
51+
</inertial>
52+
<collision name="collision">
53+
<geometry>
54+
<box>
55+
<size>0.1 0.1 0.1</size>
56+
</box>
57+
</geometry>
58+
</collision>
59+
<visual name="visual">
60+
<geometry>
61+
<box>
62+
<size>0.1 0.1 0.1</size>
63+
</box>
64+
</geometry>
65+
</visual>
66+
<sensor name="camera" type="camera">
67+
<camera>
68+
<horizontal_fov>1.047</horizontal_fov>
69+
<image>
70+
<width>320</width>
71+
<height>240</height>
72+
</image>
73+
<clip>
74+
<near>0.1</near>
75+
<far>100</far>
76+
</clip>
77+
</camera>
78+
<always_on>1</always_on>
79+
<update_rate>30</update_rate>
80+
<visualize>true</visualize>
81+
<topic>camera</topic>
82+
</sensor>
83+
</link>
84+
</model>
85+
86+
<model name="model_with_lidar">
87+
<static>true</static>
88+
<pose>4 0 0.5 0 0.0 3.14</pose>
89+
<link name="link">
90+
<pose>0.05 0.05 0.05 0 0 0</pose>
91+
<inertial>
92+
<mass>0.1</mass>
93+
<inertia>
94+
<ixx>0.000166667</ixx>
95+
<iyy>0.000166667</iyy>
96+
<izz>0.000166667</izz>
97+
</inertia>
98+
</inertial>
99+
<collision name="collision">
100+
<geometry>
101+
<box>
102+
<size>0.1 0.1 0.1</size>
103+
</box>
104+
</geometry>
105+
</collision>
106+
<visual name="visual">
107+
<geometry>
108+
<box>
109+
<size>0.1 0.1 0.1</size>
110+
</box>
111+
</geometry>
112+
</visual>
113+
114+
<sensor name='gpu_lidar' type='gpu_lidar'>
115+
<topic>lidar</topic>
116+
<update_rate>10</update_rate>
117+
<lidar>
118+
<scan>
119+
<horizontal>
120+
<samples>640</samples>
121+
<resolution>1</resolution>
122+
<min_angle>-1.396263</min_angle>
123+
<max_angle>1.396263</max_angle>
124+
</horizontal>
125+
<vertical>
126+
<samples>16</samples>
127+
<resolution>1</resolution>
128+
<min_angle>-0.261799</min_angle>
129+
<max_angle>0.261799</max_angle>
130+
</vertical>
131+
</scan>
132+
<range>
133+
<min>0.08</min>
134+
<max>10.0</max>
135+
<resolution>0.01</resolution>
136+
</range>
137+
</lidar>
138+
<alwaysOn>1</alwaysOn>
139+
<visualize>true</visualize>
140+
</sensor>
141+
</link>
142+
</model>
143+
</world>
144+
</sdf>

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

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
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 SDF_USD_SDF_PARSER_SENSOR_HH_
19+
#define SDF_USD_SDF_PARSER_SENSOR_HH_
20+
21+
#include <string>
22+
23+
// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings
24+
// are reported using #pragma message so normal diagnostic flags cannot remove
25+
// them. This workaround requires this block to be used whenever usd is
26+
// included.
27+
#pragma push_macro ("__DEPRECATED")
28+
#undef __DEPRECATED
29+
#include <pxr/usd/usd/stage.h>
30+
#pragma pop_macro ("__DEPRECATED")
31+
32+
#include "sdf/Sensor.hh"
33+
#include "sdf/usd/Export.hh"
34+
#include "sdf/usd/UsdError.hh"
35+
#include "sdf/sdf_config.h"
36+
37+
namespace sdf
38+
{
39+
// Inline bracket to help doxygen filtering.
40+
inline namespace SDF_VERSION_NAMESPACE {
41+
//
42+
namespace usd
43+
{
44+
/// \brief Parse an SDF sensor into a USD stage.
45+
/// \param[in] _sensor The SDF sensor to parse.
46+
/// \param[in] _stage The stage that should contain the USD representation
47+
/// of _sensor.
48+
/// \param[in] _path The USD path of the parsed sensor in _stage, which must
49+
/// be a valid USD path.
50+
/// \return UsdErrors, which is a vector of UsdError objects. Each UsdError
51+
/// includes an error code and message. An empty vector indicates no error.
52+
UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfSensor(
53+
const sdf::Sensor &_sensor,
54+
pxr::UsdStageRefPtr &_stage,
55+
const std::string &_path);
56+
}
57+
}
58+
}
59+
60+
#endif

‎usd/include/sdf/usd/usd_parser/utils.hh ‎usd/include/sdf/usd/usd_parser/USDMaterial.hh

File renamed without changes.

‎usd/src/CMakeLists.txt

+3-1
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ set(sources
55
sdf_parser/Light.cc
66
sdf_parser/Link.cc
77
sdf_parser/Model.cc
8+
sdf_parser/Sensor.cc
89
sdf_parser/Visual.cc
910
sdf_parser/World.cc
1011
usd_parser/Parse_USD.cc
@@ -13,7 +14,7 @@ set(sources
1314
usd_parser/USD2SDF.cc
1415
usd_parser/USDData.cc
1516
usd_parser/USDStage.cc
16-
usd_parser/utils.cc
17+
usd_parser/USDMaterial.cc
1718
)
1819

1920
ign_add_component(usd SOURCES ${sources} GET_TARGET_NAME usd_target)
@@ -37,6 +38,7 @@ set(gtest_sources
3738
sdf_parser/Joint_Sdf2Usd_TEST.cc
3839
sdf_parser/Light_Sdf2Usd_TEST.cc
3940
sdf_parser/Link_Sdf2Usd_TEST.cc
41+
sdf_parser/Sensors_Sdf2Usd_TEST.cc
4042
sdf_parser/Visual_Sdf2Usd_TEST.cc
4143
sdf_parser/World_Sdf2Usd_TEST.cc
4244
usd_parser/USDData_TEST.cc

‎usd/src/sdf_parser/Link.cc

+17
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737

3838
#include "sdf/Link.hh"
3939
#include "sdf/usd/sdf_parser/Light.hh"
40+
#include "sdf/usd/sdf_parser/Sensor.hh"
4041
#include "sdf/usd/sdf_parser/Visual.hh"
4142
#include "../UsdUtils.hh"
4243

@@ -140,6 +141,22 @@ namespace usd
140141
}
141142
}
142143

144+
// convert the link's sensors
145+
for (uint64_t i = 0; i < _link.SensorCount(); ++i)
146+
{
147+
const auto sensor = *(_link.SensorByIndex(i));
148+
const auto sensorPath = std::string(_path + "/" + sensor.Name());
149+
UsdErrors errorsSensor = ParseSdfSensor(sensor, _stage, sensorPath);
150+
if (!errorsSensor.empty())
151+
{
152+
errors.push_back(
153+
UsdError(sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR,
154+
"Error parsing sensor [" + sensor.Name() + "]"));
155+
errors.insert(errors.end(), errorsSensor.begin(), errorsSensor.end());
156+
return errors;
157+
}
158+
}
159+
143160
// links can have lights attached to them
144161
for (uint64_t i = 0; i < _link.LightCount(); ++i)
145162
{

0 commit comments

Comments
 (0)