diff --git a/.github/ci/packages-focal.apt b/.github/ci/packages-focal.apt deleted file mode 100644 index ddd00e29a0..0000000000 --- a/.github/ci/packages-focal.apt +++ /dev/null @@ -1,7 +0,0 @@ -libdart-collision-ode-dev -libdart-dev -libdart-external-ikfast-dev -libdart-external-odelcpsolver-dev -libdart-utils-urdf-dev -libogre-next-2.3-dev -python3-gz-math7 diff --git a/.github/ci/packages-jammy.apt b/.github/ci/packages-jammy.apt deleted file mode 100644 index ddd00e29a0..0000000000 --- a/.github/ci/packages-jammy.apt +++ /dev/null @@ -1,7 +0,0 @@ -libdart-collision-ode-dev -libdart-dev -libdart-external-ikfast-dev -libdart-external-odelcpsolver-dev -libdart-utils-urdf-dev -libogre-next-2.3-dev -python3-gz-math7 diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 950a5d4217..6b0ee204b1 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -1,10 +1,15 @@ freeglut3-dev +libdart-collision-ode-dev +libdart-dev +libdart-external-ikfast-dev +libdart-external-odelcpsolver-dev +libdart-utils-urdf-dev libfreeimage-dev libglew-dev libgz-cmake3-dev libgz-common5-dev -libgz-gui7-dev libgz-fuel-tools8-dev +libgz-gui7-dev libgz-math7-eigen3-dev libgz-msgs9-dev libgz-physics6-dev @@ -15,6 +20,7 @@ libgz-tools2-dev libgz-transport12-dev libgz-utils2-cli-dev libogre-1.9-dev +libogre-next-2.3-dev libprotobuf-dev libprotoc-dev libsdformat13-dev @@ -22,12 +28,13 @@ libtinyxml2-dev libxi-dev libxmu-dev python3-distutils +python3-gz-math7 python3-pybind11 python3-pytest qml-module-qt-labs-folderlistmodel qml-module-qt-labs-settings -qml-module-qtqml-models2 qml-module-qtgraphicaleffects +qml-module-qtqml-models2 qml-module-qtquick-controls qml-module-qtquick-controls2 qml-module-qtquick-dialogs diff --git a/.gitignore b/.gitignore index 51c7dd858d..40d7c0f5e3 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,6 @@ build_* .ycm_extra_conf.py *.orig + +# clangd index +.cache diff --git a/CMakeLists.txt b/CMakeLists.txt index 8eff281618..f186f9dc55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) # Initialize the project #============================================================================ project(gz-sim7 VERSION 7.0.0) +set (GZ_DISTRIBUTION "Garden") #============================================================================ # Find gz-cmake diff --git a/Migration.md b/Migration.md index 298ae2c31b..467c930b09 100644 --- a/Migration.md +++ b/Migration.md @@ -116,7 +116,7 @@ since pose information is being logged in the `changed_state` topic. + `ViewAndle`: Move camera to preset angles * The `gui.config` and `server.config` files are now located in a versioned - folder inside `$HOME/.gz/sim`, i.e. `$HOME/.gz/sim/6/gui.config`. + folder inside `$HOME/.ignition/gazebo`, i.e. `$HOME/.ignition/gazebo/6/gui.config`. * The `Component::Clone` method has been marked `const` to reflect that it should not mutate internal component state. Component implementations that diff --git a/README.md b/README.md index 2fea94f30f..36c1796029 100644 --- a/README.md +++ b/README.md @@ -9,12 +9,12 @@ Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-sim/branch/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-sim) +Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-sim/branch/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-sim/branch/main) Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_gazebo-ci-main-focal-amd64)](https://build.osrfoundation.org/job/ignition_gazebo-ci-main-focal-amd64) Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_gazebo-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_gazebo-ci-main-homebrew-amd64) Windows | [![Build Status](https://build.osrfoundation.org/job/ign_gazebo-ci-win/badge/icon)](https://build.osrfoundation.org/job/ign_gazebo-ci-win/) -Gazebo Sim is an open source robotics simulator. Through Gazebo, users have access to high fidelity physics, rendering, and sensor models. Additionally, users and developers have multiple points of entry to simulation including a graphical user interface, plugins, and asynchronous message passing and services. +Gazebo Sim is an open source robotics simulator. Through Gazebo Sim, users have access to high fidelity physics, rendering, and sensor models. Additionally, users and developers have multiple points of entry to simulation including a graphical user interface, plugins, and asynchronous message passing and services. Gazebo Sim is derived from [Gazebo Classic](http://classic.gazebosim.org) and represents over 16 years of development and experience in robotics and simulation. This library is part of the [Gazebo](https://gazebosim.org) project. @@ -59,7 +59,7 @@ force-torque, IMU, GPS, and more, all powered by * **Plugins**: Develop custom plugins for robot, sensor, and environment control. -* **Graphical interface**: Create, instrospect and interact with your simulations +* **Graphical interface**: Create, introspect and interact with your simulations through plugin-based graphical interfaces powered by [Gazebo GUI](https://github.com/gazebosim/gz-gui). diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt index 820f3e7038..bcb45f8826 100644 --- a/examples/plugin/custom_sensor_system/CMakeLists.txt +++ b/examples/plugin/custom_sensor_system/CMakeLists.txt @@ -20,7 +20,7 @@ include(FetchContent) FetchContent_Declare( sensors_clone GIT_REPOSITORY https://github.com/gazebosim/gz-sensors - GIT_TAG gz-sensors7 + GIT_TAG main ) FetchContent_Populate(sensors_clone) add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_clone_BINARY_DIR}) diff --git a/examples/worlds/CMakeLists.txt b/examples/worlds/CMakeLists.txt index 6306342c97..fee3a9e41c 100644 --- a/examples/worlds/CMakeLists.txt +++ b/examples/worlds/CMakeLists.txt @@ -1,3 +1,5 @@ file(GLOB files "*.sdf") install(FILES ${files} DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds) + +add_subdirectory(thumbnails) diff --git a/examples/worlds/apply_link_wrench.sdf b/examples/worlds/apply_link_wrench.sdf new file mode 100644 index 0000000000..03509d9766 --- /dev/null +++ b/examples/worlds/apply_link_wrench.sdf @@ -0,0 +1,163 @@ + + + + + + + + + + + box + model + -10 0 0 + 0 0 0.1 + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 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.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 2 0.5 0 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + diff --git a/examples/worlds/thumbnails/CMakeLists.txt b/examples/worlds/thumbnails/CMakeLists.txt new file mode 100644 index 0000000000..d96190f888 --- /dev/null +++ b/examples/worlds/thumbnails/CMakeLists.txt @@ -0,0 +1,3 @@ +file(GLOB files "*.png") +install(FILES ${files} + DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds/thumbnails) diff --git a/examples/worlds/thumbnails/OpenRobotics.NAO joint control.png b/examples/worlds/thumbnails/OpenRobotics.NAO joint control.png new file mode 100644 index 0000000000..4b2b8551bc Binary files /dev/null and b/examples/worlds/thumbnails/OpenRobotics.NAO joint control.png differ diff --git a/examples/worlds/thumbnails/OpenRobotics.Panda joint control world.png b/examples/worlds/thumbnails/OpenRobotics.Panda joint control world.png new file mode 100644 index 0000000000..7cccc8667e Binary files /dev/null and b/examples/worlds/thumbnails/OpenRobotics.Panda joint control world.png differ diff --git a/examples/worlds/thumbnails/OpenRobotics.Prius on Sonoma Raceway.png b/examples/worlds/thumbnails/OpenRobotics.Prius on Sonoma Raceway.png new file mode 100644 index 0000000000..3f889a9bbb Binary files /dev/null and b/examples/worlds/thumbnails/OpenRobotics.Prius on Sonoma Raceway.png differ diff --git a/examples/worlds/thumbnails/OpenRobotics.Quadcopter teleop.png b/examples/worlds/thumbnails/OpenRobotics.Quadcopter teleop.png new file mode 100644 index 0000000000..486dffb841 Binary files /dev/null and b/examples/worlds/thumbnails/OpenRobotics.Quadcopter teleop.png differ diff --git a/examples/worlds/thumbnails/OpenRobotics.Tugbot in Warehouse.png b/examples/worlds/thumbnails/OpenRobotics.Tugbot in Warehouse.png new file mode 100644 index 0000000000..36798d8169 Binary files /dev/null and b/examples/worlds/thumbnails/OpenRobotics.Tugbot in Warehouse.png differ diff --git a/examples/worlds/thumbnails/openrobotics.empty.png b/examples/worlds/thumbnails/openrobotics.empty.png new file mode 100644 index 0000000000..f05996d50d Binary files /dev/null and b/examples/worlds/thumbnails/openrobotics.empty.png differ diff --git a/include/gz/sim/Model.hh b/include/gz/sim/Model.hh index 583da66b3e..c3b668ac02 100644 --- a/include/gz/sim/Model.hh +++ b/include/gz/sim/Model.hh @@ -175,6 +175,12 @@ namespace gz public: void SetWorldPoseCmd(EntityComponentManager &_ecm, const math::Pose3d &_pose); + /// \brief Get the model's canonical link entity. + /// \param[in] _ecm Entity-component manager. + /// \return Link entity. + public: sim::Entity CanonicalLink( + const EntityComponentManager &_ecm) const; + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index 1cc6276843..734a7e1436 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -17,6 +17,8 @@ #ifndef GZ_SIM_UTIL_HH_ #define GZ_SIM_UTIL_HH_ +#include + #include #include #include @@ -257,6 +259,26 @@ namespace gz return changed; } + /// \brief Helper function to get an entity from an entity message. + /// The returned entity is not guaranteed to exist. + /// + /// The message is used as follows: + /// + /// if id not null + /// use id + /// else if name not null and type not null + /// use name + type + /// else + /// error + /// end + /// + /// \param[in] _ecm Entity component manager + /// \param[in] _msg Entity message + /// \return Entity ID, or kNullEntity if a matching entity couldn't be + /// found. + Entity GZ_SIM_VISIBLE entityFromMsg( + const EntityComponentManager &_ecm, const msgs::Entity &_msg); + /// \brief Get the spherical coordinates for an entity. /// \param[in] _entity Entity whose coordinates we want. /// \param[in] _ecm Entity component manager diff --git a/include/gz/sim/config.hh.in b/include/gz/sim/config.hh.in index 747a8816b4..894bd0d754 100644 --- a/include/gz/sim/config.hh.in +++ b/include/gz/sim/config.hh.in @@ -39,5 +39,6 @@ #define GZ_SIM_PLUGIN_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins" #define GZ_SIM_GUI_PLUGIN_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins/gui" #define GZ_SIM_WORLD_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}/worlds" +#define GZ_DISTRIBUTION "${GZ_DISTRIBUTION}" #endif diff --git a/include/gz/sim/gui/Gui.hh b/include/gz/sim/gui/Gui.hh index 5dfb1eaf44..adafaccf67 100644 --- a/include/gz/sim/gui/Gui.hh +++ b/include/gz/sim/gui/Gui.hh @@ -19,6 +19,7 @@ #define GZ_SIM_GUI_GUI_HH_ #include +#include #include #include "gz/sim/config.hh" @@ -41,10 +42,29 @@ namespace gui /// gz-tools. Set to the name of the application if using gz-tools) /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default /// configuration from GZ_HOMEDIR/.gz/sim/gui.config will be used. - /// \param[in] _renderEngineGui --render-engine-gui option + /// \param[in] _renderEngine --render-engine-gui option /// \return -1 on failure, 0 on success GZ_SIM_GUI_VISIBLE int runGui(int &_argc, - char **_argv, const char *_guiConfig, const char * _renderEngine = nullptr); + char **_argv, const char *_guiConfig, const char *_renderEngine = nullptr); + + /// \brief Run GUI application + /// \param[in] _argc Number of command line arguments (Used when running + /// without gz-tools. Set to 1 if using gz-tools). Note: The object + /// referenced by this variable must continue to exist for the lifetime of the + /// application. + /// \param[in] _argv Command line arguments (Used when running without + /// gz-tools. Set to the name of the application if using gz-tools) + /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default + /// configuration from GZ_HOMEDIR/.gz/sim/gui.config will be used. + /// \param[in] _sdfFile The world file path passed as a command line argument. + /// If set, QuickStart Dialog will not be shown. + /// \param[in] _waitGui Flag indicating whether the server waits until + /// it receives a world path from GUI. + /// \param[in] _renderEngine --render-engine-gui option + /// \return -1 on failure, 0 on success + GZ_SIM_GUI_VISIBLE int runGui(int &_argc, char **_argv, + const char *_guiConfig, const char *_sdfFile, int _waitGui, + const char *_renderEngine = nullptr); /// \brief Create a Gazebo GUI application /// \param[in] _argc Number of command line arguments (Used when running @@ -62,7 +82,7 @@ namespace gui /// GZ_HOMEDIR/.gz/sim/gui.config will be used. /// \param[in] _loadPluginsFromSdf If true, plugins specified in the world /// SDFormat file will get loaded. - /// \param[in] _renderEngineGui --render-engine-gui option + /// \param[in] _renderEngine --render-engine-gui option /// \return Newly created application. GZ_SIM_GUI_VISIBLE std::unique_ptr createGui( @@ -70,6 +90,32 @@ namespace gui const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true, const char *_renderEngine = nullptr); + /// \brief Create a Gazebo GUI application + /// \param[in] _argc Number of command line arguments (Used when running + /// without gz-tools. Set to 1 if using gz-tools). Note: The object + /// referenced by this variable must continue to exist for the lifetime of the + /// application. + /// \param[in] _argv Command line arguments (Used when running without + /// gz-tools. Set to the name of the application if using gz-tools) + /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default + /// configuration from GZ_HOMEDIR/.gz/sim/gui.config will be used. + /// \param[in] _defaultGuiConfig The default GUI configuration file. If no + /// plugins were added from a world file or from _guiConfig, this + /// configuration file will be loaded. If this argument is a nullptr or if the + /// file does not exist, the default configuration from + /// GZ_HOMEDIR/.gz/sim/gui.config will be used. + /// \param[in] _loadPluginsFromSdf If true, plugins specified in the world + /// SDFormat file will get loaded. + /// \param[in] _sdfFile SDF world file, or nullptr if not set. + /// \param[in] _waitGui True if the server is waiting for the GUI to decide on + /// a starting world. + /// \param[in] _renderEngine --render-engine-gui option + /// \return Newly created application. + GZ_SIM_GUI_VISIBLE + std::unique_ptr createGui( + int &_argc, char **_argv, const char *_guiConfig, + const char *_defaultGuiConfig, bool _loadPluginsFromSdf, + const char *_sdfFile, int _waitGui, const char *_renderEngine = nullptr); } // namespace gui } // namespace GZ_SIM_VERSION_NAMESPACE } // namespace sim diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 65f4a57119..83fca53e39 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -539,7 +539,17 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, auto originalComp = this->ComponentImplementation(_entity, type); auto clonedComp = originalComp->Clone(); - this->CreateComponentImplementation(clonedEntity, type, clonedComp.get()); + auto updateData = + this->CreateComponentImplementation(clonedEntity, type, clonedComp.get()); + if (updateData) + { + // When a cloned entity is removed, it erases all components/data so a new + // cloned entity should not have components to be updated + // LCOV_EXCL_START + gzerr << "Internal error: The component's data needs to be updated but " + << "this should not happen." << std::endl; + // LCOV_EXCL_STOP + } } // keep track of canonical link information (for clones of models, the cloned @@ -1772,11 +1782,11 @@ void EntityComponentManager::SetState( // Get Component auto comp = this->ComponentImplementation(entity, type); - std::istringstream istr(compMsg.component()); - // Create if new if (nullptr == comp) { + std::istringstream istr(compMsg.component()); + auto newComp = components::Factory::Instance()->New(type); if (nullptr == newComp) { @@ -1785,11 +1795,20 @@ void EntityComponentManager::SetState( continue; } newComp->Deserialize(istr); - this->CreateComponentImplementation(entity, type, newComp.get()); + + auto updateData = + this->CreateComponentImplementation(entity, type, newComp.get()); + if (updateData) + { + // Set comp so we deserialize the data below again + comp = this->ComponentImplementation(entity, type); + } } + // Update component value - else + if (comp) { + std::istringstream istr(compMsg.component()); comp->Deserialize(istr); this->dataPtr->AddModifiedComponent(entity); } @@ -1855,29 +1874,34 @@ void EntityComponentManager::SetState( components::BaseComponent *comp = this->ComponentImplementation(entity, compIter.first); - std::istringstream istr(compMsg.component()); - // Create if new if (nullptr == comp) { + std::istringstream istr(compMsg.component()); + // Create component auto newComp = components::Factory::Instance()->New(compMsg.type()); - if (nullptr == newComp) { gzerr << "Failed to create component of type [" << compMsg.type() << "]" << std::endl; continue; } - newComp->Deserialize(istr); - this->CreateComponentImplementation(entity, - newComp->TypeId(), newComp.get()); + auto updateData = this->CreateComponentImplementation( + entity, newComp->TypeId(), newComp.get()); + if (updateData) + { + // Set comp so we deserialize the data below again + comp = this->ComponentImplementation(entity, compIter.first); + } } + // Update component value - else + if (comp) { + std::istringstream istr(compMsg.component()); comp->Deserialize(istr); this->SetChanged(entity, compIter.first, _stateMsg.has_one_time_component_changes() ? diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 3a7e0c1175..5ed8a0f88f 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -3339,6 +3339,94 @@ TEST_P(EntityComponentManagerFixture, ResetToWithAddedEntity) } } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(AddRemoveAddComponentsStateMap)) +{ + Entity e1 = manager.CreateEntity(); + EXPECT_EQ(1u, manager.EntityCount()); + EXPECT_EQ(0, eachCount(manager)); + + // add a component + auto comp = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(1, eachCount(manager)); + EXPECT_EQ(123, comp->Data()); + + // Serialize into a message + msgs::SerializedStateMap stateMsg; + manager.State(stateMsg); + ASSERT_EQ(1, stateMsg.entities_size()); + + // remove a component + EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(e1)); + manager.RunClearNewlyCreatedEntities(); + auto changedStateMsg = manager.ChangedState(); + EXPECT_EQ(0, changedStateMsg.entities_size()); + + // add same type of component back in using SetState + auto iter = stateMsg.mutable_entities()->find(e1); + ASSERT_TRUE(iter != stateMsg.mutable_entities()->end()); + msgs::SerializedEntityMap &e1Msg = iter->second; + + auto compIter = e1Msg.mutable_components()->find(comp->TypeId()); + ASSERT_TRUE(compIter != e1Msg.mutable_components()->end()); + msgs::SerializedComponent &e1c1Msg = compIter->second; + e1c1Msg.set_component(std::to_string(321)); + (*e1Msg.mutable_components())[e1c1Msg.type()] = e1c1Msg; + (*stateMsg.mutable_entities())[static_cast(e1)] = e1Msg; + manager.SetState(stateMsg); + changedStateMsg = manager.ChangedState(); + EXPECT_EQ(1, changedStateMsg.entities_size()); + + // check component + comp = manager.Component(e1); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(321, comp->Data()); +} + +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(AddRemoveAddComponentsState)) +{ + Entity e1 = manager.CreateEntity(); + EXPECT_EQ(1u, manager.EntityCount()); + EXPECT_EQ(0, eachCount(manager)); + + // add a component + auto comp = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(1, eachCount(manager)); + EXPECT_EQ(123, comp->Data()); + + // Serialize into a message + msgs::SerializedState stateMsg = manager.State(); + ASSERT_EQ(1, stateMsg.entities_size()); + + // remove a component + EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(e1)); + manager.RunClearNewlyCreatedEntities(); + auto changedStateMsg = manager.ChangedState(); + EXPECT_EQ(0, changedStateMsg.entities_size()); + + // add same type of component back in using SetState + auto entityMsg = stateMsg.mutable_entities(0); + EXPECT_EQ(1, entityMsg->components().size()); + auto compMsg = entityMsg->mutable_components(0); + compMsg->set_component(std::to_string(321)); + + manager.SetState(stateMsg); + changedStateMsg = manager.ChangedState(); + EXPECT_EQ(1, changedStateMsg.entities_size()); + + // check component + comp = manager.Component(e1); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(321, comp->Data()); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat, diff --git a/src/Model.cc b/src/Model.cc index 8d5e9024e4..c27ea760ec 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -15,6 +15,7 @@ * */ +#include "gz/sim/components/CanonicalLink.hh" #include "gz/sim/components/Joint.hh" #include "gz/sim/components/Link.hh" #include "gz/sim/components/Model.hh" @@ -202,3 +203,10 @@ void Model::SetWorldPoseCmd(EntityComponentManager &_ecm, } } +////////////////////////////////////////////////// +Entity Model::CanonicalLink(const EntityComponentManager &_ecm) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::CanonicalLink()); +} diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index ed6805f8b3..fca2648512 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -205,9 +205,9 @@ std::optional SystemLoader::LoadPlugin( { if (_plugin.Filename() == "" || _plugin.Name() == "") { - ignerr << "Failed to instantiate system plugin: empty argument " - "[(filename): " << _plugin.Filename() << "] " << - "[(name): " << _plugin.Name() << "]." << std::endl; + gzerr << "Failed to instantiate system plugin: empty argument " + "[(filename): " << _plugin.Filename() << "] " << + "[(name): " << _plugin.Name() << "]." << std::endl; return {}; } diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 5cc9de193f..4b194e18a6 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -37,7 +37,7 @@ SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, std::string entitySystemService{"entity/system/add"}; this->node->Advertise(entitySystemService, &SystemManager::EntitySystemAddService, this); - ignmsg << "Serving entity system service on [" + gzmsg << "Serving entity system service on [" << "/" << entitySystemService << "]" << std::endl; } @@ -313,8 +313,8 @@ void SystemManager::ProcessPendingEntitySystems() if (req.plugins().empty()) { - ignwarn << "Unable to add plugins to Entity: '" << entity - << "'. No plugins specified." << std::endl; + gzwarn << "Unable to add plugins to Entity: '" << entity + << "'. No plugins specified." << std::endl; continue; } diff --git a/src/Util.cc b/src/Util.cc index 72f7410554..56f0fca50f 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -421,13 +421,14 @@ void addResourcePaths(const std::vector &_paths) sdfPaths = common::Split(sdfPathCStr, common::SystemPaths::Delimiter()); } - // Gazebo file paths (for s) + // Gazebo Common file paths (for s) auto systemPaths = common::systemPaths(); - std::vector ignPaths; - char *ignPathCStr = std::getenv(systemPaths->FilePathEnv().c_str()); - if (ignPathCStr && *ignPathCStr != '\0') + std::vector commonPaths; + char *commonPathCStr = std::getenv(systemPaths->FilePathEnv().c_str()); + if (commonPathCStr && *commonPathCStr != '\0') { - ignPaths = common::Split(ignPathCStr, common::SystemPaths::Delimiter()); + commonPaths = common::Split(commonPathCStr, + common::SystemPaths::Delimiter()); } // Gazebo resource paths @@ -468,9 +469,10 @@ void addResourcePaths(const std::vector &_paths) sdfPaths.push_back(path); } - if (std::find(ignPaths.begin(), ignPaths.end(), path) == ignPaths.end()) + if (std::find(commonPaths.begin(), + commonPaths.end(), path) == commonPaths.end()) { - ignPaths.push_back(path); + commonPaths.push_back(path); } } @@ -479,23 +481,23 @@ void addResourcePaths(const std::vector &_paths) for (const auto &path : sdfPaths) sdfPathsStr += common::SystemPaths::Delimiter() + path; - gz::common::setenv(kSdfPathEnv.c_str(), sdfPathsStr.c_str()); + common::setenv(kSdfPathEnv.c_str(), sdfPathsStr.c_str()); - std::string ignPathsStr; - for (const auto &path : ignPaths) - ignPathsStr += common::SystemPaths::Delimiter() + path; + std::string commonPathsStr; + for (const auto &path : commonPaths) + commonPathsStr += common::SystemPaths::Delimiter() + path; - gz::common::setenv( - systemPaths->FilePathEnv().c_str(), ignPathsStr.c_str()); + common::setenv( + systemPaths->FilePathEnv().c_str(), commonPathsStr.c_str()); std::string gzPathsStr; for (const auto &path : gzPaths) gzPathsStr += common::SystemPaths::Delimiter() + path; - gz::common::setenv(kResourcePathEnv.c_str(), gzPathsStr.c_str()); + common::setenv(kResourcePathEnv.c_str(), gzPathsStr.c_str()); // TODO(CH3): Deprecated. Remove on tock. - gz::common::setenv(kResourcePathEnvDeprecated.c_str(), gzPathsStr.c_str()); + common::setenv(kResourcePathEnvDeprecated.c_str(), gzPathsStr.c_str()); // Force re-evaluation // SDF is evaluated at find call @@ -503,7 +505,7 @@ void addResourcePaths(const std::vector &_paths) } ////////////////////////////////////////////////// -gz::sim::Entity topLevelModel(const Entity &_entity, +sim::Entity topLevelModel(const Entity &_entity, const EntityComponentManager &_ecm) { auto entity = _entity; @@ -565,6 +567,86 @@ std::string validTopic(const std::vector &_topics) return std::string(); } +////////////////////////////////////////////////// +Entity entityFromMsg(const EntityComponentManager &_ecm, + const msgs::Entity &_msg) +{ + if (_msg.id() != kNullEntity) + { + return _msg.id(); + } + + // If there's no ID, check name + type + if (_msg.type() == msgs::Entity::NONE) + { + return kNullEntity; + } + + auto entities = entitiesFromScopedName(_msg.name(), _ecm); + if (entities.empty()) + { + return kNullEntity; + } + + for (const auto &entity : entities) + { + if (_msg.type() == msgs::Entity::LIGHT && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::MODEL && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::LINK && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::VISUAL && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::COLLISION && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::SENSOR && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::JOINT && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::ACTOR && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::WORLD && + _ecm.Component(entity)) + { + return entity; + } + } + return kNullEntity; +} + ////////////////////////////////////////////////// std::optional sphericalCoordinates(Entity _entity, const EntityComponentManager &_ecm) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 41f151d8b1..d7faaffe19 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -283,8 +283,7 @@ TEST_F(UtilTest, EntitiesFromScopedName) Entity _relativeTo, const std::unordered_set &_result, const std::string &_delim) { - auto res = sim::entitiesFromScopedName(_scopedName, ecm, _relativeTo, - _delim); + auto res = entitiesFromScopedName(_scopedName, ecm, _relativeTo, _delim); EXPECT_EQ(_result.size(), res.size()) << _scopedName; for (auto it : _result) @@ -719,6 +718,244 @@ TEST_F(UtilTest, EnableComponent) EXPECT_EQ(nullptr, ecm.Component(entity1)); } +///////////////////////////////////////////////// +TEST_F(UtilTest, EntityFromMsg) +{ + EntityComponentManager ecm; + + // world + // - lightA (light) + // - modelB + // - linkB (link) + // - lightB (light) + // - sensorB (banana) + // - modelC + // - linkC (link) + // - collisionC + // - visualC (banana) + // - jointC + // - modelCC + // - linkCC (link) + // - actorD + + // World + auto worldEntity = ecm.CreateEntity(); + EXPECT_EQ(kNullEntity, sim::worldEntity(ecm)); + EXPECT_EQ(kNullEntity, sim::worldEntity(worldEntity, ecm)); + ecm.CreateComponent(worldEntity, components::World()); + ecm.CreateComponent(worldEntity, components::Name("world")); + + // Light A + auto lightAEntity = ecm.CreateEntity(); + ecm.CreateComponent(lightAEntity, components::Light(sdf::Light())); + ecm.CreateComponent(lightAEntity, components::Name("light")); + ecm.CreateComponent(lightAEntity, components::ParentEntity(worldEntity)); + + // Model B + auto modelBEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelBEntity, components::Model()); + ecm.CreateComponent(modelBEntity, components::Name("modelB")); + ecm.CreateComponent(modelBEntity, components::ParentEntity(worldEntity)); + + // Link B + auto linkBEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkBEntity, components::Link()); + ecm.CreateComponent(linkBEntity, components::Name("link")); + ecm.CreateComponent(linkBEntity, components::ParentEntity(modelBEntity)); + + // Light B + auto lightBEntity = ecm.CreateEntity(); + ecm.CreateComponent(lightBEntity, components::Light(sdf::Light())); + ecm.CreateComponent(lightBEntity, components::Name("light")); + ecm.CreateComponent(lightBEntity, components::ParentEntity(linkBEntity)); + + // Sensor B + auto sensorBEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorBEntity, components::Sensor()); + ecm.CreateComponent(sensorBEntity, components::Name("banana")); + ecm.CreateComponent(sensorBEntity, components::ParentEntity(linkBEntity)); + + // Model C + auto modelCEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelCEntity, components::Model()); + ecm.CreateComponent(modelCEntity, components::Name("modelC")); + ecm.CreateComponent(modelCEntity, components::ParentEntity(worldEntity)); + + // Link C + auto linkCEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkCEntity, components::Link()); + ecm.CreateComponent(linkCEntity, components::Name("link")); + ecm.CreateComponent(linkCEntity, components::ParentEntity(modelCEntity)); + + // Collision C + auto collisionCEntity = ecm.CreateEntity(); + ecm.CreateComponent(collisionCEntity, components::Collision()); + ecm.CreateComponent(collisionCEntity, components::Name("collisionC")); + ecm.CreateComponent(collisionCEntity, components::ParentEntity(linkCEntity)); + + // Visual C + auto visualCEntity = ecm.CreateEntity(); + ecm.CreateComponent(visualCEntity, components::Visual()); + ecm.CreateComponent(visualCEntity, components::Name("banana")); + ecm.CreateComponent(visualCEntity, components::ParentEntity(linkCEntity)); + + // Link C + auto jointCEntity = ecm.CreateEntity(); + ecm.CreateComponent(jointCEntity, components::Joint()); + ecm.CreateComponent(jointCEntity, components::Name("jointC")); + ecm.CreateComponent(jointCEntity, components::ParentEntity(modelCEntity)); + + // Model CC + auto modelCCEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelCCEntity, components::Model()); + ecm.CreateComponent(modelCCEntity, components::Name("modelCC")); + ecm.CreateComponent(modelCCEntity, components::ParentEntity(modelCEntity)); + + // Link CC + auto linkCCEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkCCEntity, components::Link()); + ecm.CreateComponent(linkCCEntity, components::Name("link")); + ecm.CreateComponent(linkCCEntity, components::ParentEntity(modelCCEntity)); + + // Actor D + auto actorDEntity = ecm.CreateEntity(); + ecm.CreateComponent(actorDEntity, components::Actor(sdf::Actor())); + ecm.CreateComponent(actorDEntity, components::Name("actorD")); + ecm.CreateComponent(actorDEntity, components::ParentEntity(worldEntity)); + + // Check entities + auto createMsg = [&ecm](Entity _id, const std::string &_name = "", + msgs::Entity::Type _type = msgs::Entity::NONE) -> msgs::Entity + { + msgs::Entity msg; + + if (_id != kNullEntity) + msg.set_id(_id); + + if (!_name.empty()) + msg.set_name(_name); + + if (_type != msgs::Entity::NONE) + msg.set_type(_type); + + return msg; + }; + + // Only ID + EXPECT_EQ(worldEntity, entityFromMsg(ecm, createMsg(worldEntity))); + EXPECT_EQ(lightAEntity, entityFromMsg(ecm, createMsg(lightAEntity))); + EXPECT_EQ(modelBEntity, entityFromMsg(ecm, createMsg(modelBEntity))); + EXPECT_EQ(linkBEntity, entityFromMsg(ecm, createMsg(linkBEntity))); + EXPECT_EQ(lightBEntity, entityFromMsg(ecm, createMsg(lightBEntity))); + EXPECT_EQ(sensorBEntity, entityFromMsg(ecm, createMsg(sensorBEntity))); + EXPECT_EQ(modelCEntity, entityFromMsg(ecm, createMsg(modelCEntity))); + EXPECT_EQ(linkCEntity, entityFromMsg(ecm, createMsg(linkCEntity))); + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(collisionCEntity))); + EXPECT_EQ(visualCEntity, entityFromMsg(ecm, createMsg(visualCEntity))); + EXPECT_EQ(jointCEntity, entityFromMsg(ecm, createMsg(jointCEntity))); + EXPECT_EQ(modelCCEntity, entityFromMsg(ecm, createMsg(modelCCEntity))); + EXPECT_EQ(linkCCEntity, entityFromMsg(ecm, createMsg(linkCCEntity))); + EXPECT_EQ(actorDEntity, entityFromMsg(ecm, createMsg(actorDEntity))); + + // Name and type, with different scopes + EXPECT_EQ(worldEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world", msgs::Entity::WORLD))); + + // There's more than one "light", so we need to scope it + EXPECT_EQ(lightAEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::light", msgs::Entity::LIGHT))); + + EXPECT_EQ(modelBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelB", msgs::Entity::MODEL))); + EXPECT_EQ(modelBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelB", msgs::Entity::MODEL))); + + // There's more than one "link", so we need to scope it + EXPECT_EQ(linkBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelB::link", msgs::Entity::LINK))); + EXPECT_EQ(linkBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelB::link", msgs::Entity::LINK))); + + // There's more than one "light", so we need to scope it + EXPECT_EQ(lightBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "link::light", msgs::Entity::LIGHT))); + EXPECT_EQ(lightBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelB::link::light", msgs::Entity::LIGHT))); + EXPECT_EQ(lightBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelB::link::light", msgs::Entity::LIGHT))); + + // There's more than one "link::banana", so we need to scope it + EXPECT_EQ(sensorBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelB::link::banana", msgs::Entity::SENSOR))); + EXPECT_EQ(sensorBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelB::link::banana", msgs::Entity::SENSOR))); + + EXPECT_EQ(modelCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC", msgs::Entity::MODEL))); + EXPECT_EQ(modelCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC", msgs::Entity::MODEL))); + + // There's more than one "link", so we need to scope it + EXPECT_EQ(linkCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::link", msgs::Entity::LINK))); + EXPECT_EQ(linkCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::link", msgs::Entity::LINK))); + + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "collisionC", msgs::Entity::COLLISION))); + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "link::collisionC", msgs::Entity::COLLISION))); + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::link::collisionC", msgs::Entity::COLLISION))); + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::link::collisionC", msgs::Entity::COLLISION))); + + // There's more than one "banana", so we need to scope it + EXPECT_EQ(visualCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "link::banana", msgs::Entity::VISUAL))); + EXPECT_EQ(visualCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::link::banana", msgs::Entity::VISUAL))); + EXPECT_EQ(visualCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::link::banana", msgs::Entity::VISUAL))); + + EXPECT_EQ(jointCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "jointC", msgs::Entity::JOINT))); + EXPECT_EQ(jointCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::jointC", msgs::Entity::JOINT))); + EXPECT_EQ(jointCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::jointC", msgs::Entity::JOINT))); + + EXPECT_EQ(modelCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelCC", msgs::Entity::MODEL))); + EXPECT_EQ(modelCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::modelCC", msgs::Entity::MODEL))); + EXPECT_EQ(modelCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::modelCC", msgs::Entity::MODEL))); + + // There's more than one "link", so we need to scope it + EXPECT_EQ(linkCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::modelCC::link", msgs::Entity::LINK))); + EXPECT_EQ(linkCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::modelCC::link", msgs::Entity::LINK))); + + EXPECT_EQ(actorDEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "actorD", msgs::Entity::ACTOR))); + EXPECT_EQ(actorDEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::actorD", msgs::Entity::ACTOR))); + + // Inexistent entity + EXPECT_EQ(1000u, entityFromMsg(ecm, createMsg(1000))); + + // Not found + EXPECT_EQ(kNullEntity, entityFromMsg(ecm, createMsg(kNullEntity))); + EXPECT_EQ(kNullEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "blueberry"))); + EXPECT_EQ(kNullEntity, entityFromMsg(ecm, createMsg(kNullEntity, "banana", + msgs::Entity::COLLISION))); + EXPECT_EQ(kNullEntity, entityFromMsg(ecm, createMsg(kNullEntity, "peach", + msgs::Entity::WORLD))); +} + ///////////////////////////////////////////////// TEST_F(UtilTest, ResolveSdfWorldFile) { diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index 2e073c0a52..c7dae12ac6 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -129,7 +129,7 @@ install( FILES ${CMAKE_CURRENT_BINARY_DIR}/sim${PROJECT_VERSION_MAJOR}.bash_completion.sh DESTINATION - ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${IGN_TOOLS_VER}.completion.d) + ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${GZ_TOOLS_VER}.completion.d) configure_file( "model.bash_completion.sh" @@ -138,4 +138,4 @@ install( FILES ${CMAKE_CURRENT_BINARY_DIR}/model${PROJECT_VERSION_MAJOR}.bash_completion.sh DESTINATION - ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${IGN_TOOLS_VER}.completion.d) + ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${GZ_TOOLS_VER}.completion.d) diff --git a/src/cmd/cmdsim.rb.in b/src/cmd/cmdsim.rb.in index 37ccc70cd0..d137647a17 100755 --- a/src/cmd/cmdsim.rb.in +++ b/src/cmd/cmdsim.rb.in @@ -223,6 +223,7 @@ class Cmd 'physics_engine' => '', 'render_engine_gui' => '', 'render_engine_server' => '', + 'wait_gui' => 1, 'headless-rendering' => 0 } @@ -259,9 +260,13 @@ class Cmd end opts.on('-g') do options['gui'] = 1 + # Runing the Gui only, don't show world loading menu + options['wait_gui'] = 0 end opts.on('-s') do options['server'] = 1 + # Runing the server only, don't wait for starting world from Gui + options['wait_gui'] = 0 end opts.on('--levels') do options['levels'] = 1 @@ -450,10 +455,10 @@ Please use [GZ_SIM_RESOURCE_PATH] instead." const char *, int, int, const char *, int, int, int, const char *, const char *, const char *, const char *, const char *, - const char *, int)' + const char *, int, int)' # Import the runGui function - Importer.extern 'int runGui(const char *, const char *)' + Importer.extern 'int runGui(const char *, const char *, int, const char *)' # If playback is specified, and the user has not specified a # custom gui config, set the gui config to load the playback @@ -484,6 +489,7 @@ See https://github.com/gazebosim/gz-sim/issues/44 for more info." options['playback'], options['physics_engine'], options['render_engine_server'], options['render_engine_gui'], options['file'], options['record-topics'].join(':'), + options['wait_gui'], options['headless-rendering']) end @@ -491,7 +497,8 @@ See https://github.com/gazebosim/gz-sim/issues/44 for more info." ENV['RMT_PORT'] = '1501' Process.setpgid(0, 0) Process.setproctitle('gz sim gui') - Importer.runGui(options['gui_config'], options['render_engine_gui']) + Importer.runGui(options['gui_config'], options['file'], + options['wait_gui'], options['render_engine_gui']) end Signal.trap("INT") { @@ -520,8 +527,8 @@ See https://github.com/gazebosim/gz-sim/issues/44 for more info." options['playback'], options['physics_engine'], options['render_engine_server'], options['render_engine_gui'], options['file'], options['record-topics'].join(':'), - options['headless-rendering']) - # Otherwise run the gui + options['wait_gui'], options['headless-rendering']) + # Otherwise run the gui else options['gui'] if plugin.end_with? ".dylib" puts "`gz sim` currently only works with the -s argument on macOS. @@ -530,7 +537,8 @@ See https://github.com/gazebosim/gz-sim/issues/44 for more info." end ENV['RMT_PORT'] = '1501' - Importer.runGui(options['gui_config'], options['render_engine_gui']) + Importer.runGui(options['gui_config'], options['file'], + options['wait_gui'], options['render_engine_gui']) end rescue puts "Library error: Problem running [#{options['command']}]() "\ diff --git a/src/gui/CMakeLists.txt b/src/gui/CMakeLists.txt index c01a1d7de1..3437aff68f 100644 --- a/src/gui/CMakeLists.txt +++ b/src/gui/CMakeLists.txt @@ -5,6 +5,7 @@ set (gui_sources GuiFileHandler.cc GuiRunner.cc PathManager.cc + QuickStartHandler.cc ) set (gtest_sources diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 041c121fbb..0ffead2bbe 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -14,15 +14,18 @@ * limitations under the License. * */ +#include #include #include #include #include +#include #include #include +#include "gz/sim/Util.hh" #include "gz/sim/config.hh" #include "gz/sim/gui/Gui.hh" @@ -30,6 +33,7 @@ #include "GuiFileHandler.hh" #include "GuiRunner.hh" #include "PathManager.hh" +#include "QuickStartHandler.hh" namespace gz { @@ -39,11 +43,160 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { namespace gui { +/// \brief Get the path to the default config file. If the file doesn't exist +/// yet, this function will copy the installed file into its location. +/// \param[in] _isPlayback True if playing back a log file +/// \param[in] _customDefaultConfig A default config passed by the CLI or +/// another caller. +/// \return Path to the default config file. +std::string defaultGuiConfigFile(bool _isPlayback, + const char *_customDefaultConfig) +{ + std::string defaultConfig; + std::string defaultGuiConfigName = "gui.config"; + if (nullptr == _customDefaultConfig) + { + // The playback flag (and not the gui-config flag) was + // specified from the command line + if (_isPlayback) + { + defaultGuiConfigName = "playback_gui.config"; + } + common::env(GZ_HOMEDIR, defaultConfig); + defaultConfig = common::joinPaths(defaultConfig, ".gz", + "sim", GZ_SIM_MAJOR_VERSION_STR, defaultGuiConfigName); + } + else + { + // Downstream applications can override the default path + defaultConfig = _customDefaultConfig; + } + + // Check if the default config file exists. If it doesn't, copy the installed + // file there first. + if (!common::exists(defaultConfig)) + { + auto defaultConfigFolder = common::parentPath(defaultConfig); + if (!gz::common::exists(defaultConfigFolder)) + { + if (!gz::common::createDirectories(defaultConfigFolder)) + { + gzerr << "Failed to create the default config folder [" + << defaultConfigFolder << "]\n"; + return nullptr; + } + } + + auto installedConfig = common::joinPaths( + GZ_SIM_GUI_CONFIG_PATH, defaultGuiConfigName); + if (!common::copyFile(installedConfig, defaultConfig)) + { + gzerr << "Failed to copy installed config [" << installedConfig + << "] to default config [" << defaultConfig << "]." + << std::endl; + return nullptr; + } + else + { + gzmsg << "Copied installed config [" << installedConfig + << "] to default config [" << defaultConfig << "]." + << std::endl; + } + } + + return defaultConfig; +} + +////////////////////////////////////////////////// +/// \brief Launch the quick start dialog +/// \param[in] _argc Number of command line arguments. +/// \param[in] _argv Command line arguments. +/// \param[in] _defaultConfig Path to the default configuration file. +/// \param[in] _configInUse The config that the user chose to load. If the user +/// didn't pass one, this will be equal to _defaultConfig +/// \return The path to the starting world or an empty string if none was +/// chosen. +std::string launchQuickStart(int &_argc, char **_argv, + const std::string &_defaultConfig, + const std::string &_configInUse) +{ + gzmsg << "Gazebo Sim Quick start dialog" << std::endl; + + // Gui application in dialog mode + auto app = std::make_unique( + _argc, _argv, gz::gui::WindowType::kDialog); + app->SetDefaultConfigPath(_defaultConfig); + + auto quickStartHandler = new gui::QuickStartHandler(); + quickStartHandler->setParent(app->Engine()); + + auto dialog = new gz::gui::Dialog(); + dialog->setObjectName("quick_start"); + + gzdbg << "Reading Quick start menu config." << std::endl; + auto showDialog = dialog->ReadConfigAttribute(_configInUse, "show_again"); + if (showDialog == "false") + { + gzmsg << "Not showing Quick start menu." << std::endl; + return ""; + } + + // This is the fixed window size for the quick start dialog + QSize winSize(960, 540); + dialog->QuickWindow()->resize(winSize); + dialog->QuickWindow()->setMaximumSize(dialog->QuickWindow()->size()); + dialog->QuickWindow()->setTitle("Gazebo quick start"); + + // Position the quick start in the center of the screen + QSize screenSize = dialog->QuickWindow()->screen()->size(); + screenSize /= 2.0; + screenSize -= winSize / 2.0; + dialog->QuickWindow()->setPosition(screenSize.width(), screenSize.height()); + + auto context = new QQmlContext(app->Engine()->rootContext()); + context->setContextProperty("QuickStartHandler", quickStartHandler); + + std::string qmlFile("qrc:/Gazebo/QuickStart.qml"); + + QQmlComponent dialogComponent(gz::gui::App()->Engine(), + QString(QString::fromStdString(qmlFile))); + + auto dialogItem = qobject_cast(dialogComponent.create(context)); + if (nullptr == dialogItem) + { + gzerr << "Failed to create quick start dialog." << std::endl; + return ""; + } + dialogItem->setParentItem(dialog->RootItem()); + + // Run qt application and show quick dialog + if (nullptr != app) + { + app->exec(); + gzdbg << "Shutting quick setup dialog" << std::endl; + } + + // Update dialog config + dialog->UpdateConfigAttribute(_configInUse, "show_again", + quickStartHandler->ShowAgain()); + return quickStartHandler->StartingWorld(); +} + ////////////////////////////////////////////////// std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, const char *_defaultGuiConfig, bool _loadPluginsFromSdf, const char *_renderEngine) +{ + return createGui(_argc, _argv, _guiConfig, _defaultGuiConfig, + _loadPluginsFromSdf, nullptr, 0, _renderEngine); +} + +////////////////////////////////////////////////// +std::unique_ptr createGui( + int &_argc, char **_argv, const char *_guiConfig, + const char *_defaultGuiConfig, bool _loadPluginsFromSdf, + const char *_sdfFile, int _waitGui, const char *_renderEngine) { gz::common::SignalHandler sigHandler; bool sigKilled = false; @@ -61,8 +214,63 @@ std::unique_ptr createGui( qputenv("QT_AUTO_SCREEN_SCALE_FACTOR", "1"); } - // Initialize Qt app - auto app = std::make_unique(_argc, _argv); + bool isPlayback = (nullptr != _guiConfig && + std::string(_guiConfig) == "_playback_"); + auto defaultConfig = defaultGuiConfigFile(isPlayback, _defaultGuiConfig); + + bool hasSdfFile = (nullptr != _sdfFile && strlen(_sdfFile) != 0); + bool configFromCli = (nullptr != _guiConfig && std::strlen(_guiConfig) > 0 && + std::string(_guiConfig) != "_playback_"); + + transport::Node node; + + // Quick start dialog if no specific SDF file was passed and it's not playback + std::string startingWorld; + if (!hasSdfFile && _waitGui && !isPlayback) + { + std::string configInUse = configFromCli ? _guiConfig : defaultConfig; + startingWorld = launchQuickStart(_argc, _argv, defaultConfig, configInUse); + } + else if (hasSdfFile) + { + startingWorld = _sdfFile; + } + + if (sigKilled) + { + gzdbg << "Received kill signal. Not starting main window." << std::endl; + return nullptr; + } + + // Publish starting world even if it's empty. The server is blocking waiting + // for it. + if (_waitGui) + { + std::string topic{"/gazebo/starting_world"}; + auto startingWorldPub = node.Advertise(topic); + msgs::StringMsg msg; + msg.set_data(startingWorld); + + // Wait for the server to be listening, so we're sure it receives the + // message. + gzdbg << "Waiting for subscribers to [" << topic << "]..." << std::endl; + for (int sleep = 0; sleep < 100 && !startingWorldPub.HasConnections(); + ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + if (!startingWorldPub.HasConnections()) + { + gzwarn << "Waited for 10s for a subscriber to [" << topic + << "] and got none." << std::endl; + } + startingWorldPub.Publish(msg); + } + + // Launch main window + auto app = std::make_unique( + _argc, _argv, gz::gui::WindowType::kMainWindow); + app->AddPluginPath(GZ_SIM_GUI_PLUGIN_INSTALL_DIR); auto aboutDialogHandler = new gz::sim::gui::AboutDialogHandler(); @@ -76,33 +284,6 @@ std::unique_ptr createGui( // add import path so we can load custom modules app->Engine()->addImportPath(GZ_SIM_GUI_PLUGIN_INSTALL_DIR); - std::string defaultGuiConfigName = "gui.config"; - - // Set default config file for Gazebo - std::string defaultConfig; - - // Default config folder. - std::string defaultConfigFolder; - - if (nullptr == _defaultGuiConfig) - { - // The playback flag (and not the gui-config flag) was - // specified from the command line - if (nullptr != _guiConfig && std::string(_guiConfig) == "_playback_") - { - defaultGuiConfigName = "playback_gui.config"; - } - gz::common::env(GZ_HOMEDIR, defaultConfig); - defaultConfigFolder = - gz::common::joinPaths(defaultConfig, ".gz", - "sim", GZ_SIM_MAJOR_VERSION_STR); - defaultConfig = gz::common::joinPaths(defaultConfigFolder, - defaultGuiConfigName); - } - else - { - defaultConfig = _defaultGuiConfig; - } app->SetDefaultConfigPath(defaultConfig); @@ -140,12 +321,11 @@ std::unique_ptr createGui( } // Get list of worlds - gz::transport::Node node; bool executed{false}; bool result{false}; unsigned int timeout{5000}; std::string service{"/gazebo/worlds"}; - gz::msgs::StringMsg_V worldsMsg; + msgs::StringMsg_V worldsMsg; // This loop is here to allow the server time to download resources. // \todo(nkoenig) Async resource download. Search for "Async resource @@ -173,8 +353,7 @@ std::unique_ptr createGui( std::size_t runnerCount = 0; // Configuration file from command line - if (_guiConfig != nullptr && std::strlen(_guiConfig) > 0 && - std::string(_guiConfig) != "_playback_") + if (configFromCli) { // Use the first world name with the config file // TODO(anyone) Most of gz-sim's transport API includes the world name, @@ -264,48 +443,6 @@ std::unique_ptr createGui( auto plugins = mainWin->findChildren(); if (plugins.empty()) { - // Check if there's a default config file under - // ~/.gz/sim and use that. If there isn't, copy - // the installed file there first. - if (!gz::common::exists(defaultConfig)) - { - if (!gz::common::exists(defaultConfigFolder)) - { - if (!gz::common::createDirectories(defaultConfigFolder)) - { - gzerr << "Failed to create the default config folder [" - << defaultConfigFolder << "]\n"; - return nullptr; - } - } - - auto installedConfig = gz::common::joinPaths( - GZ_SIM_GUI_CONFIG_PATH, defaultGuiConfigName); - if (!gz::common::exists(installedConfig)) - { - gzerr << "Failed to copy installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << "(file " << installedConfig << " doesn't exist)" - << std::endl; - return nullptr; - } - - if (!gz::common::copyFile(installedConfig, defaultConfig)) - { - gzerr << "Failed to copy installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << std::endl; - return nullptr; - } - else - { - gzmsg << "Copied installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << std::endl; - } - } - - // Also set ~/.gz/sim/ver/gui.config as the default path if (!app->LoadConfig(defaultConfig)) { gzerr << "Failed to load config file[" << defaultConfig << "]." @@ -313,7 +450,6 @@ std::unique_ptr createGui( return nullptr; } } - return app; } @@ -321,8 +457,16 @@ std::unique_ptr createGui( int runGui(int &_argc, char **_argv, const char *_guiConfig, const char *_renderEngine) { - auto app = sim::gui::createGui( - _argc, _argv, _guiConfig, nullptr, true, _renderEngine); + return runGui(_argc, _argv, _guiConfig, nullptr, 0, _renderEngine); +} + +////////////////////////////////////////////////// +int runGui(int &_argc, char **_argv, + const char *_guiConfig, const char *_sdfFile, int _waitGui, + const char *_renderEngine) +{ + auto app = sim::gui::createGui(_argc, _argv, _guiConfig, nullptr, true, + _sdfFile, _waitGui, _renderEngine); if (nullptr != app) { // Run main window. diff --git a/src/gui/GuiFileHandler.hh b/src/gui/GuiFileHandler.hh index fc4b5c591e..582d563ab4 100644 --- a/src/gui/GuiFileHandler.hh +++ b/src/gui/GuiFileHandler.hh @@ -24,8 +24,8 @@ #include -#include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Export.hh" +#include "gz/sim/config.hh" namespace gz { diff --git a/src/gui/Gui_TEST.cc b/src/gui/Gui_TEST.cc index 6bd906dd99..2833b33221 100644 --- a/src/gui/Gui_TEST.cc +++ b/src/gui/Gui_TEST.cc @@ -22,11 +22,14 @@ #include #include #include +#include +#include #include #include #include "gz/sim/gui/Gui.hh" #include "test_config.hh" +#include "QuickStartHandler.hh" #include "../../test/helpers/EnvTestFixture.hh" @@ -34,7 +37,7 @@ int gg_argc = 1; char **gg_argv = new char *[gg_argc]; using namespace gz; -using namespace sim; +using namespace gz::sim::gui; class GuiTest : public InternalFixture<::testing::Test> { @@ -167,3 +170,103 @@ TEST_F(GuiTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PathManager)) EXPECT_EQ("/new/path", paths[3]); } } + +///////////////////////////////////////////////// +TEST_F(GuiTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(QuickStart)) +{ + common::Console::SetVerbosity(4); + gzdbg << "Start test" << std::endl; + + transport::Node node; + + // Worlds callback + bool worldsCalled{false}; + std::function worldsCb = + [&worldsCalled](msgs::StringMsg_V &_res) + { + _res.add_data("world_name"); + worldsCalled = true; + return true; + }; + node.Advertise("/gazebo/worlds", worldsCb); + igndbg << "Worlds advertised" << std::endl; + + // Starting world callback + bool startingWorldCalled{false}; + std::function topicCb = + [&startingWorldCalled](const auto &_msg) + { + EXPECT_EQ("banana", _msg.data()); + startingWorldCalled = true; + }; + + std::string topic{"/gazebo/starting_world"}; + node.Subscribe(topic, topicCb); + igndbg << "Subscribed to [" << topic << "]" << std::endl; + + // Custom config + // TODO(chapulina) Make it not Linux-specific + std::string configFilePath{"/tmp/quick_start_test.config"}; + std::ofstream configFile(configFilePath); + configFile << "false" << + ""; + configFile.close(); + + // Thread to check and close quick start dialog + std::thread checkingThread([&]() + { + igndbg << "Started checking thread" << std::endl; + for (int sleep = 0; + (nullptr == gui::App( ) || + gui::App()->allWindows().empty() || + !gui::App()->allWindows()[0]->isVisible()) + && sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(nullptr, gui::App()); + ASSERT_EQ(1, gui::App()->allWindows().count()); + igndbg << "Found app" << std::endl; + + auto handler = gui::App()->Engine()->findChild(); + ASSERT_NE(nullptr, handler); + + EXPECT_EQ(GZ_DISTRIBUTION, handler->Distribution()); + EXPECT_EQ(GZ_SIM_VERSION_FULL, handler->SimVersion()); + EXPECT_TRUE(handler->ShowAgain()); + + handler->SetStartingWorld("banana"); + EXPECT_EQ("banana", handler->StartingWorld()); + + gui::App()->allWindows()[0]->close(); + + // Close main window + for (int sleep = 0; + (nullptr == gui::App()->findChild() || + !gui::App()->findChild()->QuickWindow()->isVisible()) + && sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + auto win = gui::App()->findChild(); + ASSERT_NE(nullptr, win); + EXPECT_TRUE(win->QuickWindow()->isVisible()); + win->QuickWindow()->close(); + }); + + auto app = createGui(gg_argc, gg_argv, + configFilePath.c_str() /* _guiConfig */, + nullptr /* _defaultGuiConfig */, + true /* _loadPluginsFromSdf */, + nullptr /* _sdfFile */, + true /* _waitGui */); + EXPECT_NE(nullptr, app); + igndbg << "GUI created" << std::endl; + + EXPECT_TRUE(worldsCalled); + EXPECT_TRUE(startingWorldCalled); + + app->exec(); + checkingThread.join(); +} + diff --git a/src/gui/Gui_clean_exit_TEST.cc b/src/gui/Gui_clean_exit_TEST.cc index 82d9a515fb..2392e6f2f9 100644 --- a/src/gui/Gui_clean_exit_TEST.cc +++ b/src/gui/Gui_clean_exit_TEST.cc @@ -54,7 +54,8 @@ void startGui() { int argc = 1; char *argv = const_cast("gz-sim-gui"); - EXPECT_EQ(0, sim::gui::runGui(argc, &argv, "", "")); + EXPECT_EQ(0, sim::gui::runGui(argc, &argv, nullptr, nullptr, 0, + nullptr)); } ///////////////////////////////////////////////// diff --git a/src/gui/QuickStartHandler.cc b/src/gui/QuickStartHandler.cc new file mode 100644 index 0000000000..c420bffc64 --- /dev/null +++ b/src/gui/QuickStartHandler.cc @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2022 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 "QuickStartHandler.hh" + +using namespace gz; +using namespace sim::gui; + +///////////////////////////////////////////////// +QString QuickStartHandler::WorldsPath() const +{ + return QString::fromUtf8(this->worldsPath.c_str()); +} + +///////////////////////////////////////////////// +std::string QuickStartHandler::StartingWorld() const +{ + return this->startingWorld; +} + +///////////////////////////////////////////////// +QString QuickStartHandler::Distribution() const +{ + return QString::fromUtf8(GZ_DISTRIBUTION); +} + +///////////////////////////////////////////////// +QString QuickStartHandler::SimVersion() const +{ + return QString::fromUtf8(GZ_SIM_VERSION_FULL); +} + +///////////////////////////////////////////////// +void QuickStartHandler::SetStartingWorld(const QString &_url) +{ + this->startingWorld = _url.toStdString(); +} + +///////////////////////////////////////////////// +void QuickStartHandler::SetShowAgain(const bool _showAgain) +{ + this->showAgain = !_showAgain; +} + +///////////////////////////////////////////////// +bool QuickStartHandler::ShowAgain() const +{ + return this->showAgain; +} diff --git a/src/gui/QuickStartHandler.hh b/src/gui/QuickStartHandler.hh new file mode 100644 index 0000000000..dc10124c71 --- /dev/null +++ b/src/gui/QuickStartHandler.hh @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2022 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 GZ_SIM_GUI_QUICKSTARTHANDLER_HH_ +#define GZ_SIM_GUI_QUICKSTARTHANDLER_HH_ + +#include +#include + +#include "gz/sim/Export.hh" +#include "gz/sim/config.hh" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace gui +{ +/// \brief Class for handling quick start dialog +class QuickStartHandler : public QObject +{ + Q_OBJECT + + /// \brief Get worlds path + /// \return worlds directory path + public: Q_INVOKABLE QString WorldsPath() const; + + /// \brief Get the distribution name + /// \return Distribution name, such as 'Citadel' + public: Q_INVOKABLE QString Distribution() const; + + /// \brief Get Gazebo Sim version + /// \return Version + public: Q_INVOKABLE QString SimVersion() const; + + /// \brief Set starting world + /// \param[in] _url Url to the world file. + public: Q_INVOKABLE void SetStartingWorld(const QString &_url); + + /// \brief Get starting world url from GUI. + /// \return World url + public: Q_INVOKABLE std::string StartingWorld() const; + + /// \brief Set the flag to show quick start menu again. + /// \param[in] _showQuickStartOpts True to show. + public: Q_INVOKABLE void SetShowAgain(const bool _showAgain); + + /// \brief Show again option. + /// \return True to show again. + public: Q_INVOKABLE bool ShowAgain() const; + + /// \brief Show the quick start menu again. + private: bool showAgain{true}; + + /// \brief Installed worlds path. + private: std::string worldsPath{GZ_SIM_WORLD_INSTALL_DIR}; + + /// \brief Get starting world url. + private: std::string startingWorld{""}; +}; +} +} +} +} +#endif diff --git a/src/gui/gui.config b/src/gui/gui.config index d226e8937b..9e7004492e 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -1,5 +1,8 @@ + + + 1000 diff --git a/src/gui/playback_gui.config b/src/gui/playback_gui.config index decf2d4988..f911c3730b 100644 --- a/src/gui/playback_gui.config +++ b/src/gui/playback_gui.config @@ -26,7 +26,7 @@ - + 3D View false @@ -37,7 +37,71 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - 6 0 6 0 0.5 3.14 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index 7e064224c2..bfb48b4b56 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -15,7 +15,6 @@ * */ import QtQuick 2.9 -import QtQuick.Controls 1.4 import QtQuick.Controls 2.2 import QtQuick.Controls.Material 2.1 import QtQuick.Layouts 1.3 @@ -25,7 +24,7 @@ import "qrc:/qml" // Item displaying 3D pose information. Rectangle { - height: header.height + content.height + height: header.height + gzPoseInstance.height width: componentInspector.width color: index % 2 == 0 ? lightGrey : darkGrey @@ -35,381 +34,103 @@ Rectangle { // Horizontal margins property int margin: 5 - // Maximum spinbox value - property double spinMax: 1000000 - - // Read-only / write - property bool readOnly: { - var isModel = entityType == "model" - return !(isModel) || nestedModel - } - - property int iconWidth: 20 - property int iconHeight: 20 - - // Loaded item for X - property var xItem: {} - - // Loaded item for Y - property var yItem: {} - - // Loaded item for Z - property var zItem: {} - - // Loaded item for roll - property var rollItem: {} - - // Loaded item for pitch - property var pitchItem: {} - - // Loaded item for yaw - property var yawItem: {} - // Send new pose to C++ - function sendPose() { + function sendPose(x, y, z, roll, pitch, yaw) { // TODO(anyone) There's a loss of precision when these values get to C++ - Pose3dImpl.OnPose( - xItem.value, - yItem.value, - zItem.value, - rollItem.value, - pitchItem.value, - yawItem.value - ); - } - - FontMetrics { - id: fontMetrics - font.family: "Roboto" - } - - /** - * Used to create a spin box - */ - Component { - id: writableNumber - GzSpinBox { - id: writableSpin - value: writableSpin.activeFocus ? writableSpin.value : numberValue - minimumValue: -spinMax - maximumValue: spinMax - decimals: getDecimals(writableSpin.width) - onEditingFinished: { - sendPose() - } - } - } - - /** - * Used to create a read-only number - */ - Component { - id: readOnlyNumber - Text { - id: numberText - anchors.fill: parent - horizontalAlignment: Text.AlignRight - verticalAlignment: Text.AlignVCenter - text: { - var decimals = getDecimals(numberText.width) - return numberValue.toFixed(decimals) - } - } + Pose3dImpl.OnPose(x, y, z, roll, pitch, yaw); } Column { anchors.fill: parent - // The expanding header. Make sure that the content to expand has an id set - // to the value "content". - ExpandingTypeHeader { - id: header - // Using the default header text values. - } - - // Content + // Header Rectangle { - id: content - property bool show: false + id: header width: parent.width - height: show ? grid.height : 0 - clip: true + height: typeHeader.height color: "transparent" - Behavior on height { - NumberAnimation { - duration: 200; - easing.type: Easing.InOutQuad - } - } - - - GridLayout { - id: grid - width: parent.width - columns: 6 - - // Left spacer + RowLayout { + anchors.fill: parent Item { - Layout.rowSpan: 3 - width: margin + indentation + width: margin } - - - Component { - id: plotIcon - Image { - property string componentInfo: "" - source: "plottable_icon.svg" - anchors.top: parent.top - anchors.left: parent.left - - Drag.mimeData: { "text/plain" : (model === null) ? "" : - "Component," + model.entity + "," + model.typeId + "," + - model.dataType + "," + componentInfo + "," + model.shortName - } - Drag.dragType: Drag.Automatic - Drag.supportedActions : Qt.CopyAction - Drag.active: dragMouse.drag.active - // a point to drag from - Drag.hotSpot.x: 0 - Drag.hotSpot.y: y - MouseArea { - id: dragMouse - anchors.fill: parent - drag.target: (model === null) ? null : parent - onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) - onReleased: parent.Drag.drop(); - cursorShape: Qt.DragCopyCursor - } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: gzPoseInstance.expand ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + TypeHeader { + id: typeHeader } - } - - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: xText.width + indentation*3 - Loader { - id: loaderX - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderX.item.componentInfo = "x" - - Text { - id : xText - text: ' X (m)' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - Item { Layout.fillWidth: true - height: 40 - Loader { - id: xLoader - anchors.fill: parent - property double numberValue: model.data[0] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - xItem = xLoader.item - } - } } - - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: rollText.width + indentation*3 - Loader { - id: loaderRoll - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderRoll.item.componentInfo = "roll" - - Text { - id: rollText - text: ' Roll (rad)' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } + } + MouseArea { + anchors.fill: parent + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + gzPoseInstance.expand = !gzPoseInstance.expand } - - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: rollLoader - anchors.fill: parent - property double numberValue: model.data[3] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - rollItem = rollLoader.item - } - } + onEntered: { + header.color = highlightColor } - - // Right spacer - Item { - Layout.rowSpan: 3 - width: margin + indentation + onExited: { + header.color = "transparent" } + } + } + Rectangle { + color: "transparent" + width: parent.width + height: gzPoseInstance.height + RowLayout { + id: gzPoseRow + width: parent.width - - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: yText.width + indentation*3 - Loader { - id: loaderY - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderY.item.componentInfo = "y" - Text { - id: yText - text: ' Y (m)' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } + // Left spacer + Item { + Layout.preferredWidth: margin + indentation } - Item { + // Content + GzPose { + id: gzPoseInstance Layout.fillWidth: true - height: 40 - Loader { - id: yLoader - anchors.fill: parent - property double numberValue: model.data[1] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - yItem = yLoader.item - } - } - } - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: pitchText.width + indentation*3 - Loader { - id: loaderPitch - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderPitch.item.componentInfo = "pitch"; - Text { - id: pitchText - text: ' Pitch (rad)' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent + readOnly: { + var isModel = entityType == "model" + return !(isModel) || nestedModel } - } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: pitchLoader - anchors.fill: parent - property double numberValue: model.data[4] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - pitchItem = pitchLoader.item - } - } - } + xValue: model.data[0] + yValue: model.data[1] + zValue: model.data[2] + rollValue: model.data[3] + pitchValue: model.data[4] + yawValue: model.data[5] - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: zText.width + indentation*3 - Loader { - id: loaderZ - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderZ.item.componentInfo = "z"; - Text { - id: zText - text: ' Z (m)' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent + onGzPoseSet: { + // _x, _y, _z, _roll, _pitch, _yaw are parameters of signal gzPoseSet + sendPose(_x, _y, _z, _roll, _pitch, _yaw) } - } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: zLoader - anchors.fill: parent - property double numberValue: model.data[2] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - zItem = zLoader.item - } - } - } + // By default it is closed + expand: false - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: yawText.width + indentation*3 - Loader { - id: loaderYaw - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderYaw.item.componentInfo = "yaw"; - Text { - id: yawText - text: ' Yaw (rad)' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } + } // end gzPoseInstance + // Right spacer Item { - Layout.fillWidth: true - height: 40 - Loader { - id: yawLoader - anchors.fill: parent - property double numberValue: model.data[5] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - onLoaded: { - yawItem = yawLoader.item - } - } + Layout.preferredWidth: margin } - } - } - } -} + } // end RowLayout + } // end Rectangle + } // end Column +} // end Rectangle diff --git a/src/gui/plugins/component_inspector/Vector3d.qml b/src/gui/plugins/component_inspector/Vector3d.qml index 9178d95a69..b33e18b426 100644 --- a/src/gui/plugins/component_inspector/Vector3d.qml +++ b/src/gui/plugins/component_inspector/Vector3d.qml @@ -15,7 +15,6 @@ * */ import QtQuick 2.9 -import QtQuick.Controls 1.4 import QtQuick.Controls 2.2 import QtQuick.Controls.Material 2.1 import QtQuick.Layouts 1.3 @@ -25,227 +24,94 @@ import "qrc:/qml" // Item displaying 3D vector information. Rectangle { - height: header.height + content.height + height: header.height + gzVectorInstance.height width: componentInspector.width color: index % 2 == 0 ? lightGrey : darkGrey // Left indentation property int indentation: 10 - // icon size - property int iconWidth: 20 - property int iconHeight: 20 - // Horizontal margins property int margin: 5 - // Maximum spinbox value - property double spinMax: 1000000 - - // Units, defaults to meters. - property string unit: model && model.unit != undefined ? model.unit : 'm' - - // Readn-only / write - property bool readOnly: true - - /** - * Used to create a spin box - */ - Component { - id: writableNumber - GzSpinBox { - id: writableSpin - value: numberValue - minimumValue: -spinMax - maximumValue: spinMax - decimals: getDecimals(writableSpin.width) - } - } - - /** - * Used to create a read-only number - */ - Component { - id: readOnlyNumber - Text { - id: numberText - anchors.fill: parent - horizontalAlignment: Text.AlignRight - verticalAlignment: Text.AlignVCenter - text: { - var decimals = getDecimals(numberText.width) - return numberValue.toFixed(decimals) - } - } - } - Component { - id: plotIcon - Image { - property string componentInfo: "" - source: "plottable_icon.svg" - anchors.top: parent.top - anchors.left: parent.left - - Drag.mimeData: { "text/plain" : (model === null) ? "" : - "Component," + model.entity + "," + model.typeId + "," + - model.dataType + "," + componentInfo + "," + model.shortName - } - Drag.dragType: Drag.Automatic - Drag.supportedActions : Qt.CopyAction - Drag.active: dragMouse.drag.active - // a point to drag from - Drag.hotSpot.x: 0 - Drag.hotSpot.y: y - MouseArea { - id: dragMouse - anchors.fill: parent - drag.target: (model === null) ? null : parent - onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) - onReleased: parent.Drag.drop(); - cursorShape: Qt.DragCopyCursor - } - } - } - Column { anchors.fill: parent - // The expanding header. Make sure that the content to expand has an id set - // to the value "content". - ExpandingTypeHeader { - id: header - // Using the default header text values. - } - - // Content + // Header Rectangle { - id: content - property bool show: false + id: header width: parent.width - height: show ? grid.height : 0 - clip: true + height: typeHeader.height color: "transparent" - Behavior on height { - NumberAnimation { - duration: 200; - easing.type: Easing.InOutQuad - } - } - - GridLayout { - id: grid - width: parent.width - columns: 4 - - // Left spacer + RowLayout { + anchors.fill: parent Item { - Layout.rowSpan: 3 - width: margin + indentation + width: margin } - - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: xText.width + indentation*3 - Loader { - id: loaderX - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderX.item.componentInfo = "x" - - Text { - id: xText - text: ' X (' + unit + ')' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: gzVectorInstance.expand ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + TypeHeader { + id: typeHeader } Item { Layout.fillWidth: true - height: 40 - Loader { - anchors.fill: parent - property double numberValue: model.data[0] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - } } - - // Right spacer - Item { - Layout.rowSpan: 3 - width: margin + indentation + } + MouseArea { + anchors.fill: parent + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + gzVectorInstance.expand = !gzVectorInstance.expand } - - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: xText.width + indentation*3 - Loader { - id: loaderY - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderY.item.componentInfo = "y" - - Text { - text: ' Y (' + unit + ')' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } + onEntered: { + header.color = highlightColor } + onExited: { + header.color = "transparent" + } + } + } + Rectangle { + color: "transparent" + width: parent.width + height: gzVectorInstance.height + RowLayout { + id: gzVectorRow + width: parent.width + // Left spacer Item { - Layout.fillWidth: true - height: 40 - Loader { - anchors.fill: parent - property double numberValue: model.data[1] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - } + Layout.preferredWidth: margin + indentation } - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: xText.width + indentation*3 - Loader { - id: loaderZ - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon - } - Component.onCompleted: loaderZ.item.componentInfo = "z" + // Content + GzVector3 { + id: gzVectorInstance + Layout.fillWidth: true + gzUnit: model && model.unit != undefined ? model.unit : 'm' - Text { - text: ' Z (' + unit + ')' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } + xValue: model.data[0] + yValue: model.data[1] + zValue: model.data[2] + + // By default it is closed + expand: false + } // GzVector3 ends + // Right spacer Item { - Layout.fillWidth: true - height: 40 - Loader { - anchors.fill: parent - property double numberValue: model.data[2] - sourceComponent: readOnly ? readOnlyNumber : writableNumber - } + Layout.preferredWidth: margin } - } - } - } -} + } // end RowLayout + } // end Rectangle + } // Column ends +} // Rectangle ends diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.qml b/src/gui/plugins/resource_spawner/ResourceSpawner.qml index ec913ac0ff..51ac1a37e2 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.qml +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.qml @@ -329,7 +329,7 @@ Rectangle { Layout.leftMargin: 15 Image { id: searchIcon - source: "Search.svg" + source: "qrc:/Gazebo/images/search.svg" anchors.verticalCenter: parent.verticalCenter } } diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.qrc b/src/gui/plugins/resource_spawner/ResourceSpawner.qrc index 663beae60c..07b219ec18 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.qrc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.qrc @@ -6,6 +6,5 @@ folder_open.png NoThumbnail.png ResourceSpawner.qml - Search.svg diff --git a/src/gui/resources/FuelThumbnail.qml b/src/gui/resources/FuelThumbnail.qml new file mode 100644 index 0000000000..aad2fcdacf --- /dev/null +++ b/src/gui/resources/FuelThumbnail.qml @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +import QtQuick 2.9 +import QtQuick.Controls 2.2 +import QtQuick.Layouts 1.3 + +Rectangle{ + id: main + property alias source: root.source + property alias text: label.text + property string owner: '' + border.color: "#e0e0e0" + border.width: 2 + + ColumnLayout { + Rectangle { + Text { + x: 10 + y: 5 + id: label + font.capitalization: Font.Capitalize + text: qsTr("Label") + color: "#443224" + font.pixelSize: 14 + } + } + + Rectangle { + Image { + id: root + source: "" + fillMode: Image.PreserveAspectCrop + width: 216 + height: 124 + + x: 2 + y: 24 + signal clicked + + MouseArea { + anchors.fill: parent + onClicked: quickStart.loadFuelWorld(main.text, main.owner); + } + } + } + } +} diff --git a/src/gui/resources/QuickStart.qml b/src/gui/resources/QuickStart.qml new file mode 100644 index 0000000000..cc209da469 --- /dev/null +++ b/src/gui/resources/QuickStart.qml @@ -0,0 +1,331 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +import QtQuick 2.9 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import QtQml.Models 2.3 +import Qt.labs.folderlistmodel 2.1 +import QtQuick.Window 2.2 +import "qrc:/qml" + +Rectangle { + + /** + * Color for search bar + */ + property color searchColor: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade200): + Material.color(Material.Grey, Material.Shade900); + + + id: quickStart + anchors.fill: parent + property var selectedWorld: "" + + function changeDefault(checked) { + QuickStartHandler.SetShowAgain(checked); + } + + function loadWorld(file){ + if (file === quickStart.selectedWorld) { + openWorld.enabled = false; + quickStart.selectedWorld = ""; + } else { + QuickStartHandler.SetStartingWorld(file); + openWorld.enabled = true; + quickStart.selectedWorld = file; + } + } + + function loadFuelWorld(fileName, owner){ + if (quickStart.selectedWorld === fileName) { + openWorld.enabled = false; + quickStart.selectedWorld = ''; + } else { + + quickStart.selectedWorld = fileName; + + if (fileName === "empty") { + QuickStartHandler.SetStartingWorld("empty.sdf"); + openWorld.enabled = true; + } else { + // Construct fuel URL + var fuelUrl = "https://fuel.gazebosim.org/1.0/" + owner + "/worlds/"; + fuelUrl += fileName; + QuickStartHandler.SetStartingWorld(fuelUrl); + openWorld.enabled = true; + } + } + } + + function getWorlds(){ + return "file://" + QuickStartHandler.WorldsPath() + } + + function getColor(fileName){ + if(fileName == selectedWorld) + return Material.primary; + return "#e0e0e0"; + } + + ColumnLayout { + anchors.fill: parent + spacing: 0 + + // Top row, which holds the logo and version. + Rectangle { + Layout.fillWidth: true + Layout.preferredHeight: 100 + color:'transparent' + + RowLayout { + anchors.fill: parent + Rectangle { + color: 'transparent' + Layout.fillWidth: true + Layout.preferredWidth: 960 + Layout.preferredHeight: 100 + Layout.minimumHeight: 100 + Image{ + source: "images/gazebo_horz_pos_topbar.svg" + fillMode: Image.PreserveAspectFit + x: 30 + y: (parent.height - height) / 2 + } + } + ColumnLayout { + Text{ + text: QuickStartHandler.Distribution() + } + Text{ + text: 'v ' + QuickStartHandler.SimVersion() + } + } + Item { + width: 30 + } + } + } + + // Middle row, which holds the world selection elements. + Rectangle { + Layout.fillWidth: true + height: 360 + RowLayout { + spacing: 0 + anchors { + fill: parent + leftMargin: 15 + rightMargin: 20 + } + + // Card grid view + Rectangle { + color: 'transparent' + Layout.fillWidth: true + Layout.fillHeight: true + + FolderListModel { + id: folderModel + showDirs: false + nameFilters: ["*.png"] + folder: getWorlds() + "/thumbnails/" + } + + Component { + id: fileDelegate + + FuelThumbnail { + id: filePath + text: fileName.split('.')[1] + owner: fileName.split('.')[0] + width: 220 + height: 150 + smooth: true + source: fileURL + border.color: getColor(fileName.split('.')[1]) + } + } + GridView { + id: gridView + width: parent.width + height: parent.height + interactive: false + + anchors { + fill: parent + leftMargin: 5 + topMargin: 5 + } + + cellWidth: 240 + cellHeight: 205 + + model: folderModel + delegate: fileDelegate + } + } + + // SDF file list with search bar + Rectangle { + color: 'transparent' + width: 200 + Layout.fillHeight: true + Layout.topMargin: 5 + border { + width: 1 + color: searchColor + } + + ColumnLayout { + anchors.fill: parent + spacing: 0 + + Rectangle { + id: searchSortBar + color: searchColor + height: 50 + width: parent.width + RowLayout { + id: rowLayout + anchors.fill: parent + spacing: 0 + Rectangle { + color: "transparent" + height: 25 + width: 25 + Layout.leftMargin: 5 + Image { + id: searchIcon + source: "images/search.svg" + anchors.verticalCenter: parent.verticalCenter + } + } + TextField { + id: searchField + Layout.fillHeight: true + Layout.preferredWidth: parent.width - 50 + selectByMouse: true + onTextEdited: { + sdfFileModel.update(); + } + } + } + } + + Component { + id: sdfFileDelegate + + ItemDelegate { + width: parent.width-11 + x: 1 + text: fileName + highlighted: selectedWorld == fileName + onClicked: { + quickStart.loadWorld(fileName); + } + } + } + + GzSortFilterModel { + id: sdfFileModel + + lessThan: function(left, right) { + var leftStr = left.fileName.toLowerCase(); + var rightStr = right.fileName.toLowerCase(); + return leftStr < rightStr; + } + + filterAcceptsItem: function(item) { + var itemStr = item.fileName.toLowerCase(); + var filterStr = searchField.text.toLowerCase(); + return itemStr.includes(filterStr); + } + + model: FolderListModel { + showDirs: false + showFiles: true + folder: getWorlds() + nameFilters: [ "*.sdf" ] + } + + delegate: sdfFileDelegate + } + + ListView { + id: pluginMenuListView + + Layout.fillHeight: true + width: parent.width + clip: true + model: sdfFileModel + + ScrollBar.vertical: ScrollBar { + active: true + } + } + } + } + } + } + + // Bottom row, which holds the run button and don't show checkbox. + Rectangle { + Layout.fillWidth: true + Layout.fillHeight: true + height: 60 + color: 'transparent' + + RowLayout { + anchors { + fill: parent + leftMargin: 20 + rightMargin: 20 + } + CheckBox { + id: showByDefault + text: "Don't show this dialog again" + Layout.fillWidth: true + onClicked: { + quickStart.changeDefault(showByDefault.checked) + } + } + Button { + id: openWorld + visible: true + text: "Run" + enabled: false + onClicked: { + quickStart.Window.window.close() + } + background: Rectangle { + implicitWidth: 60 + implicitHeight: 40 + radius: 4 + color: openWorld.hovered ? "#efefef" : 'transparent' + border { + width: 2 + color: openWorld.enabled ? Material.primary : "#efefef" + } + } + } + } + } + } +} diff --git a/src/gui/resources/gazebo.qrc b/src/gui/resources/gazebo.qrc index 96fa71b8ff..eb860136ce 100644 --- a/src/gui/resources/gazebo.qrc +++ b/src/gui/resources/gazebo.qrc @@ -1,10 +1,13 @@ + FuelThumbnail.qml GazeboDrawer.qml + QuickStart.qml images/actor.png images/chevron-down.svg images/chevron-right.svg images/collision.png + images/gazebo_horz_pos_topbar.svg images/joint.png images/light.png images/link.png @@ -14,6 +17,7 @@ images/plus.png images/plus-link.png images/plus-sensor.png + images/search.svg images/sensor.png images/unlock.svg images/visual.png diff --git a/src/gui/resources/images/gazebo_horz_pos_topbar.svg b/src/gui/resources/images/gazebo_horz_pos_topbar.svg new file mode 100644 index 0000000000..1a1bc0473e --- /dev/null +++ b/src/gui/resources/images/gazebo_horz_pos_topbar.svg @@ -0,0 +1,131 @@ + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + diff --git a/src/gui/plugins/resource_spawner/Search.svg b/src/gui/resources/images/search.svg similarity index 100% rename from src/gui/plugins/resource_spawner/Search.svg rename to src/gui/resources/images/search.svg diff --git a/src/gz.cc b/src/gz.cc index ae71bb2833..7f5f045e02 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -21,12 +21,16 @@ #include #include +#include + #include #include #include #include #include #include +#include +#include #include "gz/sim/config.hh" #include "gz/sim/Server.hh" @@ -34,6 +38,8 @@ #include "gz/sim/gui/Gui.hh" +using namespace gz; + ////////////////////////////////////////////////// extern "C" char *gzSimVersion() { @@ -50,7 +56,15 @@ extern "C" char *simVersionHeader() extern "C" void cmdVerbosity( const char *_verbosity) { - gz::common::Console::SetVerbosity(std::atoi(_verbosity)); + int verbosity = std::atoi(_verbosity); + gz::common::Console::SetVerbosity(verbosity); + + // SDFormat only has 2 levels: quiet / loud. Let sim users suppress all SDF + // console output with zero verbosity. + if (verbosity == 0) + { + sdf::Console::Instance()->SetQuiet(true); + } } ////////////////////////////////////////////////// @@ -121,11 +135,40 @@ extern "C" int runServer(const char *_sdfString, int _recordResources, int _logOverwrite, int _logCompress, const char *_playback, const char *_physicsEngine, const char *_renderEngineServer, const char *_renderEngineGui, - const char *_file, const char *_recordTopics, + const char *_file, const char *_recordTopics, int _waitGui, int _headless) { + std::string startingWorldPath{""}; gz::sim::ServerConfig serverConfig; + // Lock until the starting world is received from Gui + if (_waitGui == 1) + { + transport::Node node; + std::condition_variable condition; + std::mutex mutex; + + // Create a subscriber just so we can check when the message has propagated + std::function topicCb = + [&startingWorldPath, &mutex, &condition](const auto &_msg) + { + std::unique_lock lock(mutex); + startingWorldPath = _msg.data(); + condition.notify_all(); + }; + + std::string topic{"/gazebo/starting_world"}; + std::unique_lock lock(mutex); + gzdbg << "Subscribing to [" << topic << "]." << std::endl; + node.Subscribe(topic, topicCb); + gzdbg << "Waiting for a world to be set from the GUI..." << std::endl; + condition.wait(lock); + gzmsg << "Received world [" << startingWorldPath << "] from the GUI." + << std::endl; + gzdbg << "Unsubscribing from [" << topic << "]." << std::endl; + node.Unsubscribe(topic); + } + // Path for logs std::string recordPathMod = serverConfig.LogRecordPath(); @@ -293,7 +336,13 @@ extern "C" int runServer(const char *_sdfString, return -1; } } - serverConfig.SetSdfFile(_file); + + // This ensures if the server was run stand alone with a world from + // command line, the correct world would be loaded. + if(_waitGui == 1) + serverConfig.SetSdfFile(startingWorldPath); + else + serverConfig.SetSdfFile(_file); // Set the update rate. if (_hz > 0.0) @@ -358,7 +407,8 @@ extern "C" int runServer(const char *_sdfString, } ////////////////////////////////////////////////// -extern "C" int runGui(const char *_guiConfig, const char *_renderEngine) +extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, + const char *_renderEngine) { // argc and argv are going to be passed to a QApplication. The Qt // documentation has a warning about these: @@ -387,5 +437,6 @@ extern "C" int runGui(const char *_guiConfig, const char *_renderEngine) }; int argc = sizeof(argv) / sizeof(argv[0]); - return gz::sim::gui::runGui(argc, argv, _guiConfig, _renderEngine); + return gz::sim::gui::runGui( + argc, argv, _guiConfig, _file, _waitGui, _renderEngine); } diff --git a/src/gz.hh b/src/gz.hh index 64436370c8..2d958a7408 100644 --- a/src/gz.hh +++ b/src/gz.hh @@ -53,6 +53,8 @@ extern "C" const char *worldInstallDir(); /// \param[in] _renderEngineGui --render-engine-gui option /// \param[in] _file Path to file being loaded /// \param[in] _recordTopics Colon separated list of topics to record. Leave +/// \param[in] _waitGui Flag indicating whether the server waits until +/// it receives a world path from GUI. /// null to record the default topics. /// \param[in] _headless True if server rendering should run headless /// \return 0 if successful, 1 if not. @@ -63,13 +65,18 @@ extern "C" int runServer(const char *_sdfString, int _logCompress, const char *_playback, const char *_physicsEngine, const char *_renderEngineServer, const char *_renderEngineGui, const char *_file, - const char *_recordTopics, int _headless); + const char *_recordTopics, int _waitGui, int _headless); /// \brief External hook to run simulation GUI. /// \param[in] _guiConfig Path to Gazebo GUI configuration file. +/// \param[in] _file The world file path passed as a command line argument. +/// If set, QuickStart Dialog will not be shown. +/// \param[in] _waitGui Flag indicating whether the server waits until +/// it receives a world path from GUI. /// \param[in] _renderEngine --render-engine-gui option /// \return 0 if successful, 1 if not. -extern "C" int runGui(const char *_guiConfig, const char *_renderEngine); +extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, + const char *_renderEngine); /// \brief External hook to find or download a fuel world provided a URL. /// \param[in] _pathToResource Path to the fuel world resource, ie, diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc index 7697bc4329..70e641a080 100644 --- a/src/rendering/MarkerManager.cc +++ b/src/rendering/MarkerManager.cc @@ -504,7 +504,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const gz::msgs::Marker &_msg) else { // Create the name for the marker - std::string name = "__IGN_MARKER_VISUAL_" + ns + "_" + + std::string name = "__GZ_MARKER_VISUAL_" + ns + "_" + std::to_string(id); // Create the new marker diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 70aa5bf7b0..ed248c1836 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -2337,7 +2337,7 @@ SceneManager::LoadAnimations(const sdf::Actor &_actor) animMesh = meshManager->Load(animFilename); if (animMesh->MeshSkeleton()->AnimationCount() > 1) { - ignwarn << "File [" << animFilename + gzwarn << "File [" << animFilename << "] has more than one animation, " << "but only the 1st one is used." << std::endl; @@ -2349,7 +2349,7 @@ SceneManager::LoadAnimations(const sdf::Actor &_actor) auto firstAnim = animMesh->MeshSkeleton()->Animation(0); if (nullptr == firstAnim) { - ignerr << "Failed to get animations from [" << animFilename + gzerr << "Failed to get animations from [" << animFilename << "]" << std::endl; mapAnimNameId.clear(); break; @@ -2375,8 +2375,8 @@ SceneManager::LoadAnimations(const sdf::Actor &_actor) // to meshSkel, changing the name that would also change the name of // other instances of the animation // todo(anyone) cloning is inefficient and error-prone. We should - // add a copy constructor to animation classes in ign-common. - // The proper fix is probably to update ign-rendering to allow it to + // add a copy constructor to animation classes in gz-common. + // The proper fix is probably to update gz-rendering to allow it to // load multiple animations of the same name common::SkeletonAnimation *skelAnim = new common::SkeletonAnimation(animName); diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 42190ed104..57e8050a75 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -99,6 +99,7 @@ add_subdirectory(ackermann_steering) add_subdirectory(air_pressure) add_subdirectory(altimeter) add_subdirectory(apply_joint_force) +add_subdirectory(apply_link_wrench) add_subdirectory(battery_plugin) add_subdirectory(breadcrumbs) add_subdirectory(buoyancy) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 89c48b70fa..7519061db2 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -143,6 +143,9 @@ class gz::sim::systems::AckermannSteeringPrivate /// \brief Ackermann steering odometry message publisher. public: transport::Node::Publisher odomPub; + /// \brief Ackermann tf message publisher. + public: transport::Node::Publisher tfPub; + /// \brief Odometry X value public: double odomX{0.0}; @@ -343,6 +346,24 @@ void AckermannSteering::Configure(const Entity &_entity, this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopic); + std::vector tfTopics; + if (_sdf->HasElement("tf_topic")) + { + tfTopics.push_back(_sdf->Get("tf_topic")); + } + tfTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/tf"); + auto tfTopic = validTopic(tfTopics); + if (tfTopic.empty()) + { + gzerr << "AckermannSteering plugin invalid tf topic name " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopic); + if (_sdf->HasElement("frame_id")) this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); @@ -667,8 +688,16 @@ void AckermannSteeringPrivate::UpdateOdometry( childFrame->add_value(this->sdfChildFrameId); } + // Construct the Pose_V/tf message and publish it. + msgs::Pose_V tfMsg; + auto *tfMsgPose = tfMsg.add_pose(); + tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); + tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); + tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); + // Publish the message this->odomPub.Publish(msg); + this->tfPub.Publish(tfMsg); } ////////////////////////////////////////////////// diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index dc856758df..8bbdc7c304 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -75,12 +75,12 @@ namespace systems /// ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// '': Minimum velocity [m/s], usually <= 0. - /// '': Maximum velocity [m/s], usually >= 0. - /// '': Minimum acceleration [m/s^2], usually <= 0. - /// '': Maximum acceleration [m/s^2], usually >= 0. - /// '': jerk [m/s^3], usually <= 0. - /// '': jerk [m/s^3], usually >= 0. + /// ``: Minimum velocity [m/s], usually <= 0. + /// ``: Maximum velocity [m/s], usually >= 0. + /// ``: Minimum acceleration [m/s^2], usually <= 0. + /// ``: Maximum acceleration [m/s^2], usually >= 0. + /// ``: jerk [m/s^3], usually <= 0. + /// ``: jerk [m/s^3], usually >= 0. /// /// ``: Custom topic that this system will subscribe to in order to /// receive command velocity messages. This element if optional, and the @@ -90,6 +90,22 @@ namespace systems /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. /// + /// ``: Custom topic on which this system will publish the + /// transform from `frame_id` to `child_frame_id`. This element is optional, + /// and the default value is `/model/{name_of_model}/tf`. + /// + /// ``: Custom `frame_id` field that this system will use as the + /// origin of the odometry transform in both the `` + /// `gz.msgs.Pose_V` message and the `` + /// `gz.msgs.Odometry` message. This element if optional, and the + /// default value is `{name_of_model}/odom`. + /// + /// ``: Custom `child_frame_id` that this system will use as + /// the target of the odometry transform in both the `` + /// `gz.msgs.Pose_V` message and the `` + /// `gz.msgs.Odometry` message. This element if optional, + /// and the default value is `{name_of_model}/{name_of_link}`. + /// /// A robot with rear drive and front steering would have one each /// of left_joint, right_joint, left_steering_joint and /// right_steering_joint diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc new file mode 100644 index 0000000000..38a5677f00 --- /dev/null +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -0,0 +1,364 @@ +/* + * Copyright (C) 2022 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 +#include + +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/World.hh" +#include "gz/sim/Util.hh" + +#include "ApplyLinkWrench.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +class gz::sim::systems::ApplyLinkWrenchPrivate +{ + /// \brief Callback for wrench subscription + /// \param[in] _msg Wrench message + public: void OnWrench(const msgs::EntityWrench &_msg); + + /// \brief Callback for persistent wrench subscription + /// \param[in] _msg Wrench message + public: void OnWrenchPersistent(const msgs::EntityWrench &_msg); + + /// \brief Callback for clearing persistent wrenches + /// \param[in] _msg Entity message + public: void OnWrenchClear(const msgs::Entity &_msg); + + /// \brief True if a console message should be printed whenever an + /// instantaneous wrench is applied, a persistent wrench is cleared, etc. + public: bool verbose{true}; + + /// \brief Queue of incoming instantaneous wrenches + public: std::queue newWrenches; + + /// \brief All persistent wrenches + public: std::vector persistentWrenches; + + /// \brief Entities whose wrenches should be cleared + public: std::queue clearWrenches; + + /// \brief Communication node. + public: transport::Node node; + + /// \brief A mutex to protect wrenches + public: std::mutex mutex; +}; + +/// \brief Extract wrench information from a message. +/// \param[in] _ecm Entity component manager +/// \param[in] _msg Entity message. If it's a link, that link is returned. If +/// it's a model, its canonical link is returned. +/// \param[out] Force to apply. +/// \param[out] Torque to apply. +/// \return Target link entity. +Link decomposeMessage(const EntityComponentManager &_ecm, + const msgs::EntityWrench &_msg, math::Vector3d &_force, + math::Vector3d &_torque) +{ + if (_msg.wrench().has_force_offset()) + { + gzwarn << "Force offset currently not supported, it will be ignored." + << std::endl; + } + + if (_msg.wrench().has_force()) + { + _force = msgs::Convert(_msg.wrench().force()); + } + + if (_msg.wrench().has_torque()) + { + _torque = msgs::Convert(_msg.wrench().torque()); + } + + auto entity = entityFromMsg(_ecm, _msg.entity()); + if (entity == kNullEntity) + { + return Link(); + } + + Link link(entity); + if (link.Valid(_ecm)) + { + return link; + } + + Model model(entity); + if (model.Valid(_ecm)) + { + return Link(model.CanonicalLink(_ecm)); + } + + gzerr << "Wrench can only be applied to a link or a model. Entity [" + << entity << "] isn't either of them." << std::endl; + return Link(); +} + +////////////////////////////////////////////////// +ApplyLinkWrench::ApplyLinkWrench() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void ApplyLinkWrench::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + auto world = World(_entity); + if (!world.Valid(_ecm)) + { + gzerr << "ApplyLinkWrench system should be attached to a world." + << std::endl; + return; + } + + this->dataPtr->verbose = _sdf->Get("verbose", true).first; + + // Initial wrenches + auto ptr = const_cast(_sdf.get()); + for (auto elem = ptr->GetElement("persistent"); + elem != nullptr; + elem = elem->GetNextElement("persistent")) + { + msgs::EntityWrench msg; + if (!elem->HasElement("entity_name") || !elem->HasElement("entity_type")) + { + gzerr << "Skipping element missing entity name or type." + << std::endl; + continue; + } + + msg.mutable_entity()->set_name(elem->Get("entity_name")); + + auto typeStr = elem->GetElement("entity_type")->Get(); + if (typeStr == "link") + { + msg.mutable_entity()->set_type(msgs::Entity::LINK); + } + else if (typeStr == "model") + { + msg.mutable_entity()->set_type(msgs::Entity::MODEL); + } + else + { + gzerr << "Skipping element, entity type [" << typeStr + << "] not supported." << std::endl; + continue; + } + + if (elem->HasElement("force")) + { + msgs::Set(msg.mutable_wrench()->mutable_force(), + elem->GetElement("force")->Get()); + } + if (elem->HasElement("torque")) + { + msgs::Set(msg.mutable_wrench()->mutable_torque(), + elem->GetElement("torque")->Get()); + } + this->dataPtr->OnWrenchPersistent(msg); + } + + // Topic to apply wrench for one time step + // TODO(chapulina) Use AsValidTopic when merging forward + std::string topic{"/world/" + world.Name(_ecm).value() + "/wrench"}; + if (_sdf->HasElement("topic")) + topic = _sdf->Get("topic"); + + this->dataPtr->node.Subscribe(topic, &ApplyLinkWrenchPrivate::OnWrench, + this->dataPtr.get()); + + gzmsg << "Listening to instantaneous wrench commands in [" << topic << "]" + << std::endl; + + // Topic to apply wrench continuously + topic = "/world/" + world.Name(_ecm).value() + "/wrench/persistent"; + if (_sdf->HasElement("topic_persistent")) + topic = _sdf->Get("topic_persistent"); + + this->dataPtr->node.Subscribe(topic, + &ApplyLinkWrenchPrivate::OnWrenchPersistent, this->dataPtr.get()); + + gzmsg << "Listening to persistent wrench commands in [" << topic << "]" + << std::endl; + + // Topic to clear persistent wrenches + topic = "/world/" + world.Name(_ecm).value() + "/wrench/clear"; + if (_sdf->HasElement("topic_clear")) + topic = _sdf->Get("topic_clear"); + + this->dataPtr->node.Subscribe(topic, + &ApplyLinkWrenchPrivate::OnWrenchClear, this->dataPtr.get()); + + gzmsg << "Listening to wrench clear commands in [" << topic << "]" + << std::endl; +} + +////////////////////////////////////////////////// +void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("ApplyLinkWrench::PreUpdate"); + + std::lock_guard lock(this->dataPtr->mutex); + + // Clear persistent wrenches + while (!this->dataPtr->clearWrenches.empty()) + { + auto clearMsg = this->dataPtr->clearWrenches.front(); + auto clearEntity = entityFromMsg(_ecm, clearMsg); + + for (auto msgIt = this->dataPtr->persistentWrenches.begin(); + msgIt != this->dataPtr->persistentWrenches.end(); msgIt++) + { + auto persistentEntity = entityFromMsg(_ecm, msgIt->entity()); + if (persistentEntity == clearEntity) + { + this->dataPtr->persistentWrenches.erase(msgIt--); + + if (this->dataPtr->verbose) + { + gzdbg << "Clearing persistent wrench for entity [" << clearEntity + << "]" << std::endl; + } + } + } + + this->dataPtr->clearWrenches.pop(); + } + + // Only apply wrenches when not paused + if (_info.paused) + return; + + // Apply instantaneous wrenches + while (!this->dataPtr->newWrenches.empty()) + { + auto msg = this->dataPtr->newWrenches.front(); + + math::Vector3d force; + math::Vector3d torque; + auto link = decomposeMessage(_ecm, msg, force, torque); + if (!link.Valid(_ecm)) + { + gzerr << "Entity not found." << std::endl + << msg.DebugString() << std::endl; + this->dataPtr->newWrenches.pop(); + continue; + } + + link.AddWorldWrench(_ecm, force, torque); + + if (this->dataPtr->verbose) + { + gzdbg << "Applying wrench [" << force << " " << torque << "] to entity [" + << link.Entity() << "] for 1 time step." << std::endl; + } + + this->dataPtr->newWrenches.pop(); + } + + // Apply persistent wrenches at every time step + for (auto msg : this->dataPtr->persistentWrenches) + { + math::Vector3d force; + math::Vector3d torque; + auto link = decomposeMessage(_ecm, msg, force, torque); + if (!link.Valid(_ecm)) + { + // Not an error, persistent wrenches can be applied preemptively before + // an entity is inserted + continue; + } + link.AddWorldWrench(_ecm, force, torque); + } +} + +////////////////////////////////////////////////// +void ApplyLinkWrenchPrivate::OnWrench(const msgs::EntityWrench &_msg) +{ + std::lock_guard lock(this->mutex); + + if (!_msg.has_entity() || !_msg.has_wrench()) + { + gzerr << "Missing entity or wrench in message: " << std::endl + << _msg.DebugString() << std::endl; + return; + } + + this->newWrenches.push(_msg); +} + +////////////////////////////////////////////////// +void ApplyLinkWrenchPrivate::OnWrenchPersistent(const msgs::EntityWrench &_msg) +{ + std::lock_guard lock(this->mutex); + + if (!_msg.has_entity() || !_msg.has_wrench()) + { + gzerr << "Missing entity or wrench in message: " << std::endl + << _msg.DebugString() << std::endl; + return; + } + + if (this->verbose) + { + gzdbg << "Queueing persistent wrench:" << std::endl + << _msg.DebugString() << std::endl; + } + + this->persistentWrenches.push_back(_msg); +} + +////////////////////////////////////////////////// +void ApplyLinkWrenchPrivate::OnWrenchClear(const msgs::Entity &_msg) +{ + std::lock_guard lock(this->mutex); + + this->clearWrenches.push(_msg); +} + +GZ_ADD_PLUGIN(ApplyLinkWrench, + System, + ApplyLinkWrench::ISystemConfigure, + ApplyLinkWrench::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(ApplyLinkWrench, + "gz::sim::systems::ApplyLinkWrench") + +// TODO(CH3): Deprecated, remove on version 8 +GZ_ADD_PLUGIN_ALIAS(ApplyLinkWrench, + "ignition::gazebo::systems::ApplyLinkWrench") diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.hh b/src/systems/apply_link_wrench/ApplyLinkWrench.hh new file mode 100644 index 0000000000..625db47578 --- /dev/null +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.hh @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2022 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 GZ_SIM_SYSTEMS_APPLYLINKWRENCH_HH_ +#define GZ_SIM_SYSTEMS_APPLYLINKWRENCH_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class ApplyLinkWrenchPrivate; + + /// \brief Exposes transport topics and SDF params for applying forces and + /// torques to links in simulation. It should be attached to a world. + /// + /// The target link is defined in each message. If a link entity is provided, + /// that will receive a wrench. If a model is provided, its canonical link + /// will receive it. No other entity types are supported. + /// + /// ## Topics + /// + /// * /world//wrench + /// * Message type: msgs::EntityWrench + /// * Effect: Applies the given wrench during a single time step. + /// + /// * /world//wrench/persistent + /// * Message type: msgs::EntityWrench + /// * Effect: Keeps applying the given wrench every time step. Persistent + /// wrenches can be applied to entities that aren't in + /// simulation yet, and will start taking effect once they do. + /// + /// * /world//wrench/clear + /// * Message type: msgs::Entity + /// * Effect: Clears any persistent wrenches that are being applied to + /// the given entity. + /// + /// ## System Parameters + /// + /// Persistent wrenches can be defined from SDF, for example: + /// + /// ``` + /// + /// box + /// model + /// -10 0 0 + /// 0 0 0.1 + /// + /// ``` + class ApplyLinkWrench + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: ApplyLinkWrench(); + + /// \brief Destructor + public: ~ApplyLinkWrench() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/apply_link_wrench/CMakeLists.txt b/src/systems/apply_link_wrench/CMakeLists.txt new file mode 100644 index 0000000000..f88520aaf6 --- /dev/null +++ b/src/systems/apply_link_wrench/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_system(apply-link-wrench + SOURCES + ApplyLinkWrench.cc +) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index b1191b2f16..1c8da9c7bb 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -571,7 +571,7 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) if (this->dataPtr->running && this->dataPtr->initialized) { - igndbg << "Resetting Sensors\n"; + gzdbg << "Resetting Sensors\n"; { std::unique_lock lock(this->dataPtr->sensorMaskMutex); @@ -584,7 +584,7 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) if (nullptr == s) { - ignwarn << "Sensor removed before reset: " << id << "\n"; + gzwarn << "Sensor removed before reset: " << id << "\n"; continue; } diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 8df9c6c384..f51daef673 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -92,62 +92,6 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { - -/// \brief Helper function to get an entity from an entity message. -/// -/// \TODO(anyone) Move to Util.hh and generalize for all entities, -/// not only top level -/// -/// The message is used as follows: -/// -/// if id not null -/// use id -/// else if name not null and type not null -/// use name + type -/// else -/// error -/// end -/// \param[in] _ecm Entity component manager -/// \param[in] _msg Entity message -/// \return Entity ID, or kNullEntity if a matching entity couldn't be -/// found. -Entity topLevelEntityFromMessage(const EntityComponentManager &_ecm, - const msgs::Entity &_msg) -{ - if (_msg.id() != kNullEntity) - { - return _msg.id(); - } - - if (!_msg.name().empty() && _msg.type() != msgs::Entity::NONE) - { - Entity entity{kNullEntity}; - if (_msg.type() == msgs::Entity::MODEL) - { - entity = _ecm.EntityByComponents(components::Model(), - components::Name(_msg.name())); - } - else if (_msg.type() == msgs::Entity::LIGHT) - { - entity = _ecm.EntityByComponents( - components::Name(_msg.name())); - - auto lightComp = _ecm.Component(entity); - if (nullptr == lightComp) - entity = kNullEntity; - } - else - { - gzerr << "Failed to handle entity type [" << _msg.type() << "]" - << std::endl; - } - return entity; - } - - gzerr << "Message missing either entity's ID or name + type" << std::endl; - return kNullEntity; -} - /// \brief This class is passed to every command and contains interfaces that /// can be shared among all commands. For example, all create and remove /// commands can use the `creator` object. @@ -1338,7 +1282,7 @@ bool RemoveCommand::Execute() return false; } - auto entity = topLevelEntityFromMessage(*this->iface->ecm, *removeMsg); + auto entity = entityFromMsg(*this->iface->ecm, *removeMsg); if (entity == kNullEntity) { gzerr << "Entity named [" << removeMsg->name() << "] of type [" @@ -1612,7 +1556,7 @@ bool SphericalCoordinatesCommand::Execute() } // Entity - auto entity = topLevelEntityFromMessage(*this->iface->ecm, + auto entity = entityFromMsg(*this->iface->ecm, sphericalCoordinatesMsg->entity()); if (!this->iface->ecm->HasEntity(entity)) @@ -1782,13 +1726,34 @@ bool VisualCommand::Execute() return false; } - if (visualMsg->id() == kNullEntity) + Entity visualEntity = kNullEntity; + if (visualMsg->id() != kNullEntity) + { + visualEntity = visualMsg->id(); + } + else if (!visualMsg->name().empty() && !visualMsg->parent_name().empty()) + { + Entity parentEntity = + this->iface->ecm->EntityByComponents( + components::Name(visualMsg->parent_name())); + + auto entities = + this->iface->ecm->ChildrenByComponents(parentEntity, + components::Name(visualMsg->name())); + + // When size > 1, we don't know which entity to modify + if (entities.size() == 1) + { + visualEntity = entities[0]; + } + } + + if (visualEntity == kNullEntity) { gzerr << "Failed to find visual entity" << std::endl; return false; } - Entity visualEntity = visualMsg->id(); auto visualCmdComp = this->iface->ecm->Component(visualEntity); if (!visualCmdComp) @@ -1813,69 +1778,6 @@ WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, { } -// TODO(ivanpauno): Move this somewhere else -Entity scopedEntityFromMsg( - const msgs::Entity & _msg, const EntityComponentManager & _ecm) -{ - if (_msg.id() != kNullEntity) { - return _msg.id(); - } - std::unordered_set entities = entitiesFromScopedName( - _msg.name(), _ecm); - if (entities.empty()) { - gzerr << "Failed to find entity with scoped name [" << _msg.name() - << "]." << std::endl; - return kNullEntity; - } - if (_msg.type() == msgs::Entity::NONE) { - return *entities.begin(); - } - const components::BaseComponent * component; - std::string componentType; - for (const auto entity : entities) { - switch (_msg.type()) { - case msgs::Entity::LIGHT: - component = _ecm.Component(entity); - componentType = "LIGHT"; - break; - case msgs::Entity::MODEL: - component = _ecm.Component(entity); - componentType = "MODEL"; - break; - case msgs::Entity::LINK: - component = _ecm.Component(entity); - componentType = "LINK"; - break; - case msgs::Entity::VISUAL: - component = _ecm.Component(entity); - componentType = "VISUAL"; - break; - case msgs::Entity::COLLISION: - component = _ecm.Component(entity); - componentType = "COLLISION"; - break; - case msgs::Entity::SENSOR: - component = _ecm.Component(entity); - componentType = "SENSOR"; - break; - case msgs::Entity::JOINT: - component = _ecm.Component(entity); - componentType = "JOINT"; - break; - default: - componentType = "unknown"; - break; - } - if (component != nullptr) { - return entity; - } - } - gzerr << "Found entity with scoped name [" << _msg.name() - << "], but it doesn't have a component of the required type [" - << componentType << "]." << std::endl; - return kNullEntity; -} - ////////////////////////////////////////////////// bool WheelSlipCommand::Execute() { @@ -1887,7 +1789,7 @@ bool WheelSlipCommand::Execute() return false; } const auto & ecm = *this->iface->ecm; - Entity entity = scopedEntityFromMsg(wheelSlipMsg->entity(), ecm); + Entity entity = entityFromMsg(ecm, wheelSlipMsg->entity()); if (kNullEntity == entity) { return false; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 3e63b412ab..2d5babb5ae 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -5,6 +5,7 @@ set(tests air_pressure_system.cc altimeter_system.cc apply_joint_force_system.cc + apply_link_wrench_system.cc battery_plugin.cc breadcrumbs.cc buoyancy.cc diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index a8822f5910..b05d8647c2 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -15,7 +15,10 @@ * */ +#include #include +#include + #include #include #include @@ -309,6 +312,126 @@ TEST_P(AckermannSteeringTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SkidPublishCmd)) EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); } +///////////////////////////////////////////////// +TEST_P(AckermannSteeringTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "ackermann_steering_slow_odom.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, + const sim::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // Publish command and check that vehicle moved + double period{1.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + // Capture Tf data to compare to odom + double periodTf{1.0}; + double lastMsgTimeTf{1.0}; + std::vector tfPoses; + std::function tfCb = + [&](const msgs::Pose_V &_msg) + { + ASSERT_TRUE(_msg.pose().Get(0).has_header()); + + double msgTime = + static_cast(_msg.pose().Get(0).header().stamp().sec()) + + static_cast(_msg.pose().Get(0).header().stamp().nsec()) * + 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTimeTf + periodTf); + lastMsgTimeTf = msgTime; + + // Use position pose to match odom (index 0) + tfPoses.push_back(msgs::Convert(_msg.pose().Get(0))); + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/odometry", odomCb); + node.Subscribe("/model/vehicle/tf", tfCb); + + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2)); + + pub.Publish(msg); + + server.Run(true, 3000, false); + + // Poses for 4s + EXPECT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 3 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + + // There should have been the same amount of odom and tf + ASSERT_FALSE(odomPoses.empty()); + ASSERT_FALSE(tfPoses.empty()); + ASSERT_EQ(odomPoses.size(), tfPoses.size()); + + // Ensure all data is equal between the two topics + for (uint64_t i = 0; i < odomPoses.size(); i++) + { + ASSERT_EQ(odomPoses[i], tfPoses[i]); + } +} + ///////////////////////////////////////////////// TEST_P(AckermannSteeringTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId)) { diff --git a/test/integration/altimeter_system.cc b/test/integration/altimeter_system.cc index 1a9b5237d0..0b94217e1f 100644 --- a/test/integration/altimeter_system.cc +++ b/test/integration/altimeter_system.cc @@ -60,8 +60,9 @@ void altimeterCb(const msgs::Altimeter &_msg) ///////////////////////////////////////////////// // The test checks the world pose and sensor readings of a falling altimeter -// See https://github.com/gazebosim/gz-sim/issues/1175 -TEST_F(AltimeterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFalling)) +// See: https://github.com/gazebosim/gz-sim/issues/1175 +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_F(AltimeterTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ModelFalling)) { // Start server ServerConfig serverConfig; diff --git a/test/integration/apply_link_wrench_system.cc b/test/integration/apply_link_wrench_system.cc new file mode 100644 index 0000000000..9c81fd84c7 --- /dev/null +++ b/test/integration/apply_link_wrench_system.cc @@ -0,0 +1,344 @@ +/* + * Copyright (C) 2022 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 "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" + +#include "gz/sim/components/ExternalWorldWrenchCmd.hh" +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/TestFixture.hh" +#include "test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +#define tol 10e-4 + +using namespace gz; +using namespace sim; +using namespace std::chrono_literals; + +/// \brief Test fixture for ApplyLinkWrench system +class ApplyLinkWrenchTestFixture : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(ApplyLinkWrenchTestFixture, FromSdf) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "apply_link_wrench.sdf")); + + std::size_t iterations{0}; + Link link1, link2; + fixture.OnConfigure([&]( + const Entity &, + const std::shared_ptr &, + EntityComponentManager &_ecm, + EventManager &) + { + Model model1(_ecm.EntityByComponents(components::Model(), + components::Name("model1"))); + EXPECT_TRUE(model1.Valid(_ecm)); + + link1 = Link(model1.CanonicalLink(_ecm)); + EXPECT_TRUE(link1.Valid(_ecm)); + link1.EnableAccelerationChecks(_ecm); + + Model model2(_ecm.EntityByComponents(components::Model(), + components::Name("model2"))); + EXPECT_TRUE(model2.Valid(_ecm)); + + link2 = Link(model2.CanonicalLink(_ecm)); + EXPECT_TRUE(link2.Valid(_ecm)); + link2.EnableAccelerationChecks(_ecm); + }) + .OnPostUpdate([&]( + const UpdateInfo &, + const EntityComponentManager &_ecm) + { + auto wrenchComp1 = _ecm.Component( + link1.Entity()); + auto wrenchComp2 = _ecm.Component( + link2.Entity()); + + EXPECT_NE(nullptr, wrenchComp1); + EXPECT_NE(nullptr, wrenchComp2); + + auto linAccel1 = link1.WorldLinearAcceleration(_ecm); + ASSERT_TRUE(linAccel1.has_value()); + EXPECT_NEAR(50.0, linAccel1.value().X(), tol); + + auto linAccel2 = link2.WorldLinearAcceleration(_ecm); + ASSERT_TRUE(linAccel2.has_value()); + EXPECT_NEAR(-100.0, linAccel2.value().X(), tol); + + ++iterations; + }).Finalize(); + + std::size_t targetIterations{100}; + fixture.Server()->Run(true, targetIterations, false); + EXPECT_EQ(targetIterations, iterations); +} + +///////////////////////////////////////////////// +TEST_F(ApplyLinkWrenchTestFixture, PersistentFromTopic) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "apply_link_wrench.sdf")); + + std::size_t iterations{0}; + std::size_t movingIterations{0}; + std::size_t clearedIterations{0}; + bool wrenchesCleared{false}; + Link link3, link4; + fixture.OnConfigure([&]( + const Entity &, + const std::shared_ptr &, + EntityComponentManager &_ecm, + EventManager &) + { + Model model3(_ecm.EntityByComponents(components::Model(), + components::Name("model3"))); + EXPECT_TRUE(model3.Valid(_ecm)); + + link3 = Link(model3.CanonicalLink(_ecm)); + EXPECT_TRUE(link3.Valid(_ecm)); + link3.EnableAccelerationChecks(_ecm); + + Model model4(_ecm.EntityByComponents(components::Model(), + components::Name("model4"))); + EXPECT_TRUE(model4.Valid(_ecm)); + + link4 = Link(model4.CanonicalLink(_ecm)); + EXPECT_TRUE(link4.Valid(_ecm)); + link4.EnableAccelerationChecks(_ecm); + }) + .OnPostUpdate([&]( + const UpdateInfo &, + const EntityComponentManager &_ecm) + { + auto wrenchComp3 = _ecm.Component( + link3.Entity()); + EXPECT_NE(nullptr, wrenchComp3); + + auto wrenchComp4 = _ecm.Component( + link4.Entity()); + EXPECT_NE(nullptr, wrenchComp4); + + auto linAccel3 = link3.WorldLinearAcceleration(_ecm); + ASSERT_TRUE(linAccel3.has_value()); + + auto linAccel4 = link4.WorldLinearAcceleration(_ecm); + ASSERT_TRUE(linAccel4.has_value()); + + if (!wrenchesCleared) + { + EXPECT_NEAR(50.0, linAccel3.value().X(), tol); + EXPECT_NEAR(-100.0, linAccel4.value().X(), tol); + + ++movingIterations; + } + else + { + EXPECT_NEAR(0.0, linAccel3.value().X(), tol); + EXPECT_NEAR(0.0, linAccel4.value().X(), tol); + + ++clearedIterations; + } + + ++iterations; + }).Finalize(); + + // Publish messages + transport::Node node; + auto pubPersistent = node.Advertise( + "/world/apply_link_wrench/wrench/persistent"); + + int sleep{0}; + int maxSleep{30}; + for (; !pubPersistent.HasConnections() && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + EXPECT_TRUE(pubPersistent.HasConnections()); + + { + msgs::EntityWrench msg; + msg.mutable_entity()->set_name("model3"); + msg.mutable_entity()->set_type(msgs::Entity::MODEL); + msg.mutable_wrench()->mutable_force()->set_x(50); + msg.mutable_wrench()->mutable_torque()->set_z(0.5); + pubPersistent.Publish(msg); + } + + { + msgs::EntityWrench msg; + msg.mutable_entity()->set_name("model4::link"); + msg.mutable_entity()->set_type(msgs::Entity::LINK); + msg.mutable_wrench()->mutable_force()->set_x(-100); + msg.mutable_wrench()->mutable_torque()->set_z(-1.0); + pubPersistent.Publish(msg); + } + + std::size_t targetIterations{100}; + fixture.Server()->Run(true, targetIterations, false); + EXPECT_EQ(targetIterations, iterations); + EXPECT_EQ(movingIterations, iterations); + + // Clear wrenches + auto pubClear = node.Advertise( + "/world/apply_link_wrench/wrench/clear"); + EXPECT_TRUE(pubClear.HasConnections()); + + { + msgs::Entity msg; + msg.set_name("model3"); + msg.set_type(msgs::Entity::MODEL); + pubClear.Publish(msg); + } + + { + msgs::Entity msg; + msg.set_name("model4::link"); + msg.set_type(msgs::Entity::LINK); + pubClear.Publish(msg); + } + + // \todo(chapulina) Arbitrarily sleeping here isn't very robust + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + wrenchesCleared = true; + fixture.Server()->Run(true, targetIterations, false); + EXPECT_EQ(targetIterations * 2, iterations); + EXPECT_EQ(movingIterations, targetIterations); + EXPECT_EQ(clearedIterations, targetIterations); +} + +///////////////////////////////////////////////// +TEST_F(ApplyLinkWrenchTestFixture, InstantaneousFromTopic) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "apply_link_wrench.sdf")); + + std::size_t iterations{0}; + std::size_t impulseIterations{0}; + Link link3, link4; + fixture.OnConfigure([&]( + const Entity &, + const std::shared_ptr &, + EntityComponentManager &_ecm, + EventManager &) + { + Model model3(_ecm.EntityByComponents(components::Model(), + components::Name("model3"))); + EXPECT_TRUE(model3.Valid(_ecm)); + + link3 = Link(model3.CanonicalLink(_ecm)); + EXPECT_TRUE(link3.Valid(_ecm)); + link3.EnableAccelerationChecks(_ecm); + + Model model4(_ecm.EntityByComponents(components::Model(), + components::Name("model4"))); + EXPECT_TRUE(model4.Valid(_ecm)); + + link4 = Link(model4.CanonicalLink(_ecm)); + EXPECT_TRUE(link4.Valid(_ecm)); + link4.EnableAccelerationChecks(_ecm); + }) + .OnPostUpdate([&]( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + auto wrenchComp3 = _ecm.Component( + link3.Entity()); + EXPECT_NE(nullptr, wrenchComp3); + + auto wrenchComp4 = _ecm.Component( + link4.Entity()); + EXPECT_NE(nullptr, wrenchComp4); + + auto linAccel3 = link3.WorldLinearAcceleration(_ecm); + ASSERT_TRUE(linAccel3.has_value()); + + auto linAccel4 = link4.WorldLinearAcceleration(_ecm); + ASSERT_TRUE(linAccel4.has_value()); + + if (_info.iterations == 1) + { + EXPECT_NEAR(50.0, linAccel3.value().X(), tol); + EXPECT_NEAR(-100.0, linAccel4.value().X(), tol); + + ++impulseIterations; + } + else + { + EXPECT_NEAR(0.0, linAccel3.value().X(), tol); + EXPECT_NEAR(0.0, linAccel4.value().X(), tol); + } + + ++iterations; + }).Finalize(); + + // Publish messages + transport::Node node; + auto pubWrench = node.Advertise( + "/world/apply_link_wrench/wrench"); + + int sleep{0}; + int maxSleep{30}; + for (; !pubWrench.HasConnections() && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + EXPECT_TRUE(pubWrench.HasConnections()); + + { + msgs::EntityWrench msg; + msg.mutable_entity()->set_name("model3"); + msg.mutable_entity()->set_type(msgs::Entity::MODEL); + msg.mutable_wrench()->mutable_force()->set_x(50); + msg.mutable_wrench()->mutable_torque()->set_z(0.5); + pubWrench.Publish(msg); + } + + { + msgs::EntityWrench msg; + msg.mutable_entity()->set_name("model4::link"); + msg.mutable_entity()->set_type(msgs::Entity::LINK); + msg.mutable_wrench()->mutable_force()->set_x(-100); + msg.mutable_wrench()->mutable_torque()->set_z(-1.0); + pubWrench.Publish(msg); + } + + std::size_t targetIterations{20}; + fixture.Server()->Run(true, targetIterations, false); + EXPECT_EQ(targetIterations, iterations); + EXPECT_EQ(1u, impulseIterations); +} + diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 2b2f49ec44..35ce6d2703 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -211,8 +211,9 @@ class DiffDriveTest : public InternalFixture<::testing::TestWithParam> }; ///////////////////////////////////////////////// -// See https://github.com/gazebosim/gz-sim/issues/1175 -TEST_P(DiffDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) +// See: https://github.com/gazebosim/gz-sim/issues/1175 +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_P(DiffDriveTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmd)) { TestPublishCmd( std::string(PROJECT_SOURCE_PATH) + "/test/worlds/diff_drive.sdf", @@ -220,7 +221,9 @@ TEST_P(DiffDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) } ///////////////////////////////////////////////// -TEST_P(DiffDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmdCustomTopics)) +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_P(DiffDriveTest, + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmdCustomTopics)) { TestPublishCmd( std::string(PROJECT_SOURCE_PATH) + diff --git a/test/integration/level_manager.cc b/test/integration/level_manager.cc index 815921a61f..0031a95239 100644 --- a/test/integration/level_manager.cc +++ b/test/integration/level_manager.cc @@ -179,8 +179,9 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> ///////////////////////////////////////////////// /// Check default level includes entities not included by other levels -// See https://github.com/gazebosim/gz-sim/issues/1175 -TEST_F(LevelManagerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(DefaultLevel)) +// See: https://github.com/gazebosim/gz-sim/issues/1175 +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_F(LevelManagerFixture, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(DefaultLevel)) { std::vector> levelEntityNamesList; @@ -432,8 +433,9 @@ TEST_F(LevelManagerFixture, /////////////////////////////////////////////// /// Check that buffers work properly with multiple performers +// See: https://github.com/gazebosim/gz-sim/issues/630 TEST_F(LevelManagerFixture, - GZ_UTILS_TEST_DISABLED_ON_WIN32(LevelBuffersWithMultiplePerformers)) + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(LevelBuffersWithMultiplePerformers)) { ModelMover perf1(*this->server->EntityByName("sphere")); ModelMover perf2(*this->server->EntityByName("box")); diff --git a/test/integration/logical_audio_sensor_plugin.cc b/test/integration/logical_audio_sensor_plugin.cc index 1be85ec72b..be992a947a 100644 --- a/test/integration/logical_audio_sensor_plugin.cc +++ b/test/integration/logical_audio_sensor_plugin.cc @@ -207,7 +207,9 @@ TEST_F(LogicalAudioTest, "world/logical_audio_sensor/model/source_model/sensor/source_1"); } -TEST_F(LogicalAudioTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogicalAudioServices)) +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_F(LogicalAudioTest, + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(LogicalAudioServices)) { ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + diff --git a/test/integration/model.cc b/test/integration/model.cc index cf7ba644e4..6d403fb32a 100644 --- a/test/integration/model.cc +++ b/test/integration/model.cc @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -232,3 +233,32 @@ TEST_F(ModelIntegrationTest, SetWorldPoseCmd) EXPECT_TRUE(ecm.HasOneTimeComponentChanges()); } +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, CanonicalLink) +{ + EntityComponentManager ecm; + + // Model + auto eModel = ecm.CreateEntity(); + Model model(eModel); + EXPECT_EQ(eModel, model.Entity()); + EXPECT_EQ(0u, model.LinkCount(ecm)); + + // Links + auto canonicalLink = ecm.CreateEntity(); + ecm.CreateComponent(canonicalLink, components::Link()); + ecm.CreateComponent(canonicalLink, components::CanonicalLink()); + ecm.CreateComponent(canonicalLink, + components::ParentEntity(eModel)); + ecm.CreateComponent(canonicalLink, components::Name("canonical")); + + auto anotherLink = ecm.CreateEntity(); + ecm.CreateComponent(anotherLink, components::Link()); + ecm.CreateComponent(anotherLink, components::ParentEntity(eModel)); + ecm.CreateComponent(anotherLink, components::Name("another")); + + // Check model + EXPECT_EQ(canonicalLink, model.CanonicalLink(ecm)); + EXPECT_EQ(2u, model.LinkCount(ecm)); +} + diff --git a/test/integration/multiple_servers.cc b/test/integration/multiple_servers.cc index eba1fbf675..8840927349 100644 --- a/test/integration/multiple_servers.cc +++ b/test/integration/multiple_servers.cc @@ -17,6 +17,8 @@ #include +#include + #include "gz/sim/Server.hh" #include "gz/sim/ServerConfig.hh" @@ -32,7 +34,8 @@ class MultipleServers : public InternalFixture<::testing::TestWithParam> }; ///////////////////////////////////////////////// -TEST_P(MultipleServers, TwoServersNonBlocking) +// See: https://github.com/gazebosim/gz-sim/issues/1544 +TEST_P(MultipleServers, GZ_UTILS_TEST_DISABLED_ON_MAC(TwoServersNonBlocking)) { gz::sim::ServerConfig serverConfig; serverConfig.SetSdfString(TestWorldSansPhysics::World()); @@ -72,7 +75,8 @@ TEST_P(MultipleServers, TwoServersNonBlocking) } ///////////////////////////////////////////////// -TEST_P(MultipleServers, TwoServersMixedBlocking) +// See: https://github.com/gazebosim/gz-sim/issues/1544 +TEST_P(MultipleServers, GZ_UTILS_TEST_DISABLED_ON_MAC(TwoServersMixedBlocking)) { gz::sim::ServerConfig serverConfig; serverConfig.SetSdfString(TestWorldSansPhysics::World()); diff --git a/test/integration/network_handshake.cc b/test/integration/network_handshake.cc index 63bbd6abbb..c51a7b3188 100644 --- a/test/integration/network_handshake.cc +++ b/test/integration/network_handshake.cc @@ -125,7 +125,8 @@ TEST_F(NetworkHandshake, GZ_UTILS_TEST_DISABLED_ON_WIN32(Handshake)) } ///////////////////////////////////////////////// -TEST_F(NetworkHandshake, GZ_UTILS_TEST_DISABLED_ON_WIN32(Updates)) +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_F(NetworkHandshake, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Updates)) { auto pluginElem = std::make_shared(); pluginElem->SetName("plugin"); diff --git a/test/integration/performer_detector.cc b/test/integration/performer_detector.cc index 2f0491cc61..d79a4fb9ad 100644 --- a/test/integration/performer_detector.cc +++ b/test/integration/performer_detector.cc @@ -61,8 +61,10 @@ class PerformerDetectorTest : public InternalFixture<::testing::Test> ///////////////////////////////////////////////// // Test that commanded motor speed is applied -// See https://github.com/gazebosim/gz-sim/issues/1175 -TEST_F(PerformerDetectorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MovingPerformer)) +// See: https://github.com/gazebosim/gz-sim/issues/1175 +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_F(PerformerDetectorTest, + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MovingPerformer)) { auto server = this->StartServer("/test/worlds/performer_detector.sdf"); diff --git a/test/integration/pose_publisher_system.cc b/test/integration/pose_publisher_system.cc index dd093e458c..a36d2c64e2 100644 --- a/test/integration/pose_publisher_system.cc +++ b/test/integration/pose_publisher_system.cc @@ -322,7 +322,9 @@ TEST_F(PosePublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) } ///////////////////////////////////////////////// -TEST_F(PosePublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(UpdateFrequency)) +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_F(PosePublisherTest, + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(UpdateFrequency)) { // Start server ServerConfig serverConfig; @@ -643,8 +645,9 @@ TEST_F(PosePublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(StaticPosePublisher)) } ///////////////////////////////////////////////// +// See: https://github.com/gazebosim/gz-sim/issues/630 TEST_F(PosePublisherTest, - GZ_UTILS_TEST_DISABLED_ON_WIN32(StaticPoseUpdateFrequency)) + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(StaticPoseUpdateFrequency)) { // Start server ServerConfig serverConfig; diff --git a/test/integration/touch_plugin.cc b/test/integration/touch_plugin.cc index 8e849943ca..58c30d4d05 100644 --- a/test/integration/touch_plugin.cc +++ b/test/integration/touch_plugin.cc @@ -187,7 +187,9 @@ TEST_F(TouchPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(StartDisabled)) } ////////////////////////////////////////////////// -TEST_F(TouchPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RemovalOfParentModel)) +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_F(TouchPluginTest, + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(RemovalOfParentModel)) { this->StartServer("/test/worlds/touch_plugin.sdf"); diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 3f896e85ec..d7e6c9e6ad 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -29,10 +30,12 @@ #include "gz/sim/components/Light.hh" #include "gz/sim/components/Link.hh" +#include "gz/sim/components/Material.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/Physics.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/VisualCmd.hh" #include "gz/sim/components/WheelSlipCmd.hh" #include "gz/sim/components/World.hh" #include "gz/sim/Model.hh" @@ -1215,4 +1218,91 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(WheelSlip)) // Run two iterations, in the first one the WheelSlipCmd component is created // and processed. // The second one is just to check everything went fine. - server.Run(true, 3, false);} + server.Run(true, 3, false); +} + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Visual)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "test", "worlds", "shapes.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const sim::UpdateInfo &, + sim::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + ASSERT_NE(nullptr, ecm); + + msgs::Visual req; + msgs::Boolean res; + transport::Node node; + bool result; + unsigned int timeout = 100; + std::string service{"/world/default/visual_config"}; + + auto boxVisualEntity = + ecm->EntityByComponents(components::Name("box_visual")); + ASSERT_NE(kNullEntity, boxVisualEntity); + + // check box visual's initial values + auto boxVisualComp = ecm->Component(boxVisualEntity); + ASSERT_NE(nullptr, boxVisualComp); + EXPECT_EQ(math::Color(1.0f, 0.0f, 0.0f, 1.0f), + boxVisualComp->Data().Diffuse()); + + msgs::Set(req.mutable_material()->mutable_diffuse(), + math::Color(0.0f, 1.0f, 0.0f, 1.0f)); + + // This will fail to find the entity in VisualCommand::Execute() + // since no id was provided + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + server.Run(true, 1, false); + // check that the VisualCmd component was not created + auto boxVisCmdComp = ecm->Component(boxVisualEntity); + EXPECT_EQ(nullptr, boxVisCmdComp); + + // add id to msg and resend request + req.set_id(boxVisualEntity); + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + server.Run(true, 1, false); + // check the VisualCmd was created and check the values + boxVisCmdComp = ecm->Component(boxVisualEntity); + ASSERT_NE(nullptr, boxVisualComp); + EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().r()); + EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().g()); + EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().b()); + EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().a()); + + // update component using visual name and parent name + req.Clear(); + req.set_name("box_visual"); + req.set_parent_name("box_link"); + msgs::Set(req.mutable_material()->mutable_diffuse(), + math::Color(0.0f, 0.0f, 1.0f, 1.0f)); + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + server.Run(true, 1, false); + // check the values + boxVisCmdComp = ecm->Component(boxVisualEntity); + ASSERT_NE(nullptr, boxVisualComp); + EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().r()); + EXPECT_FLOAT_EQ(0.0f, boxVisCmdComp->Data().material().diffuse().g()); + EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().b()); + EXPECT_FLOAT_EQ(1.0f, boxVisCmdComp->Data().material().diffuse().a()); +} diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc index dd59aed8d4..c355d4ba4d 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -231,8 +231,9 @@ class VelocityControlTest }; ///////////////////////////////////////////////// -// See https://github.com/gazebosim/gz-sim/issues/1175 -TEST_P(VelocityControlTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) +// See: https://github.com/gazebosim/gz-sim/issues/1175 +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_P(VelocityControlTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmd)) { TestPublishCmd( std::string(PROJECT_SOURCE_PATH) + "/test/worlds/velocity_control.sdf", diff --git a/test/worlds/apply_link_wrench.sdf b/test/worlds/apply_link_wrench.sdf new file mode 100644 index 0000000000..10c3d6e030 --- /dev/null +++ b/test/worlds/apply_link_wrench.sdf @@ -0,0 +1,123 @@ + + + + 0 0 0 + + + + + + model1 + model + 50 0 0 + 0 0 0.5 + + + model2::link + link + -100 0 0 + 0 0 -1.0 + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 0 2 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 0 4 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 0 6 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + +