From 1668f316e8e93c7f276d4bdbc46b75dd3ce066ac Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Wed, 5 Mar 2025 23:33:49 +0800 Subject: [PATCH] Standalone `gz sdf` executable (#1539) Signed-off-by: Steve Peters Signed-off-by: Saurabh Kamat Signed-off-by: Addisu Z. Taddese Co-authored-by: Steve Peters Co-authored-by: Addisu Z. Taddese Co-authored-by: Jose Luis Rivero --- sdf/CMakeLists.txt | 36 ++-- src/CMakeLists.txt | 12 -- src/cmd/CMakeLists.txt | 48 ++++- src/cmd/cmdsdformat.rb.in | 248 ++-------------------- src/cmd/gz.cc | 356 ++++++++++++++++++++++++++++++++ src/cmd/gz.hh | 69 +++++++ src/{ => cmd}/gz_TEST.cc | 0 src/cmd/sdf.bash_completion.sh | 1 + src/cmd/sdf_main.cc | 218 ++++++++++++++++++++ src/gz.cc | 361 --------------------------------- src/gz.hh | 40 ---- 11 files changed, 717 insertions(+), 672 deletions(-) create mode 100644 src/cmd/gz.cc create mode 100644 src/cmd/gz.hh rename src/{ => cmd}/gz_TEST.cc (100%) create mode 100644 src/cmd/sdf_main.cc delete mode 100644 src/gz.cc delete mode 100644 src/gz.hh diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt index 432df07bc..3bc620b3e 100644 --- a/sdf/CMakeLists.txt +++ b/sdf/CMakeLists.txt @@ -44,23 +44,23 @@ endif() # Generate aggregated SDF description files for use by the sdformat.org # website. If the description files change, the generated full*.sdf files need # to be removed before running this target. -if (GZ_PROGRAM) - # Update this list as new sdformat spec versions are added. - set(sdf_desc_versions 1.4 1.5 1.6 1.7 1.8 1.9 1.10 1.11 1.12) +# Update this list as new sdformat spec versions are added. +set(sdf_desc_versions 1.4 1.5 1.6 1.7 1.8 1.9 1.10 1.11 1.12) - set(description_targets) - foreach(desc_ver ${sdf_desc_versions}) - string(REPLACE "." "-" desc_ver_dash ${desc_ver}) - list(APPEND description_targets ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf) - add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf - COMMAND - ${CMAKE_COMMAND} -E env GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf/$ - ${GZ_PROGRAM} - ARGS sdf -d ${desc_ver} > ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf - COMMENT "Generating full description for spec ${desc_ver}" - VERBATIM) - endforeach() - add_custom_target(sdf_descriptions DEPENDS ${description_targets} ${PROJECT_LIBRARY_TARGET_NAME}) -endif() +set(description_targets) +foreach(desc_ver ${sdf_desc_versions}) + string(REPLACE "." "-" desc_ver_dash ${desc_ver}) + list(APPEND description_targets ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf) + add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf + COMMAND + ${CMAKE_COMMAND} -E env GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf/$ + $ + ARGS sdf -d ${desc_ver} > ${CMAKE_CURRENT_BINARY_DIR}/full_${desc_ver_dash}.sdf + COMMENT "Generating full description for spec ${desc_ver}" + VERBATIM) +endforeach() +add_custom_target(sdf_descriptions DEPENDS ${description_targets}) +# Add a dependency on the gz-sdformat-sdf target which is created in in ../cmd/CMakeLists +add_dependencies(sdf_descriptions gz-sdformat-sdf) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3ffa05625..571e80086 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -70,11 +70,6 @@ target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} ) if (BUILD_TESTING) - # Build this test file only if Gazebo Tools is installed. - if (NOT GZ_PROGRAM) - list(REMOVE_ITEM gtest_sources gz_TEST.cc) - endif() - add_library(library_for_tests OBJECT Converter.cc EmbeddedSdf.cc @@ -120,13 +115,6 @@ if (BUILD_TESTING) ENVIRONMENT SDF_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) - - if (TARGET UNIT_gz_TEST) - target_compile_definitions(UNIT_gz_TEST PRIVATE - -DGZ_PATH="${GZ_PROGRAM}" - -DDETAIL_GZ_CONFIG_PATH="${CMAKE_BINARY_DIR}/test/conf/$" - -DGZ_TEST_LIBRARY_PATH="${PROJECT_BINARY_DIR}/src") - endif() endif() add_subdirectory(cmd) diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index c1c91078b..581f7cafb 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -1,15 +1,54 @@ +# Build sdf CLI executable +set(sdf_executable gz-sdformat-sdf) +add_executable(${sdf_executable} sdf_main.cc gz.cc ../FrameSemantics.cc) +target_link_libraries(${sdf_executable} + gz-utils${GZ_UTILS_VER}::cli + ${PROJECT_LIBRARY_TARGET_NAME} + TINYXML2::TINYXML2 +) +install( + TARGETS ${sdf_executable} + DESTINATION ${CMAKE_INSTALL_LIBEXECDIR}/gz/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}/ +) + +# Build the unit tests only if Gazebo tools is installed +if(BUILD_TESTING AND GZ_PROGRAM) + gz_build_tests(TYPE UNIT + SOURCES + gz_TEST.cc + INCLUDE_DIRS + ${PROJECT_SOURCE_DIR}/test + TEST_LIST + test_list + LIB_DEPS + gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} + ${EXTRA_TEST_LIB_DEPS} + ENVIRONMENT + SDF_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + ) + + if(TARGET UNIT_gz_TEST) + target_compile_definitions(UNIT_gz_TEST + PRIVATE + "GZ_PATH=\"${GZ_PROGRAM}\"" + "DETAIL_GZ_CONFIG_PATH=\"${CMAKE_BINARY_DIR}/test/conf/$\"" + "GZ_TEST_LIBRARY_PATH=\"$\"" + ) + endif() +endif() + #=============================================================================== # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. # Ex: cmdsdformat0.rb -set(cmd_script_generated_test +set(cmd_script_generated_test "${CMAKE_BINARY_DIR}/test/lib/$/ruby/gz/cmd${PROJECT_NAME}.rb") -set(cmd_script_configured_test +set(cmd_script_configured_test "${CMAKE_CURRENT_BINARY_DIR}/test_cmd${PROJECT_NAME}.rb.configured") # Set the library_location variable to the full path of the library file within # the build directory. -set(library_location "$") +set(library_location "$") configure_file( "cmd${PROJECT_NAME_NO_VERSION_LOWER}.rb.in" @@ -20,7 +59,6 @@ file(GENERATE OUTPUT "${cmd_script_generated_test}" INPUT "${cmd_script_configured_test}") - #=============================================================================== # Used for the installed version. # Generate the ruby script that gets installed. @@ -37,7 +75,7 @@ else() set(library_location_prefix "${CMAKE_INSTALL_LIBDIR}") endif() -set(library_location "../../../${library_location_prefix}/$") +set(library_location "../../../${CMAKE_INSTALL_LIBEXECDIR}/gz/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}/$") configure_file( "cmd${PROJECT_NAME_NO_VERSION_LOWER}.rb.in" diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 8ba2c25fb..3297e3690 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -14,263 +14,39 @@ # See the License for the specific language governing permissions and # limitations under the License. -# We use 'dl' for Ruby <= 1.9.x and 'fiddle' for Ruby >= 2.0.x -if RUBY_VERSION.split('.')[0] < '2' - require 'dl' - require 'dl/import' - include DL -else - require 'fiddle' - require 'fiddle/import' - include Fiddle -end - -require 'optparse' require 'pathname' - # Constants. -LIBRARY_NAME = '@library_location@' LIBRARY_VERSION = '@PROJECT_VERSION_FULL@' -COMMON_OPTIONS = - " -h [ --help ] Print this help message.\n"\ - " --force-version Use a specific library version.\n"\ - ' --versions Show the available versions.' -COMMANDS = { 'sdf' => - "Utilities for SDF files.\n\n"\ - " gz sdf [options]\n\n"\ - "Options:\n\n"\ - " -k [ --check ] arg Check if an SDFormat file is valid.\n" + - " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + - " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph. (WARNING: This is for advanced\n" + - " use only and the output may change without any promise of stability)\n" + - " --inertial-stats arg Prints moment of inertia, centre of mass, and total mass from a model sdf file.\n" + - " -p [ --print ] arg Print converted arg. Note the quaternion representation of the\n" + - " rotational part of poses and unit vectors will be normalized.\n" + - " -i [ --preserve-includes ] Preserve included tags when printing converted arg (does not preserve merge-includes).\n" + - " --degrees Pose rotation angles are printed in degrees.\n" + - " --expand-auto-inertials Prints auto-computed inertial values for simple shapes. For meshes and other unsupported\n" + - " shapes, the default inertial values will be printed.\n" + - " --snap-to-degrees arg Snap pose rotation angles to this specified interval in degrees. This value must be\n" + - " larger than 0, less than or equal to 360, and larger than the defined snap tolerance.\n" + - " --snap-tolerance arg Used in conjunction with --snap-to-degrees, specifies the tolerance at which snapping\n" + - " occurs. This value must be larger than 0, less than 360, and less than the defined\n" + - " degrees value to snap to. If unspecified, its default value is 0.01.\n" + - " --precision arg Set the output stream precision for floating point numbers. The arg must be a positive integer.\n" + - - COMMON_OPTIONS - } +COMMANDS = { + "sdf" => "@library_location@", +} # # Class for the SDF command line tools. # class Cmd - - # - # Return a structure describing the options. - # - def parse(args) - options = {} - options['degrees'] = 0 - options['expand_auto_inertials'] = 0 - options['snap_tolerance'] = 0.01 - options['preserve_includes'] = 0 - - usage = COMMANDS[args[0]] - - # Read the command line arguments. - opt_parser = OptionParser.new do |opts| - opts.banner = usage - - opts.on('-h', '--help", "Print this help message') do - puts usage - exit(0) - end - - opts.on('-k arg', '--check arg', String, - 'Check if an SDFormat file is valid.') do |arg| - options['check'] = arg - end - opts.on('--inertial-stats arg', String, - 'Prints moment of inertia, centre of mass, and total mass from a model sdf file.') do |arg| - options['inertial_stats'] = arg - end - opts.on('-d', '--describe [VERSION]', 'Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@)') do |v| - options['describe'] = v - end - opts.on('-p', '--print', 'Print converted arg') do - options['print'] = 1 - end - opts.on('-i', '--preserve-includes', 'Preserve included tags when printing converted arg (does not preserve merge-includes)') do - options['preserve_includes'] = 1 - end - opts.on('--degrees', 'Printed pose rotations are will be in degrees') do |degrees| - options['degrees'] = 1 - end - opts.on('--expand-auto-inertials', 'Auto-computed inertial values will be printed') do - options['expand_auto_inertials'] = 1 - end - opts.on('--snap-to-degrees arg', Integer, - 'Printed rotations are snapped to specified degree intervals') do |arg| - if arg == 0 || arg > 360 - puts "Degree interval to snap to must be more than 0, and less than or equal to 360." - exit(-1) - end - options['snap_to_degrees'] = arg - end - opts.on('--snap-tolerance arg', Float, - 'Printed rotations are snapped if they are within this specified tolerance') do |arg| - if arg < 0 || arg > 360 - puts "Rotation snapping tolerance must be more than 0, and less than 360." - exit(-1) - end - options['snap_tolerance'] = arg - end - opts.on('--precision arg', Integer, - 'Set the output stream precision for floating point numbers.') do |arg| - options['precision'] = arg - end - opts.on('-g arg', '--graph type', String, - 'Print PoseRelativeTo or FrameAttachedTo graph') do |graph_type| - options['graph'] = {:type => graph_type} - end - end - begin - opt_parser.parse!(args) - rescue - puts usage - exit(-1) - end - - # Check that there is at least one command and there is a plugin that knows - # how to handle it. - if ARGV.empty? || !COMMANDS.key?(ARGV[0]) || - options.empty? - puts usage - exit(-1) - end - - options['command'] = ARGV[0] - - if (options['preserve_includes'] != 0 and not options['print']) || - (options['precision'] and not options['print']) - puts usage - exit(-1) - end - - if options['print'] - filename = args.pop - if filename - options['print'] = filename - else - puts usage - exit(-1) - end - end - - options - end - - # - # Execute the command - # def execute(args) - options = parse(args) + command = args[0] + exe_name = COMMANDS[command] - # Debugging: - # puts 'Parsed:' - # puts options - - # Read the plugin that handles the command. - if Pathname.new(LIBRARY_NAME).absolute? - plugin = LIBRARY_NAME - else + unless Pathname.new(exe_name).absolute? # We're assuming that the library path is relative to the current # location of this script. - plugin = File.expand_path(File.join(File.dirname(__FILE__), LIBRARY_NAME)) + exe_name = File.expand_path(File.join(File.dirname(__FILE__), exe_name)) end conf_version = LIBRARY_VERSION - - if defined? RubyInstaller - # RubyInstaller does not search for dlls in PATH or the directory that tests are running from, - # so we'll add the parent directory of the plugin to the search path. - # https://github.com/oneclick/rubyinstaller2/wiki/For-gem-developers#-dll-loading - RubyInstaller::Runtime.add_dll_directory(File.dirname(plugin)) - end - - begin - Importer.dlload plugin - rescue DLError => error - puts "Library error: [#{plugin}] not found." - puts "DLError: #{error.message}" - exit(-1) - end - - # Read the library version. - Importer.extern 'char* gzVersion()' - begin - plugin_version = Importer.gzVersion.to_s - rescue DLError - puts "Library error: Problem running 'gzVersion()' from #{plugin}." - exit(-1) - end + exe_version = `#{exe_name} --version`.strip # Sanity check: Verify that the version of the yaml file matches the version # of the library that we are using. - unless plugin_version.eql? conf_version + unless exe_version.eql? conf_version puts "Error: Version mismatch. Your configuration file version is - [#{conf_version}] but #{plugin} version is [#{plugin_version}]." + [#{conf_version}] but #{exe_name} version is [#{exe_version}]." exit(-1) end - begin - case options['command'] - when 'sdf' - if options.key?('check') - Importer.extern 'int cmdCheck(const char *)' - exit(Importer.cmdCheck(File.expand_path(options['check']))) - elsif options.key?('inertial_stats') - Importer.extern 'int cmdInertialStats(const char *)' - exit(Importer.cmdInertialStats(options['inertial_stats'])) - elsif options.key?('describe') - Importer.extern 'int cmdDescribe(const char *)' - exit(Importer.cmdDescribe(options['describe'])) - elsif options.key?('print') - snap_to_degrees = 0 - precision = 0 - - if options.key?('snap_to_degrees') - if options['snap_to_degrees'] < options['snap_tolerance'] - puts "Rotation snapping tolerance must be larger than the snapping tolerance." - exit(-1) - end - snap_to_degrees = options['snap_to_degrees'] - end - if options.key?('precision') - precision = options['precision'] - end - Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance, int, int, int)' - exit(Importer.cmdPrint(File.expand_path(options['print']), - options['degrees'], - snap_to_degrees, - options['snap_tolerance'], - options['preserve_includes'], - precision, - options['expand_auto_inertials'])) - elsif options.key?('graph') - Importer.extern 'int cmdGraph(const char *, const char *)' - exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1]))) - else - puts 'Command error: I do not have an implementation '\ - 'for this command.' - end - else - puts 'Command error: I do not have an implementation for '\ - "command [gz #{options['command']}]." - end - rescue - puts "Library error: Problem running [#{options['command']}]() "\ - "from #{plugin}." - end + # Drop command from list of arguments + exec(exe_name, *args[1..-1]) end end diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc new file mode 100644 index 000000000..751ab5691 --- /dev/null +++ b/src/cmd/gz.cc @@ -0,0 +1,356 @@ +/* + * Copyright (C) 2017 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 +#include +#include +#include +#include +#include +#include +#include + +#include "sdf/config.hh" +#include "sdf/Filesystem.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" +#include "sdf/Root.hh" +#include "sdf/parser.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/PrintConfig.hh" +#include "sdf/system_util.hh" + +#include "gz/math/Inertial.hh" + +#include "../FrameSemantics.hh" +#include "../ScopedGraph.hh" +#include "gz.hh" + +namespace sdf +{ + inline namespace SDF_VERSION_NAMESPACE { + + ////////////////////////////////////////////////// + int cmdCheck(const char *_path) + { + int result = 0; + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + for (auto &error : errors) + { + std::cerr << error << std::endl; + } + return -1; + } + + if (!sdf::checkCanonicalLinkNames(&root)) + { + result = -1; + } + + if (!sdf::checkJointParentChildNames(&root)) + { + result = -1; + } + + if (!sdf::checkFrameAttachedToGraph(&root)) + { + result = -1; + } + + if (!sdf::checkPoseRelativeToGraph(&root)) + { + result = -1; + } + + if (!sdf::recursiveSiblingUniqueNames(root.Element())) + { + result = -1; + } + + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::SDFPtr sdf(new sdf::SDF()); + + if (!sdf::init(sdf)) + { + std::cerr << "Error: SDF schema initialization failed.\n"; + return -1; + } + + if (!sdf::readFile(_path, sdf)) + { + std::cerr << "Error: SDF parsing the xml failed.\n"; + return -1; + } + + if (result == 0) + { + std::cout << "Valid.\n"; + } + return result; + } + + ////////////////////////////////////////////////// + int cmdDescribe(const char *_version) + { + sdf::SDFPtr sdf(new sdf::SDF()); + + if (nullptr != _version) + { + sdf->Version(_version); + } + if (!sdf::init(sdf)) + { + std::cerr << "Error: SDF schema initialization failed.\n"; + return -1; + } + + sdf->PrintDescription(); + + return 0; + } + + ////////////////////////////////////////////////// + int cmdPrint(const char *_path, bool _inDegrees, int _snapToDegrees, + float _snapTolerance, bool _preserveIncludes, int _outPrecision, + bool _expandAutoInertials) + { + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::ParserConfig parserConfig; + if (_expandAutoInertials) + { + parserConfig.SetCalculateInertialConfiguration( + sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT); + } + + sdf::Root root; + sdf::Errors errors = root.Load(_path, parserConfig); + + sdf::PrintConfig config; + if (_inDegrees) + { + config.SetRotationInDegrees(true); + } + + if (_snapToDegrees > 0) + { + config.SetRotationSnapToDegrees(static_cast(_snapToDegrees), + static_cast(_snapTolerance)); + } + + if (_preserveIncludes) + config.SetPreserveIncludes(true); + + if (_outPrecision > 0) + config.SetOutPrecision(_outPrecision); + + if (root.Element()) + { + root.Element()->PrintValues(errors, "", config); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + return -1; + } + return 0; + } + + ////////////////////////////////////////////////// + int cmdGraph(const char *_graphType, const char *_path) + { + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + + if (std::strcmp(_graphType, "pose") == 0) + { + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + if (root.WorldCount() > 0) + { + errors = sdf::buildPoseRelativeToGraph(graph, root.WorldByIndex(0)); + } + else if (root.Model() != nullptr) + { + errors = + sdf::buildPoseRelativeToGraph(graph, root.Model()); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + std::cout << graph.Graph() << std::endl; + } + else if (std::strcmp(_graphType, "frame") == 0) + { + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + if (root.WorldCount() > 0) + { + errors = sdf::buildFrameAttachedToGraph(graph, root.WorldByIndex(0)); + } + else if (root.Model() != nullptr) + { + errors = + sdf::buildFrameAttachedToGraph(graph, root.Model()); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + std::cout << graph.Graph() << std::endl; + } + else + { + std::cerr << R"(Only "pose" and "frame" graph types are supported)" + << std::endl; + } + + return 0; + } + + ////////////////////////////////////////////////// + int cmdInertialStats(const char *_path) + { + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + + if (root.WorldCount() > 0) + { + std::cerr << "Error: Expected a model file but received a world file." + << std::endl; + return -1; + } + + const sdf::Model *model = root.Model(); + if (!model) + { + std::cerr << "Error: Could not find the model." << std::endl; + return -1; + } + + if (model->ModelCount() > 0) + { + std::cout << "Warning: Inertial properties of links in nested" + " models will not be included." << std::endl; + } + + gz::math::Inertiald totalInertial; + + for (uint64_t i = 0; i < model->LinkCount(); i++) + { + gz::math::Inertiald currentLinkInertial; + model->LinkByIndex(i)->ResolveInertial(currentLinkInertial, "__model__"); + + totalInertial += currentLinkInertial; + } + + auto totalMass = totalInertial.MassMatrix().Mass(); + auto xCentreOfMass = totalInertial.Pose().Pos().X(); + auto yCentreOfMass = totalInertial.Pose().Pos().Y(); + auto zCentreOfMass = totalInertial.Pose().Pos().Z(); + + std::cout << "Inertial statistics for model: " + << model->Name() << std::endl; + std::cout << "---" << std::endl; + std::cout << "Total mass of the model: " << totalMass << std::endl; + std::cout << "---" << std::endl; + + std::cout << "Centre of mass in model frame: " << std::endl; + std::cout << "X: " << xCentreOfMass << std::endl; + std::cout << "Y: " << yCentreOfMass << std::endl; + std::cout << "Z: " << zCentreOfMass << std::endl; + std::cout << "---" << std::endl; + + std::cout << "Moment of inertia matrix: " << std::endl; + + // Pretty print the MOI matrix + std::stringstream ss; + ss << totalInertial.Moi(); + + std::string s; + size_t maxLength = 0u; + std::vector moiVector; + while ( std::getline(ss, s, ' ' ) ) + { + moiVector.push_back(s); + if (s.size() > maxLength) + { + maxLength = s.size(); + } + } + + for (int i = 0; i < 9; i++) + { + size_t spacePadding = maxLength - moiVector[i].size(); + // Print the matrix element + std::cout << moiVector[i]; + for (size_t j = 0; j < spacePadding; j++) + { + std::cout << " "; + } + // Add space for the next element + std::cout << " "; + // Add '\n' if the next row is about to start + if ((i+1)%3 == 0) + { + std::cout << "\n"; + } + } + std::cout << "---" << std::endl; + + return 0; + } + } +} diff --git a/src/cmd/gz.hh b/src/cmd/gz.hh new file mode 100644 index 000000000..dc3beb8e5 --- /dev/null +++ b/src/cmd/gz.hh @@ -0,0 +1,69 @@ +/* + * Copyright (C) 2017 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_GZ_HH_ +#define SDF_GZ_HH_ + +#include + +#include "sdf/system_util.hh" + +namespace sdf +{ + // Inline bra to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + + /// \brief External hook to execute 'gz sdf -k' from the command line. + /// \param[in] _path Path to the SDF file to validate. + /// \return Zero on success, negative one otherwise. + int cmdCheck(const char *_path); + + /// \brief External hook to execute 'gz sdf -d' from the command line. + /// \param[in] _version SDFormat version. + /// \return int Zero on success, negative one otherwise. + int cmdDescribe(const char *_version); + + /// \brief External hook to execute 'gz sdf -p' from the command line. + /// \param[in] _path Path to SDF file. + /// \param[in] _inDegrees Print angles in degrees. + /// \param[in] _snapToDegrees Snap pose rotation angles in degrees. + /// \param[in] _snapTolerance Specfies tolerance for snapping. + /// \param[in] _preserveIncludes Preserve included tags when printing. + /// \param[in] _outPrecision Output stream precision for floating point. + /// \param[in] _expandAutoInertials Print auto-computed inertial values. + /// \return int Zero on success, negative one otherwise. + int cmdPrint(const char *_path, bool _inDegrees, int _snapToDegrees, + float _snapTolerance, bool _preserveIncludes, int _outPrecision, + bool _expandAutoInertials); + + /// \brief External hook to execute 'gz sdf --graph' from the command line. + /// \param[in] _graphType Graph type. + /// \param[in] _path Path to SDF file. + /// \return int Zero on success, negative one otherwise. + int cmdGraph(const char *_graphType, const char *_path); + + /// \brief External hook to execute 'gz sdf --inertial-stats' + /// from command line + /// \param[in] _path Path to SDF file. + /// \return int Zero on success, negative one otherwise. + int cmdInertialStats(const char *_path); + + } +} + +#endif diff --git a/src/gz_TEST.cc b/src/cmd/gz_TEST.cc similarity index 100% rename from src/gz_TEST.cc rename to src/cmd/gz_TEST.cc diff --git a/src/cmd/sdf.bash_completion.sh b/src/cmd/sdf.bash_completion.sh index c896949fc..04b3162f2 100644 --- a/src/cmd/sdf.bash_completion.sh +++ b/src/cmd/sdf.bash_completion.sh @@ -24,6 +24,7 @@ GZ_SDF_COMPLETION_LIST=" -k --check -d --describe -p --print + -g --graph --inertial-stats -h --help --force-version diff --git a/src/cmd/sdf_main.cc b/src/cmd/sdf_main.cc new file mode 100644 index 000000000..a492b23a0 --- /dev/null +++ b/src/cmd/sdf_main.cc @@ -0,0 +1,218 @@ +/* + * Copyright (C) 2025 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 +#include +#include + +#include +#include + +#include "sdf/config.hh" +#include "gz.hh" + +using namespace sdf; + +////////////////////////////////////////////////// +/// \brief Enumeration of available commands +enum class SdfCommand +{ + kNone, + kSdfCheck, + kSdfDescribe, + kSdfGraph, + kSdfPrintInertialStats, + kSdfPrint +}; + +////////////////////////////////////////////////// +/// \brief Structure to hold all available topic options +struct SdfOptions +{ + /// \brief Command to execute + SdfCommand command{SdfCommand::kNone}; + + /// \brief Path to the SDFormat file to print or check + std::string filepath{""}; + + /// \brief Version of the SDFormat specification to describe + std::string version{""}; + + /// \brief Type of SDFormat graph to print: + /// * "frame" for FrameAttachedToGraph + /// * "pose" for PoseRelativeToGraph + std::string graphType{""}; + + /// \brief When true, preserve included tags when printing converted arg + /// (does not preserve merge-includes) + bool preserveIncludes{false}; + + /// \brief When true, print pose rotations in degrees. + bool degrees{false}; + + /// \brief When true, auto-computed inertial values will be printed. + bool expandAutoInertials{false}; + + /// \brief Output stream precision for floating point numbers. + int precision{0}; + + /// \brief If set, printed rotations are snapped to specified degree + /// intervals. + int snapToDegrees{0}; + + /// \brief Printed rotations are snapped if they are less than this specified + /// tolerance. + double snapTolerance{0.01}; +}; + +////////////////////////////////////////////////// +/// \brief Callback fired when options are successfully parsed +void runSdfCommand(const SdfOptions &_opt) +{ + switch(_opt.command) + { + case SdfCommand::kSdfCheck: + cmdCheck(_opt.filepath.c_str()); + break; + case SdfCommand::kSdfDescribe: + cmdDescribe(_opt.version.c_str()); + break; + case SdfCommand::kSdfGraph: + cmdGraph(_opt.graphType.c_str(), _opt.filepath.c_str()); + break; + case SdfCommand::kSdfPrintInertialStats: + cmdInertialStats(_opt.filepath.c_str()); + break; + case SdfCommand::kSdfPrint: + cmdPrint(_opt.filepath.c_str(), _opt.degrees, _opt.snapToDegrees, + _opt.snapTolerance, _opt.preserveIncludes, _opt.precision, + _opt.expandAutoInertials); + break; + case SdfCommand::kNone: + default: + // In the event that there is no command, display help + throw CLI::CallForHelp(); + } +} + +////////////////////////////////////////////////// +void addSdfFlags(CLI::App &_app) +{ + auto opt = std::make_shared(); + + auto filepathOpt = + _app.add_option("filepath", opt->filepath, + "Path to an SDFormat file."); + + auto command = _app.add_option_group("command", "Command to be executed."); + + command->add_flag_callback("-k,--check", + [opt](){ + opt->command = SdfCommand::kSdfCheck; + }, + "Check if an SDFormat file is valid.") + ->needs(filepathOpt); + + command->add_option_function("-d,--describe", + [opt](const std::string &_version){ + opt->command = SdfCommand::kSdfDescribe; + opt->version = _version; + }, + "Print the aggregated SDFormat spec description. Latest version (" + SDF_PROTOCOL_VERSION ")") + ->expected(0, 1) + ->default_val(SDF_PROTOCOL_VERSION); + + command->add_option_function("-g,--graph", + [opt](const std::string &_graphType){ + opt->command = SdfCommand::kSdfGraph; + opt->graphType = _graphType; + }, + "Print the PoseRelativeTo or FrameAttachedTo graph by following\n" + "with either pose or frame as argument respectively.\n" + "(WARNING: This is for advanced use only and the output may change\n" + "without any promise of stability)") + ->needs(filepathOpt) + ->check(CLI::IsMember({"pose", "frame"})); + + command->add_flag_callback("--inertial-stats", + [opt](){ + opt->command = SdfCommand::kSdfPrintInertialStats; + }, + "Prints moment of inertia, centre of mass, and total mass from a model " + "sdf file.") + ->needs(filepathOpt); + + auto printCmd = command->add_flag_callback("-p,--print", + [opt](){ + opt->command = SdfCommand::kSdfPrint; + }, + "Print converted arg. Note the quaternion representation of the\n" + "rotational part of poses and unit vectors will be normalized.") + ->needs(filepathOpt); + + _app.add_flag("-i,--preserve-includes", opt->preserveIncludes, + "Preserve included tags when printing converted arg (does " + "not preserve merge-includes).") + ->needs(printCmd); + + _app.add_flag("--degrees", opt->degrees, + "Printed pose rotations are will be in degrees.") + ->needs(printCmd); + + _app.add_flag("--expand-auto-inertials", opt->expandAutoInertials, + "Auto-computed inertial values will be printed.") + ->needs(printCmd); + + _app.add_option("--precision", opt->precision, + "Set the output stream precision for floating point " + "numbers.") + ->needs(printCmd); + + _app.add_option("--snap-to-degrees", opt->snapToDegrees, + "Printed rotations are snapped to specified degree " + "intervals.") + ->needs(printCmd); + + _app.add_option("--snap-tolerance", opt->snapTolerance, + "Printed rotations are snapped if they are within this " + "specified tolerance.") + ->needs(printCmd); + + _app.callback([opt](){runSdfCommand(*opt); }); +} + +////////////////////////////////////////////////// +int main(int argc, char** argv) +{ + CLI::App app{"Utilities for SDFormat files."}; + + app.add_flag_callback("-v,--version", [](){ + std::cout << SDF_VERSION_FULL << std::endl; + throw CLI::Success(); + }, + "Print the current library version"); + + // Dummy flags handled by gz-tools + app.add_flag("--force-version", "Use a specific library version."); + app.add_flag("--versions", "Show the available versions."); + + addSdfFlags(app); + app.formatter(std::make_shared(&app)); + CLI11_PARSE(app, argc, argv); +} diff --git a/src/gz.cc b/src/gz.cc deleted file mode 100644 index 54b4794d1..000000000 --- a/src/gz.cc +++ /dev/null @@ -1,361 +0,0 @@ -/* - * Copyright (C) 2017 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 -#include -#include -#include -#include - -#include "sdf/config.hh" -#include "sdf/Filesystem.hh" -#include "sdf/Link.hh" -#include "sdf/Model.hh" -#include "sdf/Root.hh" -#include "sdf/parser.hh" -#include "sdf/ParserConfig.hh" -#include "sdf/PrintConfig.hh" -#include "sdf/system_util.hh" - -#include "gz/math/Inertial.hh" - -#include "FrameSemantics.hh" -#include "ScopedGraph.hh" -#include "gz.hh" - -////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path) -{ - int result = 0; - - sdf::Root root; - sdf::Errors errors = root.Load(_path); - if (!errors.empty()) - { - for (auto &error : errors) - { - std::cerr << error << std::endl; - } - return -1; - } - - if (!sdf::checkCanonicalLinkNames(&root)) - { - result = -1; - } - - if (!sdf::checkJointParentChildNames(&root)) - { - result = -1; - } - - if (!sdf::checkFrameAttachedToGraph(&root)) - { - result = -1; - } - - if (!sdf::checkPoseRelativeToGraph(&root)) - { - result = -1; - } - - if (!sdf::recursiveSiblingUniqueNames(root.Element())) - { - result = -1; - } - - if (!sdf::filesystem::exists(_path)) - { - std::cerr << "Error: File [" << _path << "] does not exist.\n"; - return -1; - } - - sdf::SDFPtr sdf(new sdf::SDF()); - - if (!sdf::init(sdf)) - { - std::cerr << "Error: SDF schema initialization failed.\n"; - return -1; - } - - if (!sdf::readFile(_path, sdf)) - { - std::cerr << "Error: SDF parsing the xml failed.\n"; - return -1; - } - - if (result == 0) - { - std::cout << "Valid.\n"; - } - return result; -} - -////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE char *gzVersion() -{ -#ifdef _MSC_VER - return _strdup(SDF_VERSION_FULL); -#else - return strdup(SDF_VERSION_FULL); -#endif -} - -////////////////////////////////////////////////// -/// \brief Print the full description of the SDF spec. -/// \return 0 on success, -1 if SDF could not be initialized. -extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version) -{ - sdf::SDFPtr sdf(new sdf::SDF()); - - if (nullptr != _version) - { - sdf->Version(_version); - } - if (!sdf::init(sdf)) - { - std::cerr << "Error: SDF schema initialization failed.\n"; - return -1; - } - - sdf->PrintDescription(); - - return 0; -} - -////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, - int _inDegrees, int _snapToDegrees, float _snapTolerance, - int _preserveIncludes, int _outPrecision, int _expandAutoInertials) -{ - if (!sdf::filesystem::exists(_path)) - { - std::cerr << "Error: File [" << _path << "] does not exist.\n"; - return -1; - } - - sdf::ParserConfig parserConfig; - if (_expandAutoInertials) - { - parserConfig.SetCalculateInertialConfiguration( - sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT); - } - - sdf::Root root; - sdf::Errors errors = root.Load(_path, parserConfig); - - sdf::PrintConfig config; - if (_inDegrees != 0) - { - config.SetRotationInDegrees(true); - } - - if (_snapToDegrees > 0) - { - config.SetRotationSnapToDegrees(static_cast(_snapToDegrees), - static_cast(_snapTolerance)); - } - - if (_preserveIncludes != 0) - config.SetPreserveIncludes(true); - - if (_outPrecision > 0) - config.SetOutPrecision(_outPrecision); - - if (root.Element()) - { - root.Element()->PrintValues(errors, "", config); - } - - if (!errors.empty()) - { - std::cerr << errors << std::endl; - return -1; - } - return 0; -} - -////////////////////////////////////////////////// -// cppcheck-suppress unusedFunction -extern "C" SDFORMAT_VISIBLE int cmdGraph( - const char *_graphType, const char *_path) -{ - if (!sdf::filesystem::exists(_path)) - { - std::cerr << "Error: File [" << _path << "] does not exist.\n"; - return -1; - } - - sdf::Root root; - sdf::Errors errors = root.Load(_path); - if (!errors.empty()) - { - std::cerr << errors << std::endl; - } - - if (std::strcmp(_graphType, "pose") == 0) - { - auto ownedGraph = std::make_shared(); - sdf::ScopedGraph graph(ownedGraph); - if (root.WorldCount() > 0) - { - errors = sdf::buildPoseRelativeToGraph(graph, root.WorldByIndex(0)); - } - else if (root.Model() != nullptr) - { - errors = - sdf::buildPoseRelativeToGraph(graph, root.Model()); - } - - if (!errors.empty()) - { - std::cerr << errors << std::endl; - } - std::cout << graph.Graph() << std::endl; - } - else if (std::strcmp(_graphType, "frame") == 0) - { - auto ownedGraph = std::make_shared(); - sdf::ScopedGraph graph(ownedGraph); - if (root.WorldCount() > 0) - { - errors = sdf::buildFrameAttachedToGraph(graph, root.WorldByIndex(0)); - } - else if (root.Model() != nullptr) - { - errors = - sdf::buildFrameAttachedToGraph(graph, root.Model()); - } - - if (!errors.empty()) - { - std::cerr << errors << std::endl; - } - std::cout << graph.Graph() << std::endl; - } - else - { - std::cerr << R"(Only "pose" and "frame" graph types are supported)" - << std::endl; - } - - return 0; -} - -////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdInertialStats( - const char *_path) -{ - if (!sdf::filesystem::exists(_path)) - { - std::cerr << "Error: File [" << _path << "] does not exist.\n"; - return -1; - } - - sdf::Root root; - sdf::Errors errors = root.Load(_path); - if (!errors.empty()) - { - std::cerr << errors << std::endl; - } - - if (root.WorldCount() > 0) - { - std::cerr << "Error: Expected a model file but received a world file." - << std::endl; - return -1; - } - - const sdf::Model *model = root.Model(); - if (!model) - { - std::cerr << "Error: Could not find the model." << std::endl; - return -1; - } - - if (model->ModelCount() > 0) - { - std::cout << "Warning: Inertial properties of links in nested" - " models will not be included." << std::endl; - } - - gz::math::Inertiald totalInertial; - - for (uint64_t i = 0; i < model->LinkCount(); i++) - { - gz::math::Inertiald currentLinkInertial; - model->LinkByIndex(i)->ResolveInertial(currentLinkInertial, "__model__"); - - totalInertial += currentLinkInertial; - } - - auto totalMass = totalInertial.MassMatrix().Mass(); - auto xCentreOfMass = totalInertial.Pose().Pos().X(); - auto yCentreOfMass = totalInertial.Pose().Pos().Y(); - auto zCentreOfMass = totalInertial.Pose().Pos().Z(); - - std::cout << "Inertial statistics for model: " << model->Name() << std::endl; - std::cout << "---" << std::endl; - std::cout << "Total mass of the model: " << totalMass << std::endl; - std::cout << "---" << std::endl; - - std::cout << "Centre of mass in model frame: " << std::endl; - std::cout << "X: " << xCentreOfMass << std::endl; - std::cout << "Y: " << yCentreOfMass << std::endl; - std::cout << "Z: " << zCentreOfMass << std::endl; - std::cout << "---" << std::endl; - - std::cout << "Moment of inertia matrix: " << std::endl; - - // Pretty print the MOI matrix - std::stringstream ss; - ss << totalInertial.Moi(); - - std::string s; - size_t maxLength = 0u; - std::vector moiVector; - while ( std::getline(ss, s, ' ' ) ) - { - moiVector.push_back(s); - if (s.size() > maxLength) - { - maxLength = s.size(); - } - } - - for (int i = 0; i < 9; i++) - { - size_t spacePadding = maxLength - moiVector[i].size(); - // Print the matrix element - std::cout << moiVector[i]; - for (size_t j = 0; j < spacePadding; j++) - { - std::cout << " "; - } - // Add space for the next element - std::cout << " "; - // Add '\n' if the next row is about to start - if ((i+1)%3 == 0) - { - std::cout << "\n"; - } - } - std::cout << "---" << std::endl; - - return 0; -} diff --git a/src/gz.hh b/src/gz.hh deleted file mode 100644 index 6f3dbd2c6..000000000 --- a/src/gz.hh +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (C) 2017 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_GZ_HH_ -#define SDF_GZ_HH_ - -#include - -#include -#include "sdf/system_util.hh" - -// Inline bracket to help doxygen filtering. -inline namespace SDF_VERSION_NAMESPACE { -// - -/// \brief External hook to execute 'gz sdf -k' from the command line. -/// \param[in] _path Path to the file to validate. -/// \return Zero on success, negative one otherwise. -extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path); - -/// \brief External hook to read the library version. -/// \return C-string representing the version. Ex.: 0.1.2 -extern "C" SDFORMAT_VISIBLE char *gzVersion(); -} - -#endif