From 275b671fc0a14b0d1bd1e89600f8149617380afb Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 22 Jul 2021 16:17:59 -0700 Subject: [PATCH 1/4] Logic tweaks Signed-off-by: Louise Poubel --- src/cmd/ModelCommandAPI.cc | 318 +++++++++++++++++++------------------ src/cmd/ModelCommandAPI.hh | 10 +- src/cmd/cmdmodel.rb.in | 8 +- src/cmd/ignmodel.yaml.in | 4 +- tutorials.md.in | 2 +- tutorials/model_command.md | 48 +++--- 6 files changed, 193 insertions(+), 197 deletions(-) diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc index d6ba45e8ad..cd43a79270 100644 --- a/src/cmd/ModelCommandAPI.cc +++ b/src/cmd/ModelCommandAPI.cc @@ -21,8 +21,12 @@ #include #include +#include +#include + #include #include +#include #include #include #include @@ -34,29 +38,28 @@ #include #include #include -#include -#include -#include +#include #include -namespace { +using namespace ignition; +using namespace gazebo; ////////////////////////////////////////////////// -// \brief Get the name of the world being used by calling -// `/gazebo/worlds` service. -// \return The name of the world if service is available, -// an empty string otherwise. +/// \brief Get the name of the world being used by calling +/// `/gazebo/worlds` service. +/// \return The name of the world if service is available, +/// an empty string otherwise. std::string getWorldName() { // Create a transport node. - ignition::transport::Node node; + transport::Node node; bool result{false}; const unsigned int timeout{5000}; const std::string service{"/gazebo/worlds"}; // Request and block - ignition::msgs::StringMsg_V res; + msgs::StringMsg_V res; if (!node.Request(service, timeout, res, result)) { @@ -75,11 +78,63 @@ std::string getWorldName() return res.data().Get(0); } +////////////////////////////////////////////////// +/// \brief Get entity info: name and entity ID +/// \param[in] _entity Entity to get info +/// \param[in] _ecm Entity component manager +/// \return " []" +std::string entityInfo(Entity _entity, const EntityComponentManager &_ecm) +{ + std::string info; + + const auto nameComp = _ecm.Component( _entity); + if (nameComp) + { + info += nameComp->Data() + " "; + } + info += "[" + std::to_string(_entity) + "]"; + return info; +} + +////////////////////////////////////////////////// +/// \brief Get entity info: name and entity ID +/// \param[in] _entity Name of entity to get info +/// \param[in] _ecm Entity component manager +/// \return " []" +std::string entityInfo(const std::string &_name, + const EntityComponentManager &_ecm) +{ + std::string info{_name}; + + auto entity = _ecm.EntityByComponents(components::Name(_name)); + if (kNullEntity != entity) + { + info += " [" + std::to_string(entity) + "]"; + } + return info; +} + +////////////////////////////////////////////////// +/// \brief Get pose info in a standard way +/// \param[in] _pose Pose to print +/// \param[in] _prefix Indentation prefix for every line +/// \return Pose formatted in a standard way +std::string poseInfo(math::Pose3d _pose, const std::string &_prefix) +{ + return + _prefix + "[" + std::to_string(_pose.X()) + " | " + + std::to_string(_pose.Y()) + " | " + + std::to_string(_pose.Z()) + "]\n" + + _prefix + "[" + std::to_string(_pose.Roll()) + " | " + + std::to_string(_pose.Pitch()) + " | " + + std::to_string(_pose.Yaw()) + "]"; +} + ////////////////////////////////////////////////// // \brief Set the state of a ECM instance with a world snapshot. // \param _ecm ECM instance to be populated. // \return boolean indicating if it was able to populate the ECM. -bool populateECM(ignition::gazebo::EntityComponentManager &_ecm) +bool populateECM(EntityComponentManager &_ecm) { const std::string world = getWorldName(); if (world.empty()) @@ -89,7 +144,7 @@ bool populateECM(ignition::gazebo::EntityComponentManager &_ecm) return false; } // Create a transport node. - ignition::transport::Node node; + transport::Node node; bool result{false}; const unsigned int timeout{5000}; const std::string service{"/world/" + world + "/state"}; @@ -98,7 +153,7 @@ bool populateECM(ignition::gazebo::EntityComponentManager &_ecm) << "]..." << std::endl << std::endl; // Request and block - ignition::msgs::SerializedStepMap res; + msgs::SerializedStepMap res; if (!node.Request(service, timeout, res, result)) { @@ -121,90 +176,56 @@ bool populateECM(ignition::gazebo::EntityComponentManager &_ecm) ////////////////////////////////////////////////// -// \brief Print the model pose information. +// \brief Print the model information. // \param[in] _entity Entity of the model requested. // \param[in] _ecm ECM ready for requests. -void printPose(const uint64_t _entity, - const ignition::gazebo::EntityComponentManager &_ecm){ - const auto modelPose = _ecm.EntitiesByComponents( - ignition::gazebo::components::ParentEntity(_entity), - ignition::gazebo::components::Pose()); - +void printModelInfo(const uint64_t _entity, + const EntityComponentManager &_ecm) +{ const auto poseComp = - _ecm.Component(_entity); + _ecm.Component(_entity); const auto nameComp = - _ecm.Component(_entity); - if (poseComp) + _ecm.Component(_entity); + if (poseComp && nameComp) { - std::string poseInfo; - poseInfo += "\n [" + std::to_string(poseComp->Data().X()) + " | " - + std::to_string(poseComp->Data().Y()) + " | " - + std::to_string(poseComp->Data().Z()) + "]\n" - " [" + std::to_string(poseComp->Data().Roll()) + " | " - + std::to_string(poseComp->Data().Pitch()) + " | " - + std::to_string(poseComp->Data().Yaw()) + "]"; - std::cout << "Model: [" << _entity << "]" << std::endl << " - Name: " << nameComp->Data() << std::endl - << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " - << poseInfo << std::endl << std::endl; + << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " << std::endl + << poseInfo(poseComp->Data(), " ") << std::endl; } } ////////////////////////////////////////////////// // \brief Print the model links information. -// \param[in] _entity Entity of the model requested. +// \param[in] _modelEntity Entity of the model requested. // \param[in] _ecm ECM ready for requests. -// \param[in] _linkName Link to be printed, if nullptr, print all links. -void printLinks(const uint64_t _entity, - const ignition::gazebo::EntityComponentManager &_ecm, +// \param[in] _linkName Link to be printed, if empty, print all links. +void printLinks(const uint64_t _modelEntity, + const EntityComponentManager &_ecm, const std::string &_linkName) { const auto links = _ecm.EntitiesByComponents( - ignition::gazebo::components::ParentEntity(_entity), - ignition::gazebo::components::Link()); + components::ParentEntity(_modelEntity), components::Link()); for (const auto &entity : links) { - const auto parentComp = - _ecm.Component(entity); - - const auto nameComp = - _ecm.Component(entity); + const auto nameComp = _ecm.Component(entity); if (_linkName.length() && _linkName != nameComp->Data()) continue; - if (parentComp) - { - std::string parentInfo; - const auto parentNameComp = - _ecm.Component( - parentComp->Data()); - - if (parentNameComp) - { - parentInfo += parentNameComp->Data() + " "; - } - parentInfo += "[" + std::to_string(parentComp->Data()) + "]"; - std::cout << " - Link [" << entity << "]" << std::endl - << " - Name: " << nameComp->Data() << std::endl - << " - Parent: " << parentInfo << std::endl; - } + std::cout << " - Link [" << entity << "]" << std::endl + << " - Name: " << nameComp->Data() << std::endl + << " - Parent: " << entityInfo(_modelEntity, _ecm) + << std::endl; - const auto inertialComp = - _ecm.Component(entity); + const auto inertialComp = _ecm.Component(entity); if (inertialComp) { - const auto inertialMatrix = inertialComp->Data().MassMatrix(); - const auto massComp = inertialComp->Data().MassMatrix().Mass(); - - const std::string inertialPose = - "\n [" + std::to_string(inertialComp->Data().Pose().X()) + " | " - + std::to_string(inertialComp->Data().Pose().Y()) + " | " - + std::to_string(inertialComp->Data().Pose().Z()) + "]"; + const auto inertialMatrix = inertialComp->Data().MassMatrix(); + const auto mass = inertialComp->Data().MassMatrix().Mass(); - const std::string massInfo = "[" + std::to_string(massComp) + "]"; + const std::string massInfo = "[" + std::to_string(mass) + "]"; const std::string inertialInfo = "\n [" + std::to_string(inertialMatrix.Ixx()) + " | " + std::to_string(inertialMatrix.Ixy()) + " | " @@ -216,45 +237,34 @@ void printLinks(const uint64_t _entity, + std::to_string(inertialMatrix.Iyz()) + " | " + std::to_string(inertialMatrix.Izz()) + "]"; std::cout << " - Mass (kg): " << massInfo << std::endl - << " - Inertial Pose:" << inertialPose << std::endl + << " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:" + << std::endl + << poseInfo(inertialComp->Data().Pose(), " ") + << std::endl << " - Inertial Matrix (kg⋅m^2): " << inertialInfo << std::endl; } - const std::string inertialPose = - "\n [" + std::to_string(inertialComp->Data().Pose().X()) + " | " - + std::to_string(inertialComp->Data().Pose().Y()) + " | " - + std::to_string(inertialComp->Data().Pose().Z()) + "]"; - - - const auto poseComp = - _ecm.Component(entity); + const auto poseComp = _ecm.Component(entity); if (poseComp) { - const std::string poseInfo = - "\n [" + std::to_string(poseComp->Data().X()) + " | " - + std::to_string(poseComp->Data().Y()) + " | " - + std::to_string(poseComp->Data().Z()) + "]\n" - " [" + std::to_string(poseComp->Data().Roll()) + " | " - + std::to_string(poseComp->Data().Pitch()) + " | " - + std::to_string(poseComp->Data().Yaw()) + "]"; - - std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " - << poseInfo << std::endl; + std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl + << poseInfo(poseComp->Data(), " ") << std::endl; } } } ////////////////////////////////////////////////// // \brief Print the model joints information. -// \param[in] _entity Entity of the model requested. +// \param[in] _modelEntity Entity of the model requested. // \param[in] _ecm ECM ready for requests. // \param[in] _jointName Joint to be printed, if nullptr, print all joints. -void printJoints(const uint64_t entity, - const ignition::gazebo::EntityComponentManager &_ecm, +void printJoints(const uint64_t _modelEntity, + const EntityComponentManager &_ecm, const std::string &_jointName) { - static const std::map jointTypes = { + static const std::map jointTypes = + { {sdf::JointType::REVOLUTE, "revolute"}, {sdf::JointType::BALL, "ball"}, {sdf::JointType::CONTINUOUS, "continuous"}, @@ -267,101 +277,95 @@ void printJoints(const uint64_t entity, }; const auto joints = _ecm.EntitiesByComponents( - ignition::gazebo::components::ParentEntity(entity), - ignition::gazebo::components::Joint()); + components::ParentEntity(_modelEntity), components::Joint()); - for (const auto &_entity : joints) + for (const auto &entity : joints) { - const auto parentComp = - _ecm.Component(_entity); - - const auto nameComp = - _ecm.Component(_entity); + const auto nameComp = _ecm.Component(entity); if (_jointName.length() && _jointName != nameComp->Data()) - continue; + continue; - if (parentComp) + std::cout << " - Joint [" << entity << "]" << std::endl + << " - Name: " << nameComp->Data() << std::endl + << " - Parent: " << entityInfo(_modelEntity, _ecm) + << std::endl; + + const auto jointTypeComp = _ecm.Component(entity); + if (jointTypeComp) { - std::string parentInfo; - auto parentNameComp = - _ecm.Component( - parentComp->Data()); - - if (parentNameComp) - { - parentInfo += parentNameComp->Data() + " "; - } - parentInfo += "[" + std::to_string(parentComp->Data()) + "]"; - - std::cout << " - Joint [" << _entity << "]" << std::endl - << " - Name: " << nameComp->Data() << std::endl - << " - Parent: " << parentInfo << std::endl; + std::cout << " - Type: " << jointTypes.at(jointTypeComp->Data()) + << std::endl; } - const auto jointComp = - _ecm.Component(_entity); const auto childLinkComp = - _ecm.Component(_entity); + _ecm.Component(entity); const auto parentLinkComp = - _ecm.Component(_entity); - const auto poseComp = - _ecm.Component(_entity); - const auto axisComp = - _ecm.Component(_entity); + _ecm.Component(entity); if (childLinkComp && parentLinkComp) { - const std::string poseInfo = - "\n [" + std::to_string(poseComp->Data().X()) + " | " - + std::to_string(poseComp->Data().Y()) + " | " - + std::to_string(poseComp->Data().Z()) + "]\n" - " [" + std::to_string(poseComp->Data().Roll()) + " | " - + std::to_string(poseComp->Data().Pitch()) + " | " - + std::to_string(poseComp->Data().Yaw()) + "]"; - - std::cout << " - Type: " << jointTypes.at(jointComp->Data()) - << "\n" << " - Parent Link: " << childLinkComp->Data() << "\n" - << " - Child Link: " << parentLinkComp->Data() << "\n" - << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " - << poseInfo << std::endl; + std::cout << " - Parent Link: " + << entityInfo(parentLinkComp->Data(), _ecm) << "\n" + << " - Child Link: " + << entityInfo(childLinkComp->Data(), _ecm) << "\n"; + } + + const auto poseComp = _ecm.Component(entity); + if (poseComp) + { + std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " << std::endl + << poseInfo(poseComp->Data(), " ") << std::endl; } + + const auto axisComp = _ecm.Component(entity); if (axisComp) { std::cout << " - Axis unit vector [ XYZ ]: \n" - " [" << axisComp->Data().Xyz().X() << " | " - << axisComp->Data().Xyz().Y() << " | " - << axisComp->Data().Xyz().Z() << "]\n"; + " [" << axisComp->Data().Xyz().X() << " | " + << axisComp->Data().Xyz().Y() << " | " + << axisComp->Data().Xyz().Z() << "]\n"; } } } -} ////////////////////////////////////////////////// extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList() { - ignition::gazebo::EntityComponentManager ecm{}; + EntityComponentManager ecm{}; if (!populateECM(ecm)) { return; } + + auto world = ecm.EntityByComponents(components::World()); + if (kNullEntity == world) + { + std::cout << "No world found." << std::endl; + return; + } + const auto models = ecm.EntitiesByComponents( - ignition::gazebo::components::ParentEntity(1), - ignition::gazebo::components::Model()); + components::ParentEntity(world), components::Model()); + + if (models.size() == 0) + { + std::cout << "No models in world [" << world << "]" << std::endl; + return; + } std::cout << "Available models:" << std::endl; for (const auto &m : models) { - const auto nameComp = - ecm.Component(m); + const auto nameComp = ecm.Component(m); std::cout << " - " << nameComp->Data() << std::endl; } } ////////////////////////////////////////////////// extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( - const char *_model, int _pose, const char *_linkName, const char *_jointName) + const char *_modelName, int _pose, const char *_linkName, const char *_jointName) { std::string linkName{""}; if (_linkName) @@ -373,34 +377,32 @@ extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( if (!_pose && !_linkName && !_jointName) printAll = true; - if (!_model) + if (!_modelName) { std::cerr << std::endl << "Model name not found" << std::endl; return; } - const std::string model{_model}; - ignition::gazebo::EntityComponentManager ecm{}; + EntityComponentManager ecm{}; if (!populateECM(ecm)) return; // Get the desired model entity. - auto entity = ecm.EntityByComponents( - ignition::gazebo::components::Name(model), - ignition::gazebo::components::Model()); + auto entity = ecm.EntityByComponents(components::Name(_modelName), + components::Model()); - if (entity == ignition::gazebo::kNullEntity) - std::cout << "No model named <" << model << "> was found" << std::endl; + if (entity == kNullEntity) + std::cout << "No model named <" << _modelName << "> was found" << std::endl; // Get the pose of the model if (printAll | _pose) - printPose(entity, ecm); + printModelInfo(entity, ecm); // Get the links information if (printAll | (_linkName != nullptr)) printLinks(entity, ecm, linkName); - // Get the links information + // Get the joints information if (printAll | (_jointName != nullptr)) printJoints(entity, ecm, jointName); } diff --git a/src/cmd/ModelCommandAPI.hh b/src/cmd/ModelCommandAPI.hh index 072ab9c55e..fc776b1c18 100644 --- a/src/cmd/ModelCommandAPI.hh +++ b/src/cmd/ModelCommandAPI.hh @@ -16,22 +16,16 @@ */ #include "ignition/gazebo/Export.hh" -#include "ignition/gazebo/Entity.hh" -#include -#include - /// \brief External hook to get a list of available models. extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList(); /// \brief External hook to dump model information. -/// \param[in] _model Model name. +/// \param[in] _modelName Model name. /// \param[in] _pose --pose option. -/// \param[in] _link --link option. /// \param[in] _link_name Link name. -/// \param[in] _joint --joint option. /// \param[in] _joint_name Joint name. extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( - const char *_model, int _pose, const char *_linkName, + const char *_modelName, int _pose, const char *_linkName, const char *_jointName); diff --git a/src/cmd/cmdmodel.rb.in b/src/cmd/cmdmodel.rb.in index 0351b24087..173b0f1a00 100644 --- a/src/cmd/cmdmodel.rb.in +++ b/src/cmd/cmdmodel.rb.in @@ -87,7 +87,7 @@ class Cmd exit end opts.on('-m', '--model [arg]', String, 'Model name') do |m| - options['model'] = m + options['model_name'] = m end opts.on('--list', String, 'List models available') do options['list'] = 1 @@ -162,10 +162,10 @@ class Cmd Importer.extern 'void cmdModelList()' Importer.cmdModelList() exit(0) - elsif options.key?('model') - Importer.extern 'void cmdModelInfo(const char *, int, const char *, + elsif options.key?('model_name') + Importer.extern 'void cmdModelInfo(const char *, int, const char *, const char *)' - Importer.cmdModelInfo(options['model'], options['pose'], + Importer.cmdModelInfo(options['model_name'], options['pose'], options['link_name'], options['joint_name']) else puts 'Command error: I do not have an implementation for '\ diff --git a/src/cmd/ignmodel.yaml.in b/src/cmd/ignmodel.yaml.in index d86419e1e5..c0775c8bab 100644 --- a/src/cmd/ignmodel.yaml.in +++ b/src/cmd/ignmodel.yaml.in @@ -1,6 +1,6 @@ ---- # Subcommands available inside ignition-model. +--- # Model subcommand available inside ignition gazebo. format: 1.0.0 -library_name: ignition-model +library_name: ignition-gazebo-ign-model library_version: @PROJECT_VERSION_FULL@ library_path: @ign_model_ruby_path@ commands: diff --git a/tutorials.md.in b/tutorials.md.in index 6580997245..7fe0b53976 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -25,7 +25,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage logicalaudiosensor "Logical Audio Sensor": Using the LogicalAudioSensor system to mimic logical audio emission and detection in simulation. * \subpage videorecorder "Video Recorder": Record videos from the 3D render window. * \subpage model_and_optimize_meshes "Model and optimize meshes": Some recomendations when creating meshes in blender for simulations. -* \subpage model_command "Model Command": Print information about models. +* \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. **Migration from Gazebo classic** diff --git a/tutorials/model_command.md b/tutorials/model_command.md index 4511467522..1fbc4ae755 100644 --- a/tutorials/model_command.md +++ b/tutorials/model_command.md @@ -10,7 +10,7 @@ For each model, it is possible to get information about its ## Example running the diff_drive world -To try out this command we need first a running simulation. Let's load the diff_drive ignition simulation. In a terminal, run: +To try out this command we need first a running simulation. Let's load the `diff_drive` example world. In a terminal, run: ign gazebo diff_drive.sdf @@ -29,7 +29,7 @@ And available models should be printed: Once you get the name of the model you want to see, you may run the following commands to get its properties. -`ign model -m ` to get the **complete information of the model**. e.g. +`ign model -m ` to get the **complete information of the model**. e.g. ign model -m vehicle_blue @@ -38,7 +38,7 @@ Once you get the name of the model you want to see, you may run the following co Model: [8] - Name: vehicle_blue - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [0.000000 | 2.000000 | 0.325000] [0.000000 | 0.000000 | 0.000000] @@ -48,11 +48,11 @@ Once you get the name of the model you want to see, you may run the following co - Mass (kg): [1.143950] - Inertial Pose: [0.000000 | 0.000000 | 0.000000] - - Inertial Matrix (kg⋅m^2): + - Inertial Matrix (kg⋅m^2): [0.126164 | 0.000000 | 0.000000] [0.000000 | 0.416519 | 0.000000] [0.000000 | 0.000000 | 0.481014] - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [-0.151427 | 0.000000 | 0.175000] [0.000000 | 0.000000 | 0.000000] - Link [12] @@ -61,11 +61,11 @@ Once you get the name of the model you want to see, you may run the following co - Mass (kg): [2.000000] - Inertial Pose: [0.000000 | 0.000000 | 0.000000] - - Inertial Matrix (kg⋅m^2): + - Inertial Matrix (kg⋅m^2): [0.145833 | 0.000000 | 0.000000] [0.000000 | 0.145833 | 0.000000] [0.000000 | 0.000000 | 0.125000] - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [0.554283 | 0.625029 | -0.025000] [-1.570700 | 0.000000 | 0.000000] - Link [15] @@ -74,11 +74,11 @@ Once you get the name of the model you want to see, you may run the following co - Mass (kg): [2.000000] - Inertial Pose: [0.000000 | 0.000000 | 0.000000] - - Inertial Matrix (kg⋅m^2): + - Inertial Matrix (kg⋅m^2): [0.145833 | 0.000000 | 0.000000] [0.000000 | 0.145833 | 0.000000] [0.000000 | 0.000000 | 0.125000] - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [0.554282 | -0.625029 | -0.025000] [-1.570700 | 0.000000 | 0.000000] - Link [18] @@ -87,11 +87,11 @@ Once you get the name of the model you want to see, you may run the following co - Mass (kg): [1.000000] - Inertial Pose: [0.000000 | 0.000000 | 0.000000] - - Inertial Matrix (kg⋅m^2): + - Inertial Matrix (kg⋅m^2): [0.100000 | 0.000000 | 0.000000] [0.000000 | 0.100000 | 0.000000] [0.000000 | 0.000000 | 0.100000] - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [-0.957138 | 0.000000 | -0.125000] [0.000000 | 0.000000 | 0.000000] - Joint [21] @@ -100,10 +100,10 @@ Once you get the name of the model you want to see, you may run the following co - Type: revolute - Parent Link: left_wheel - Child Link: chassis - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [0.000000 | 0.000000 | 0.000000] [0.000000 | 0.000000 | 0.000000] - - Axis position [ XYZ ]: + - Axis position [ XYZ ]: [0 | 0 | 1] - Joint [22] - Name: right_wheel_joint @@ -111,10 +111,10 @@ Once you get the name of the model you want to see, you may run the following co - Type: revolute - Parent Link: right_wheel - Child Link: chassis - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [0.000000 | 0.000000 | 0.000000] [0.000000 | 0.000000 | 0.000000] - - Axis position [ XYZ ]: + - Axis position [ XYZ ]: [0 | 0 | 1] - Joint [23] - Name: caster_wheel @@ -122,14 +122,14 @@ Once you get the name of the model you want to see, you may run the following co - Type: ball - Parent Link: caster - Child Link: chassis - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [0.000000 | 0.000000 | 0.000000] [0.000000 | 0.000000 | 0.000000] ``` -`ign model -m --pose` to get the **pose** information. e.g. +`ign model -m --pose` to get the **pose** information. e.g. ign model -m vehicle_blue --pose @@ -145,12 +145,12 @@ Once you get the name of the model you want to see, you may run the following co ``` -To get the information of **all the model links** enter +To get the information of **all the model links** enter ign model -m --link -Or you can get the information of a **single link** by adding the name as argument. e.g. +Or you can get the information of a **single link** by adding the name as argument. e.g. ign model -m vehicle_blue --link caster @@ -163,21 +163,21 @@ Or you can get the information of a **single link** by adding the name as argume - Mass (kg): [1.000000] - Inertial Pose: [0.000000 | 0.000000 | 0.000000] - - Inertial Matrix (kg⋅m^2): + - Inertial Matrix (kg⋅m^2): [0.100000 | 0.000000 | 0.000000] [0.000000 | 0.100000 | 0.000000] [0.000000 | 0.000000 | 0.100000] - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [-0.957138 | 0.000000 | -0.125000] [0.000000 | -0.000000 | 0.000000] ``` -To get the information of **all the model joints** enter +To get the information of **all the model joints** enter ign model -m --joint -Or you can get the information of a **single joint** by adding the name as argument. e.g. +Or you can get the information of a **single joint** by adding the name as argument. e.g. ign model -m vehicle_blue --joint caster_wheel @@ -190,7 +190,7 @@ Or you can get the information of a **single joint** by adding the name as argum - Type: ball - Parent Link: caster - Child Link: chassis - - Pose [ XYZ (m) ] [ RPY (rad) ]: + - Pose [ XYZ (m) ] [ RPY (rad) ]: [0.000000 | 0.000000 | 0.000000] [0.000000 | -0.000000 | 0.000000] ``` From 75da302b3b8d5dd9b69d0210deed63e11260716e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 22 Jul 2021 16:26:03 -0700 Subject: [PATCH 2/4] No need for another component Signed-off-by: Louise Poubel --- src/CMakeLists.txt | 7 +++++-- src/cmd/CMakeLists.txt | 8 -------- src/cmd/cmdmodel.rb.in | 2 +- 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 27777aba49..44f8bbab97 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -31,8 +31,11 @@ set(gui_sources PARENT_SCOPE ) -ign_add_component(model SOURCES cmd/ModelCommandAPI.cc GET_TARGET_NAME model_lib_target) -ign_add_component(ign SOURCES ign.cc GET_TARGET_NAME ign_lib_target) +ign_add_component(ign + SOURCES + ign.cc + cmd/ModelCommandAPI.cc + GET_TARGET_NAME ign_lib_target) target_link_libraries(${ign_lib_target} PRIVATE ${PROJECT_LIBRARY_TARGET_NAME} diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index dba0388bdb..f80aba4ef6 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -43,10 +43,6 @@ install( FILES set(cmd_model_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") set(cmd_model_script_configured "${cmd_model_script_generated}.configured") -# Set the model_library_location variable to the relative path to the library file -# within the install directory structure. -set(model_library_location "../../../${CMAKE_INSTALL_LIBDIR}/$") - configure_file( "cmdmodel.rb.in" "${cmd_model_script_configured}" @@ -106,10 +102,6 @@ set(cmd_model_ruby_test_dir "${CMAKE_BINARY_DIR}/model/test/lib/ruby/ignition") set(cmd_model_script_generated_test "${cmd_model_ruby_test_dir}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") set(cmd_model_script_configured_test "${cmd_model_script_generated_test}.configured") -# Set the model_library_location variable to the full path of the library file -# within the build directory -set(model_library_location "$") - configure_file( "cmdmodel.rb.in" "${cmd_model_script_configured_test}" diff --git a/src/cmd/cmdmodel.rb.in b/src/cmd/cmdmodel.rb.in index 173b0f1a00..e11ae958e1 100644 --- a/src/cmd/cmdmodel.rb.in +++ b/src/cmd/cmdmodel.rb.in @@ -29,7 +29,7 @@ require 'date' require 'optparse' # Constants. -LIBRARY_NAME = '@model_library_location@' +LIBRARY_NAME = '@library_location@' LIBRARY_VERSION = '@PROJECT_VERSION_FULL@' COMMON_OPTIONS = " -h [ --help ] Print this help message. \n"\ From 5f99aecea7911f601b6e2982ac47369a3ba1e828 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 22 Jul 2021 20:11:26 -0700 Subject: [PATCH 3/4] tests, more cleanup, 'ign model' Signed-off-by: Louise Poubel --- src/CMakeLists.txt | 25 +-- src/ModelCommandAPI_TEST.cc | 180 +++++++++++--------- src/cmd/CMakeLists.txt | 12 +- src/cmd/ModelCommandAPI.cc | 13 +- src/cmd/cmdmodel.rb.in | 28 +-- src/cmd/{ignmodel.yaml.in => model.yaml.in} | 2 +- 6 files changed, 137 insertions(+), 123 deletions(-) rename src/cmd/{ignmodel.yaml.in => model.yaml.in} (83%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 44f8bbab97..339988e4c7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -140,7 +140,14 @@ ign_build_tests(TYPE UNIT ignition-gazebo${PROJECT_VERSION_MAJOR} ) -if(TARGET UNIT_ign_TEST) +# Command line tests need extra settings +foreach(CMD_TEST + UNIT_ign_TEST + UNIT_ModelCommandAPI_TEST) + + if(NOT TARGET ${CMD_TEST}) + continue() + endif() # Running `ign gazebo` on macOS has problems when run with /usr/bin/ruby # due to System Integrity Protection (SIP). Try to find ruby from @@ -149,33 +156,27 @@ if(TARGET UNIT_ign_TEST) find_program(BREW_RUBY ruby HINTS /usr/local/opt/ruby/bin) endif() - add_dependencies(UNIT_ign_TEST + add_dependencies(${CMD_TEST} ${ign_lib_target} TestModelSystem TestSensorSystem TestWorldSystem ) - target_compile_definitions(UNIT_ign_TEST PRIVATE + target_compile_definitions(${CMD_TEST} PRIVATE "BREW_RUBY=\"${BREW_RUBY} \"") - target_compile_definitions(UNIT_ign_TEST PRIVATE + target_compile_definitions(${CMD_TEST} PRIVATE "IGN_PATH=\"${IGNITION-TOOLS_BINARY_DIRS}\"") set(_env_vars) list(APPEND _env_vars "IGN_CONFIG_PATH=${CMAKE_BINARY_DIR}/test/conf") list(APPEND _env_vars "IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$") - set_tests_properties(UNIT_ign_TEST PROPERTIES + set_tests_properties(${CMD_TEST} PROPERTIES ENVIRONMENT "${_env_vars}") -endif() -if(TARGET UNIT_ModelCommandAPI_TEST) - - target_compile_definitions(UNIT_ModelCommandAPI_TEST PRIVATE - "IGN_PATH=\"${IGNITION-TOOLS_BINARY_DIRS}\"") - -endif() +endforeach() if(NOT WIN32) add_subdirectory(cmd) diff --git a/src/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc index 1a0b7108c5..efb6225411 100644 --- a/src/ModelCommandAPI_TEST.cc +++ b/src/ModelCommandAPI_TEST.cc @@ -15,8 +15,6 @@ * */ -#include "cmd/ModelCommandAPI.hh" - #include #include #include @@ -26,8 +24,8 @@ #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/test_config.hh" // NOLINT(build/include) -static const std::string kIgnModelCommand(std::string(IGN_PATH) + - "/ign model "); +static const std::string kIgnModelCommand( + std::string(BREW_RUBY) + std::string(IGN_PATH) + "/ign model "); ///////////////////////////////////////////////// @@ -49,6 +47,8 @@ void ReplaceNegativeZeroValues(std::string &_text) ///////////////////////////////////////////////// std::string customExecStr(std::string _cmd) { + std::cout << "Running command [" << _cmd << "]" << std::endl; + _cmd += " 2>&1"; FILE *pipe = popen(_cmd.c_str(), "r"); @@ -70,6 +70,7 @@ std::string customExecStr(std::string _cmd) return result; } +///////////////////////////////////////////////// // Test `ign model` command when no Gazebo server is running. TEST(ModelCommandAPI, NoServerRunning) { @@ -82,6 +83,7 @@ TEST(ModelCommandAPI, NoServerRunning) EXPECT_EQ(expectedOutput, output); } +///////////////////////////////////////////////// // Tests `ign model` command. TEST(ModelCommandAPI, Commands) { @@ -117,93 +119,96 @@ TEST(ModelCommandAPI, Commands) "\nRequesting state for world [diff_drive]...\n\n" "Model: [8]\n" " - Name: vehicle_blue\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.000000 | 2.000000 | 0.325000]\n" " [0.000000 | 0.000000 | 0.000000]\n" - "\n" " - Link [9]\n" " - Name: chassis\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [1.143950]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " - Inertial Matrix (kg.m^2):\n" " [0.126164 | 0.000000 | 0.000000]\n" " [0.000000 | 0.416519 | 0.000000]\n" " [0.000000 | 0.000000 | 0.481014]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [-0.151427 | 0.000000 | 0.175000]\n" " [0.000000 | 0.000000 | 0.000000]\n" " - Link [12]\n" " - Name: left_wheel\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [2.000000]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " - Inertial Matrix (kg.m^2):\n" " [0.145833 | 0.000000 | 0.000000]\n" " [0.000000 | 0.145833 | 0.000000]\n" " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.554283 | 0.625029 | -0.025000]\n" " [-1.570700 | 0.000000 | 0.000000]\n" " - Link [15]\n" " - Name: right_wheel\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [2.000000]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg.m^2):\n" " [0.145833 | 0.000000 | 0.000000]\n" " [0.000000 | 0.145833 | 0.000000]\n" " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.554282 | -0.625029 | -0.025000]\n" " [-1.570700 | 0.000000 | 0.000000]\n" " - Link [18]\n" " - Name: caster\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [1.000000]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " - Inertial Matrix (kg.m^2):\n" " [0.100000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.100000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.100000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [-0.957138 | 0.000000 | -0.125000]\n" " [0.000000 | 0.000000 | 0.000000]\n" " - Joint [21]\n" " - Name: left_wheel_joint\n" " - Parent: vehicle_blue [8]\n" - " - Type: revolute\n" - " - Parent Link: left_wheel\n" - " - Child Link: chassis\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Axis unit vector [ XYZ ]: \n" - " [0 | 0 | 1]\n" + " - Type: revolute\n" + " - Parent Link: chassis [9]\n" + " - Child Link: left_wheel [12]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Axis unit vector [ XYZ ]:\n" + " [0 | 0 | 1]\n" " - Joint [22]\n" " - Name: right_wheel_joint\n" " - Parent: vehicle_blue [8]\n" - " - Type: revolute\n" - " - Parent Link: right_wheel\n" - " - Child Link: chassis\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Axis unit vector [ XYZ ]: \n" - " [0 | 0 | 1]\n" + " - Type: revolute\n" + " - Parent Link: chassis [9]\n" + " - Child Link: right_wheel [15]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Axis unit vector [ XYZ ]:\n" + " [0 | 0 | 1]\n" " - Joint [23]\n" " - Name: caster_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Type: ball\n" - " - Parent Link: caster\n" - " - Child Link: chassis\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + " - Type: ball\n" + " - Parent Link: chassis [9]\n" + " - Child Link: caster [18]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -216,9 +221,9 @@ TEST(ModelCommandAPI, Commands) "\nRequesting state for world [diff_drive]...\n\n" "Model: [8]\n" " - Name: vehicle_blue\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.000000 | 2.000000 | 0.325000]\n" - " [0.000000 | 0.000000 | 0.000000]\n\n"; + " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -234,52 +239,56 @@ TEST(ModelCommandAPI, Commands) " - Name: chassis\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [1.143950]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg.m^2):\n" " [0.126164 | 0.000000 | 0.000000]\n" " [0.000000 | 0.416519 | 0.000000]\n" " [0.000000 | 0.000000 | 0.481014]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [-0.151427 | 0.000000 | 0.175000]\n" " [0.000000 | 0.000000 | 0.000000]\n" " - Link [12]\n" " - Name: left_wheel\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [2.000000]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " - Inertial Matrix (kg.m^2):\n" " [0.145833 | 0.000000 | 0.000000]\n" " [0.000000 | 0.145833 | 0.000000]\n" " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.554283 | 0.625029 | -0.025000]\n" " [-1.570700 | 0.000000 | 0.000000]\n" " - Link [15]\n" " - Name: right_wheel\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [2.000000]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " - Inertial Matrix (kg.m^2):\n" " [0.145833 | 0.000000 | 0.000000]\n" " [0.000000 | 0.145833 | 0.000000]\n" " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.554282 | -0.625029 | -0.025000]\n" " [-1.570700 | 0.000000 | 0.000000]\n" " - Link [18]\n" " - Name: caster\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [1.000000]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg.m^2):\n" " [0.100000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.100000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.100000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [-0.957138 | 0.000000 | -0.125000]\n" " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); @@ -297,13 +306,14 @@ TEST(ModelCommandAPI, Commands) " - Name: caster\n" " - Parent: vehicle_blue [8]\n" " - Mass (kg): [1.000000]\n" - " - Inertial Pose:\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg⋅m^2): \n" + " - Inertial Matrix (kg.m^2):\n" " [0.100000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.100000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.100000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" " [-0.957138 | 0.000000 | -0.125000]\n" " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); @@ -320,34 +330,34 @@ TEST(ModelCommandAPI, Commands) " - Joint [21]\n" " - Name: left_wheel_joint\n" " - Parent: vehicle_blue [8]\n" - " - Type: revolute\n" - " - Parent Link: left_wheel\n" - " - Child Link: chassis\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Axis unit vector [ XYZ ]: \n" - " [0 | 0 | 1]\n" + " - Type: revolute\n" + " - Parent Link: chassis [9]\n" + " - Child Link: left_wheel [12]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Axis unit vector [ XYZ ]:\n" + " [0 | 0 | 1]\n" " - Joint [22]\n" " - Name: right_wheel_joint\n" " - Parent: vehicle_blue [8]\n" - " - Type: revolute\n" - " - Parent Link: right_wheel\n" - " - Child Link: chassis\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Axis unit vector [ XYZ ]: \n" - " [0 | 0 | 1]\n" + " - Type: revolute\n" + " - Parent Link: chassis [9]\n" + " - Child Link: right_wheel [15]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Axis unit vector [ XYZ ]:\n" + " [0 | 0 | 1]\n" " - Joint [23]\n" " - Name: caster_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Type: ball\n" - " - Parent Link: caster\n" - " - Child Link: chassis\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + " - Type: ball\n" + " - Parent Link: chassis [9]\n" + " - Child Link: caster [18]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -362,12 +372,12 @@ TEST(ModelCommandAPI, Commands) " - Joint [23]\n" " - Name: caster_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Type: ball\n" - " - Parent Link: caster\n" - " - Child Link: chassis\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + " - Type: ball\n" + " - Parent Link: chassis [9]\n" + " - Child Link: caster [18]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } } diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index f80aba4ef6..e7bcb36718 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -56,12 +56,12 @@ install(FILES ${cmd_model_script_generated} DESTINATION lib/ruby/ignition) # Used for the installed version. set(ign_model_ruby_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/ignition/cmdmodel${PROJECT_VERSION_MAJOR}") -set(ignmodel_configured "${CMAKE_CURRENT_BINARY_DIR}/ignmodel${PROJECT_VERSION_MAJOR}.yaml") +set(model_configured "${CMAKE_CURRENT_BINARY_DIR}/model${PROJECT_VERSION_MAJOR}.yaml") configure_file( - "ignmodel.yaml.in" - ${ignmodel_configured}) + "model.yaml.in" + ${model_configured}) -install(FILES ${ignmodel_configured} DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/ignition/) +install(FILES ${model_configured} DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/ignition/) #=============================================================================== @@ -115,5 +115,5 @@ file(GENERATE set(ign_model_ruby_path "${cmd_model_script_generated_test}") configure_file( - "ignmodel.yaml.in" - "${cmd_model_ruby_test_dir}/ignmodel${PROJECT_VERSION_MAJOR}.yaml") + "model.yaml.in" + "${cmd_model_ruby_test_dir}/model${PROJECT_VERSION_MAJOR}.yaml") diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc index cd43a79270..7cebd40593 100644 --- a/src/cmd/ModelCommandAPI.cc +++ b/src/cmd/ModelCommandAPI.cc @@ -190,7 +190,7 @@ void printModelInfo(const uint64_t _entity, { std::cout << "Model: [" << _entity << "]" << std::endl << " - Name: " << nameComp->Data() << std::endl - << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " << std::endl + << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl << poseInfo(poseComp->Data(), " ") << std::endl; } } @@ -237,11 +237,11 @@ void printLinks(const uint64_t _modelEntity, + std::to_string(inertialMatrix.Iyz()) + " | " + std::to_string(inertialMatrix.Izz()) + "]"; std::cout << " - Mass (kg): " << massInfo << std::endl - << " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:" + << " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl << poseInfo(inertialComp->Data().Pose(), " ") << std::endl - << " - Inertial Matrix (kg⋅m^2): " + << " - Inertial Matrix (kg.m^2):" << inertialInfo << std::endl; } @@ -314,14 +314,14 @@ void printJoints(const uint64_t _modelEntity, const auto poseComp = _ecm.Component(entity); if (poseComp) { - std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " << std::endl + std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl << poseInfo(poseComp->Data(), " ") << std::endl; } const auto axisComp = _ecm.Component(entity); if (axisComp) { - std::cout << " - Axis unit vector [ XYZ ]: \n" + std::cout << " - Axis unit vector [ XYZ ]:\n" " [" << axisComp->Data().Xyz().X() << " | " << axisComp->Data().Xyz().Y() << " | " << axisComp->Data().Xyz().Z() << "]\n"; @@ -365,7 +365,8 @@ extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList() ////////////////////////////////////////////////// extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( - const char *_modelName, int _pose, const char *_linkName, const char *_jointName) + const char *_modelName, int _pose, const char *_linkName, + const char *_jointName) { std::string linkName{""}; if (_linkName) diff --git a/src/cmd/cmdmodel.rb.in b/src/cmd/cmdmodel.rb.in index e11ae958e1..8070bfec63 100644 --- a/src/cmd/cmdmodel.rb.in +++ b/src/cmd/cmdmodel.rb.in @@ -25,15 +25,18 @@ else include Fiddle end -require 'date' require 'optparse' # Constants. LIBRARY_NAME = '@library_location@' LIBRARY_VERSION = '@PROJECT_VERSION_FULL@' + COMMON_OPTIONS = - " -h [ --help ] Print this help message. \n"\ - " \n"\ + " -h [--help] Print this help message.\n"\ + " \n" + + " --force-version Use a specific library version.\n"\ + " \n" + + ' --versions Show the available versions.' COMMANDS = { 'model' => "Print information about models.\n\n"+ @@ -83,8 +86,8 @@ class Cmd opts.banner = usage opts.on('-h', '--help') do - puts usage - exit + puts usage + exit end opts.on('-m', '--model [arg]', String, 'Model name') do |m| options['model_name'] = m @@ -118,23 +121,23 @@ class Cmd # 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]) - puts usage - exit(-1) - end + if ARGV.empty? || !COMMANDS.key?(ARGV[0]) || + options.empty? + puts usage + exit(-1) + end # Check that an option was selected. - if options.length == 5 + if !(options.key?('list') || options.key?('model_name')) puts usage exit(-1) - end + end options['command'] = args[0] options end # parse() - def execute(args) options = parse(args) @@ -157,7 +160,6 @@ class Cmd exit(-1) end - if options.key?('list') Importer.extern 'void cmdModelList()' Importer.cmdModelList() diff --git a/src/cmd/ignmodel.yaml.in b/src/cmd/model.yaml.in similarity index 83% rename from src/cmd/ignmodel.yaml.in rename to src/cmd/model.yaml.in index c0775c8bab..b9d2ec9184 100644 --- a/src/cmd/ignmodel.yaml.in +++ b/src/cmd/model.yaml.in @@ -1,6 +1,6 @@ --- # Model subcommand available inside ignition gazebo. format: 1.0.0 -library_name: ignition-gazebo-ign-model +library_name: ignition-gazebo-ign library_version: @PROJECT_VERSION_FULL@ library_path: @ign_model_ruby_path@ commands: From fe44f8f2b3f85fa4b769cf8d163c2ead65184047 Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Fri, 23 Jul 2021 18:39:18 -0300 Subject: [PATCH 4/4] Fixes ruby script for internal testing. Signed-off-by: Franco Cipollone --- src/cmd/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index e7bcb36718..a1ff8091c4 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -98,7 +98,7 @@ configure_file( #=============================================================================== # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. -set(cmd_model_ruby_test_dir "${CMAKE_BINARY_DIR}/model/test/lib/ruby/ignition") +set(cmd_model_ruby_test_dir "${CMAKE_BINARY_DIR}/test/lib/ruby/ignition") set(cmd_model_script_generated_test "${cmd_model_ruby_test_dir}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") set(cmd_model_script_configured_test "${cmd_model_script_generated_test}.configured") @@ -116,4 +116,4 @@ set(ign_model_ruby_path "${cmd_model_script_generated_test}") configure_file( "model.yaml.in" - "${cmd_model_ruby_test_dir}/model${PROJECT_VERSION_MAJOR}.yaml") + "${CMAKE_BINARY_DIR}/test/conf/model${PROJECT_VERSION_MAJOR}.yaml" @ONLY)