diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a3521cf28b..339988e4c7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -31,7 +31,11 @@ set(gui_sources PARENT_SCOPE ) -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} @@ -59,6 +63,7 @@ set (sources Util.cc View.cc World.cc + cmd/ModelCommandAPI.cc ${PROTO_PRIVATE_SRC} ${network_sources} ) @@ -74,6 +79,7 @@ set (gtest_sources ign_TEST.cc Link_TEST.cc Model_TEST.cc + ModelCommandAPI_TEST.cc SdfEntityCreator_TEST.cc SdfGenerator_TEST.cc Server_TEST.cc @@ -134,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 @@ -143,26 +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() + +endforeach() if(NOT WIN32) add_subdirectory(cmd) diff --git a/src/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc new file mode 100644 index 0000000000..efb6225411 --- /dev/null +++ b/src/ModelCommandAPI_TEST.cc @@ -0,0 +1,390 @@ +/* + * 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 "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" // NOLINT(build/include) + +static const std::string kIgnModelCommand( + std::string(BREW_RUBY) + std::string(IGN_PATH) + "/ign model "); + + +///////////////////////////////////////////////// +/// \brief Used to avoid the cases where the zero is +/// represented as a negative number. +/// \param _text Output string that may have negative zero values. +void ReplaceNegativeZeroValues(std::string &_text) +{ + std::string neg_zero{"-0.000000"}; + std::string zero{"0.000000"}; + size_t pos = 0; + while ((pos = _text.find(neg_zero, pos)) != std::string::npos) + { + _text.replace(pos, neg_zero.length(), zero); + pos += zero.length(); + } +} + +///////////////////////////////////////////////// +std::string customExecStr(std::string _cmd) +{ + std::cout << "Running command [" << _cmd << "]" << std::endl; + + _cmd += " 2>&1"; + FILE *pipe = popen(_cmd.c_str(), "r"); + + if (!pipe) + return "ERROR"; + + char buffer[128]; + std::string result = ""; + + while (!feof(pipe)) + { + if (fgets(buffer, 128, pipe) != nullptr) + { + result += buffer; + } + } + + pclose(pipe); + return result; +} + +///////////////////////////////////////////////// +// Test `ign model` command when no Gazebo server is running. +TEST(ModelCommandAPI, NoServerRunning) +{ + const std::string cmd = kIgnModelCommand + "--list "; + const std::string output = customExecStr(cmd); + const std::string expectedOutput = + "\nService call to [/gazebo/worlds] timed out\n" + "Command failed when trying to get the world name " + "of the running simulation.\n"; + EXPECT_EQ(expectedOutput, output); +} + +///////////////////////////////////////////////// +// Tests `ign model` command. +TEST(ModelCommandAPI, Commands) +{ + ignition::gazebo::ServerConfig serverConfig; + // Using an static model to avoid any movements in the simulation. + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/static_diff_drive_vehicle.sdf"); + + ignition::gazebo::Server server(serverConfig); + // Run at least one iteration before continuing to guarantee correctly set up. + ASSERT_TRUE(server.Run(true, 5, false)); + // Run without blocking. + server.Run(false, 0, false); + + // Tested command: ign model --list + { + const std::string cmd = kIgnModelCommand + "--list"; + const std::string output = customExecStr(cmd); + const std::string expectedOutput = + "\nRequesting state for world [diff_drive]..." + "\n\nAvailable models:\n" + " - ground_plane\n" + " - vehicle_blue\n"; + EXPECT_EQ(expectedOutput, output); + } + + // Tested command: ign model -m vehicle_blue + { + const std::string cmd = kIgnModelCommand + "-m vehicle_blue"; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [diff_drive]...\n\n" + "Model: [8]\n" + " - Name: vehicle_blue\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 2.000000 | 0.325000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Link [9]\n" + " - Name: chassis\n" + " - Parent: vehicle_blue [8]\n" + " - Mass (kg): [1.143950]\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" + " [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" + " [-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 [ 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" + " [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" + " [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 [ 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" + " [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" + " [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 [ 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" + " [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" + " [-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: 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: 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: 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); + } + + // Tested command: ign model -m vehicle_blue --pose + { + const std::string cmd = kIgnModelCommand + "-m vehicle_blue --pose "; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [diff_drive]...\n\n" + "Model: [8]\n" + " - Name: vehicle_blue\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 | 2.000000 | 0.325000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; + EXPECT_EQ(expectedOutput, output); + } + + // Tested command: ign model -m vehicle_blue --link + { + const std::string cmd = kIgnModelCommand + + "-m vehicle_blue --link"; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [diff_drive]...\n\n" + " - Link [9]\n" + " - Name: chassis\n" + " - Parent: vehicle_blue [8]\n" + " - Mass (kg): [1.143950]\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" + " [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" + " [-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 [ 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" + " [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" + " [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 [ 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" + " [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" + " [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 [ 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" + " [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" + " [-0.957138 | 0.000000 | -0.125000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; + EXPECT_EQ(expectedOutput, output); + } + + // Tested command: ign model -m vehicle_blue --link caster + { + const std::string cmd = kIgnModelCommand + + "-m vehicle_blue --link caster"; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [diff_drive]...\n\n" + " - Link [18]\n" + " - Name: caster\n" + " - Parent: vehicle_blue [8]\n" + " - Mass (kg): [1.000000]\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" + " [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" + " [-0.957138 | 0.000000 | -0.125000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; + EXPECT_EQ(expectedOutput, output); + } + + // Tested command: ign model -m vehicle_blue --joint + { + const std::string cmd = kIgnModelCommand + + "-m vehicle_blue --joint"; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [diff_drive]...\n\n" + " - Joint [21]\n" + " - Name: left_wheel_joint\n" + " - Parent: vehicle_blue [8]\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: 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: 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); + } + + // Tested command: ign model -m vehicle_blue --joint caster_wheel + { + const std::string cmd = kIgnModelCommand + + "-m vehicle_blue --joint caster_wheel"; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [diff_drive]...\n\n" + " - Joint [23]\n" + " - Name: caster_wheel\n" + " - Parent: vehicle_blue [8]\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); + } +} + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index c82f1d776a..a1ff8091c4 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -36,6 +36,34 @@ install( FILES DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/ignition/) +#=============================================================================== +# Used for the installed model command version. +# Generate the ruby script that gets installed. +# Note that the major version of the library is included in the name. +set(cmd_model_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") +set(cmd_model_script_configured "${cmd_model_script_generated}.configured") + +configure_file( + "cmdmodel.rb.in" + "${cmd_model_script_configured}" + @ONLY) +file(GENERATE + OUTPUT "${cmd_model_script_generated}" + INPUT "${cmd_model_script_configured}") + +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(model_configured "${CMAKE_CURRENT_BINARY_DIR}/model${PROJECT_VERSION_MAJOR}.yaml") +configure_file( + "model.yaml.in" + ${model_configured}) + +install(FILES ${model_configured} DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/ignition/) + + #=============================================================================== # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. @@ -66,3 +94,26 @@ set(ign_library_path configure_file( "${IGN_DESIGNATION}.yaml.in" "${CMAKE_BINARY_DIR}/test/conf/${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}.yaml" @ONLY) + +#=============================================================================== +# 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}/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") + +configure_file( + "cmdmodel.rb.in" + "${cmd_model_script_configured_test}" + @ONLY) + +file(GENERATE + OUTPUT "${cmd_model_script_generated_test}" + INPUT "${cmd_model_script_configured_test}") + +# Used for internal testing. +set(ign_model_ruby_path "${cmd_model_script_generated_test}") + +configure_file( + "model.yaml.in" + "${CMAKE_BINARY_DIR}/test/conf/model${PROJECT_VERSION_MAJOR}.yaml" @ONLY) diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc new file mode 100644 index 0000000000..7cebd40593 --- /dev/null +++ b/src/cmd/ModelCommandAPI.cc @@ -0,0 +1,409 @@ +/* + * 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 "ModelCommandAPI.hh" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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. +std::string getWorldName() +{ + // Create a transport node. + transport::Node node; + + bool result{false}; + const unsigned int timeout{5000}; + const std::string service{"/gazebo/worlds"}; + + // Request and block + msgs::StringMsg_V res; + + if (!node.Request(service, timeout, res, result)) + { + std::cerr << std::endl << "Service call to [" << service << "] timed out" + << std::endl; + return ""; + } + + if (!result) + { + std::cerr << std::endl << "Service call to [" << service << "] failed" + << std::endl; + return ""; + } + + 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(EntityComponentManager &_ecm) +{ + const std::string world = getWorldName(); + if (world.empty()) + { + std::cerr << "Command failed when trying to get the world name of " + << "the running simulation." << std::endl; + return false; + } + // Create a transport node. + transport::Node node; + bool result{false}; + const unsigned int timeout{5000}; + const std::string service{"/world/" + world + "/state"}; + + std::cout << std::endl << "Requesting state for world [" << world + << "]..." << std::endl << std::endl; + + // Request and block + msgs::SerializedStepMap res; + + if (!node.Request(service, timeout, res, result)) + { + std::cerr << std::endl << "Service call to [" << service << "] timed out" + << std::endl; + return false; + } + + if (!result) + { + std::cerr << std::endl << "Service call to [" << service << "] failed" + << std::endl; + return false; + } + + // Instantiate an ECM and populate with data from message + _ecm.SetState(res.state()); + return true; +} + + +////////////////////////////////////////////////// +// \brief Print the model information. +// \param[in] _entity Entity of the model requested. +// \param[in] _ecm ECM ready for requests. +void printModelInfo(const uint64_t _entity, + const EntityComponentManager &_ecm) +{ + const auto poseComp = + _ecm.Component(_entity); + const auto nameComp = + _ecm.Component(_entity); + if (poseComp && nameComp) + { + std::cout << "Model: [" << _entity << "]" << std::endl + << " - Name: " << nameComp->Data() << std::endl + << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl + << poseInfo(poseComp->Data(), " ") << std::endl; + } +} + +////////////////////////////////////////////////// +// \brief Print the model links information. +// \param[in] _modelEntity Entity of the model requested. +// \param[in] _ecm ECM ready for requests. +// \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( + components::ParentEntity(_modelEntity), components::Link()); + for (const auto &entity : links) + { + const auto nameComp = _ecm.Component(entity); + + if (_linkName.length() && _linkName != nameComp->Data()) + continue; + + std::cout << " - Link [" << entity << "]" << std::endl + << " - Name: " << nameComp->Data() << std::endl + << " - Parent: " << entityInfo(_modelEntity, _ecm) + << std::endl; + + const auto inertialComp = _ecm.Component(entity); + + if (inertialComp) + { + const auto inertialMatrix = inertialComp->Data().MassMatrix(); + const auto mass = inertialComp->Data().MassMatrix().Mass(); + + const std::string massInfo = "[" + std::to_string(mass) + "]"; + const std::string inertialInfo = + "\n [" + std::to_string(inertialMatrix.Ixx()) + " | " + + std::to_string(inertialMatrix.Ixy()) + " | " + + std::to_string(inertialMatrix.Ixz()) + "]\n" + " [" + std::to_string(inertialMatrix.Ixy()) + " | " + + std::to_string(inertialMatrix.Iyy()) + " | " + + std::to_string(inertialMatrix.Iyz()) + "]\n" + " [" + std::to_string(inertialMatrix.Ixz()) + " | " + + std::to_string(inertialMatrix.Iyz()) + " | " + + std::to_string(inertialMatrix.Izz()) + "]"; + std::cout << " - Mass (kg): " << massInfo << 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 auto poseComp = _ecm.Component(entity); + if (poseComp) + { + std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl + << poseInfo(poseComp->Data(), " ") << std::endl; + } + } +} + +////////////////////////////////////////////////// +// \brief Print the model joints information. +// \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 _modelEntity, + const EntityComponentManager &_ecm, + const std::string &_jointName) +{ + static const std::map jointTypes = + { + {sdf::JointType::REVOLUTE, "revolute"}, + {sdf::JointType::BALL, "ball"}, + {sdf::JointType::CONTINUOUS, "continuous"}, + {sdf::JointType::FIXED, "fixed"}, + {sdf::JointType::GEARBOX, "gearbox"}, + {sdf::JointType::PRISMATIC, "prismatic"}, + {sdf::JointType::REVOLUTE2, "revolute2"}, + {sdf::JointType::SCREW, "screw"}, + {sdf::JointType::UNIVERSAL, "universal"} + }; + + const auto joints = _ecm.EntitiesByComponents( + components::ParentEntity(_modelEntity), components::Joint()); + + for (const auto &entity : joints) + { + const auto nameComp = _ecm.Component(entity); + + if (_jointName.length() && _jointName != nameComp->Data()) + continue; + + 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::cout << " - Type: " << jointTypes.at(jointTypeComp->Data()) + << std::endl; + } + + const auto childLinkComp = + _ecm.Component(entity); + const auto parentLinkComp = + _ecm.Component(entity); + + if (childLinkComp && parentLinkComp) + { + 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"; + } + } +} + +////////////////////////////////////////////////// +extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList() +{ + 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( + 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); + std::cout << " - " << nameComp->Data() << std::endl; + } +} + +////////////////////////////////////////////////// +extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( + const char *_modelName, int _pose, const char *_linkName, + const char *_jointName) +{ + std::string linkName{""}; + if (_linkName) + linkName = _linkName; + std::string jointName{""}; + if (_jointName) + jointName = _jointName; + bool printAll{false}; + if (!_pose && !_linkName && !_jointName) + printAll = true; + + if (!_modelName) + { + std::cerr << std::endl << "Model name not found" << std::endl; + return; + } + + EntityComponentManager ecm{}; + if (!populateECM(ecm)) + return; + + // Get the desired model entity. + auto entity = ecm.EntityByComponents(components::Name(_modelName), + components::Model()); + + if (entity == kNullEntity) + std::cout << "No model named <" << _modelName << "> was found" << std::endl; + + // Get the pose of the model + if (printAll | _pose) + printModelInfo(entity, ecm); + + // Get the links information + if (printAll | (_linkName != nullptr)) + printLinks(entity, ecm, linkName); + + // Get the joints information + if (printAll | (_jointName != nullptr)) + printJoints(entity, ecm, jointName); +} diff --git a/src/cmd/ModelCommandAPI.hh b/src/cmd/ModelCommandAPI.hh new file mode 100644 index 0000000000..fc776b1c18 --- /dev/null +++ b/src/cmd/ModelCommandAPI.hh @@ -0,0 +1,31 @@ +/* + * 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 "ignition/gazebo/Export.hh" + +/// \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] _modelName Model name. +/// \param[in] _pose --pose option. +/// \param[in] _link_name Link name. +/// \param[in] _joint_name Joint name. +extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( + 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 new file mode 100644 index 0000000000..8070bfec63 --- /dev/null +++ b/src/cmd/cmdmodel.rb.in @@ -0,0 +1,179 @@ +#!/usr/bin/ruby + +# 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. + +# 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' + +# Constants. +LIBRARY_NAME = '@library_location@' +LIBRARY_VERSION = '@PROJECT_VERSION_FULL@' + +COMMON_OPTIONS = + " -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"+ + " \n"\ + " ign model [options] \n"\ + " \n"\ + "Available Options: \n"\ + " --list Get a list of the available models. \n"\ + " -m [--model] arg Select the model to be shown. \n"\ + " -p [--pose] Print the pose of the model. \n"\ + " -l [--link] arg Select a link to show its properties. \n"\ + " If no arg is passed all links are printed \n"\ + " Requires the -m option \n"\ + " \n"\ + " E.g. to get information about the \n"\ + " caster link in the diff_drive world, run: \n"\ + " ign model -m vehicle_blue -l caster \n"\ + " \n"\ + " -j [--joint] arg Select a joint to show its properties. \n"\ + " If no arg is passed all joints are printed \n"\ + " Requires the -m option \n"\ + " \n"\ + " E.g. to get information about the \n"\ + " caster_wheel joint in the diff_drive \n"\ + " world, run: \n"\ + " ign model -m vehicle_blue -j caster_wheel \n\n"+ + COMMON_OPTIONS +} + +# +# Class for the Ignition Gazebo Model command line tools. +# +class Cmd + + # + # Return a structure describing the options. + # + def parse(args) + options = { + 'pose' => 0, + 'link_name' => 0, + 'joint_name' => 0 + } + usage = COMMANDS[args[0]] + + opt_parser = OptionParser.new do |opts| + opts.banner = usage + + opts.on('-h', '--help') do + puts usage + exit + end + opts.on('-m', '--model [arg]', String, 'Model name') do |m| + options['model_name'] = m + end + opts.on('--list', String, 'List models available') do + options['list'] = 1 + end + opts.on('-p', '--pose', Integer, 'Request pose') do + options['pose'] = 1 + end + opts.on('-l', '--link [arg]', String, 'Request link information') do |l| + options['link_name'] = '' + if l + options['link_name'] = l + end + end + opts.on('-j', '--joint [arg]', String, 'Request joint information') do |j| + # options['joint'] = 1 + options['joint_name'] = '' + if j + options['joint_name'] = j + end + end + end # opt_parser do + 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 + + # Check that an option was selected. + if !(options.key?('list') || options.key?('model_name')) + puts usage + exit(-1) + end + + options['command'] = args[0] + + options + end # parse() + + def execute(args) + options = parse(args) + + # Read the plugin that handles the command. + if LIBRARY_NAME[0] == '/' + # If the first character is a slash, we'll assume that we've been given an + # absolute path to the library. This is only used during test mode. + plugin = LIBRARY_NAME + else + # 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)) + end + conf_version = LIBRARY_VERSION + + begin + Importer.dlload plugin + rescue DLError => e + puts "Library error for [#{plugin}]: #{e.to_s}" + exit(-1) + end + + if options.key?('list') + Importer.extern 'void cmdModelList()' + Importer.cmdModelList() + exit(0) + elsif options.key?('model_name') + Importer.extern 'void cmdModelInfo(const char *, int, const char *, + const char *)' + Importer.cmdModelInfo(options['model_name'], options['pose'], + options['link_name'], options['joint_name']) + else + puts 'Command error: I do not have an implementation for '\ + "command [ign #{options['command']}]." + end + # # execute + end +# class +end diff --git a/src/cmd/model.yaml.in b/src/cmd/model.yaml.in new file mode 100644 index 0000000000..b9d2ec9184 --- /dev/null +++ b/src/cmd/model.yaml.in @@ -0,0 +1,8 @@ +--- # Model subcommand available inside ignition gazebo. +format: 1.0.0 +library_name: ignition-gazebo-ign +library_version: @PROJECT_VERSION_FULL@ +library_path: @ign_model_ruby_path@ +commands: + - model : Print information about models. +--- diff --git a/test/worlds/static_diff_drive_vehicle.sdf b/test/worlds/static_diff_drive_vehicle.sdf new file mode 100644 index 0000000000..179a11ef20 --- /dev/null +++ b/test/worlds/static_diff_drive_vehicle.sdf @@ -0,0 +1,226 @@ + + + + + + + 0.001 + 1.0 + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + + true + + + + diff --git a/tutorials.md.in b/tutorials.md.in index f8c32bdf00..7fe0b53976 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -25,6 +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": 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 new file mode 100644 index 0000000000..1fbc4ae755 --- /dev/null +++ b/tutorials/model_command.md @@ -0,0 +1,196 @@ +\page model_command Model Command + +## Overview +`ign model` command allows you to get information about the models for a given running Ignition Gazebo simulation. + +For each model, it is possible to get information about its + - Pose: Pose of the model + - Links: Pose, mass, and inertial matrix of the link + - Joints: Parent link, child link and joint type. + +## Example running the diff_drive world + +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 + +Once Ignition Gazebo is up, we can use the ign model command to get information of the simulation. +Open a new terminal and enter: + + ign model --list + +And available models should be printed: + + + Available models: + - ground_plane + - vehicle_blue + - vehicle_green + +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 vehicle_blue + +``` + Requesting state for world [diff_drive]... + + Model: [8] + - Name: vehicle_blue + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 2.000000 | 0.325000] + [0.000000 | 0.000000 | 0.000000] + + - Link [9] + - Name: chassis + - Parent: vehicle_blue [8] + - Mass (kg): [1.143950] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - 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) ]: + [-0.151427 | 0.000000 | 0.175000] + [0.000000 | 0.000000 | 0.000000] + - Link [12] + - Name: left_wheel + - Parent: vehicle_blue [8] + - Mass (kg): [2.000000] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - 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) ]: + [0.554283 | 0.625029 | -0.025000] + [-1.570700 | 0.000000 | 0.000000] + - Link [15] + - Name: right_wheel + - Parent: vehicle_blue [8] + - Mass (kg): [2.000000] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - 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) ]: + [0.554282 | -0.625029 | -0.025000] + [-1.570700 | 0.000000 | 0.000000] + - Link [18] + - Name: caster + - Parent: vehicle_blue [8] + - Mass (kg): [1.000000] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - 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) ]: + [-0.957138 | 0.000000 | -0.125000] + [0.000000 | 0.000000 | 0.000000] + - Joint [21] + - Name: left_wheel_joint + - Parent: vehicle_blue [8] + - Type: revolute + - Parent Link: left_wheel + - Child Link: chassis + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 0.000000 | 0.000000] + [0.000000 | 0.000000 | 0.000000] + - Axis position [ XYZ ]: + [0 | 0 | 1] + - Joint [22] + - Name: right_wheel_joint + - Parent: vehicle_blue [8] + - Type: revolute + - Parent Link: right_wheel + - Child Link: chassis + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 0.000000 | 0.000000] + [0.000000 | 0.000000 | 0.000000] + - Axis position [ XYZ ]: + [0 | 0 | 1] + - Joint [23] + - Name: caster_wheel + - Parent: vehicle_blue [8] + - Type: ball + - Parent Link: caster + - Child Link: chassis + - 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 vehicle_blue --pose + + +``` + Requesting state for world [diff_drive]... + + Model: [8] + - Name: vehicle_blue + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 2.000000 | 0.325000] + [0.000000 | 0.000000 | 0.000000] +``` + + +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. + + ign model -m vehicle_blue --link caster + +``` + Requesting state for world [diff_drive]... + + - Link [18] + - Name: caster + - Parent: vehicle_blue [8] + - Mass (kg): [1.000000] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - 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) ]: + [-0.957138 | 0.000000 | -0.125000] + [0.000000 | -0.000000 | 0.000000] +``` + + +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. + + ign model -m vehicle_blue --joint caster_wheel + +``` + Requesting state for world [diff_drive]... + + - Joint [23] + - Name: caster_wheel + - Parent: vehicle_blue [8] + - Type: ball + - Parent Link: caster + - Child Link: chassis + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 0.000000 | 0.000000] + [0.000000 | -0.000000 | 0.000000] +```