Skip to content

Commit a593db2

Browse files
ahcordeadlarkin
ahcorde
and
adlarkin
authored
USD to SDF: Added diff drive plugin (#904)
Signed-off-by: ahcorde <ahcorde@gmail.com> Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>
1 parent 2247225 commit a593db2

File tree

7 files changed

+112
-29
lines changed

7 files changed

+112
-29
lines changed

‎usd/include/sdf/usd/usd_parser/Parser.hh

+4-1
Original file line numberDiff line numberDiff line change
@@ -34,12 +34,15 @@ namespace sdf
3434
/// \brief Parse a USD file and convert it to a SDF file
3535
/// \param[in] _inputFilenameUsd Path of the USD file to parse
3636
/// \param[in] _outputFilenameSdf Path where the SDF file will be located
37+
/// \param[in] _useGazeboPlugins Whether _outputFilenameSdf should have
38+
/// gazebo plugins in it (true) or not (false)
3739
/// \return UsdErrors, which is a vector of UsdError objects. Each UsdError
3840
/// includes an error code and message. An empty vector indicates no error
3941
/// occurred when parsing the USD file to its SDF representation.
4042
UsdErrors IGNITION_SDFORMAT_USD_VISIBLE parseUSDFile(
4143
const std::string &_inputFilenameUsd,
42-
const std::string &_outputFilenameSdf);
44+
const std::string &_outputFilenameSdf,
45+
bool _useGazeboPlugins = true);
4346
}
4447
}
4548
}

‎usd/src/cmd/usd2sdf.cc

+11-1
Original file line numberDiff line numberDiff line change
@@ -43,12 +43,17 @@ struct Options
4343

4444
/// \brief output filename
4545
std::string outputFilename{"output.sdf"};
46+
47+
/// \brief Whether gazebo plugins should be used in the parsed sdf file
48+
/// (true) or not (false)
49+
bool useGazeboPlugins{true};
4650
};
4751

4852
void runCommand(const Options &_opt)
4953
{
5054
const auto errors =
51-
sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename);
55+
sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename,
56+
_opt.useGazeboPlugins);
5257
if (!errors.empty())
5358
{
5459
std::cerr << "Errors occurred when generating [" << _opt.outputFilename
@@ -70,6 +75,11 @@ void addFlags(CLI::App &_app)
7075
opt->outputFilename,
7176
"Output filename. Defaults to output.sdf unless otherwise specified.");
7277

78+
_app.add_option("use-gazebo-plugins",
79+
opt->useGazeboPlugins,
80+
"Whether gazebo plugins should be added to the output sdf file or not. "
81+
"Defaults to true.");
82+
7383
_app.callback([&_app, opt](){
7484
runCommand(*opt);
7585
});

‎usd/src/usd_parser/Parser.cc

+3-2
Original file line numberDiff line numberDiff line change
@@ -27,12 +27,13 @@ namespace usd
2727
{
2828
UsdErrors parseUSDFile(
2929
const std::string &_inputFilenameUsd,
30-
const std::string &_outputFilenameSdf)
30+
const std::string &_outputFilenameSdf,
31+
bool _useGazeboPlugins)
3132
{
3233
UsdErrors errors;
3334
USD2SDF usd2sdf;
3435
sdf::Root root;
35-
errors = usd2sdf.Read(_inputFilenameUsd, root);
36+
errors = usd2sdf.Read(_inputFilenameUsd, _useGazeboPlugins, root);
3637
if (!errors.empty())
3738
{
3839
return errors;

‎usd/src/usd_parser/USD2SDF.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -31,14 +31,14 @@ namespace sdf {
3131
inline namespace SDF_VERSION_NAMESPACE {
3232
namespace usd {
3333
////////////////////////////////////////////////
34-
UsdErrors USD2SDF::Read(const std::string &_fileName,
34+
UsdErrors USD2SDF::Read(const std::string &_fileName, bool _useGazeboPlugins,
3535
sdf::Root &_root)
3636
{
3737
UsdErrors errors;
3838

3939
sdf::World sdfWorld;
4040

41-
errors = parseUSDWorld(_fileName, sdfWorld);
41+
errors = parseUSDWorld(_fileName, _useGazeboPlugins, sdfWorld);
4242
if (!errors.empty())
4343
{
4444
errors.emplace_back(UsdError(

‎usd/src/usd_parser/USD2SDF.hh

+3
Original file line numberDiff line numberDiff line change
@@ -40,13 +40,16 @@ inline namespace SDF_VERSION_NAMESPACE {
4040

4141
/// \brief convert USD file to a sdf::Root object
4242
/// \param[in] _fileName string containing USD filename.
43+
/// \param[in] _useGazeboPlugins Whether _root should have gazebo plugins
44+
/// in it (true) or not (false)
4345
/// \param[out] _root Root element to populate with the equivalent sdf
4446
/// information from _fileName.
4547
/// \return UsdErrors, which is a list of UsdError objects. An empty list
4648
/// means no errors occurred when populating _root with the contents
4749
/// of _fileName
4850
public: UsdErrors Read(
4951
const std::string &_fileName,
52+
bool _useGazeboPlugins,
5053
sdf::Root &_root);
5154
};
5255
}

‎usd/src/usd_parser/USDWorld.cc

+86-23
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,7 @@ inline namespace SDF_VERSION_NAMESPACE {
6464
namespace usd
6565
{
6666
UsdErrors parseUSDWorld(const std::string &_inputFileName,
67-
sdf::World &_world)
67+
bool _useGazeboPlugins, sdf::World &_world)
6868
{
6969
UsdErrors errors;
7070
USDData usdData(_inputFileName);
@@ -189,6 +189,66 @@ namespace usd
189189
}
190190
}
191191

192+
if (_useGazeboPlugins && primType == "RosDifferentialBase")
193+
{
194+
if (!modelPtr)
195+
{
196+
errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR,
197+
"Unable to store RosDifferentialBase in a DiffDrive plugin "
198+
"because the corresponding sdf::Model object wasn't found."));
199+
return errors;
200+
}
201+
202+
auto leftWheelAttr = prim.GetAttribute(
203+
pxr::TfToken("leftWheelJointName"));
204+
auto rightWheelAttr = prim.GetAttribute(
205+
pxr::TfToken("rightWheelJointName"));
206+
auto wheelBaseAttr = prim.GetAttribute(
207+
pxr::TfToken("wheelBase"));
208+
auto wheelRadiusAttr = prim.GetAttribute(
209+
pxr::TfToken("wheelRadius"));
210+
211+
sdf::Plugin diffDrivePlugin;
212+
diffDrivePlugin.SetName("ignition::gazebo::systems::DiffDrive");
213+
diffDrivePlugin.SetFilename("ignition-gazebo-diff-drive-system");
214+
215+
std::string leftWheelName;
216+
std::string rightWheelName;
217+
float wheelBase;
218+
float wheelRadius;
219+
wheelBaseAttr.Get<float>(&wheelBase);
220+
wheelRadiusAttr.Get<float>(&wheelRadius);
221+
leftWheelAttr.Get<std::string>(&leftWheelName);
222+
rightWheelAttr.Get<std::string>(&rightWheelName);
223+
224+
sdf::ElementPtr leftJointContent(new sdf::Element);
225+
leftJointContent->SetName("left_joint");
226+
leftJointContent->AddValue("string", "", false);
227+
leftJointContent->Set(leftWheelName + "_joint");
228+
diffDrivePlugin.InsertContent(leftJointContent);
229+
230+
sdf::ElementPtr rightJointContent(new sdf::Element);
231+
rightJointContent->SetName("right_joint");
232+
rightJointContent->AddValue("string", "", false);
233+
rightJointContent->Set(rightWheelName + "_joint");
234+
diffDrivePlugin.InsertContent(rightJointContent);
235+
236+
sdf::ElementPtr wheelSeparationContent(new sdf::Element);
237+
wheelSeparationContent->SetName("wheel_separation");
238+
wheelSeparationContent->AddValue("float", "0.0", false);
239+
wheelSeparationContent->Set(wheelBase);
240+
diffDrivePlugin.InsertContent(wheelSeparationContent);
241+
242+
sdf::ElementPtr wheelRadiusContent(new sdf::Element);
243+
wheelRadiusContent->SetName("wheel_radius");
244+
wheelRadiusContent->AddValue("float", "0.0", false);
245+
wheelRadiusContent->Set(wheelRadius);
246+
diffDrivePlugin.InsertContent(wheelRadiusContent);
247+
248+
modelPtr->AddPlugin(diffDrivePlugin);
249+
continue;
250+
}
251+
192252
if (prim.IsA<pxr::UsdLuxBoundableLightBase>() ||
193253
prim.IsA<pxr::UsdLuxNonboundableLightBase>())
194254
{
@@ -340,28 +400,31 @@ namespace usd
340400
}
341401
}
342402

343-
// Add some plugins to run the Ignition Gazebo simulation
344-
sdf::Plugin physicsPlugin;
345-
physicsPlugin.SetName("ignition::gazebo::systems::Physics");
346-
physicsPlugin.SetFilename("ignition-gazebo-physics-system");
347-
_world.AddPlugin(physicsPlugin);
348-
349-
sdf::Plugin sensorsPlugin;
350-
sensorsPlugin.SetName("ignition::gazebo::systems::Sensors");
351-
sensorsPlugin.SetFilename("ignition-gazebo-sensors-system");
352-
_world.AddPlugin(sensorsPlugin);
353-
354-
sdf::Plugin userCommandsPlugin;
355-
userCommandsPlugin.SetName("ignition::gazebo::systems::UserCommands");
356-
userCommandsPlugin.SetFilename("ignition-gazebo-user-commands-system");
357-
_world.AddPlugin(userCommandsPlugin);
358-
359-
sdf::Plugin sceneBroadcasterPlugin;
360-
sceneBroadcasterPlugin.SetName(
361-
"ignition::gazebo::systems::SceneBroadcaster");
362-
sceneBroadcasterPlugin.SetFilename(
363-
"ignition-gazebo-scene-broadcaster-system");
364-
_world.AddPlugin(sceneBroadcasterPlugin);
403+
if (_useGazeboPlugins)
404+
{
405+
// Add some plugins to run the Ignition Gazebo simulation
406+
sdf::Plugin physicsPlugin;
407+
physicsPlugin.SetName("ignition::gazebo::systems::Physics");
408+
physicsPlugin.SetFilename("ignition-gazebo-physics-system");
409+
_world.AddPlugin(physicsPlugin);
410+
411+
sdf::Plugin sensorsPlugin;
412+
sensorsPlugin.SetName("ignition::gazebo::systems::Sensors");
413+
sensorsPlugin.SetFilename("ignition-gazebo-sensors-system");
414+
_world.AddPlugin(sensorsPlugin);
415+
416+
sdf::Plugin userCommandsPlugin;
417+
userCommandsPlugin.SetName("ignition::gazebo::systems::UserCommands");
418+
userCommandsPlugin.SetFilename("ignition-gazebo-user-commands-system");
419+
_world.AddPlugin(userCommandsPlugin);
420+
421+
sdf::Plugin sceneBroadcasterPlugin;
422+
sceneBroadcasterPlugin.SetName(
423+
"ignition::gazebo::systems::SceneBroadcaster");
424+
sceneBroadcasterPlugin.SetFilename(
425+
"ignition-gazebo-scene-broadcaster-system");
426+
_world.AddPlugin(sceneBroadcasterPlugin);
427+
}
365428

366429
return errors;
367430
}

‎usd/src/usd_parser/USDWorld.hh

+3
Original file line numberDiff line numberDiff line change
@@ -34,12 +34,15 @@ namespace sdf
3434
{
3535
/// \brief Parse the world information of a USD file
3636
/// \param[in] _inputFileNameUsd Path where the USD is located
37+
/// \param[in] _useGazeboPlugins Whether _world should have gazebo plugins
38+
/// in it (true) or not (false)
3739
/// \param[out] _world World interface where all USD world data is placed
3840
/// \return UsdErrors, which is a vector of UsdError objects. Each UsdError
3941
/// includes an error code and message. An empty vector indicates no error
4042
/// occurred when parsing the world information of _inputFileNameUsd
4143
UsdErrors parseUSDWorld(
4244
const std::string &_inputFileNameUsd,
45+
bool _useGazeboPlugins,
4346
sdf::World &_world);
4447
}
4548
}

0 commit comments

Comments
 (0)