From d8238203c21dc36ea6f68c3c158d9325b2e39d24 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 9 Jun 2024 00:00:45 -0700 Subject: [PATCH 01/19] Implement gz sdf as standalone executable Signed-off-by: Steve Peters --- src/cmd/CMakeLists.txt | 45 +++++ src/cmd/gz.cc | 361 +++++++++++++++++++++++++++++++++++++++++ src/cmd/gz.hh | 52 ++++++ src/cmd/sdf_main.cc | 200 +++++++++++++++++++++++ 4 files changed, 658 insertions(+) create mode 100644 src/cmd/gz.cc create mode 100644 src/cmd/gz.hh create mode 100644 src/cmd/sdf_main.cc diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index c1c91078b..9ea570b43 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -1,3 +1,48 @@ +# Collect source files into the "sources" variable and unit test files into the +# "gtest_sources" variable. +gz_get_libsources_and_unittests(sources gtest_sources) + +if (NOT HAVE_GZ_TOOLS) + list(REMOVE_ITEM gtest_sources gz_TEST.cc) +endif() + +# Make a small static lib of command line functions +add_library(gz STATIC gz.cc) +target_link_libraries(gz + ${PROJECT_LIBRARY_TARGET_NAME} +) + +# Build sdf CLI executable +set(sdf_executable gz-sdformat-sdf) +add_executable(${sdf_executable} sdf_main.cc) +target_link_libraries(${sdf_executable} + gz + gz-utils${GZ_UTILS_VER}::cli + ${PROJECT_LIBRARY_TARGET_NAME} +) +install(TARGETS ${sdf_executable} DESTINATION ${CMAKE_INSTALL_LIBEXECDIR}/gz/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}/) + +if (FALSE AND BUILD_TESTING) + # Build the unit tests. + gz_build_tests(TYPE UNIT SOURCES ${gtest_sources} + TEST_LIST test_list + LIB_DEPS + gz + gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} + ${EXTRA_TEST_LIB_DEPS} test_config) + + if (TARGET UNIT_gz_TEST) + + set_tests_properties( + UNIT_gz_TEST + PROPERTIES + ENVIRONMENT + "GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf/$" + # "GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf/$;LD_LIBRARY_PATH=\"$\":$ENV{LD_LIBRARY_PATH}" + ) + endif() +endif() + #=============================================================================== # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc new file mode 100644 index 000000000..ffcbd433a --- /dev/null +++ b/src/cmd/gz.cc @@ -0,0 +1,361 @@ +/* + * 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/sdf_config.h" +#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/cmd/gz.hh b/src/cmd/gz.hh new file mode 100644 index 000000000..22afb9a79 --- /dev/null +++ b/src/cmd/gz.hh @@ -0,0 +1,52 @@ +/* + * 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(); + +extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version); + +extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, + int _inDegrees, int _snapToDegrees, float _snapTolerance, + int _preserveIncludes, int _outPrecision, int _expandAutoInertials); + +extern "C" SDFORMAT_VISIBLE int cmdGraph( + const char *_graphType, const char *_path); + +extern "C" SDFORMAT_VISIBLE int cmdInertialStats( + const char *_path); +} + +#endif diff --git a/src/cmd/sdf_main.cc b/src/cmd/sdf_main.cc new file mode 100644 index 000000000..1103e913d --- /dev/null +++ b/src/cmd/sdf_main.cc @@ -0,0 +1,200 @@ +/* + * Copyright (C) 2021 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 "sdf/config.hh" +#include "gz.hh" + +////////////////////////////////////////////////// +/// \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 nonzero, preserve included tags when printing converted arg + /// (does not preserve merge-includes) + int preserveIncludes{0}; + + /// \brief When nonzero, print pose rotations in degrees. + int degrees{0}; + + /// \brief When nonzero, auto-computed inertial values will be printed. + int expandAutoInertials{0}; + + /// \brief Output stream precision for floating point numbers. + std::optional precision; + + /// \brief If set, printed rotations are snapped to specified degree + /// intervals. + std::optional snapToDegrees; + + /// \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 preserveIncludesOpt = + _app.add_option("-i,--preserve-includes", opt->preserveIncludes, + "Preserve included tags when printing converted arg (does " + "not preserve merge-includes)."); + auto degreesOpt = + _app.add_option("--degrees", opt->degrees, + "Printed pose rotations are will be in degrees."); + auto expandAutoInertialsOpt = + _app.add_option("--expand-auto-inertials", opt->expandAutoInertials, + "Auto-computed inertial values will be printed."); + auto precisionOpt = + _app.add_option("--precision", opt->precision, + "Set the output stream precision for floating point " + "numbers."); + auto snapToDegreesOpt = + _app.add_option("--snap-to-degrees", opt->snapToDegrees, + "Printed rotations are snapped to specified degree " + "intervals."); + auto snapToleranceOpt = + _app.add_option("--snap-tolerance", opt->snapTolerance, + "Printed rotations are snapped if they are within this " + "specified tolerance."); + + 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 ")"); + + command->add_option_function("-g,--graph", + [opt](const std::string &_graphType){ + opt->command = SdfCommand::kSdfGraph; + opt->graphType = _graphType; + }, + " filepath Print the PoseRelativeTo or FrameAttachedTo " + "graph.\n" + " (WARNING: This is for advanced use only and the output may change \n" + "without any promise of stability)") + ->needs(filepathOpt); + + 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); + + command->add_flag_callback("-p,--print", + [opt](){ + opt->command = SdfCommand::kSdfPrint; + }, + "Prints moment of inertia, centre of mass, and total mass from a model " + "sdf file.") + ->needs(filepathOpt); + + _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(); + }); + + addSdfFlags(app); + app.formatter(std::make_shared(&app)); + CLI11_PARSE(app, argc, argv); +} From 891e4901eeae77c817362a214b9abbdc515b7eda Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Fri, 24 Jan 2025 07:40:55 +0800 Subject: [PATCH 02/19] Updating ruby script to use standalone executable (#1489) * Fixed cmake to find source file for FrameSemantics * Updated exe location in ruby script * Add a dependency on gz-sdformat-sdf to the sdf_descriptions target * Updated cmd line arguments in executable tests * Added dummy flags to make tests pass --------- Signed-off-by: Saurabh Kamat Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- sdf/CMakeLists.txt | 4 +- src/cmd/CMakeLists.txt | 15 +- src/cmd/cmdsdformat.rb.in | 248 ++------------------------------- src/cmd/sdf.bash_completion.sh | 1 + src/cmd/sdf_main.cc | 47 +++---- 5 files changed, 48 insertions(+), 267 deletions(-) diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt index aec064bd3..336ed237f 100644 --- a/sdf/CMakeLists.txt +++ b/sdf/CMakeLists.txt @@ -47,5 +47,7 @@ if (GZ_PROGRAM) COMMENT "Generating full description for spec ${desc_ver}" VERBATIM) endforeach() - add_custom_target(sdf_descriptions DEPENDS ${description_targets} ${PROJECT_LIBRARY_TARGET_NAME}) + 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) endif() diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index 9ea570b43..ce7120bca 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -7,9 +7,12 @@ if (NOT HAVE_GZ_TOOLS) endif() # Make a small static lib of command line functions -add_library(gz STATIC gz.cc) +add_library(gz STATIC gz.cc ../FrameSemantics.cc) target_link_libraries(gz - ${PROJECT_LIBRARY_TARGET_NAME} + PUBLIC + ${PROJECT_LIBRARY_TARGET_NAME} + PRIVATE + TINYXML2::TINYXML2 ) # Build sdf CLI executable @@ -47,14 +50,14 @@ 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" @@ -82,7 +85,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/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 index 1103e913d..5a26658a1 100644 --- a/src/cmd/sdf_main.cc +++ b/src/cmd/sdf_main.cc @@ -114,28 +114,22 @@ void addSdfFlags(CLI::App &_app) auto filepathOpt = _app.add_option("filepath", opt->filepath, "Path to an SDFormat file."); - auto preserveIncludesOpt = - _app.add_option("-i,--preserve-includes", opt->preserveIncludes, - "Preserve included tags when printing converted arg (does " - "not preserve merge-includes)."); - auto degreesOpt = - _app.add_option("--degrees", opt->degrees, - "Printed pose rotations are will be in degrees."); - auto expandAutoInertialsOpt = - _app.add_option("--expand-auto-inertials", opt->expandAutoInertials, - "Auto-computed inertial values will be printed."); - auto precisionOpt = - _app.add_option("--precision", opt->precision, - "Set the output stream precision for floating point " - "numbers."); - auto snapToDegreesOpt = - _app.add_option("--snap-to-degrees", opt->snapToDegrees, - "Printed rotations are snapped to specified degree " - "intervals."); - auto snapToleranceOpt = - _app.add_option("--snap-tolerance", opt->snapTolerance, - "Printed rotations are snapped if they are within this " - "specified tolerance."); + _app.add_flag("-i,--preserve-includes", opt->preserveIncludes, + "Preserve included tags when printing converted arg (does " + "not preserve merge-includes)."); + _app.add_flag("--degrees", opt->degrees, + "Printed pose rotations are will be in degrees."); + _app.add_flag("--expand-auto-inertials", opt->expandAutoInertials, + "Auto-computed inertial values will be printed."); + _app.add_option("--precision", opt->precision, + "Set the output stream precision for floating point " + "numbers."); + _app.add_option("--snap-to-degrees", opt->snapToDegrees, + "Printed rotations are snapped to specified degree " + "intervals."); + _app.add_option("--snap-tolerance", opt->snapTolerance, + "Printed rotations are snapped if they are within this " + "specified tolerance."); auto command = _app.add_option_group("command", "Command to be executed."); @@ -152,7 +146,9 @@ void addSdfFlags(CLI::App &_app) opt->version = _version; }, "Print the aggregated SDFormat spec description. Latest version (" - SDF_PROTOCOL_VERSION ")"); + SDF_PROTOCOL_VERSION ")") + ->expected(0, 1) + ->default_val(SDF_PROTOCOL_VERSION); command->add_option_function("-g,--graph", [opt](const std::string &_graphType){ @@ -192,7 +188,10 @@ int main(int argc, char** argv) app.add_flag_callback("-v,--version", [](){ std::cout << SDF_VERSION_FULL << std::endl; throw CLI::Success(); - }); + }, + "Print the current library version"); + 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)); From c4f155fee607184ad566f81ac0186b16cdb0f1d2 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Fri, 7 Feb 2025 14:28:54 +0800 Subject: [PATCH 03/19] Moved tests and deleted redundant files Signed-off-by: Saurabh Kamat --- src/{ => cmd}/gz_TEST.cc | 3 - src/gz.cc | 361 --------------------------------------- src/gz.hh | 40 ----- 3 files changed, 404 deletions(-) rename src/{ => cmd}/gz_TEST.cc (99%) delete mode 100644 src/gz.cc delete mode 100644 src/gz.hh diff --git a/src/gz_TEST.cc b/src/cmd/gz_TEST.cc similarity index 99% rename from src/gz_TEST.cc rename to src/cmd/gz_TEST.cc index 864de685e..0071ba1cd 100644 --- a/src/gz_TEST.cc +++ b/src/cmd/gz_TEST.cc @@ -37,9 +37,6 @@ #define pclose _pclose #endif -// DETAIL_GZ_CONFIG_PATH is compiler definition set in CMake. -#define GZ_CONFIG_PATH DETAIL_GZ_CONFIG_PATH - static std::string SdfVersion() { return " --force-version " + std::string(SDF_VERSION_FULL); 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 From 756ba94b4edcf22a97cdfa63b289ddd305b636aa Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Fri, 7 Feb 2025 15:39:33 +0800 Subject: [PATCH 04/19] Updated cmake to build moved test Signed-off-by: Saurabh Kamat --- src/CMakeLists.txt | 12 ----------- src/cmd/CMakeLists.txt | 48 +++++++++++++++++++++--------------------- src/cmd/gz.cc | 23 ++++++++------------ src/cmd/gz.hh | 46 +++++++++++++++++++++++++--------------- src/cmd/sdf_main.cc | 10 +++++---- 5 files changed, 68 insertions(+), 71 deletions(-) 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 ce7120bca..1aaddb0bc 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -1,11 +1,3 @@ -# Collect source files into the "sources" variable and unit test files into the -# "gtest_sources" variable. -gz_get_libsources_and_unittests(sources gtest_sources) - -if (NOT HAVE_GZ_TOOLS) - list(REMOVE_ITEM gtest_sources gz_TEST.cc) -endif() - # Make a small static lib of command line functions add_library(gz STATIC gz.cc ../FrameSemantics.cc) target_link_libraries(gz @@ -23,25 +15,34 @@ target_link_libraries(${sdf_executable} gz-utils${GZ_UTILS_VER}::cli ${PROJECT_LIBRARY_TARGET_NAME} ) -install(TARGETS ${sdf_executable} DESTINATION ${CMAKE_INSTALL_LIBEXECDIR}/gz/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}/) +install( + TARGETS ${sdf_executable} + DESTINATION ${CMAKE_INSTALL_LIBEXECDIR}/gz/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}/ +) -if (FALSE AND BUILD_TESTING) - # Build the unit tests. - gz_build_tests(TYPE UNIT SOURCES ${gtest_sources} - TEST_LIST test_list +# 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 gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} - ${EXTRA_TEST_LIB_DEPS} test_config) - - if (TARGET UNIT_gz_TEST) - - set_tests_properties( - UNIT_gz_TEST - PROPERTIES - ENVIRONMENT - "GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf/$" - # "GZ_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf/$;LD_LIBRARY_PATH=\"$\":$ENV{LD_LIBRARY_PATH}" + ${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}\"" + "GZ_CONFIG_PATH=\"${CMAKE_BINARY_DIR}/test/conf/$\"" + "GZ_TEST_LIBRARY_PATH=\"$\"" ) endif() endif() @@ -68,7 +69,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. diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc index ffcbd433a..51e94fdb5 100644 --- a/src/cmd/gz.cc +++ b/src/cmd/gz.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -39,7 +39,7 @@ #include "gz.hh" ////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path) +int cmdCheck(const char *_path) { int result = 0; @@ -107,7 +107,7 @@ extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path) } ////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE char *gzVersion() +char *gzVersion() { #ifdef _MSC_VER return _strdup(SDF_VERSION_FULL); @@ -117,9 +117,7 @@ extern "C" SDFORMAT_VISIBLE char *gzVersion() } ////////////////////////////////////////////////// -/// \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) +int cmdDescribe(const char *_version) { sdf::SDFPtr sdf(new sdf::SDF()); @@ -139,9 +137,9 @@ extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version) } ////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, - int _inDegrees, int _snapToDegrees, float _snapTolerance, - int _preserveIncludes, int _outPrecision, int _expandAutoInertials) +int cmdPrint(const char *_path, int _inDegrees, int _snapToDegrees, + float _snapTolerance, int _preserveIncludes, int _outPrecision, + int _expandAutoInertials) { if (!sdf::filesystem::exists(_path)) { @@ -191,9 +189,7 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, } ////////////////////////////////////////////////// -// cppcheck-suppress unusedFunction -extern "C" SDFORMAT_VISIBLE int cmdGraph( - const char *_graphType, const char *_path) +int cmdGraph(const char *_graphType, const char *_path) { if (!sdf::filesystem::exists(_path)) { @@ -258,8 +254,7 @@ extern "C" SDFORMAT_VISIBLE int cmdGraph( } ////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdInertialStats( - const char *_path) +int cmdInertialStats(const char *_path) { if (!sdf::filesystem::exists(_path)) { diff --git a/src/cmd/gz.hh b/src/cmd/gz.hh index 22afb9a79..1bd2a8562 100644 --- a/src/cmd/gz.hh +++ b/src/cmd/gz.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -23,30 +23,42 @@ #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. +/// \param[in] _path Path to the SDF file to validate. /// \return Zero on success, negative one otherwise. -extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path); +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(); +char *gzVersion(); -extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version); +/// \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); -extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, - int _inDegrees, int _snapToDegrees, float _snapTolerance, - int _preserveIncludes, int _outPrecision, int _expandAutoInertials); +/// \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, int _inDegrees, int _snapToDegrees, + float _snapTolerance, int _preserveIncludes, int _outPrecision, + int _expandAutoInertials); -extern "C" SDFORMAT_VISIBLE int cmdGraph( - const char *_graphType, const char *_path); +/// \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); -extern "C" SDFORMAT_VISIBLE int cmdInertialStats( - 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/cmd/sdf_main.cc b/src/cmd/sdf_main.cc index 5a26658a1..40df6b3d8 100644 --- a/src/cmd/sdf_main.cc +++ b/src/cmd/sdf_main.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -157,7 +157,7 @@ void addSdfFlags(CLI::App &_app) }, " filepath Print the PoseRelativeTo or FrameAttachedTo " "graph.\n" - " (WARNING: This is for advanced use only and the output may change \n" + "(WARNING: This is for advanced use only and the output may change\n" "without any promise of stability)") ->needs(filepathOpt); @@ -173,8 +173,8 @@ void addSdfFlags(CLI::App &_app) [opt](){ opt->command = SdfCommand::kSdfPrint; }, - "Prints moment of inertia, centre of mass, and total mass from a model " - "sdf file.") + "Print converted arg. Note the quaternion representation of the\n" + "rotational part of poses and unit vectors will be normalized.") ->needs(filepathOpt); _app.callback([opt](){runSdfCommand(*opt); }); @@ -190,6 +190,8 @@ int main(int argc, char** argv) 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."); From 193a1f1326ad83228bc448c4bcb44f90d37ec5ef Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Fri, 7 Feb 2025 16:04:46 +0800 Subject: [PATCH 05/19] Skipped creation of static lib for standalone exe creation Signed-off-by: Saurabh Kamat --- src/cmd/CMakeLists.txt | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index 1aaddb0bc..ec30947db 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -1,19 +1,10 @@ -# Make a small static lib of command line functions -add_library(gz STATIC gz.cc ../FrameSemantics.cc) -target_link_libraries(gz - PUBLIC - ${PROJECT_LIBRARY_TARGET_NAME} - PRIVATE - TINYXML2::TINYXML2 -) - # Build sdf CLI executable set(sdf_executable gz-sdformat-sdf) -add_executable(${sdf_executable} sdf_main.cc) +add_executable(${sdf_executable} sdf_main.cc gz.cc ../FrameSemantics.cc) target_link_libraries(${sdf_executable} - gz gz-utils${GZ_UTILS_VER}::cli ${PROJECT_LIBRARY_TARGET_NAME} + TINYXML2::TINYXML2 ) install( TARGETS ${sdf_executable} @@ -30,7 +21,6 @@ if(BUILD_TESTING AND GZ_PROGRAM) TEST_LIST test_list LIB_DEPS - gz gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} ${EXTRA_TEST_LIB_DEPS} ENVIRONMENT From 699c5aec99ac71af6fe26cfa77c6cc219fab4b9d Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Fri, 7 Feb 2025 21:24:09 +0800 Subject: [PATCH 06/19] Added includes Signed-off-by: Saurabh Kamat --- src/cmd/sdf_main.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/cmd/sdf_main.cc b/src/cmd/sdf_main.cc index 40df6b3d8..55c280a53 100644 --- a/src/cmd/sdf_main.cc +++ b/src/cmd/sdf_main.cc @@ -15,6 +15,8 @@ * */ +#include +#include #include #include @@ -155,7 +157,7 @@ void addSdfFlags(CLI::App &_app) opt->command = SdfCommand::kSdfGraph; opt->graphType = _graphType; }, - " filepath Print the PoseRelativeTo or FrameAttachedTo " + " filepath Print the PoseRelativeTo or FrameAttachedTo " "graph.\n" "(WARNING: This is for advanced use only and the output may change\n" "without any promise of stability)") From 7b27baaadb9b57f5c2b32000d4e41998f315183e Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Tue, 11 Feb 2025 18:12:49 +0800 Subject: [PATCH 07/19] Restoring previous files to save commit history Signed-off-by: Saurabh Kamat --- src/gz.cc | 361 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/gz.hh | 40 ++++++ 2 files changed, 401 insertions(+) create mode 100644 src/gz.cc create mode 100644 src/gz.hh diff --git a/src/gz.cc b/src/gz.cc new file mode 100644 index 000000000..54b4794d1 --- /dev/null +++ b/src/gz.cc @@ -0,0 +1,361 @@ +/* + * 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 new file mode 100644 index 000000000..6f3dbd2c6 --- /dev/null +++ b/src/gz.hh @@ -0,0 +1,40 @@ +/* + * 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 From 97146589d9a0412d0b0f66f1437c79e5049c0a6c Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Tue, 11 Feb 2025 18:19:18 +0800 Subject: [PATCH 08/19] Removed gz files from cmd Signed-off-by: Saurabh Kamat --- src/cmd/gz.cc | 356 -------------------------------------------------- src/cmd/gz.hh | 64 --------- 2 files changed, 420 deletions(-) delete mode 100644 src/cmd/gz.cc delete mode 100644 src/cmd/gz.hh diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc deleted file mode 100644 index 51e94fdb5..000000000 --- a/src/cmd/gz.cc +++ /dev/null @@ -1,356 +0,0 @@ -/* - * 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/sdf_config.h" -#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" - -////////////////////////////////////////////////// -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; -} - -////////////////////////////////////////////////// -char *gzVersion() -{ -#ifdef _MSC_VER - return _strdup(SDF_VERSION_FULL); -#else - return strdup(SDF_VERSION_FULL); -#endif -} - -////////////////////////////////////////////////// -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, 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; -} - -////////////////////////////////////////////////// -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 deleted file mode 100644 index 1bd2a8562..000000000 --- a/src/cmd/gz.hh +++ /dev/null @@ -1,64 +0,0 @@ -/* - * 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. - * -*/ - -#ifndef SDF_GZ_HH_ -#define SDF_GZ_HH_ - -#include - -#include -#include "sdf/system_util.hh" - -/// \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 read the library version. -/// \return C-string representing the version. Ex.: 0.1.2 -char *gzVersion(); - -/// \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, int _inDegrees, int _snapToDegrees, - float _snapTolerance, int _preserveIncludes, int _outPrecision, - int _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 From 5dae7f49501184b84555c600e8fa5c6101af9cf8 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Tue, 11 Feb 2025 18:29:52 +0800 Subject: [PATCH 09/19] Moved cmd files by preserving history Signed-off-by: Saurabh Kamat --- src/{ => cmd}/gz.cc | 0 src/{ => cmd}/gz.hh | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/{ => cmd}/gz.cc (100%) rename src/{ => cmd}/gz.hh (100%) diff --git a/src/gz.cc b/src/cmd/gz.cc similarity index 100% rename from src/gz.cc rename to src/cmd/gz.cc diff --git a/src/gz.hh b/src/cmd/gz.hh similarity index 100% rename from src/gz.hh rename to src/cmd/gz.hh From 71edff4186b78ac279ed0db7dc3f21c26c755357 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Tue, 11 Feb 2025 20:02:13 +0800 Subject: [PATCH 10/19] Added namespace to cmd line hooks Signed-off-by: Saurabh Kamat --- src/cmd/gz.cc | 501 ++++++++++++++++++++++---------------------- src/cmd/gz.hh | 54 ++++- src/cmd/sdf_main.cc | 2 + 3 files changed, 296 insertions(+), 261 deletions(-) diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc index 54b4794d1..0a497bc42 100644 --- a/src/cmd/gz.cc +++ b/src/cmd/gz.cc @@ -34,328 +34,329 @@ #include "gz/math/Inertial.hh" -#include "FrameSemantics.hh" -#include "ScopedGraph.hh" +#include "../FrameSemantics.hh" +#include "../ScopedGraph.hh" #include "gz.hh" -////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path) +namespace sdf { - int result = 0; + inline namespace SDF_VERSION_NAMESPACE { - sdf::Root root; - sdf::Errors errors = root.Load(_path); - if (!errors.empty()) + ////////////////////////////////////////////////// + int cmdCheck(const char *_path) { - for (auto &error : errors) + int result = 0; + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) { - std::cerr << error << std::endl; + for (auto &error : errors) + { + std::cerr << error << std::endl; + } + return -1; } - return -1; - } - if (!sdf::checkCanonicalLinkNames(&root)) - { - result = -1; - } + if (!sdf::checkCanonicalLinkNames(&root)) + { + result = -1; + } - if (!sdf::checkJointParentChildNames(&root)) - { - result = -1; - } + if (!sdf::checkJointParentChildNames(&root)) + { + result = -1; + } - if (!sdf::checkFrameAttachedToGraph(&root)) - { - result = -1; - } + if (!sdf::checkFrameAttachedToGraph(&root)) + { + result = -1; + } - if (!sdf::checkPoseRelativeToGraph(&root)) - { - result = -1; - } + if (!sdf::checkPoseRelativeToGraph(&root)) + { + result = -1; + } - if (!sdf::recursiveSiblingUniqueNames(root.Element())) - { - 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; - } + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } - sdf::SDFPtr sdf(new sdf::SDF()); + sdf::SDFPtr sdf(new sdf::SDF()); - if (!sdf::init(sdf)) - { - std::cerr << "Error: SDF schema initialization failed.\n"; - return -1; - } + 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 (!sdf::readFile(_path, sdf)) + { + std::cerr << "Error: SDF parsing the xml failed.\n"; + return -1; + } - if (result == 0) - { - std::cout << "Valid.\n"; + if (result == 0) + { + std::cout << "Valid.\n"; + } + return result; } - 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)) + ////////////////////////////////////////////////// + char *gzVersion() { - std::cerr << "Error: SDF schema initialization failed.\n"; - return -1; + #ifdef _MSC_VER + return _strdup(SDF_VERSION_FULL); + #else + return strdup(SDF_VERSION_FULL); + #endif } - 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)) + ////////////////////////////////////////////////// + int cmdDescribe(const char *_version) { - std::cerr << "Error: File [" << _path << "] does not exist.\n"; - return -1; - } + sdf::SDFPtr sdf(new sdf::SDF()); - sdf::ParserConfig parserConfig; - if (_expandAutoInertials) - { - parserConfig.SetCalculateInertialConfiguration( - sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT); - } + if (nullptr != _version) + { + sdf->Version(_version); + } + if (!sdf::init(sdf)) + { + std::cerr << "Error: SDF schema initialization failed.\n"; + return -1; + } - sdf::Root root; - sdf::Errors errors = root.Load(_path, parserConfig); + sdf->PrintDescription(); - sdf::PrintConfig config; - if (_inDegrees != 0) - { - config.SetRotationInDegrees(true); + return 0; } - if (_snapToDegrees > 0) + ////////////////////////////////////////////////// + int cmdPrint(const char *_path, int _inDegrees, int _snapToDegrees, + float _snapTolerance, int _preserveIncludes, int _outPrecision, + int _expandAutoInertials) { - config.SetRotationSnapToDegrees(static_cast(_snapToDegrees), - static_cast(_snapTolerance)); - } + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } - if (_preserveIncludes != 0) - config.SetPreserveIncludes(true); + sdf::ParserConfig parserConfig; + if (_expandAutoInertials) + { + parserConfig.SetCalculateInertialConfiguration( + sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT); + } - if (_outPrecision > 0) - config.SetOutPrecision(_outPrecision); + sdf::Root root; + sdf::Errors errors = root.Load(_path, parserConfig); - if (root.Element()) - { - root.Element()->PrintValues(errors, "", config); - } + sdf::PrintConfig config; + if (_inDegrees != 0) + { + config.SetRotationInDegrees(true); + } - if (!errors.empty()) - { - std::cerr << errors << std::endl; - return -1; - } - return 0; -} + if (_snapToDegrees > 0) + { + config.SetRotationSnapToDegrees(static_cast(_snapToDegrees), + static_cast(_snapTolerance)); + } -////////////////////////////////////////////////// -// 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; - } + if (_preserveIncludes != 0) + config.SetPreserveIncludes(true); - sdf::Root root; - sdf::Errors errors = root.Load(_path); - if (!errors.empty()) - { - std::cerr << errors << std::endl; - } + if (_outPrecision > 0) + config.SetOutPrecision(_outPrecision); - 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) + if (root.Element()) { - errors = - sdf::buildPoseRelativeToGraph(graph, root.Model()); + root.Element()->PrintValues(errors, "", config); } if (!errors.empty()) { std::cerr << errors << std::endl; + return -1; } - std::cout << graph.Graph() << std::endl; + return 0; } - else if (std::strcmp(_graphType, "frame") == 0) + + ////////////////////////////////////////////////// + int cmdGraph(const char *_graphType, const char *_path) { - 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) + if (!sdf::filesystem::exists(_path)) { - errors = - sdf::buildFrameAttachedToGraph(graph, root.Model()); + 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; } - std::cout << graph.Graph() << std::endl; - } - else - { - std::cerr << R"(Only "pose" and "frame" graph types are supported)" - << std::endl; - } - return 0; -} + 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; + } -////////////////////////////////////////////////// -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; + return 0; } - sdf::Root root; - sdf::Errors errors = root.Load(_path); - if (!errors.empty()) + ////////////////////////////////////////////////// + int cmdInertialStats(const char *_path) { - std::cerr << errors << std::endl; - } + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } - if (root.WorldCount() > 0) - { - std::cerr << "Error: Expected a model file but received a world file." - << std::endl; - return -1; - } + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } - const sdf::Model *model = root.Model(); - if (!model) - { - std::cerr << "Error: Could not find the model." << std::endl; - return -1; - } + if (root.WorldCount() > 0) + { + std::cerr << "Error: Expected a model file but received a world file." + << std::endl; + return -1; + } - if (model->ModelCount() > 0) - { - std::cout << "Warning: Inertial properties of links in nested" - " models will not be included." << std::endl; - } + const sdf::Model *model = root.Model(); + if (!model) + { + std::cerr << "Error: Could not find the model." << std::endl; + return -1; + } - gz::math::Inertiald totalInertial; + if (model->ModelCount() > 0) + { + std::cout << "Warning: Inertial properties of links in nested" + " models will not be included." << std::endl; + } - for (uint64_t i = 0; i < model->LinkCount(); i++) - { - gz::math::Inertiald currentLinkInertial; - model->LinkByIndex(i)->ResolveInertial(currentLinkInertial, "__model__"); + gz::math::Inertiald totalInertial; - totalInertial += currentLinkInertial; - } + for (uint64_t i = 0; i < model->LinkCount(); i++) + { + gz::math::Inertiald currentLinkInertial; + model->LinkByIndex(i)->ResolveInertial(currentLinkInertial, "__model__"); - auto totalMass = totalInertial.MassMatrix().Mass(); - auto xCentreOfMass = totalInertial.Pose().Pos().X(); - auto yCentreOfMass = totalInertial.Pose().Pos().Y(); - auto zCentreOfMass = totalInertial.Pose().Pos().Z(); + totalInertial += currentLinkInertial; + } - 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; + 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 << "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 << "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 << "Moment of inertia matrix: " << 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; - // Pretty print the MOI matrix - std::stringstream ss; - ss << totalInertial.Moi(); + std::cout << "Moment of inertia matrix: " << std::endl; - 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(); - } - } + // Pretty print the MOI matrix + std::stringstream ss; + ss << totalInertial.Moi(); - 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::string s; + size_t maxLength = 0u; + std::vector moiVector; + while ( std::getline(ss, s, ' ' ) ) { - std::cout << " "; + moiVector.push_back(s); + if (s.size() > maxLength) + { + maxLength = s.size(); + } } - // Add space for the next element - std::cout << " "; - // Add '\n' if the next row is about to start - if ((i+1)%3 == 0) + + for (int i = 0; i < 9; i++) { - std::cout << "\n"; + 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; + std::cout << "---" << std::endl; - return 0; -} + return 0; + } + } +} \ No newline at end of file diff --git a/src/cmd/gz.hh b/src/cmd/gz.hh index 6f3dbd2c6..15cf1a6b3 100644 --- a/src/cmd/gz.hh +++ b/src/cmd/gz.hh @@ -20,21 +20,53 @@ #include -#include #include "sdf/system_util.hh" -// Inline bracket to help doxygen filtering. -inline namespace SDF_VERSION_NAMESPACE { -// +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 file to validate. -/// \return Zero on success, negative one otherwise. -extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path); + /// \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 read the library version. -/// \return C-string representing the version. Ex.: 0.1.2 -extern "C" SDFORMAT_VISIBLE char *gzVersion(); + /// \brief External hook to read the library version. + /// \return C-string representing the version. Ex.: 0.1.2 + char *gzVersion(); + + /// \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, int _inDegrees, int _snapToDegrees, + float _snapTolerance, int _preserveIncludes, int _outPrecision, + int _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/cmd/sdf_main.cc b/src/cmd/sdf_main.cc index 55c280a53..1f3d3d4d7 100644 --- a/src/cmd/sdf_main.cc +++ b/src/cmd/sdf_main.cc @@ -26,6 +26,8 @@ #include "sdf/config.hh" #include "gz.hh" +using namespace sdf; + ////////////////////////////////////////////////// /// \brief Enumeration of available commands enum class SdfCommand From d8343553ba27ca37c5d3d3d6ac05ecc5be994ae0 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Tue, 11 Feb 2025 21:06:57 +0800 Subject: [PATCH 11/19] Changed types from int to bool for flags Signed-off-by: Saurabh Kamat --- src/cmd/gz.cc | 18 +++++++++++------- src/cmd/gz.hh | 9 +++++---- src/cmd/sdf_main.cc | 20 ++++++++++---------- 3 files changed, 26 insertions(+), 21 deletions(-) diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc index 0a497bc42..73fce2062 100644 --- a/src/cmd/gz.cc +++ b/src/cmd/gz.cc @@ -15,9 +15,12 @@ * */ +#include #include +#include #include #include +#include #include #include #include @@ -141,9 +144,9 @@ namespace sdf } ////////////////////////////////////////////////// - int cmdPrint(const char *_path, int _inDegrees, int _snapToDegrees, - float _snapTolerance, int _preserveIncludes, int _outPrecision, - int _expandAutoInertials) + int cmdPrint(const char *_path, bool _inDegrees, int _snapToDegrees, + float _snapTolerance, bool _preserveIncludes, int _outPrecision, + bool _expandAutoInertials) { if (!sdf::filesystem::exists(_path)) { @@ -162,7 +165,7 @@ namespace sdf sdf::Errors errors = root.Load(_path, parserConfig); sdf::PrintConfig config; - if (_inDegrees != 0) + if (_inDegrees) { config.SetRotationInDegrees(true); } @@ -173,7 +176,7 @@ namespace sdf static_cast(_snapTolerance)); } - if (_preserveIncludes != 0) + if (_preserveIncludes) config.SetPreserveIncludes(true); if (_outPrecision > 0) @@ -308,7 +311,8 @@ namespace sdf auto yCentreOfMass = totalInertial.Pose().Pos().Y(); auto zCentreOfMass = totalInertial.Pose().Pos().Z(); - std::cout << "Inertial statistics for model: " << model->Name() << std::endl; + 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; @@ -359,4 +363,4 @@ namespace sdf return 0; } } -} \ No newline at end of file +} diff --git a/src/cmd/gz.hh b/src/cmd/gz.hh index 15cf1a6b3..053bbfb35 100644 --- a/src/cmd/gz.hh +++ b/src/cmd/gz.hh @@ -51,9 +51,9 @@ namespace sdf /// \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, int _inDegrees, int _snapToDegrees, - float _snapTolerance, int _preserveIncludes, int _outPrecision, - int _expandAutoInertials); + 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. @@ -61,7 +61,8 @@ namespace sdf /// \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 + /// \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); diff --git a/src/cmd/sdf_main.cc b/src/cmd/sdf_main.cc index 1f3d3d4d7..4a095bb2f 100644 --- a/src/cmd/sdf_main.cc +++ b/src/cmd/sdf_main.cc @@ -58,22 +58,22 @@ struct SdfOptions /// * "pose" for PoseRelativeToGraph std::string graphType{""}; - /// \brief When nonzero, preserve included tags when printing converted arg + /// \brief When true, preserve included tags when printing converted arg /// (does not preserve merge-includes) - int preserveIncludes{0}; + bool preserveIncludes{false}; - /// \brief When nonzero, print pose rotations in degrees. - int degrees{0}; + /// \brief When true, print pose rotations in degrees. + bool degrees{false}; - /// \brief When nonzero, auto-computed inertial values will be printed. - int expandAutoInertials{0}; + /// \brief When true, auto-computed inertial values will be printed. + bool expandAutoInertials{false}; /// \brief Output stream precision for floating point numbers. - std::optional precision; + int precision{0}; /// \brief If set, printed rotations are snapped to specified degree /// intervals. - std::optional snapToDegrees; + int snapToDegrees{0}; /// \brief Printed rotations are snapped if they are less than this specified /// tolerance. @@ -99,8 +99,8 @@ void runSdfCommand(const SdfOptions &_opt) cmdInertialStats(_opt.filepath.c_str()); break; case SdfCommand::kSdfPrint: - cmdPrint(_opt.filepath.c_str(), _opt.degrees, *_opt.snapToDegrees, - _opt.snapTolerance, _opt.preserveIncludes, *_opt.precision, + cmdPrint(_opt.filepath.c_str(), _opt.degrees, _opt.snapToDegrees, + _opt.snapTolerance, _opt.preserveIncludes, _opt.precision, _opt.expandAutoInertials); break; case SdfCommand::kNone: From 22d78cc9a00a91041d195b14121d001b2252027e Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Tue, 11 Feb 2025 21:08:10 +0800 Subject: [PATCH 12/19] Defined test config path as alias and populated it from cmake Signed-off-by: Saurabh Kamat --- src/cmd/CMakeLists.txt | 2 +- src/cmd/gz_TEST.cc | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index ec30947db..581f7cafb 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -31,7 +31,7 @@ if(BUILD_TESTING AND GZ_PROGRAM) target_compile_definitions(UNIT_gz_TEST PRIVATE "GZ_PATH=\"${GZ_PROGRAM}\"" - "GZ_CONFIG_PATH=\"${CMAKE_BINARY_DIR}/test/conf/$\"" + "DETAIL_GZ_CONFIG_PATH=\"${CMAKE_BINARY_DIR}/test/conf/$\"" "GZ_TEST_LIBRARY_PATH=\"$\"" ) endif() diff --git a/src/cmd/gz_TEST.cc b/src/cmd/gz_TEST.cc index 0071ba1cd..864de685e 100644 --- a/src/cmd/gz_TEST.cc +++ b/src/cmd/gz_TEST.cc @@ -37,6 +37,9 @@ #define pclose _pclose #endif +// DETAIL_GZ_CONFIG_PATH is compiler definition set in CMake. +#define GZ_CONFIG_PATH DETAIL_GZ_CONFIG_PATH + static std::string SdfVersion() { return " --force-version " + std::string(SDF_VERSION_FULL); From 51e848570a02f31031316922a9561e4ecc493cdd Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Mon, 24 Feb 2025 21:14:30 +0800 Subject: [PATCH 13/19] Removed GZ_PROGRAM dependency from sdf_descriptions target Signed-off-by: Saurabh Kamat --- sdf/CMakeLists.txt | 38 ++++++++++++++++++-------------------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt index 433a4954e..987a1188d 100644 --- a/sdf/CMakeLists.txt +++ b/sdf/CMakeLists.txt @@ -29,25 +29,23 @@ execute_process( # 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}) - # Add a dependency on the gz-sdformat-sdf target which is created in in ../cmd/CMakeLists - add_dependencies(sdf_descriptions gz-sdformat-sdf) -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) From b15e7679025bc5209faea5ee2135e0e546550ce8 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Wed, 26 Feb 2025 14:29:53 +0800 Subject: [PATCH 14/19] Added concise description and validation to -g option Signed-off-by: Saurabh Kamat --- src/cmd/sdf_main.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/cmd/sdf_main.cc b/src/cmd/sdf_main.cc index 4a095bb2f..69c2cb743 100644 --- a/src/cmd/sdf_main.cc +++ b/src/cmd/sdf_main.cc @@ -159,11 +159,12 @@ void addSdfFlags(CLI::App &_app) opt->command = SdfCommand::kSdfGraph; opt->graphType = _graphType; }, - " filepath Print the PoseRelativeTo or FrameAttachedTo " - "graph.\n" + "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); + ->needs(filepathOpt) + ->check(CLI::IsMember({"pose", "frame"})); command->add_flag_callback("--inertial-stats", [opt](){ From 3505c3d0d59693b66d7ad97144edef667fd573f3 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 27 Feb 2025 21:35:03 +0800 Subject: [PATCH 15/19] Removed function to print gz version Signed-off-by: Saurabh Kamat --- src/cmd/gz.cc | 10 ---------- src/cmd/gz.hh | 4 ---- 2 files changed, 14 deletions(-) diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc index 73fce2062..751ab5691 100644 --- a/src/cmd/gz.cc +++ b/src/cmd/gz.cc @@ -113,16 +113,6 @@ namespace sdf return result; } - ////////////////////////////////////////////////// - char *gzVersion() - { - #ifdef _MSC_VER - return _strdup(SDF_VERSION_FULL); - #else - return strdup(SDF_VERSION_FULL); - #endif - } - ////////////////////////////////////////////////// int cmdDescribe(const char *_version) { diff --git a/src/cmd/gz.hh b/src/cmd/gz.hh index 053bbfb35..dc3beb8e5 100644 --- a/src/cmd/gz.hh +++ b/src/cmd/gz.hh @@ -33,10 +33,6 @@ namespace sdf /// \return Zero on success, negative one otherwise. int cmdCheck(const char *_path); - /// \brief External hook to read the library version. - /// \return C-string representing the version. Ex.: 0.1.2 - char *gzVersion(); - /// \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. From eacb7b38d4b2231359e0e8d0d16e4c78b17c44da Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 27 Feb 2025 21:36:23 +0800 Subject: [PATCH 16/19] Added dependency on -p for print specific flags Signed-off-by: Saurabh Kamat --- src/cmd/sdf_main.cc | 46 ++++++++++++++++++++++++++++----------------- 1 file changed, 29 insertions(+), 17 deletions(-) diff --git a/src/cmd/sdf_main.cc b/src/cmd/sdf_main.cc index 69c2cb743..a492b23a0 100644 --- a/src/cmd/sdf_main.cc +++ b/src/cmd/sdf_main.cc @@ -118,22 +118,6 @@ void addSdfFlags(CLI::App &_app) auto filepathOpt = _app.add_option("filepath", opt->filepath, "Path to an SDFormat file."); - _app.add_flag("-i,--preserve-includes", opt->preserveIncludes, - "Preserve included tags when printing converted arg (does " - "not preserve merge-includes)."); - _app.add_flag("--degrees", opt->degrees, - "Printed pose rotations are will be in degrees."); - _app.add_flag("--expand-auto-inertials", opt->expandAutoInertials, - "Auto-computed inertial values will be printed."); - _app.add_option("--precision", opt->precision, - "Set the output stream precision for floating point " - "numbers."); - _app.add_option("--snap-to-degrees", opt->snapToDegrees, - "Printed rotations are snapped to specified degree " - "intervals."); - _app.add_option("--snap-tolerance", opt->snapTolerance, - "Printed rotations are snapped if they are within this " - "specified tolerance."); auto command = _app.add_option_group("command", "Command to be executed."); @@ -174,7 +158,7 @@ void addSdfFlags(CLI::App &_app) "sdf file.") ->needs(filepathOpt); - command->add_flag_callback("-p,--print", + auto printCmd = command->add_flag_callback("-p,--print", [opt](){ opt->command = SdfCommand::kSdfPrint; }, @@ -182,6 +166,34 @@ void addSdfFlags(CLI::App &_app) "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); }); } From 3f35036e3bc0ed76a0e3885435fa7976515a151e Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 27 Feb 2025 22:21:00 +0800 Subject: [PATCH 17/19] Added back files for eventual git rename Signed-off-by: Saurabh Kamat --- src/cmd/gz.cc | 356 ------------------------------------------------- src/cmd/gz.hh | 69 ---------- src/gz.cc | 361 ++++++++++++++++++++++++++++++++++++++++++++++++++ src/gz.hh | 40 ++++++ 4 files changed, 401 insertions(+), 425 deletions(-) delete mode 100644 src/cmd/gz.cc delete mode 100644 src/cmd/gz.hh create mode 100644 src/gz.cc create mode 100644 src/gz.hh diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc deleted file mode 100644 index 751ab5691..000000000 --- a/src/cmd/gz.cc +++ /dev/null @@ -1,356 +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 -#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 deleted file mode 100644 index dc3beb8e5..000000000 --- a/src/cmd/gz.hh +++ /dev/null @@ -1,69 +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 "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.cc b/src/gz.cc new file mode 100644 index 000000000..3f59b0b99 --- /dev/null +++ b/src/gz.cc @@ -0,0 +1,361 @@ +/* + * 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; +} \ No newline at end of file diff --git a/src/gz.hh b/src/gz.hh new file mode 100644 index 000000000..e040be6af --- /dev/null +++ b/src/gz.hh @@ -0,0 +1,40 @@ +/* + * 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 \ No newline at end of file From 5fdfb42d9677b3ae75f83f430c0afcfbafcf8b44 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 27 Feb 2025 22:36:54 +0800 Subject: [PATCH 18/19] Moved cmd source files Signed-off-by: Saurabh Kamat --- src/{ => cmd}/gz.cc | 0 src/{ => cmd}/gz.hh | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/{ => cmd}/gz.cc (100%) rename src/{ => cmd}/gz.hh (100%) diff --git a/src/gz.cc b/src/cmd/gz.cc similarity index 100% rename from src/gz.cc rename to src/cmd/gz.cc diff --git a/src/gz.hh b/src/cmd/gz.hh similarity index 100% rename from src/gz.hh rename to src/cmd/gz.hh From 0f40dd57ef29673bf6d3c4f4fd4a271bdc018ffb Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 27 Feb 2025 22:38:20 +0800 Subject: [PATCH 19/19] Added modification to cmd source files Signed-off-by: Saurabh Kamat --- src/cmd/gz.cc | 509 +++++++++++++++++++++++++------------------------- src/cmd/gz.hh | 53 ++++-- 2 files changed, 293 insertions(+), 269 deletions(-) diff --git a/src/cmd/gz.cc b/src/cmd/gz.cc index 3f59b0b99..751ab5691 100644 --- a/src/cmd/gz.cc +++ b/src/cmd/gz.cc @@ -15,9 +15,12 @@ * */ +#include #include +#include #include #include +#include #include #include #include @@ -34,328 +37,320 @@ #include "gz/math/Inertial.hh" -#include "FrameSemantics.hh" -#include "ScopedGraph.hh" +#include "../FrameSemantics.hh" +#include "../ScopedGraph.hh" #include "gz.hh" -////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path) +namespace sdf { - int result = 0; + inline namespace SDF_VERSION_NAMESPACE { - sdf::Root root; - sdf::Errors errors = root.Load(_path); - if (!errors.empty()) + ////////////////////////////////////////////////// + int cmdCheck(const char *_path) { - for (auto &error : errors) + int result = 0; + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) { - std::cerr << error << std::endl; + for (auto &error : errors) + { + std::cerr << error << std::endl; + } + return -1; } - 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::checkCanonicalLinkNames(&root)) + { + result = -1; + } - if (!sdf::recursiveSiblingUniqueNames(root.Element())) - { - result = -1; - } + if (!sdf::checkJointParentChildNames(&root)) + { + result = -1; + } - if (!sdf::filesystem::exists(_path)) - { - std::cerr << "Error: File [" << _path << "] does not exist.\n"; - return -1; - } + if (!sdf::checkFrameAttachedToGraph(&root)) + { + result = -1; + } - sdf::SDFPtr sdf(new sdf::SDF()); + if (!sdf::checkPoseRelativeToGraph(&root)) + { + result = -1; + } - if (!sdf::init(sdf)) - { - std::cerr << "Error: SDF schema initialization failed.\n"; - return -1; - } + if (!sdf::recursiveSiblingUniqueNames(root.Element())) + { + result = -1; + } - if (!sdf::readFile(_path, sdf)) - { - std::cerr << "Error: SDF parsing the xml failed.\n"; - return -1; - } + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } - if (result == 0) - { - std::cout << "Valid.\n"; - } - return result; -} + sdf::SDFPtr sdf(new sdf::SDF()); -////////////////////////////////////////////////// -extern "C" SDFORMAT_VISIBLE char *gzVersion() -{ -#ifdef _MSC_VER - return _strdup(SDF_VERSION_FULL); -#else - return strdup(SDF_VERSION_FULL); -#endif -} + if (!sdf::init(sdf)) + { + std::cerr << "Error: SDF schema initialization failed.\n"; + return -1; + } -////////////////////////////////////////////////// -/// \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 (!sdf::readFile(_path, sdf)) + { + std::cerr << "Error: SDF parsing the xml failed.\n"; + return -1; + } - if (nullptr != _version) - { - sdf->Version(_version); - } - if (!sdf::init(sdf)) - { - std::cerr << "Error: SDF schema initialization failed.\n"; - return -1; + if (result == 0) + { + std::cout << "Valid.\n"; + } + return result; } - 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)) + ////////////////////////////////////////////////// + int cmdDescribe(const char *_version) { - std::cerr << "Error: File [" << _path << "] does not exist.\n"; - return -1; - } + sdf::SDFPtr sdf(new sdf::SDF()); - sdf::ParserConfig parserConfig; - if (_expandAutoInertials) - { - parserConfig.SetCalculateInertialConfiguration( - sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT); - } + if (nullptr != _version) + { + sdf->Version(_version); + } + if (!sdf::init(sdf)) + { + std::cerr << "Error: SDF schema initialization failed.\n"; + return -1; + } - sdf::Root root; - sdf::Errors errors = root.Load(_path, parserConfig); + sdf->PrintDescription(); - sdf::PrintConfig config; - if (_inDegrees != 0) - { - config.SetRotationInDegrees(true); + return 0; } - if (_snapToDegrees > 0) + ////////////////////////////////////////////////// + int cmdPrint(const char *_path, bool _inDegrees, int _snapToDegrees, + float _snapTolerance, bool _preserveIncludes, int _outPrecision, + bool _expandAutoInertials) { - config.SetRotationSnapToDegrees(static_cast(_snapToDegrees), - static_cast(_snapTolerance)); - } + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } - if (_preserveIncludes != 0) - config.SetPreserveIncludes(true); + sdf::ParserConfig parserConfig; + if (_expandAutoInertials) + { + parserConfig.SetCalculateInertialConfiguration( + sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT); + } - if (_outPrecision > 0) - config.SetOutPrecision(_outPrecision); + sdf::Root root; + sdf::Errors errors = root.Load(_path, parserConfig); - if (root.Element()) - { - root.Element()->PrintValues(errors, "", config); - } + sdf::PrintConfig config; + if (_inDegrees) + { + config.SetRotationInDegrees(true); + } - if (!errors.empty()) - { - std::cerr << errors << std::endl; - return -1; - } - return 0; -} + if (_snapToDegrees > 0) + { + config.SetRotationSnapToDegrees(static_cast(_snapToDegrees), + static_cast(_snapTolerance)); + } -////////////////////////////////////////////////// -// 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; - } + if (_preserveIncludes) + config.SetPreserveIncludes(true); - sdf::Root root; - sdf::Errors errors = root.Load(_path); - if (!errors.empty()) - { - std::cerr << errors << std::endl; - } + if (_outPrecision > 0) + config.SetOutPrecision(_outPrecision); - 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) + if (root.Element()) { - errors = - sdf::buildPoseRelativeToGraph(graph, root.Model()); + root.Element()->PrintValues(errors, "", config); } if (!errors.empty()) { std::cerr << errors << std::endl; + return -1; } - std::cout << graph.Graph() << std::endl; + return 0; } - else if (std::strcmp(_graphType, "frame") == 0) + + ////////////////////////////////////////////////// + int cmdGraph(const char *_graphType, const char *_path) { - 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) + if (!sdf::filesystem::exists(_path)) { - errors = - sdf::buildFrameAttachedToGraph(graph, root.Model()); + 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; } - 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 (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; + } - if (model->ModelCount() > 0) - { - std::cout << "Warning: Inertial properties of links in nested" - " models will not be included." << std::endl; + return 0; } - gz::math::Inertiald totalInertial; - - for (uint64_t i = 0; i < model->LinkCount(); i++) + ////////////////////////////////////////////////// + int cmdInertialStats(const char *_path) { - gz::math::Inertiald currentLinkInertial; - model->LinkByIndex(i)->ResolveInertial(currentLinkInertial, "__model__"); - - totalInertial += currentLinkInertial; - } + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } - auto totalMass = totalInertial.MassMatrix().Mass(); - auto xCentreOfMass = totalInertial.Pose().Pos().X(); - auto yCentreOfMass = totalInertial.Pose().Pos().Y(); - auto zCentreOfMass = totalInertial.Pose().Pos().Z(); + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } - 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; + if (root.WorldCount() > 0) + { + std::cerr << "Error: Expected a model file but received a world file." + << std::endl; + return -1; + } - 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; + const sdf::Model *model = root.Model(); + if (!model) + { + std::cerr << "Error: Could not find the model." << std::endl; + return -1; + } - std::cout << "Moment of inertia matrix: " << std::endl; + if (model->ModelCount() > 0) + { + std::cout << "Warning: Inertial properties of links in nested" + " models will not be included." << std::endl; + } - // Pretty print the MOI matrix - std::stringstream ss; - ss << totalInertial.Moi(); + gz::math::Inertiald totalInertial; - std::string s; - size_t maxLength = 0u; - std::vector moiVector; - while ( std::getline(ss, s, ' ' ) ) - { - moiVector.push_back(s); - if (s.size() > maxLength) + for (uint64_t i = 0; i < model->LinkCount(); i++) { - maxLength = s.size(); + gz::math::Inertiald currentLinkInertial; + model->LinkByIndex(i)->ResolveInertial(currentLinkInertial, "__model__"); + + totalInertial += currentLinkInertial; } - } - 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++) + 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, ' ' ) ) { - std::cout << " "; + moiVector.push_back(s); + if (s.size() > maxLength) + { + maxLength = s.size(); + } } - // Add space for the next element - std::cout << " "; - // Add '\n' if the next row is about to start - if ((i+1)%3 == 0) + + for (int i = 0; i < 9; i++) { - std::cout << "\n"; + 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; + std::cout << "---" << std::endl; - return 0; -} \ No newline at end of file + return 0; + } + } +} diff --git a/src/cmd/gz.hh b/src/cmd/gz.hh index e040be6af..dc3beb8e5 100644 --- a/src/cmd/gz.hh +++ b/src/cmd/gz.hh @@ -20,21 +20,50 @@ #include -#include #include "sdf/system_util.hh" -// Inline bracket to help doxygen filtering. -inline namespace SDF_VERSION_NAMESPACE { -// +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 file to validate. -/// \return Zero on success, negative one otherwise. -extern "C" SDFORMAT_VISIBLE int cmdCheck(const char *_path); + /// \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 read the library version. -/// \return C-string representing the version. Ex.: 0.1.2 -extern "C" SDFORMAT_VISIBLE char *gzVersion(); + /// \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 \ No newline at end of file +#endif