From 3a7d09ca8335289662a6eedf3267a99c0a18c1c0 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 12 May 2021 14:08:26 -0700 Subject: [PATCH 01/35] Add scene manager GUI plugin that works with ign-gui's Scene3D Signed-off-by: Louise Poubel --- .../ignition/gazebo/rendering/RenderUtil.hh | 4 + src/gui/gui.config | 16 ++- src/gui/plugins/CMakeLists.txt | 1 + src/gui/plugins/scene_manager/CMakeLists.txt | 7 + .../plugins/scene_manager/GzSceneManager.cc | 135 ++++++++++++++++++ .../plugins/scene_manager/GzSceneManager.hh | 66 +++++++++ .../plugins/scene_manager/GzSceneManager.qml | 28 ++++ .../plugins/scene_manager/GzSceneManager.qrc | 5 + src/rendering/RenderUtil.cc | 12 ++ 9 files changed, 273 insertions(+), 1 deletion(-) create mode 100644 src/gui/plugins/scene_manager/CMakeLists.txt create mode 100644 src/gui/plugins/scene_manager/GzSceneManager.cc create mode 100644 src/gui/plugins/scene_manager/GzSceneManager.hh create mode 100644 src/gui/plugins/scene_manager/GzSceneManager.qml create mode 100644 src/gui/plugins/scene_manager/GzSceneManager.qrc diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 8f21ff3625..daecee7422 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -92,6 +92,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Name of the rendering scene. public: std::string SceneName() const; + /// \brief Set the scene to use. + /// \param[in] _scene Pointer to the scene. + public: void SetScene(const rendering::ScenePtr &_scene); + /// \brief Set background color of render window /// \param[in] _color Color of render window background public: void SetBackgroundColor(const math::Color &_color); diff --git a/src/gui/gui.config b/src/gui/gui.config index 072f631f63..bb2b9f7e42 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -26,7 +26,7 @@ - + 3D View false @@ -40,6 +40,20 @@ 6 0 6 0 0.5 3.14 + + + + + + + false + 5 + 5 + floating + false + + + diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 6d7d140fe7..7df38dfb35 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -119,6 +119,7 @@ add_subdirectory(playback_scrubber) add_subdirectory(plotting) add_subdirectory(resource_spawner) add_subdirectory(scene3d) +add_subdirectory(scene_manager) add_subdirectory(shapes) add_subdirectory(tape_measure) add_subdirectory(transform_control) diff --git a/src/gui/plugins/scene_manager/CMakeLists.txt b/src/gui/plugins/scene_manager/CMakeLists.txt new file mode 100644 index 0000000000..6e5e884b94 --- /dev/null +++ b/src/gui/plugins/scene_manager/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_gui_plugin(GzSceneManager + SOURCES GzSceneManager.cc + QT_HEADERS GzSceneManager.hh + PRIVATE_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} +) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc new file mode 100644 index 0000000000..ee3a204a29 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "GzSceneManager.hh" + +#include + +#include + +#include +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + /// \brief Private data class for GzSceneManager + class GzSceneManagerPrivate + { + /// \brief Update the 3D scene based on the latest state of the ECM. + public: void OnRender(); + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene; + + /// \brief Name of the world + public: std::string worldName; + + /// \brief Rendering utility + public: RenderUtil renderUtil; + }; +} +} +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +GzSceneManager::GzSceneManager() + : GuiSystem(), dataPtr(std::make_unique) +{ +} + +///////////////////////////////////////////////// +GzSceneManager::~GzSceneManager() = default; + +///////////////////////////////////////////////// +void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Scene Manager"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +////////////////////////////////////////////////// +void GzSceneManager::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("GzSceneManager::Update"); + if (this->dataPtr->worldName.empty()) + { + // TODO(anyone) Only one scene is supported for now + Entity worldEntity; + _ecm.Each( + [&](const Entity &_entity, + const components::World * /* _world */ , + const components::Name *_name)->bool + { + this->dataPtr->worldName = _name->Data(); + worldEntity = _entity; + return false; + }); + } + + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); +} + +///////////////////////////////////////////////// +bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gui::events::Render::kType) + { + this->dataPtr->OnRender(); + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void GzSceneManagerPrivate::OnRender() +{ + if (nullptr == this->scene) + { + this->scene = rendering::someInitializedScene(); + if (nullptr == this->scene) + return; + + this->renderUtil.SetScene(this->scene); + } + + this->renderUtil.Update(); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::GzSceneManager, + ignition::gui::Plugin) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.hh b/src/gui/plugins/scene_manager/GzSceneManager.hh new file mode 100644 index 0000000000..ab0d5f1be0 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.hh @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_GAZEBO_GUI_GZSCENEMANAGER_HH_ +#define IGNITION_GAZEBO_GUI_GZSCENEMANAGER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + class GzSceneManagerPrivate; + + /// \brief Updates a 3D scene based on information coming from the ECM. + /// This plugin doesn't instantiate a new 3D scene. Instead, it relies on + /// another plugin being loaded alongside it that will create and paint the + /// scene to the window, such as `ignition::gui::plugins::Scene3D`. + class GzSceneManager : public GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: GzSceneManager(); + + /// \brief Destructor + public: ~GzSceneManager() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} +} + +#endif diff --git a/src/gui/plugins/scene_manager/GzSceneManager.qml b/src/gui/plugins/scene_manager/GzSceneManager.qml new file mode 100644 index 0000000000..873da30014 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.qml @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +import QtQuick 2.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 + +// TODO: remove invisible rectangle, see +// https://github.com/ignitionrobotics/ign-gui/issues/220 +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 +} diff --git a/src/gui/plugins/scene_manager/GzSceneManager.qrc b/src/gui/plugins/scene_manager/GzSceneManager.qrc new file mode 100644 index 0000000000..b28c13c3b4 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.qrc @@ -0,0 +1,5 @@ + + + GzSceneManager.qml + + diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 382a48ae86..65a076b52e 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1827,6 +1827,10 @@ void RenderUtilPrivate::RemoveRenderingEntities( ///////////////////////////////////////////////// void RenderUtil::Init() { + // Already initialized + if (nullptr != this->dataPtr->scene) + return; + ignition::common::SystemPaths pluginPath; pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); rendering::setPluginPaths(pluginPath.PluginPaths()); @@ -1927,6 +1931,14 @@ void RenderUtil::SetSceneName(const std::string &_name) this->dataPtr->sceneName = _name; } +///////////////////////////////////////////////// +void RenderUtil::SetScene(const rendering::ScenePtr &_scene) +{ + this->dataPtr->scene = _scene; + this->dataPtr->sceneManager.SetScene(_scene); + this->dataPtr->engine = _scene == nullptr ? nullptr : _scene->Engine(); +} + ///////////////////////////////////////////////// std::string RenderUtil::SceneName() const { From 80161fb7fe13c8c1dabcbe4c7af8bafa38d59549 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 12 May 2021 14:29:17 -0700 Subject: [PATCH 02/35] cleanup Signed-off-by: Louise Poubel --- .../plugins/scene_manager/GzSceneManager.cc | 21 ++----------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index ee3a204a29..7491687688 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -47,9 +47,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { //// \brief Pointer to the rendering scene public: rendering::ScenePtr scene; - /// \brief Name of the world - public: std::string worldName; - /// \brief Rendering utility public: RenderUtil renderUtil; }; @@ -62,7 +59,7 @@ using namespace gazebo; ///////////////////////////////////////////////// GzSceneManager::GzSceneManager() - : GuiSystem(), dataPtr(std::make_unique) + : GuiSystem(), dataPtr(std::make_unique()) { } @@ -70,7 +67,7 @@ GzSceneManager::GzSceneManager() GzSceneManager::~GzSceneManager() = default; ///////////////////////////////////////////////// -void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *) { if (this->title.empty()) this->title = "Scene Manager"; @@ -84,20 +81,6 @@ void GzSceneManager::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("GzSceneManager::Update"); - if (this->dataPtr->worldName.empty()) - { - // TODO(anyone) Only one scene is supported for now - Entity worldEntity; - _ecm.Each( - [&](const Entity &_entity, - const components::World * /* _world */ , - const components::Name *_name)->bool - { - this->dataPtr->worldName = _name->Data(); - worldEntity = _entity; - return false; - }); - } this->dataPtr->renderUtil.UpdateECM(_info, _ecm); this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); From 400974d71a16162d06e32392550fd67e5b30fb5a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 5 May 2021 17:23:54 +0200 Subject: [PATCH 03/35] Added some-process flag Signed-off-by: ahcorde --- include/ignition/gazebo/Server.hh | 2 + include/ignition/gazebo/ServerConfig.hh | 12 +- include/ignition/gazebo/gui/Gui.hh | 7 +- include/ignition/gazebo/gui/GuiRunner.hh | 4 +- .../ignition/gazebo/rendering/RenderUtil.hh | 2 +- src/Server.cc | 8 ++ src/ServerConfig.cc | 21 ++- src/SimulationRunner.cc | 5 +- src/SimulationRunner.hh | 2 +- src/cmd/cmdgazebo.rb.in | 30 ++++- src/gui/Gui.cc | 14 +- src/gui/GuiRunner.cc | 20 ++- src/ign.cc | 125 ++++++++++++++---- src/ign.hh | 31 +++++ src/rendering/RenderUtil.cc | 61 ++++++--- src/rendering/SceneManager.cc | 22 ++- src/systems/sensors/Sensors.cc | 3 + 17 files changed, 305 insertions(+), 64 deletions(-) diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index ff8edae5b6..588dfab55b 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -116,6 +116,8 @@ namespace ignition /// \brief Destructor public: ~Server(); + public: EntityComponentManager &GetEntityComponentManager(); + /// \brief Set the update period. The update period is the wall-clock time /// between ECS updates. /// Note that this is different from the simulation update rate. ECS diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 199ccdee35..0acc13ad06 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -175,6 +175,16 @@ namespace ignition /// \return The full contents of the SDF string, or empty string. public: std::string SdfString() const; + /// \brief Set if the server and GUI should run in the same process. + /// \param[in] _sameProcessAsGUI True if the server and GUI will run in + /// the same process, False otherwise + public: void SetSameProcessAsGUI(const bool &_sameProcessAsGUI); + + /// \brief Get if the server and GUI are running in the same process + /// \return True if the server and GUI will run in + /// the same process, False otherwise + public: bool SameProcessAsGUI() const; + /// \brief Set the update rate in Hertz. Value <=0 are ignored. /// \param[in] _hz The desired update rate of the server in Hertz. public: void SetUpdateRate(const double &_hz); @@ -426,7 +436,7 @@ namespace ignition /// \return A list of plugins to load, based on above ordering std::list IGNITION_GAZEBO_VISIBLE - loadPluginInfo(bool _isPlayback = false); + loadPluginInfo(bool _isPlayback = false, bool _sameProcessAsGUI = false); } } } diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index e23f99cfea..e5e387bd52 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -22,6 +22,7 @@ #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -42,7 +43,9 @@ namespace gui /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, char **_argv, - const char *_guiConfig); + const char *_guiConfig, + EntityComponentManager &_ecm, + bool sameProcess); /// \brief Create a Gazebo GUI application /// \param[in] _argc Number of command line arguments (Used when running @@ -63,6 +66,8 @@ namespace gui IGNITION_GAZEBO_GUI_VISIBLE std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, + EntityComponentManager &_ecm, + bool sameProcess, const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); } // namespace gui diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index e64b4e83fb..3a91ecc5e2 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/include/ignition/gazebo/gui/GuiRunner.hh @@ -26,6 +26,7 @@ #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -45,7 +46,8 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \param[in] _worldName World name. /// \todo Move to src/gui on v6. public: explicit IGN_DEPRECATED(5.0) GuiRunner( - const std::string &_worldName); + const std::string &_worldName, EntityComponentManager &_ecm, + bool sameProcess); /// \brief Destructor public: ~GuiRunner() override; diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index daecee7422..408d0c4d36 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -44,7 +44,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { class IGNITION_GAZEBO_RENDERING_VISIBLE RenderUtil { /// \brief Constructor - public: explicit RenderUtil(); + public: explicit RenderUtil(bool updateNewEntities); /// \brief Destructor public: ~RenderUtil(); diff --git a/src/Server.cc b/src/Server.cc index aee4486823..f7329e6f9d 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -206,6 +206,14 @@ Server::Server(const ServerConfig &_config) ///////////////////////////////////////////////// Server::~Server() = default; +EntityComponentManager &Server::GetEntityComponentManager() +{ + for (std::unique_ptr &runner : this->dataPtr->simRunners) + { + return runner->EntityCompMgr(); + } +} + ///////////////////////////////////////////////// bool Server::Run(const bool _blocking, const uint64_t _iterations, const bool _paused) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 8c051d1914..20e7ae6fe7 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -236,7 +236,8 @@ class ignition::gazebo::ServerConfigPrivate networkRole(_cfg->networkRole), networkSecondaries(_cfg->networkSecondaries), seed(_cfg->seed), - logRecordTopics(_cfg->logRecordTopics) { } + logRecordTopics(_cfg->logRecordTopics), + sameProcessAsGUI(_cfg->sameProcessAsGUI) { } // \brief The SDF file that the server should load public: std::string sdfFile = ""; @@ -301,6 +302,10 @@ class ignition::gazebo::ServerConfigPrivate /// \brief Topics to record. public: std::vector logRecordTopics; + + /// \brief Boolean to define if the server and gui run in the same process + /// True gui and server will run in the same process, False otherwise + public: bool sameProcessAsGUI; }; ////////////////////////////////////////////////// @@ -346,6 +351,18 @@ std::string ServerConfig::SdfString() const return this->dataPtr->sdfString; } +////////////////////////////////////////////////// +void ServerConfig::SetSameProcessAsGUI(const bool &_sameProcessAsGUI) +{ + this->dataPtr->sameProcessAsGUI = _sameProcessAsGUI; +} + +////////////////////////////////////////////////// +bool ServerConfig::SameProcessAsGUI() const +{ + return this->dataPtr->sameProcessAsGUI; +} + ////////////////////////////////////////////////// void ServerConfig::SetUpdateRate(const double &_hz) { @@ -824,7 +841,7 @@ ignition::gazebo::parsePluginsFromString(const std::string &_str) ///////////////////////////////////////////////// std::list -ignition::gazebo::loadPluginInfo(bool _isPlayback) +ignition::gazebo::loadPluginInfo(bool _isPlayback, bool _sameProcessAsGUI) { std::list ret; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 1e52182873..b261993cc5 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -168,7 +168,8 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, { ignmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); - auto plugins = ignition::gazebo::loadPluginInfo(isPlayback); + auto plugins = ignition::gazebo::loadPluginInfo( + isPlayback, this->serverConfig.SameProcessAsGUI()); this->LoadServerPlugins(plugins); } @@ -1172,7 +1173,7 @@ bool SimulationRunner::Paused() const } ///////////////////////////////////////////////// -const EntityComponentManager &SimulationRunner::EntityCompMgr() const +EntityComponentManager &SimulationRunner::EntityCompMgr() { return this->entityCompMgr; } diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 2c5dda2763..98bf457019 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -254,7 +254,7 @@ namespace ignition /// \brief Get the EntityComponentManager /// \return Reference to the entity component manager. - public: const EntityComponentManager &EntityCompMgr() const; + public: EntityComponentManager &EntityCompMgr(); /// \brief Return an entity with the provided name. /// \details If multiple entities with the same name exist, the first diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 30f783c6eb..069819cd87 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -54,6 +54,8 @@ COMMANDS = { 'gazebo' => " which loads all models. It's always true \n"\ " with --network-role. \n"\ "\n"\ + " --same-process Server and Client will run in the same process \n"\ + "\n"\ " --network-role [arg] Participant role used in a distributed \n"\ " simulation environment. Role is one of \n"\ " [primary, secondary]. It implies --levels. \n"\ @@ -219,6 +221,7 @@ class Cmd 'log-compress' => 0, 'playback' => '', 'run' => 0, + 'same-process' => 0, 'server' => 0, 'verbose' => '1', 'gui_config' => '', @@ -261,6 +264,9 @@ class Cmd opts.on('--levels') do options['levels'] = 1 end + opts.on('--same-process') do + options['same-process'] = 1 + end opts.on('--record') do options['record'] = 1 end @@ -436,6 +442,13 @@ has properly set the DYLD_LIBRARY_PATH environment variables." # Import the runGui function Importer.extern 'int runGui(const char *)' + # Import the runCombined function + Importer.extern 'int runCombined(const char *, int, int, float, int, + const char *, int, int, const char *, + int, int, int, const char *, const char *, + const char *, const char *, const char *, + const char *, const char *)' + # If playback is specified, and the user has not specified a # custom gui config, set the gui config to load the playback # gui config @@ -443,10 +456,23 @@ has properly set the DYLD_LIBRARY_PATH environment variables." options['gui_config'] = "_playback_" end + if options['same-process'] == 1 + + ENV['RMT_PORT'] = '1500' + Process.setproctitle('ign gazebo') + Importer.runCombined(parsed, options['iterations'], options['run'], + options['hz'], options['levels'], options['network_role'], + options['network_secondaries'], options['record'], + options['record-path'], options['record-resources'], + options['log-overwrite'], options['log-compress'], + options['playback'], options['physics_engine'], + options['render_engine_server'], options['render_engine_gui'], + options['file'], options['record-topics'].join(':'), + options['gui_config']) + # Neither the -s nor -g options were used, so run both the server # and gui. - if options['server'] == 0 && options['gui'] == 0 - + elsif options['server'] == 0 && options['gui'] == 0 if plugin.end_with? ".dylib" puts "`ign gazebo` currently only works with the -s argument on macOS. See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 58056f40cf..9329b22bd0 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -43,8 +43,11 @@ namespace gui ////////////////////////////////////////////////// std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, + EntityComponentManager &_ecm, bool sameProcess, const char *_defaultGuiConfig, bool _loadPluginsFromSdf) { + auto &sharedEcm = _ecm; + ignition::common::SignalHandler sigHandler; bool sigKilled = false; sigHandler.AddCallback([&](const int /*_sig*/) @@ -177,7 +180,7 @@ std::unique_ptr createGui( # pragma warning(push) # pragma warning(disable: 4996) #endif - auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0)); + auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0), sharedEcm, sameProcess); #ifndef _WIN32 # pragma GCC diagnostic pop #else @@ -241,7 +244,7 @@ std::unique_ptr createGui( # pragma warning(push) # pragma warning(disable: 4996) #endif - auto runner = new ignition::gazebo::GuiRunner(worldName); + auto runner = new ignition::gazebo::GuiRunner(worldName, sharedEcm, sameProcess); #ifndef _WIN32 # pragma GCC diagnostic pop #else @@ -318,9 +321,12 @@ std::unique_ptr createGui( } ////////////////////////////////////////////////// -int runGui(int &_argc, char **_argv, const char *_guiConfig) +int runGui(int &_argc, char **_argv, const char *_guiConfig, + EntityComponentManager &_ecm, bool sameProcess) { - auto app = gazebo::gui::createGui(_argc, _argv, _guiConfig); + auto &sharedEcm = _ecm; + auto app = gazebo::gui::createGui(_argc, _argv, _guiConfig, + sharedEcm, sameProcess); if (nullptr != app) { // Run main window. diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index d06032fa22..dc927da3eb 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -35,11 +35,17 @@ using namespace gazebo; ///////////////////////////////////////////////// class ignition::gazebo::GuiRunner::Implementation { + + public: explicit Implementation(gazebo::EntityComponentManager &_ecm) + : ecm(_ecm){} + /// \brief Update the plugins. public: void UpdatePlugins(); /// \brief Entity-component manager. - public: gazebo::EntityComponentManager ecm; + public: gazebo::EntityComponentManager &ecm; + + public: bool sameProcess; /// \brief Transport node. public: transport::Node node{}; @@ -61,10 +67,15 @@ class ignition::gazebo::GuiRunner::Implementation }; ///////////////////////////////////////////////// -GuiRunner::GuiRunner(const std::string &_worldName) - : dataPtr(utils::MakeUniqueImpl()) +GuiRunner::GuiRunner(const std::string &_worldName, + EntityComponentManager &_ecm, bool _sameProcess) + : dataPtr(utils::MakeUniqueImpl(_ecm)) { + this->dataPtr->sameProcess = _sameProcess; + this->setProperty("worldName", QString::fromStdString(_worldName)); + this->setProperty("sameProcess", + QString::fromStdString(std::to_string(_sameProcess))); auto win = gui::App()->findChild(); auto winWorldNames = win->property("worldNames").toStringList(); @@ -118,6 +129,9 @@ GuiRunner::~GuiRunner() ///////////////////////////////////////////////// void GuiRunner::RequestState() { + if (this->dataPtr->sameProcess) + return; + // set up service for async state response callback std::string id = std::to_string(gui::App()->applicationPid()); std::string reqSrv = diff --git a/src/ign.cc b/src/ign.cc index 9faf652e09..6c3ba8ccc5 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -31,6 +31,7 @@ #include "ignition/gazebo/config.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/ServerConfig.hh" +#include "SimulationRunner.hh" #include "ignition/gazebo/gui/Gui.hh" @@ -115,18 +116,19 @@ extern "C" const char *findFuelResource( } ////////////////////////////////////////////////// -extern "C" int runServer(const char *_sdfString, - int _iterations, int _run, float _hz, int _levels, const char *_networkRole, +int createServerConfig(ignition::gazebo::ServerConfig &_serverConfig, + const char *_sdfString, + float _hz, int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, 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, bool _sameProcessAsGUI) { - ignition::gazebo::ServerConfig serverConfig; - // Path for logs - std::string recordPathMod = serverConfig.LogRecordPath(); + std::string recordPathMod = _serverConfig.LogRecordPath(); + + _serverConfig.SetSameProcessAsGUI(_sameProcessAsGUI); // Path for compressed log, used to check for duplicates std::string cmpPath = std::string(recordPathMod); @@ -148,8 +150,8 @@ extern "C" int runServer(const char *_sdfString, return -1; } - serverConfig.SetUseLogRecord(true); - serverConfig.SetLogRecordResources(_recordResources); + _serverConfig.SetUseLogRecord(true); + _serverConfig.SetLogRecordResources(_recordResources); // If a record path is specified if (_recordPath != nullptr && std::strlen(_recordPath) > 0) @@ -261,23 +263,23 @@ extern "C" int runServer(const char *_sdfString, ignmsg << "Recording states to default path [" << recordPathMod << "]" << std::endl; } - serverConfig.SetLogRecordPath(recordPathMod); + _serverConfig.SetLogRecordPath(recordPathMod); std::vector topics = ignition::common::split( _recordTopics, ":"); for (const std::string &topic : topics) { - serverConfig.AddLogRecordTopic(topic); + _serverConfig.AddLogRecordTopic(topic); } } else { - ignLogInit(serverConfig.LogRecordPath(), "server_console.log"); + ignLogInit(_serverConfig.LogRecordPath(), "server_console.log"); } if (_logCompress > 0) { - serverConfig.SetLogRecordCompressPath(cmpPath); + _serverConfig.SetLogRecordCompressPath(cmpPath); } ignmsg << "Ignition Gazebo Server v" << IGNITION_GAZEBO_VERSION_FULL @@ -286,31 +288,31 @@ extern "C" int runServer(const char *_sdfString, // Set the SDF string to user if (_sdfString != nullptr && std::strlen(_sdfString) > 0) { - if (!serverConfig.SetSdfString(_sdfString)) + if (!_serverConfig.SetSdfString(_sdfString)) { ignerr << "Failed to set SDF string [" << _sdfString << "]" << std::endl; return -1; } } - serverConfig.SetSdfFile(_file); + _serverConfig.SetSdfFile(_file); // Set the update rate. if (_hz > 0.0) - serverConfig.SetUpdateRate(_hz); + _serverConfig.SetUpdateRate(_hz); // Set whether levels should be used. if (_levels > 0) { ignmsg << "Using the level system\n"; - serverConfig.SetUseLevels(true); + _serverConfig.SetUseLevels(true); } if (_networkRole && std::strlen(_networkRole) > 0) { ignmsg << "Using the distributed simulation and levels systems\n"; - serverConfig.SetNetworkRole(_networkRole); - serverConfig.SetNetworkSecondaries(_networkSecondaries); - serverConfig.SetUseLevels(true); + _serverConfig.SetNetworkRole(_networkRole); + _serverConfig.SetNetworkSecondaries(_networkSecondaries); + _serverConfig.SetUseLevels(true); } if (_playback != nullptr && std::strlen(_playback) > 0) @@ -324,36 +326,104 @@ extern "C" int runServer(const char *_sdfString, else { ignmsg << "Playing back states" << _playback << std::endl; - serverConfig.SetLogPlaybackPath(ignition::common::absPath( + _serverConfig.SetLogPlaybackPath(ignition::common::absPath( std::string(_playback))); } } if (_physicsEngine != nullptr && std::strlen(_physicsEngine) > 0) { - serverConfig.SetPhysicsEngine(_physicsEngine); + _serverConfig.SetPhysicsEngine(_physicsEngine); } if (_renderEngineServer != nullptr && std::strlen(_renderEngineServer) > 0) { - serverConfig.SetRenderEngineServer(_renderEngineServer); + _serverConfig.SetRenderEngineServer(_renderEngineServer); } if (_renderEngineGui != nullptr && std::strlen(_renderEngineGui) > 0) { - serverConfig.SetRenderEngineGui(_renderEngineGui); + _serverConfig.SetRenderEngineGui(_renderEngineGui); } + return 0; +} + +////////////////////////////////////////////////// +extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, + int _iterations, int _run, float _hz, int _levels, const char *_networkRole, + int _networkSecondaries, int _record, const char *_recordPath, + int _recordResources, int _logOverwrite, int _logCompress, + const char *_playback, const char *_physicsEngine, + const char *_renderEngineServer, const char *_renderEngineGui, + const char *_file, const char *_recordTopics) +{ // Create the Gazebo server - ignition::gazebo::Server server(serverConfig); + ignition::gazebo::ServerConfig serverConfig; - // Run the server - server.Run(true, _iterations, _run == 0); + if (createServerConfig(serverConfig, + _sdfString, _hz, _levels, _networkRole, + _networkSecondaries, _record, _recordPath, + _recordResources, _logOverwrite, _logCompress, + _playback, _physicsEngine, _renderEngineServer, + _renderEngineGui, _file, _recordTopics, false) == 0) + { + ignition::gazebo::Server server(serverConfig); + // Run the server + server.Run(true, _iterations, _run == 0); + igndbg << "Shutting down ign-gazebo-server" << std::endl; + return 0; + } igndbg << "Shutting down ign-gazebo-server" << std::endl; return 0; } +////////////////////////////////////////////////// +extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, + int _iterations, int _run, float _hz, int _levels, const char *_networkRole, + int _networkSecondaries, int _record, const char *_recordPath, + 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 *_guiConfig) +{ + ignition::gazebo::ServerConfig serverConfig; + + if (createServerConfig(serverConfig, + _sdfString, _hz, _levels, _networkRole, + _networkSecondaries, _record, _recordPath, + _recordResources, _logOverwrite, _logCompress, + _playback, _physicsEngine, _renderEngineServer, + _renderEngineGui, _file, _recordTopics, true) == 0) + { + // Create the Gazebo server + ignition::gazebo::Server server(serverConfig); + + // Run the server + server.Run(false, _iterations, _run == 0); + + auto &sharedEcm = server.GetEntityComponentManager(); + + // argc and argv are going to be passed to a QApplication. The Qt + // documentation has a warning about these: + // "Warning: The data referred to by argc and argv must stay valid for the + // entire lifetime of the QApplication object. In addition, argc must be + // greater than zero and argv must contain at least one valid character + // string." + int argc = 1; + // Converting a string literal to char * is forbidden as of C++11. It can only + // be converted to a const char *. The const cast is here to prevent a warning + // since we do need to pass a char* to runGui + char *argv = const_cast("ign-gazebo-gui"); + return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig, sharedEcm, true); + } + + ignerr << "Unable to create server config\n"; + + return -1; +} + ////////////////////////////////////////////////// extern "C" int runGui(const char *_guiConfig) { @@ -368,5 +438,6 @@ extern "C" int runGui(const char *_guiConfig) // be converted to a const char *. The const cast is here to prevent a warning // since we do need to pass a char* to runGui char *argv = const_cast("ign-gazebo-gui"); - return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig); + ignition::gazebo::v6::EntityComponentManager guiEcm; + return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig, guiEcm, false); } diff --git a/src/ign.hh b/src/ign.hh index 3b7cd3c5e4..2d807615d2 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -76,4 +76,35 @@ extern "C" int runGui(const char *_guiConfig); extern "C" const char *findFuelResource( char *_pathToResource); +/// \brief External hook to run simulation server and GUI. +/// \param[in] _sdfString SDF file to run, as a string. +/// \param[in] _iterations --iterations option +/// \param[in] _run -r option +/// \param[in] _hz -z option +/// \param[in] _levels --levels option +/// \param[in] _networkRole --network-role option +/// \param[in] _networkSecondaries --network-secondaries option +/// \param[in] _record --record option +/// \param[in] _recordPath --record-path option +/// \param[in] _recordResources --record-resources option +/// \param[in] _logOverwrite --log-overwrite option +/// \param[in] _logCompress --log-compress option +/// \param[in] _playback --playback option +/// \param[in] _physicsEngine --physics-engine option +/// \param[in] _renderEngineServer --render-engine-server option +/// \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 +/// null to record the default topics. +/// \param[in] _guiConfig Path to Ignition GUI configuration file. +/// \return 0 if successful, 1 if not. +extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, + int _iterations, int _run, float _hz, int _levels, const char *_networkRole, + int _networkSecondaries, int _record, const char *_recordPath, + 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 *_guiConfig); + + #endif diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 65a076b52e..fa4ab2eb8b 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -327,13 +327,16 @@ class ignition::gazebo::RenderUtilPrivate /// \brief A map of entity id to thermal camera sensor configuration /// properties. The elements in the tuple are: /// - public:std::unordered_map> thermalCameraData; + + public: bool sameProcess; }; ////////////////////////////////////////////////// -RenderUtil::RenderUtil() : dataPtr(std::make_unique()) +RenderUtil::RenderUtil(bool _sameProcess) : dataPtr(std::make_unique()) { + this->dataPtr->sameProcess = _sameProcess; } ////////////////////////////////////////////////// @@ -625,6 +628,7 @@ void RenderUtil::Update() // create new entities { IGN_PROFILE("RenderUtil::Update Create"); + // std::cerr << "newModels size " << newModels.size() << '\n'; for (const auto &model : newModels) { uint64_t iteration = std::get<3>(model); @@ -709,13 +713,18 @@ void RenderUtil::Update() // scoped names // TODO(anyone) do this in ign-sensors? auto parentNode = this->dataPtr->sceneManager.NodeById(parent); - if (!parentNode) - { - ignerr << "Failed to create sensor with name[" << dataSdf.Name() - << "] for entity [" << entity - << "]. Parent not found with ID[" << parent << "]." - << std::endl; - continue; + while (!parentNode) { + using namespace std::chrono_literals; + std::this_thread::sleep_for(5ms); + if (!parentNode) + { + ignerr << "Failed to create sensor with name[" << dataSdf.Name() + << "] for entity [" << entity + << "]. Parent not found with ID[" << parent << "]." + << std::endl; + continue; + } + parentNode = this->dataPtr->sceneManager.NodeById(parent); } std::string sensorName = @@ -1151,9 +1160,18 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - this->newModels.push_back( - std::make_tuple(_entity, model, _parent->Data(), - _info.iterations)); + auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), + _info.iterations); + bool found = false; + for (auto & data: this->newModels) + { + if (std::get<0>(data) == std::get<0>(tupleTemp)) + { + found = true; + } + } + if (!found) + this->newModels.push_back(tupleTemp); return true; }); @@ -1379,9 +1397,18 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - this->newModels.push_back( - std::make_tuple(_entity, model, _parent->Data(), - _info.iterations)); + auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), + _info.iterations); + bool found = false; + for (auto & data: this->newModels) + { + if (std::get<0>(data) == std::get<0>(tupleTemp)) + { + found = true; + } + } + if (!found) + this->newModels.push_back(tupleTemp); return true; }); @@ -1824,12 +1851,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( }); } +static std::mutex mutexInit; ///////////////////////////////////////////////// void RenderUtil::Init() { // Already initialized if (nullptr != this->dataPtr->scene) return; + std::lock_guard lock(mutexInit); ignition::common::SystemPaths pluginPath; pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); @@ -1849,11 +1878,13 @@ void RenderUtil::Init() // Scene this->dataPtr->scene = this->dataPtr->engine->SceneByName(this->dataPtr->sceneName); + std::cerr << "this->dataPtr->scene " << this->dataPtr->scene << '\n'; if (!this->dataPtr->scene) { igndbg << "Create scene [" << this->dataPtr->sceneName << "]" << std::endl; this->dataPtr->scene = this->dataPtr->engine->CreateScene(this->dataPtr->sceneName); + std::cerr << "this->dataPtr->scene " << this->dataPtr->scene << '\n'; if (this->dataPtr->scene) { this->dataPtr->scene->SetAmbientLight(this->dataPtr->ambientLight); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 1345e98bef..5603421e84 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -453,6 +453,8 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, else if (_geom.Type() == sdf::GeometryType::PLANE) { geom = this->dataPtr->scene->CreatePlane(); + if (geom == nullptr) + return rendering::GeometryPtr(); scale.X() = _geom.PlaneShape()->Size().X(); scale.Y() = _geom.PlaneShape()->Size().Y(); @@ -1008,11 +1010,17 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, rendering::LightPtr lightParent; auto it = this->dataPtr->lights.find(_parentId); - if (it != this->dataPtr->lights.end()) + int attemps = 10; + while (it != this->dataPtr->lights.end()) { - lightParent = it->second; + if (attemps < 0) + break; + attemps--; + using namespace std::chrono_literals; + std::this_thread::sleep_for(5ms); } - else + lightParent = it->second; + if (it == this->dataPtr->lights.end()) { ignerr << "Parent entity with Id: [" << _parentId << "] not found. " << "Not adding light visual with ID[" << _id @@ -1070,7 +1078,7 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, { ignerr << "Light with Id: [" << _id << "] already exists in the scene" << std::endl; - return rendering::LightPtr(); + return this->dataPtr->lights.find(_id)->second; } rendering::VisualPtr parent; @@ -1096,10 +1104,14 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, { case sdf::LightType::POINT: light = this->dataPtr->scene->CreatePointLight(name); + if (light == nullptr) + return rendering::LightPtr(); break; case sdf::LightType::SPOT: { light = this->dataPtr->scene->CreateSpotLight(name); + if (light == nullptr) + return rendering::LightPtr(); rendering::SpotLightPtr spotLight = std::dynamic_pointer_cast(light); spotLight->SetInnerAngle(_light.SpotInnerAngle()); @@ -1111,6 +1123,8 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, case sdf::LightType::DIRECTIONAL: { light = this->dataPtr->scene->CreateDirectionalLight(name); + if (light == nullptr) + return rendering::LightPtr(); rendering::DirectionalLightPtr dirLight = std::dynamic_pointer_cast(light); diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a20b5c3280..f54cc9cf45 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -57,6 +57,9 @@ using namespace systems; // Private data class. class ignition::gazebo::systems::SensorsPrivate { + public: explicit SensorsPrivate() + : renderUtil(false){} + /// \brief Sensor manager object. This manages the lifecycle of the /// instantiated sensors. public: sensors::Manager sensorManager; From ae902c339f4de2ac6ef43be5c370ff10d6b53015 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 2 Jun 2021 17:27:11 +0200 Subject: [PATCH 04/35] Avoid some crashes Signed-off-by: ahcorde --- .../ignition/gazebo/rendering/RenderUtil.hh | 2 +- src/gui/gui.config | 2 +- src/gui/plugins/CMakeLists.txt | 2 +- .../plugins/scene_manager/GzSceneManager.cc | 2 +- src/rendering/RenderUtil.cc | 20 +++++++---------- src/rendering/SceneManager.cc | 22 ++++++++++++++++--- 6 files changed, 31 insertions(+), 19 deletions(-) diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 408d0c4d36..e0b39addb5 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -44,7 +44,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { class IGNITION_GAZEBO_RENDERING_VISIBLE RenderUtil { /// \brief Constructor - public: explicit RenderUtil(bool updateNewEntities); + public: explicit RenderUtil(bool updateNewEntities = false); /// \brief Destructor public: ~RenderUtil(); diff --git a/src/gui/gui.config b/src/gui/gui.config index bb2b9f7e42..5cef892e45 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -33,7 +33,7 @@ docked - ogre2 + ogre scene 0.4 0.4 0.4 0.8 0.8 0.8 diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 7df38dfb35..a155b91637 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -118,7 +118,7 @@ add_subdirectory(lights) add_subdirectory(playback_scrubber) add_subdirectory(plotting) add_subdirectory(resource_spawner) -add_subdirectory(scene3d) +# add_subdirectory(scene3d) add_subdirectory(scene_manager) add_subdirectory(shapes) add_subdirectory(tape_measure) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 7491687688..61c86a21d2 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -103,7 +103,7 @@ void GzSceneManagerPrivate::OnRender() { if (nullptr == this->scene) { - this->scene = rendering::someInitializedScene(); + this->scene = rendering::sceneFromFirstRenderEngine(); if (nullptr == this->scene) return; diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index fa4ab2eb8b..9e64a4608b 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -713,19 +713,15 @@ void RenderUtil::Update() // scoped names // TODO(anyone) do this in ign-sensors? auto parentNode = this->dataPtr->sceneManager.NodeById(parent); - while (!parentNode) { - using namespace std::chrono_literals; - std::this_thread::sleep_for(5ms); - if (!parentNode) - { - ignerr << "Failed to create sensor with name[" << dataSdf.Name() - << "] for entity [" << entity - << "]. Parent not found with ID[" << parent << "]." - << std::endl; - continue; - } - parentNode = this->dataPtr->sceneManager.NodeById(parent); + if (!parentNode) + { + ignerr << "Failed to create sensor with name[" << dataSdf.Name() + << "] for entity [" << entity + << "]. Parent not found with ID[" << parent << "]." + << std::endl; + continue; } + parentNode = this->dataPtr->sceneManager.NodeById(parent); std::string sensorName = this->dataPtr->createSensorCb(entity, dataSdf, parentNode->Name()); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 5603421e84..4d3cd3b9eb 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -175,8 +175,10 @@ rendering::VisualPtr SceneManager::CreateModel(Entity _id, if (this->dataPtr->scene->HasVisualName(name)) { - ignerr << "Visual: [" << name << "] already exists" << std::endl; - return rendering::VisualPtr(); + ignerr << "Visual: [" << name << " - " << _id << "] already exists" << std::endl; + auto vis = this->dataPtr->scene->VisualByName(name); + this->dataPtr->visuals[_id] = vis; + return vis; } rendering::VisualPtr modelVis = this->dataPtr->scene->CreateVisual(name); @@ -204,7 +206,9 @@ rendering::VisualPtr SceneManager::CreateLink(Entity _id, { ignerr << "Entity with Id: [" << _id << "] already exists in the scene" << std::endl; - return rendering::VisualPtr(); + auto vis = this->dataPtr->scene->VisualById(_id); + this->dataPtr->visuals[_id] = vis; + return vis; } rendering::VisualPtr parent; @@ -224,6 +228,12 @@ rendering::VisualPtr SceneManager::CreateLink(Entity _id, _link.Name(); if (parent) name = parent->Name() + "::" + name; + if (this->dataPtr->scene->HasVisualName(name)) + { + auto vis = this->dataPtr->scene->VisualByName(name); + this->dataPtr->visuals[_id] = vis; + return vis; + } rendering::VisualPtr linkVis = this->dataPtr->scene->CreateVisual(name); linkVis->SetUserData("gazebo-entity", static_cast(_id)); linkVis->SetUserData("pause-update", static_cast(0)); @@ -271,6 +281,12 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, _visual.Name(); if (parent) name = parent->Name() + "::" + name; + if (this->dataPtr->scene->HasVisualName(name)) + { + auto vis = this->dataPtr->scene->VisualByName(name); + this->dataPtr->visuals[_id] = vis; + return vis; + } rendering::VisualPtr visualVis = this->dataPtr->scene->CreateVisual(name); visualVis->SetUserData("gazebo-entity", static_cast(_id)); visualVis->SetUserData("pause-update", static_cast(0)); From 57cf18fce33f145dbe3f54a33156464d8c9ab063 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 3 Jun 2021 22:09:16 +0200 Subject: [PATCH 05/35] Improved implementation Signed-off-by: ahcorde --- include/ignition/gazebo/Events.hh | 2 + include/ignition/gazebo/Server.hh | 6 + include/ignition/gazebo/System.hh | 5 + include/ignition/gazebo/gui/Gui.hh | 3 + include/ignition/gazebo/gui/GuiRunner.hh | 2 + include/ignition/gazebo/gui/GuiSystem.hh | 3 + src/Server.cc | 8 ++ src/SimulationRunner.cc | 1 + src/gui/Gui.cc | 15 ++- src/gui/GuiRunner.cc | 17 ++- .../plugins/scene_manager/GzSceneManager.cc | 10 ++ .../plugins/scene_manager/GzSceneManager.hh | 4 + src/ign.cc | 6 +- src/rendering/RenderUtil.cc | 2 - src/systems/sensors/Sensors.cc | 111 ++++++++++++++---- src/systems/sensors/Sensors.hh | 3 + 16 files changed, 162 insertions(+), 36 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 92c04c6e5e..2e8a469fac 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -58,6 +58,8 @@ namespace ignition /// the entity, which may contain multiple tags. using LoadPlugins = common::EventT; + + using Render = ignition::common::EventT; } } // namespace events } // namespace gazebo diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 588dfab55b..737f4874de 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -116,8 +116,14 @@ namespace ignition /// \brief Destructor public: ~Server(); + /// \brief Get the Entity Component Manager reference + /// \return The Entity Component Manager reference public: EntityComponentManager &GetEntityComponentManager(); + /// \brief Get the Event Manager reference + /// \return The Event Manager reference + public: EventManager &GetEventManager(); + /// \brief Set the update period. The update period is the wall-clock time /// between ECS updates. /// Note that this is different from the simulation update rate. ECS diff --git a/include/ignition/gazebo/System.hh b/include/ignition/gazebo/System.hh index 467815f202..d012430402 100644 --- a/include/ignition/gazebo/System.hh +++ b/include/ignition/gazebo/System.hh @@ -98,6 +98,11 @@ namespace ignition const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &_eventMgr) = 0; + + /// \brief + /// \param[in] _sameProcess True if the server and client are running in + /// the same process, False otherwise. + public: virtual void SetSameProcess(bool /*_sameProcess*/){}; }; /// \class ISystemPreUpdate ISystem.hh ignition/gazebo/System.hh diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index e5e387bd52..ed9989cdbd 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -23,6 +23,7 @@ #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -45,6 +46,7 @@ namespace gui IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, char **_argv, const char *_guiConfig, EntityComponentManager &_ecm, + EventManager &_eventMgr, bool sameProcess); /// \brief Create a Gazebo GUI application @@ -67,6 +69,7 @@ namespace gui std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, EntityComponentManager &_ecm, + EventManager &_eventMgr, bool sameProcess, const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index 3a91ecc5e2..ede6a18651 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/include/ignition/gazebo/gui/GuiRunner.hh @@ -27,6 +27,7 @@ #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -47,6 +48,7 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \todo Move to src/gui on v6. public: explicit IGN_DEPRECATED(5.0) GuiRunner( const std::string &_worldName, EntityComponentManager &_ecm, + EventManager &_event, bool sameProcess); /// \brief Destructor diff --git a/include/ignition/gazebo/gui/GuiSystem.hh b/include/ignition/gazebo/gui/GuiSystem.hh index df6e2b6c66..ac1030671b 100644 --- a/include/ignition/gazebo/gui/GuiSystem.hh +++ b/include/ignition/gazebo/gui/GuiSystem.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -53,6 +54,8 @@ namespace gazebo /// and write entities and their components. public: virtual void Update(const UpdateInfo &/*_info*/, EntityComponentManager &/*_ecm*/){} + + public: virtual void Configure(EventManager &/*_event*/){}; }; } } diff --git a/src/Server.cc b/src/Server.cc index f7329e6f9d..2e5377f164 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -214,6 +214,14 @@ EntityComponentManager &Server::GetEntityComponentManager() } } +EventManager &Server::GetEventManager() +{ + for (std::unique_ptr &runner : this->dataPtr->simRunners) + { + return runner->EventMgr(); + } +} + ///////////////////////////////////////////////// bool Server::Run(const bool _blocking, const uint64_t _iterations, const bool _paused) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index b261993cc5..a1a7db8870 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -834,6 +834,7 @@ void SimulationRunner::LoadPlugin(const Entity _entity, systemConfig->Configure(_entity, _sdf, this->entityCompMgr, this->eventMgr); + systemConfig->SetSameProcess(serverConfig.SameProcessAsGUI()); } this->AddSystem(system.value()); diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 9329b22bd0..71f37efce5 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -43,10 +43,12 @@ namespace gui ////////////////////////////////////////////////// std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, - EntityComponentManager &_ecm, bool sameProcess, - const char *_defaultGuiConfig, bool _loadPluginsFromSdf) + EntityComponentManager &_ecm, EventManager &_eventMgr, + bool sameProcess, const char *_defaultGuiConfig, + bool _loadPluginsFromSdf) { auto &sharedEcm = _ecm; + auto &sharedEventManager = _eventMgr; ignition::common::SignalHandler sigHandler; bool sigKilled = false; @@ -180,7 +182,7 @@ std::unique_ptr createGui( # pragma warning(push) # pragma warning(disable: 4996) #endif - auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0), sharedEcm, sameProcess); + auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0), sharedEcm, sharedEventManager, sameProcess); #ifndef _WIN32 # pragma GCC diagnostic pop #else @@ -244,7 +246,7 @@ std::unique_ptr createGui( # pragma warning(push) # pragma warning(disable: 4996) #endif - auto runner = new ignition::gazebo::GuiRunner(worldName, sharedEcm, sameProcess); + auto runner = new ignition::gazebo::GuiRunner(worldName, sharedEcm, sharedEventManager, sameProcess); #ifndef _WIN32 # pragma GCC diagnostic pop #else @@ -322,11 +324,12 @@ std::unique_ptr createGui( ////////////////////////////////////////////////// int runGui(int &_argc, char **_argv, const char *_guiConfig, - EntityComponentManager &_ecm, bool sameProcess) + EntityComponentManager &_ecm, EventManager &_eventMgr, bool sameProcess) { auto &sharedEcm = _ecm; + auto &sharedEventManager = _eventMgr; auto app = gazebo::gui::createGui(_argc, _argv, _guiConfig, - sharedEcm, sameProcess); + sharedEcm, sharedEventManager, sameProcess); if (nullptr != app) { // Run main window. diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index dc927da3eb..1d592f10a6 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -26,6 +26,7 @@ #include "ignition/gazebo/components/components.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/GuiRunner.hh" #include "ignition/gazebo/gui/GuiSystem.hh" @@ -35,9 +36,9 @@ using namespace gazebo; ///////////////////////////////////////////////// class ignition::gazebo::GuiRunner::Implementation { - - public: explicit Implementation(gazebo::EntityComponentManager &_ecm) - : ecm(_ecm){} + public: explicit Implementation(gazebo::EntityComponentManager &_ecm, + gazebo::EventManager &_eventMgr) + : ecm(_ecm), eventMgr(_eventMgr){} /// \brief Update the plugins. public: void UpdatePlugins(); @@ -45,6 +46,9 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief Entity-component manager. public: gazebo::EntityComponentManager &ecm; + /// \brief Event manager. + public: gazebo::EventManager &eventMgr; + public: bool sameProcess; /// \brief Transport node. @@ -68,8 +72,9 @@ class ignition::gazebo::GuiRunner::Implementation ///////////////////////////////////////////////// GuiRunner::GuiRunner(const std::string &_worldName, - EntityComponentManager &_ecm, bool _sameProcess) - : dataPtr(utils::MakeUniqueImpl(_ecm)) + EntityComponentManager &_ecm, gazebo::EventManager &_eventMgr, + bool _sameProcess) + : dataPtr(utils::MakeUniqueImpl(_ecm, _eventMgr)) { this->dataPtr->sameProcess = _sameProcess; @@ -173,7 +178,7 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) << "]" << std::endl; return; } - + plugin->Configure(this->dataPtr->eventMgr); this->RequestState(); } diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 61c86a21d2..22a4a7436a 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -28,6 +28,7 @@ #include #include +#include "ignition/gazebo/Events.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -47,6 +48,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { //// \brief Pointer to the rendering scene public: rendering::ScenePtr scene; + public: EventManager *eventManager{nullptr}; + /// \brief Rendering utility public: RenderUtil renderUtil; }; @@ -66,6 +69,12 @@ GzSceneManager::GzSceneManager() ///////////////////////////////////////////////// GzSceneManager::~GzSceneManager() = default; +///////////////////////////////////////////////// +void GzSceneManager::Configure(EventManager &_eventMgr) +{ + this->dataPtr->eventManager = &_eventMgr; +} + ///////////////////////////////////////////////// void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *) { @@ -92,6 +101,7 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) if (_event->type() == gui::events::Render::kType) { this->dataPtr->OnRender(); + this->dataPtr->eventManager->Emit(); } // Standard event processing diff --git a/src/gui/plugins/scene_manager/GzSceneManager.hh b/src/gui/plugins/scene_manager/GzSceneManager.hh index ab0d5f1be0..446deca324 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.hh +++ b/src/gui/plugins/scene_manager/GzSceneManager.hh @@ -21,6 +21,7 @@ #include #include +#include "ignition/gazebo/EventManager.hh" namespace ignition { @@ -52,6 +53,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) override; + // Documentation inherited + public: void Configure(EventManager &_eventMgr) override; + // Documentation inherited private: bool eventFilter(QObject *_obj, QEvent *_event) override; diff --git a/src/ign.cc b/src/ign.cc index 6c3ba8ccc5..6413bf550c 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -404,6 +404,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, server.Run(false, _iterations, _run == 0); auto &sharedEcm = server.GetEntityComponentManager(); + auto &sharedEventManager = server.GetEventManager(); // argc and argv are going to be passed to a QApplication. The Qt // documentation has a warning about these: @@ -416,7 +417,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, // be converted to a const char *. The const cast is here to prevent a warning // since we do need to pass a char* to runGui char *argv = const_cast("ign-gazebo-gui"); - return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig, sharedEcm, true); + return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig, sharedEcm, sharedEventManager, true); } ignerr << "Unable to create server config\n"; @@ -439,5 +440,6 @@ extern "C" int runGui(const char *_guiConfig) // since we do need to pass a char* to runGui char *argv = const_cast("ign-gazebo-gui"); ignition::gazebo::v6::EntityComponentManager guiEcm; - return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig, guiEcm, false); + ignition::gazebo::v6::EventManager guiEventEcm; + return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig, guiEcm, guiEventEcm, false); } diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 9e64a4608b..be1751de8b 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1874,13 +1874,11 @@ void RenderUtil::Init() // Scene this->dataPtr->scene = this->dataPtr->engine->SceneByName(this->dataPtr->sceneName); - std::cerr << "this->dataPtr->scene " << this->dataPtr->scene << '\n'; if (!this->dataPtr->scene) { igndbg << "Create scene [" << this->dataPtr->sceneName << "]" << std::endl; this->dataPtr->scene = this->dataPtr->engine->CreateScene(this->dataPtr->sceneName); - std::cerr << "this->dataPtr->scene " << this->dataPtr->scene << '\n'; if (this->dataPtr->scene) { this->dataPtr->scene->SetAmbientLight(this->dataPtr->ambientLight); diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index f54cc9cf45..2f797c051f 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -30,6 +30,7 @@ #include +#include #include #include #include @@ -122,6 +123,9 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Connection to events::Stop event, used to stop thread public: ignition::common::ConnectionPtr stopConn; + /// \brief Connection to events::Render event, used to render thread + public: ignition::common::ConnectionPtr renderConn; + /// \brief Update time for the next rendering iteration public: std::chrono::steady_clock::duration updateTime; @@ -138,6 +142,8 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Pointer to the event manager public: EventManager *eventManager{nullptr}; + public: bool sameProcess = false; + /// \brief Wait for initialization to happen private: void WaitForInit(); @@ -177,6 +183,9 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Stop the rendering thread public: void Stop(); + + /// \brief Stop the rendering thread + public: void Render(); }; ////////////////////////////////////////////////// @@ -225,8 +234,11 @@ void SensorsPrivate::RunOnce() IGN_PROFILE("SensorsPrivate::RunOnce"); { - IGN_PROFILE("Update"); - this->renderUtil.Update(); + if (!this->sameProcess) + { + IGN_PROFILE("Update"); + this->renderUtil.Update(); + } } @@ -251,26 +263,27 @@ void SensorsPrivate::RunOnce() } this->sensorMaskMutex.unlock(); + if (!this->sameProcess) { - IGN_PROFILE("PreRender"); - this->eventManager->Emit(); - // Update the scene graph manually to improve performance - // We only need to do this once per frame It is important to call - // sensors::RenderingSensor::SetManualSceneUpdate and set it to true - // so we don't waste cycles doing one scene graph update per sensor - this->scene->PreRender(); - } + { + IGN_PROFILE("PreRender"); + this->eventManager->Emit(); + // Update the scene graph manually to improve performance + // We only need to do this once per frame It is important to call + // sensors::RenderingSensor::SetManualSceneUpdate and set it to true + // so we don't waste cycles doing one scene graph update per sensor + this->scene->PreRender(); + } - { - // publish data - IGN_PROFILE("RunOnce"); - this->sensorManager.RunOnce(this->updateTime); - this->eventManager->Emit(); + { + // publish data + IGN_PROFILE("RunOnce"); + this->sensorManager.RunOnce(this->updateTime); + this->eventManager->Emit(); + } } - this->activeSensors.clear(); } - this->updateAvailable = false; lock.unlock(); this->renderCv.notify_one(); @@ -306,6 +319,42 @@ void SensorsPrivate::Run() this->renderThread = std::thread(&SensorsPrivate::RenderThread, this); } +////////////////////////////////////////////////// +void SensorsPrivate::Render() +{ + if (!this->sameProcess) + return; + + if (!this->running) + return; + + if (!this->scene) + return; + + IGN_PROFILE("SensorsPrivate::RunOnce"); + { + IGN_PROFILE("Update"); + this->renderUtil.Update(); + } + + { + IGN_PROFILE("PreRender"); + this->eventManager->Emit(); + // Update the scene graph manually to improve performance + // We only need to do this once per frame It is important to call + // sensors::RenderingSensor::SetManualSceneUpdate and set it to true + // so we don't waste cycles doing one scene graph update per sensor + this->scene->PreRender(); + } + + { + // publish data + IGN_PROFILE("RunOnce"); + this->sensorManager.RunOnce(this->updateTime); + this->eventManager->Emit(); + } +} + ////////////////////////////////////////////////// void SensorsPrivate::Stop() { @@ -365,6 +414,11 @@ Sensors::~Sensors() this->dataPtr->Stop(); } +void Sensors::SetSameProcess(bool _sameProcess) +{ + this->dataPtr->sameProcess = _sameProcess; +} + ////////////////////////////////////////////////// void Sensors::Configure(const Entity &/*_id*/, const std::shared_ptr &_sdf, @@ -411,6 +465,9 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->stopConn = _eventMgr.Connect( std::bind(&SensorsPrivate::Stop, this->dataPtr.get())); + this->dataPtr->renderConn = _eventMgr.Connect( + std::bind(&SensorsPrivate::Render, this->dataPtr.get())); + // Kick off worker thread this->dataPtr->Run(); } @@ -447,10 +504,24 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::RgbdCamera::typeId) || _ecm.HasComponentType(components::ThermalCamera::typeId))) { - igndbg << "Initialization needed" << std::endl; std::unique_lock lock(this->dataPtr->renderMutex); - this->dataPtr->doInit = true; - this->dataPtr->renderCv.notify_one(); + + // This will allow to load first the scene3D + if (this->dataPtr->sameProcess) + { + if (rendering::isEngineLoaded("ogre") || rendering::isEngineLoaded("ogre2")) + { + igndbg << "Initialization needed" << std::endl; + this->dataPtr->doInit = true; + this->dataPtr->renderCv.notify_one(); + } + else + { + igndbg << "Initialization needed" << std::endl; + this->dataPtr->doInit = true; + this->dataPtr->renderCv.notify_one(); + } + } } if (this->dataPtr->running && this->dataPtr->initialized) diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index 3f988fa146..e936d57415 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -56,6 +56,9 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; + // Documentation inherited + public: void SetSameProcess(bool _sameProcess) final; + // Documentation inherited public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) final; From 343dba897f9f2dd293f0dd494aed7b05fae606b4 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 4 Jun 2021 13:12:45 +0200 Subject: [PATCH 06/35] added more improvemetns Signed-off-by: ahcorde --- include/ignition/gazebo/ServerConfig.hh | 2 +- include/ignition/gazebo/gui/GuiSystem.hh | 2 +- .../ignition/gazebo/rendering/RenderUtil.hh | 4 ++ src/ServerConfig.cc | 2 +- src/SimulationRunner.cc | 3 +- src/gui/GuiRunner.cc | 8 ++-- src/gui/gui.config | 2 +- .../plugins/scene_manager/GzSceneManager.cc | 21 +++++++-- .../plugins/scene_manager/GzSceneManager.hh | 2 +- src/rendering/RenderUtil.cc | 47 +++++++++---------- src/rendering/SceneManager.cc | 12 ++--- src/systems/sensors/Sensors.cc | 19 ++++---- 12 files changed, 67 insertions(+), 57 deletions(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 0acc13ad06..9a6ce7224f 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -436,7 +436,7 @@ namespace ignition /// \return A list of plugins to load, based on above ordering std::list IGNITION_GAZEBO_VISIBLE - loadPluginInfo(bool _isPlayback = false, bool _sameProcessAsGUI = false); + loadPluginInfo(bool _isPlayback = false); } } } diff --git a/include/ignition/gazebo/gui/GuiSystem.hh b/include/ignition/gazebo/gui/GuiSystem.hh index ac1030671b..5c601968d7 100644 --- a/include/ignition/gazebo/gui/GuiSystem.hh +++ b/include/ignition/gazebo/gui/GuiSystem.hh @@ -55,7 +55,7 @@ namespace gazebo public: virtual void Update(const UpdateInfo &/*_info*/, EntityComponentManager &/*_ecm*/){} - public: virtual void Configure(EventManager &/*_event*/){}; + public: virtual void Configure(EventManager &/*_event*/, bool /*sameProcess*/){}; }; } } diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index e0b39addb5..0e50cbb926 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -76,6 +76,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void UpdateFromECM(const UpdateInfo &_info, const EntityComponentManager &_ecm); + /// \brief Helper PostUpdate function for updating the scene + public: void UpdateFromECM2(const UpdateInfo &_info, + const EntityComponentManager &_ecm); + /// \brief Set the rendering engine to use /// \param[in] _engineName Name of the rendering engine. public: void SetEngineName(const std::string &_engineName); diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 20e7ae6fe7..f3640eb25f 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -841,7 +841,7 @@ ignition::gazebo::parsePluginsFromString(const std::string &_str) ///////////////////////////////////////////////// std::list -ignition::gazebo::loadPluginInfo(bool _isPlayback, bool _sameProcessAsGUI) +ignition::gazebo::loadPluginInfo(bool _isPlayback) { std::list ret; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index a1a7db8870..9e05256377 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -168,8 +168,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, { ignmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); - auto plugins = ignition::gazebo::loadPluginInfo( - isPlayback, this->serverConfig.SameProcessAsGUI()); + auto plugins = ignition::gazebo::loadPluginInfo(isPlayback); this->LoadServerPlugins(plugins); } diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 1d592f10a6..2b68ef07ec 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -134,8 +134,10 @@ GuiRunner::~GuiRunner() ///////////////////////////////////////////////// void GuiRunner::RequestState() { - if (this->dataPtr->sameProcess) - return; + std::cerr << "RequestState sameProcess " << this->dataPtr->sameProcess << '\n'; + + // if (this->dataPtr->sameProcess) + // return; // set up service for async state response callback std::string id = std::to_string(gui::App()->applicationPid()); @@ -178,7 +180,7 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) << "]" << std::endl; return; } - plugin->Configure(this->dataPtr->eventMgr); + plugin->Configure(this->dataPtr->eventMgr, this->dataPtr->sameProcess); this->RequestState(); } diff --git a/src/gui/gui.config b/src/gui/gui.config index 5cef892e45..bb2b9f7e42 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -33,7 +33,7 @@ docked - ogre + ogre2 scene 0.4 0.4 0.4 0.8 0.8 0.8 diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 22a4a7436a..74d6d28b89 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -50,6 +50,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: EventManager *eventManager{nullptr}; + public: bool sameProcess{false}; + /// \brief Rendering utility public: RenderUtil renderUtil; }; @@ -70,9 +72,10 @@ GzSceneManager::GzSceneManager() GzSceneManager::~GzSceneManager() = default; ///////////////////////////////////////////////// -void GzSceneManager::Configure(EventManager &_eventMgr) +void GzSceneManager::Configure(EventManager &_eventMgr, bool _sameProcess) { this->dataPtr->eventManager = &_eventMgr; + this->dataPtr->sameProcess = _sameProcess; } ///////////////////////////////////////////////// @@ -90,9 +93,16 @@ void GzSceneManager::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("GzSceneManager::Update"); - + std::cerr << "GzSceneManager sameProcess " << this->dataPtr->sameProcess << '\n'; this->dataPtr->renderUtil.UpdateECM(_info, _ecm); - this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + if (this->dataPtr->sameProcess) + { + // this->dataPtr->renderUtil.UpdateFromECM2(_info, _ecm); + } + else + { + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + } } ///////////////////////////////////////////////// @@ -101,7 +111,10 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) if (_event->type() == gui::events::Render::kType) { this->dataPtr->OnRender(); - this->dataPtr->eventManager->Emit(); + if (this->dataPtr->sameProcess) + { + // this->dataPtr->eventManager->Emit(); + } } // Standard event processing diff --git a/src/gui/plugins/scene_manager/GzSceneManager.hh b/src/gui/plugins/scene_manager/GzSceneManager.hh index 446deca324..3077ea740e 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.hh +++ b/src/gui/plugins/scene_manager/GzSceneManager.hh @@ -54,7 +54,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { EntityComponentManager &_ecm) override; // Documentation inherited - public: void Configure(EventManager &_eventMgr) override; + public: void Configure(EventManager &_eventMgr, bool _sameProcess) override; // Documentation inherited private: bool eventFilter(QObject *_obj, QEvent *_event) override; diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index be1751de8b..0793fec6df 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -470,6 +470,21 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->FindCollisionLinks(_ecm); } +////////////////////////////////////////////////// +void RenderUtil::UpdateFromECM2(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("RenderUtil::UpdateFromECM"); + std::lock_guard lock(this->dataPtr->updateMutex); + this->dataPtr->simTime = _info.simTime; + + // this->dataPtr->CreateRenderingEntities(_ecm, _info); + this->dataPtr->UpdateRenderingEntities(_ecm); + this->dataPtr->RemoveRenderingEntities(_ecm, _info); + this->dataPtr->markerManager.SetSimTime(_info.simTime); + this->dataPtr->FindCollisionLinks(_ecm); +} + ////////////////////////////////////////////////// void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) { @@ -628,7 +643,6 @@ void RenderUtil::Update() // create new entities { IGN_PROFILE("RenderUtil::Update Create"); - // std::cerr << "newModels size " << newModels.size() << '\n'; for (const auto &model : newModels) { uint64_t iteration = std::get<3>(model); @@ -721,7 +735,6 @@ void RenderUtil::Update() << std::endl; continue; } - parentNode = this->dataPtr->sceneManager.NodeById(parent); std::string sensorName = this->dataPtr->createSensorCb(entity, dataSdf, parentNode->Name()); @@ -1156,18 +1169,9 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), - _info.iterations); - bool found = false; - for (auto & data: this->newModels) - { - if (std::get<0>(data) == std::get<0>(tupleTemp)) - { - found = true; - } - } - if (!found) - this->newModels.push_back(tupleTemp); + this->newModels.push_back( + std::make_tuple(_entity, model, _parent->Data(), + _info.iterations)); return true; }); @@ -1393,18 +1397,9 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), - _info.iterations); - bool found = false; - for (auto & data: this->newModels) - { - if (std::get<0>(data) == std::get<0>(tupleTemp)) - { - found = true; - } - } - if (!found) - this->newModels.push_back(tupleTemp); + this->newModels.push_back( + std::make_tuple(_entity, model, _parent->Data(), + _info.iterations)); return true; }); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 4d3cd3b9eb..bf535a919e 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -1026,17 +1026,11 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, rendering::LightPtr lightParent; auto it = this->dataPtr->lights.find(_parentId); - int attemps = 10; - while (it != this->dataPtr->lights.end()) + if (it != this->dataPtr->lights.end()) { - if (attemps < 0) - break; - attemps--; - using namespace std::chrono_literals; - std::this_thread::sleep_for(5ms); + lightParent = it->second; } - lightParent = it->second; - if (it == this->dataPtr->lights.end()) + else { ignerr << "Parent entity with Id: [" << _parentId << "] not found. " << "Not adding light visual with ID[" << _id diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 2f797c051f..b1d32708e6 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -465,8 +465,11 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->stopConn = _eventMgr.Connect( std::bind(&SensorsPrivate::Stop, this->dataPtr.get())); - this->dataPtr->renderConn = _eventMgr.Connect( - std::bind(&SensorsPrivate::Render, this->dataPtr.get())); + if (this->dataPtr->sameProcess) + { + this->dataPtr->renderConn = _eventMgr.Connect( + std::bind(&SensorsPrivate::Render, this->dataPtr.get())); + } // Kick off worker thread this->dataPtr->Run(); @@ -515,12 +518,12 @@ void Sensors::PostUpdate(const UpdateInfo &_info, this->dataPtr->doInit = true; this->dataPtr->renderCv.notify_one(); } - else - { - igndbg << "Initialization needed" << std::endl; - this->dataPtr->doInit = true; - this->dataPtr->renderCv.notify_one(); - } + } + else + { + igndbg << "Initialization needed" << std::endl; + this->dataPtr->doInit = true; + this->dataPtr->renderCv.notify_one(); } } From 999620da55e18158facda14a6b60be659f03cc44 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 4 Jun 2021 13:29:43 +0200 Subject: [PATCH 07/35] uncomment code Signed-off-by: ahcorde --- src/gui/plugins/scene_manager/GzSceneManager.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 74d6d28b89..b5d7014726 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -97,7 +97,7 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateECM(_info, _ecm); if (this->dataPtr->sameProcess) { - // this->dataPtr->renderUtil.UpdateFromECM2(_info, _ecm); + this->dataPtr->renderUtil.UpdateFromECM2(_info, _ecm); } else { @@ -113,7 +113,7 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->OnRender(); if (this->dataPtr->sameProcess) { - // this->dataPtr->eventManager->Emit(); + this->dataPtr->eventManager->Emit(); } } From 94982551ab4c3985d54f21cef045b22e8a4e4a23 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 4 Jun 2021 18:48:32 +0200 Subject: [PATCH 08/35] some fixes Signed-off-by: ahcorde --- src/gui/GuiRunner.cc | 4 ++-- src/gui/plugins/scene_manager/GzSceneManager.cc | 1 - src/systems/sensors/Sensors.cc | 12 ++++++------ 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 2b68ef07ec..ac2c959e81 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -136,8 +136,8 @@ void GuiRunner::RequestState() { std::cerr << "RequestState sameProcess " << this->dataPtr->sameProcess << '\n'; - // if (this->dataPtr->sameProcess) - // return; + if (this->dataPtr->sameProcess) + return; // set up service for async state response callback std::string id = std::to_string(gui::App()->applicationPid()); diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index b5d7014726..f03acbb1aa 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -93,7 +93,6 @@ void GzSceneManager::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("GzSceneManager::Update"); - std::cerr << "GzSceneManager sameProcess " << this->dataPtr->sameProcess << '\n'; this->dataPtr->renderUtil.UpdateECM(_info, _ecm); if (this->dataPtr->sameProcess) { diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index b1d32708e6..23bea6a50e 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -417,6 +417,12 @@ Sensors::~Sensors() void Sensors::SetSameProcess(bool _sameProcess) { this->dataPtr->sameProcess = _sameProcess; + + if (this->dataPtr->sameProcess) + { + this->dataPtr->renderConn = this->dataPtr->eventManager->Connect( + std::bind(&SensorsPrivate::Render, this->dataPtr.get())); + } } ////////////////////////////////////////////////// @@ -465,12 +471,6 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->stopConn = _eventMgr.Connect( std::bind(&SensorsPrivate::Stop, this->dataPtr.get())); - if (this->dataPtr->sameProcess) - { - this->dataPtr->renderConn = _eventMgr.Connect( - std::bind(&SensorsPrivate::Render, this->dataPtr.get())); - } - // Kick off worker thread this->dataPtr->Run(); } From ea7107f84bc9889451368f2c005540890c3258c7 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 8 Jun 2021 10:16:14 +0200 Subject: [PATCH 09/35] Updates Signed-off-by: ahcorde --- include/ignition/gazebo/Events.hh | 45 +++ .../ignition/gazebo/rendering/RenderUtil.hh | 13 +- src/SdfEntityCreator.cc | 1 + src/gui/GuiRunner.cc | 15 +- src/gui/plugins/entity_tree/EntityTree.cc | 260 ++++++++++++++---- src/gui/plugins/entity_tree/EntityTree.hh | 3 + .../plugins/scene_manager/GzSceneManager.cc | 50 +++- src/rendering/RenderUtil.cc | 220 ++++++++++++--- src/rendering/SceneManager.cc | 8 + src/systems/sensors/Sensors.cc | 6 +- 10 files changed, 494 insertions(+), 127 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 2e8a469fac..3c74990b3b 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -59,7 +59,52 @@ namespace ignition using LoadPlugins = common::EventT; + /// \brief Event used to emit a render event when running in one process. + /// This is required because we have two RenderUtils classes when there + /// is a render sensor in the scene (camera, depth sensor, etc). + /// We could only have one thread updating the renderscene, with this + /// signal we are able to call grom the GzSceneManager the render calls + /// required by the sensor + /// + /// For example: + /// \code + /// eventManager.Emit(); + /// \endcode using Render = ignition::common::EventT; + + /// \brief Event used to emit render event when running in one process + using EnableSensors = + ignition::common::EventT; + + /// \brief Event used to emit a render event when running in one process. + /// This will allow to emit a signal to remove an entity, this event is + /// used for example in the entitytree. The ECM is udpated at 30Hz in + /// GUI thread, which means it will miss some additions or removals + /// This event will allow us to remove entities indenpendly from the + /// update rate. + /// + /// For example: + /// \code + /// eventManager.Emit(_entity); + /// \endcode + using RemoveFromECM = + ignition::common::EventT; + + /// \brief Event used to emit a render event when running in one process. + /// This will allow to emit a signal to add an entity, this event is + /// used for example in the entitytree. The ECM is udpated at 30Hz in + /// GUI thread, which means it will miss some additions or removals + /// This event will allow us to add entities indenpendly from the + /// update rate. + /// + /// For example: + /// \code + /// eventManager.Emit( + /// _entity, _name, _parent); + /// \endcode + using AddToECM = + ignition::common::EventT; } } // namespace events } // namespace gazebo diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 0e50cbb926..83bc46e965 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -27,8 +27,9 @@ #include #include -#include "ignition/gazebo/rendering/SceneManager.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/rendering/MarkerManager.hh" +#include "ignition/gazebo/rendering/SceneManager.hh" namespace ignition @@ -44,7 +45,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { class IGNITION_GAZEBO_RENDERING_VISIBLE RenderUtil { /// \brief Constructor - public: explicit RenderUtil(bool updateNewEntities = false); + public: RenderUtil(); /// \brief Destructor public: ~RenderUtil(); @@ -62,6 +63,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Main update function. Must be called in the rendering thread. public: void Update(); + /// brief Set the event Manager + /// \param[in] _eventMgr Reference to the event Manager + public: void SetEventManager(EventManager &_eventMgr); + /// \brief Get a pointer to the scene /// \return Pointer to the scene public: rendering::ScenePtr Scene() const; @@ -76,10 +81,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void UpdateFromECM(const UpdateInfo &_info, const EntityComponentManager &_ecm); - /// \brief Helper PostUpdate function for updating the scene - public: void UpdateFromECM2(const UpdateInfo &_info, - const EntityComponentManager &_ecm); - /// \brief Set the rendering engine to use /// \param[in] _engineName Name of the rendering engine. public: void SetEngineName(const std::string &_engineName); diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 91a8aed645..5904b8e5a1 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -839,6 +839,7 @@ void SdfEntityCreator::RequestRemoveEntity(Entity _entity, bool _recursive) } this->dataPtr->ecm->RequestRemoveEntity(_entity, _recursive); + this->dataPtr->eventManager->Emit(_entity); } ////////////////////////////////////////////////// diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index ac2c959e81..7472ada6b2 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -134,11 +134,6 @@ GuiRunner::~GuiRunner() ///////////////////////////////////////////////// void GuiRunner::RequestState() { - std::cerr << "RequestState sameProcess " << this->dataPtr->sameProcess << '\n'; - - if (this->dataPtr->sameProcess) - return; - // set up service for async state response callback std::string id = std::to_string(gui::App()->applicationPid()); std::string reqSrv = @@ -180,7 +175,15 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) << "]" << std::endl; return; } - plugin->Configure(this->dataPtr->eventMgr, this->dataPtr->sameProcess); + + // The call above always return the same plugin, which the first that was + // load. This plugin will set again the eventMgr and the sameProcess + // state. + auto plugins = gui::App()->findChildren(); + for (auto &p: plugins) + { + p->Configure(this->dataPtr->eventMgr, this->dataPtr->sameProcess); + } this->RequestState(); } diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 8a0ae74152..2cf497ab00 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -26,6 +26,7 @@ #include #include +#include "ignition/gazebo/Events.hh" #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" @@ -44,6 +45,13 @@ namespace ignition::gazebo { + struct AddNewEntity + { + unsigned int entity; + std::string name; + unsigned int parent; + }; + class EntityTreePrivate { /// \brief Model holding all the current entities in the world. @@ -52,6 +60,36 @@ namespace ignition::gazebo /// \brief True if initialized public: bool initialized{false}; + /// \brief Event Manager + public: EventManager *eventManager{nullptr}; + + /// \brief is the simulation running in the same process + public: bool sameProcess{false}; + + /// \brief is any sensor available in the simulation + public: bool enableSensors{false}; + + /// \brief Connection to events::removeFromECM event, used to remove + /// entities. + public: ignition::common::ConnectionPtr removeFromECMConn; + + /// \brief Connection to events::removeFromECMConn event, used to add + /// entities. + public: ignition::common::ConnectionPtr addToECMConn; + + /// \brief vector with entities to add to the entity tree. + public: std::vector entitiesToAdd; + + /// \brief method to handle the removeFromECM event + public: void removeEntityEvent(unsigned int _entity); + + /// \brief method to handle the addToECM event + /// \param[in] _entity entity to add + /// \param[in] _name name of the entity + /// \param[in] _parentEntity parant of the entity + public: void addEntityEvent(unsigned int _entity, std::string _name, + unsigned int _parentEntity); + /// \brief World entity public: Entity worldEntity{kNullEntity}; }; @@ -274,6 +312,43 @@ EntityTree::EntityTree() ///////////////////////////////////////////////// EntityTree::~EntityTree() = default; +void EntityTreePrivate::removeEntityEvent(unsigned int _entity) +{ + QMetaObject::invokeMethod(&this->treeModel, "RemoveEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, _entity)); +} + +void EntityTreePrivate::addEntityEvent(unsigned int _entity, std::string _name, unsigned int _parentEntity) +{ + entitiesToAdd.push_back(AddNewEntity {_entity, _name, _parentEntity}); +} + +///////////////////////////////////////////////// +void EntityTree::Configure(EventManager &_eventMgr, bool _sameProcess) +{ + if (this->dataPtr->eventManager) + return; + + this->dataPtr->eventManager = &_eventMgr; + this->dataPtr->sameProcess = _sameProcess; + + if (this->dataPtr->sameProcess) + { + this->dataPtr->removeFromECMConn = + _eventMgr.Connect( + std::bind(&EntityTreePrivate::removeEntityEvent, this->dataPtr.get(), + std::placeholders::_1)); + + this->dataPtr->addToECMConn = + _eventMgr.Connect( + std::bind(&EntityTreePrivate::addEntityEvent, this->dataPtr.get(), + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); + } +} + ///////////////////////////////////////////////// void EntityTree::LoadConfig(const tinyxml2::XMLElement *) { @@ -288,87 +363,150 @@ void EntityTree::LoadConfig(const tinyxml2::XMLElement *) void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) { IGN_PROFILE("EntityTree::Update"); - // Treat all pre-existent entities as new at startup - if (!this->dataPtr->initialized) + if (this->dataPtr->sameProcess) { - _ecm.Each( - [&](const Entity &_entity, - const components::Name *_name)->bool + for (auto &entityToAdd: this->dataPtr->entitiesToAdd) { - auto worldComp = _ecm.Component(_entity); - if (worldComp) + if (!this->dataPtr->initialized) { - this->dataPtr->worldEntity = _entity; + auto worldComp = _ecm.Component(entityToAdd.entity); + if (worldComp) + { + this->dataPtr->worldEntity = entityToAdd.entity; + continue; + } - // Skipping the world for now to keep the tree shallow - return true; - } + // Parent + Entity parentEntity{kNullEntity}; - // Parent - Entity parentEntity{kNullEntity}; + auto parentComp = + _ecm.Component(entityToAdd.entity); + if (parentComp) + { + parentEntity = parentComp->Data(); + } - auto parentComp = _ecm.Component(_entity); - if (parentComp) - { - parentEntity = parentComp->Data(); + // World children are top-level + if (this->dataPtr->worldEntity != kNullEntity && + parentEntity == this->dataPtr->worldEntity) + { + parentEntity = kNullEntity; + } + + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, entityToAdd.entity), + Q_ARG(QString, QString::fromStdString(entityToAdd.name)), + Q_ARG(unsigned int, parentEntity), + Q_ARG(QString, entityType(entityToAdd.entity, _ecm))); + if (this->dataPtr->worldEntity != kNullEntity) + this->dataPtr->initialized = true; } + else{ + auto parentEntity = entityToAdd.parent; - // World children are top-level - if (this->dataPtr->worldEntity != kNullEntity && - parentEntity == this->dataPtr->worldEntity) - { - parentEntity = kNullEntity; + // World children are top-level + if (this->dataPtr->worldEntity != kNullEntity && + parentEntity == this->dataPtr->worldEntity) + { + parentEntity = kNullEntity; + } + + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, entityToAdd.entity), + Q_ARG(QString, QString::fromStdString(entityToAdd.name)), + Q_ARG(unsigned int, parentEntity), + Q_ARG(QString, entityType(entityToAdd.entity, _ecm))); } - - QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", - Qt::QueuedConnection, - Q_ARG(unsigned int, _entity), - Q_ARG(QString, QString::fromStdString(_name->Data())), - Q_ARG(unsigned int, parentEntity), - Q_ARG(QString, entityType(_entity, _ecm))); - return true; - }); - - if (this->dataPtr->worldEntity != kNullEntity) - this->dataPtr->initialized = true; + } + this->dataPtr->entitiesToAdd.clear(); } else { - // Requiring a parent entity because we're not adding the world, which is - // parentless, to the tree - _ecm.EachNew( - [&](const Entity &_entity, - const components::Name *_name, - const components::ParentEntity *_parentEntity)->bool + // Treat all pre-existent entities as new at startup + if (!this->dataPtr->initialized) { - auto parentEntity = _parentEntity->Data(); + _ecm.Each( + [&](const Entity &_entity, + const components::Name *_name)->bool + { + auto worldComp = _ecm.Component(_entity); + if (worldComp) + { + this->dataPtr->worldEntity = _entity; + + // Skipping the world for now to keep the tree shallow + return true; + } + + // Parent + Entity parentEntity{kNullEntity}; + + auto parentComp = _ecm.Component(_entity); + if (parentComp) + { + parentEntity = parentComp->Data(); + } - // World children are top-level - if (this->dataPtr->worldEntity != kNullEntity && - parentEntity == this->dataPtr->worldEntity) + // World children are top-level + if (this->dataPtr->worldEntity != kNullEntity && + parentEntity == this->dataPtr->worldEntity) + { + parentEntity = kNullEntity; + } + + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, _entity), + Q_ARG(QString, QString::fromStdString(_name->Data())), + Q_ARG(unsigned int, parentEntity), + Q_ARG(QString, entityType(_entity, _ecm))); + return true; + }); + + if (this->dataPtr->worldEntity != kNullEntity) + this->dataPtr->initialized = true; + } + else + { + // Requiring a parent entity because we're not adding the world, which is + // parentless, to the tree + _ecm.EachNew( + [&](const Entity &_entity, + const components::Name *_name, + const components::ParentEntity *_parentEntity)->bool { - parentEntity = kNullEntity; - } + auto parentEntity = _parentEntity->Data(); + + // World children are top-level + if (this->dataPtr->worldEntity != kNullEntity && + parentEntity == this->dataPtr->worldEntity) + { + parentEntity = kNullEntity; + } + + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, _entity), + Q_ARG(QString, QString::fromStdString(_name->Data())), + Q_ARG(unsigned int, parentEntity), + Q_ARG(QString, entityType(_entity, _ecm))); + return true; + }); + } - QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::Name *)->bool + { + std::cerr << "Remove Entity tree" << '\n'; + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, _entity), - Q_ARG(QString, QString::fromStdString(_name->Data())), - Q_ARG(unsigned int, parentEntity), - Q_ARG(QString, entityType(_entity, _ecm))); + Q_ARG(unsigned int, _entity)); return true; }); } - - _ecm.EachRemoved( - [&](const Entity &_entity, - const components::Name *)->bool - { - QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", - Qt::QueuedConnection, - Q_ARG(unsigned int, _entity)); - return true; - }); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/entity_tree/EntityTree.hh b/src/gui/plugins/entity_tree/EntityTree.hh index 4f882de9f7..1f0824ad57 100644 --- a/src/gui/plugins/entity_tree/EntityTree.hh +++ b/src/gui/plugins/entity_tree/EntityTree.hh @@ -120,6 +120,9 @@ namespace gazebo // Documentation inherited public: void Update(const UpdateInfo &, EntityComponentManager &) override; + // Documentation inherited + public: void Configure(EventManager &_eventMgr, bool _sameProcess) override; + /// \brief Callback when an entity has been selected. This should be /// called from QML. /// \param[in] _entity Entity being selected. diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index f03acbb1aa..a4326d3801 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -45,13 +45,29 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Update the 3D scene based on the latest state of the ECM. public: void OnRender(); + /// \brief This method is used to connect with the event + /// events::EnableSensors. It will set if the simulation is running any + /// sensors + public: void EnableSensors(bool _enable); + //// \brief Pointer to the rendering scene public: rendering::ScenePtr scene; + /// \brief Event Manager public: EventManager *eventManager{nullptr}; + /// \brief Is the simulation running the GUI and server in the same process public: bool sameProcess{false}; + /// \brief is the sensors system plugin running ? + public: bool enableSensors{false}; + + /// \brief did the first render event occur? + public: bool emitFirstRender{false}; + + /// \brief Track connection to "EnableSensors" Event + public: ignition::common::ConnectionPtr enableSensorsConn; + /// \brief Rendering utility public: RenderUtil renderUtil; }; @@ -74,8 +90,21 @@ GzSceneManager::~GzSceneManager() = default; ///////////////////////////////////////////////// void GzSceneManager::Configure(EventManager &_eventMgr, bool _sameProcess) { + if (this->dataPtr->eventManager) + return; + this->dataPtr->eventManager = &_eventMgr; this->dataPtr->sameProcess = _sameProcess; + + this->dataPtr->renderUtil.SetEventManager(_eventMgr); + + if (_sameProcess) + { + this->dataPtr->enableSensorsConn = + _eventMgr.Connect( + std::bind(&GzSceneManagerPrivate::EnableSensors, this->dataPtr.get(), + std::placeholders::_1)); + } } ///////////////////////////////////////////////// @@ -93,13 +122,10 @@ void GzSceneManager::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("GzSceneManager::Update"); - this->dataPtr->renderUtil.UpdateECM(_info, _ecm); - if (this->dataPtr->sameProcess) - { - this->dataPtr->renderUtil.UpdateFromECM2(_info, _ecm); - } - else + if (this->dataPtr->emitFirstRender && + (!this->dataPtr->sameProcess || !this->dataPtr->enableSensors)) { + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); } } @@ -113,6 +139,7 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) if (this->dataPtr->sameProcess) { this->dataPtr->eventManager->Emit(); + this->dataPtr->emitFirstRender = true; } } @@ -120,6 +147,12 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) return QObject::eventFilter(_obj, _event); } +///////////////////////////////////////////////// +void GzSceneManagerPrivate::EnableSensors(bool _enable) +{ + this->enableSensors = _enable; +} + ///////////////////////////////////////////////// void GzSceneManagerPrivate::OnRender() { @@ -132,7 +165,10 @@ void GzSceneManagerPrivate::OnRender() this->renderUtil.SetScene(this->scene); } - this->renderUtil.Update(); + if (this->emitFirstRender && (!this->sameProcess || !this->enableSensors)) + { + this->renderUtil.Update(); + } } // Register this plugin diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 0793fec6df..a9f3e2f2a6 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -77,6 +77,7 @@ #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Events.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" #include "ignition/gazebo/rendering/SceneManager.hh" @@ -330,13 +331,13 @@ class ignition::gazebo::RenderUtilPrivate public: std::unordered_map> thermalCameraData; - public: bool sameProcess; + /// \brief Event Manager + public: EventManager *eventManager{nullptr}; }; ////////////////////////////////////////////////// -RenderUtil::RenderUtil(bool _sameProcess) : dataPtr(std::make_unique()) +RenderUtil::RenderUtil() : dataPtr(std::make_unique()) { - this->dataPtr->sameProcess = _sameProcess; } ////////////////////////////////////////////////// @@ -348,6 +349,11 @@ rendering::ScenePtr RenderUtil::Scene() const return this->dataPtr->scene; } +void RenderUtil::SetEventManager(EventManager &_eventMgr) +{ + this->dataPtr->eventManager = &_eventMgr; +} + ////////////////////////////////////////////////// void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) @@ -470,21 +476,6 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->FindCollisionLinks(_ecm); } -////////////////////////////////////////////////// -void RenderUtil::UpdateFromECM2(const UpdateInfo &_info, - const EntityComponentManager &_ecm) -{ - IGN_PROFILE("RenderUtil::UpdateFromECM"); - std::lock_guard lock(this->dataPtr->updateMutex); - this->dataPtr->simTime = _info.simTime; - - // this->dataPtr->CreateRenderingEntities(_ecm, _info); - this->dataPtr->UpdateRenderingEntities(_ecm); - this->dataPtr->RemoveRenderingEntities(_ecm, _info); - this->dataPtr->markerManager.SetSimTime(_info.simTime); - this->dataPtr->FindCollisionLinks(_ecm); -} - ////////////////////////////////////////////////// void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) { @@ -1154,6 +1145,7 @@ void RenderUtilPrivate::CreateRenderingEntities( this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); + this->eventManager->Emit(_entity, "None", 0); return true; }); @@ -1169,9 +1161,23 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - this->newModels.push_back( - std::make_tuple(_entity, model, _parent->Data(), - _info.iterations)); + auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), + _info.iterations); + bool found = false; + for (auto & data: this->newModels) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newModels.push_back(tupleTemp); + std::cerr << "Emit new Model " << _entity << '\n'; + this->eventManager->Emit(_entity, _name->Data(), _parent->Data()); + } return true; }); @@ -1186,8 +1192,19 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Link link; link.SetName(_name->Data()); link.SetRawPose(_pose->Data()); - this->newLinks.push_back( - std::make_tuple(_entity, link, _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, link, _parent->Data()); + bool found = false; + for (auto & data: this->newLinks) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newLinks.push_back(tupleTemp); + // used for collsions this->modelToLinkEntities[_parent->Data()].push_back(_entity); return true; @@ -1256,8 +1273,19 @@ void RenderUtilPrivate::CreateRenderingEntities( } } - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, visual, _parent->Data()); + bool found = false; + for (auto & data: this->newVisuals) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newVisuals.push_back(tupleTemp); + return true; }); @@ -1267,8 +1295,19 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::Actor *_actor, const components::ParentEntity *_parent) -> bool { - this->newActors.push_back( - std::make_tuple(_entity, _actor->Data(), _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, _actor->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newActors) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newActors.push_back(tupleTemp); + return true; }); @@ -1278,8 +1317,19 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::Light *_light, const components::ParentEntity *_parent) -> bool { - this->newLights.push_back( - std::make_tuple(_entity, _light->Data(), _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, _light->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newLights) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newLights.push_back(tupleTemp); + return true; }); @@ -1306,8 +1356,19 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::ParticleEmitter *_emitter, const components::ParentEntity *_parent) -> bool { - this->newParticleEmitters.push_back( - std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, _emitter->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newParticleEmitters) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newParticleEmitters.push_back(tupleTemp); + return true; }); @@ -1383,6 +1444,7 @@ void RenderUtilPrivate::CreateRenderingEntities( this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); + this->eventManager->Emit(_entity, "None", 0); return true; }); @@ -1397,9 +1459,23 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - this->newModels.push_back( - std::make_tuple(_entity, model, _parent->Data(), - _info.iterations)); + auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), + _info.iterations); + bool found = false; + for (auto & data: this->newModels) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newModels.push_back(tupleTemp); + std::cerr << "Emit new Model2 " << _entity << '\n'; + this->eventManager->Emit(_entity, _name->Data(), _parent->Data()); + } return true; }); @@ -1414,8 +1490,19 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Link link; link.SetName(_name->Data()); link.SetRawPose(_pose->Data()); - this->newLinks.push_back( - std::make_tuple(_entity, link, _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, link, _parent->Data()); + bool found = false; + for (auto & data: this->newLinks) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newLinks.push_back(tupleTemp); + // used for collsions this->modelToLinkEntities[_parent->Data()].push_back(_entity); return true; @@ -1484,8 +1571,19 @@ void RenderUtilPrivate::CreateRenderingEntities( } } - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, visual, _parent->Data()); + bool found = false; + for (auto & data: this->newVisuals) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newVisuals.push_back(tupleTemp); + return true; }); @@ -1495,8 +1593,19 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::Actor *_actor, const components::ParentEntity *_parent) -> bool { - this->newActors.push_back( - std::make_tuple(_entity, _actor->Data(), _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, _actor->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newActors) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newActors.push_back(tupleTemp); + return true; }); @@ -1506,8 +1615,19 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::Light *_light, const components::ParentEntity *_parent) -> bool { - this->newLights.push_back( - std::make_tuple(_entity, _light->Data(), _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, _light->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newLights) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newLights.push_back(tupleTemp); + return true; }); @@ -1534,8 +1654,19 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::ParticleEmitter *_emitter, const components::ParentEntity *_parent) -> bool { - this->newParticleEmitters.push_back( - std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + auto tupleTemp = std::make_tuple(_entity, _emitter->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newParticleEmitters) + { + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + this->newParticleEmitters.push_back(tupleTemp); + return true; }); @@ -1753,6 +1884,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( { this->removeEntities[_entity] = _info.iterations; this->modelToLinkEntities.erase(_entity); + this->eventManager->Emit(_entity); return true; }); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index bf535a919e..474044e547 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -1109,6 +1109,14 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, if (parent) name = parent->Name() + "::" + name; + if (this->dataPtr->scene->HasLightName(name)) + { + ignerr << "Visual: [" << name << " - " << _id << "] already exists" << std::endl; + auto l = this->dataPtr->scene->LightByName(name); + this->dataPtr->lights[_id] = l; + return l; + } + rendering::LightPtr light; switch (_light.Type()) { diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 23bea6a50e..fbc12bbec4 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -58,9 +58,6 @@ using namespace systems; // Private data class. class ignition::gazebo::systems::SensorsPrivate { - public: explicit SensorsPrivate() - : renderUtil(false){} - /// \brief Sensor manager object. This manages the lifecycle of the /// instantiated sensors. public: sensors::Manager sensorManager; @@ -471,6 +468,8 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->stopConn = _eventMgr.Connect( std::bind(&SensorsPrivate::Stop, this->dataPtr.get())); + this->dataPtr->renderUtil.SetEventManager(_eventMgr); + // Kick off worker thread this->dataPtr->Run(); } @@ -515,6 +514,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (rendering::isEngineLoaded("ogre") || rendering::isEngineLoaded("ogre2")) { igndbg << "Initialization needed" << std::endl; + this->dataPtr->eventManager->Emit(true); this->dataPtr->doInit = true; this->dataPtr->renderCv.notify_one(); } From cc4cf71f45836ad6478ab8c69af5dc4a19201d5d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 8 Jun 2021 12:11:06 +0200 Subject: [PATCH 10/35] Updates Signed-off-by: ahcorde --- include/ignition/gazebo/Events.hh | 9 +++ src/SdfEntityCreator.cc | 1 - src/gui/plugins/entity_tree/EntityTree.cc | 1 - .../plugins/scene_manager/GzSceneManager.cc | 2 +- src/rendering/RenderUtil.cc | 81 ++++++++++++++++--- src/systems/physics/Physics.cc | 20 ++++- src/systems/sensors/Sensors.cc | 4 +- 7 files changed, 100 insertions(+), 18 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 3c74990b3b..33933d20e7 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -23,6 +23,7 @@ #include "ignition/gazebo/config.hh" #include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/EntityComponentManager.hh" namespace ignition { @@ -105,6 +106,14 @@ namespace ignition using AddToECM = ignition::common::EventT; + + /// \brief Event used to emit a render event when running in one process. + /// Same remove event are lost because of the rate when running in the + /// same process without sensors. This event is launched in the physics + /// system plugin to remove entities in the renderUtil + using UpdateGUIECM = common::EventT; } } // namespace events } // namespace gazebo diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 5904b8e5a1..91a8aed645 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -839,7 +839,6 @@ void SdfEntityCreator::RequestRemoveEntity(Entity _entity, bool _recursive) } this->dataPtr->ecm->RequestRemoveEntity(_entity, _recursive); - this->dataPtr->eventManager->Emit(_entity); } ////////////////////////////////////////////////// diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 2cf497ab00..88b391803f 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -500,7 +500,6 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) [&](const Entity &_entity, const components::Name *)->bool { - std::cerr << "Remove Entity tree" << '\n'; QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", Qt::QueuedConnection, Q_ARG(unsigned int, _entity)); diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index a4326d3801..626bbb30a6 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -139,8 +139,8 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) if (this->dataPtr->sameProcess) { this->dataPtr->eventManager->Emit(); - this->dataPtr->emitFirstRender = true; } + this->dataPtr->emitFirstRender = true; } // Standard event processing diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index a9f3e2f2a6..0b838084f7 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -333,6 +333,10 @@ class ignition::gazebo::RenderUtilPrivate /// \brief Event Manager public: EventManager *eventManager{nullptr}; + + /// \brief Connection to events::UpdateGUIECMConn event, used to remove + /// entities. + public: ignition::common::ConnectionPtr UpdateGUIECMConn; }; ////////////////////////////////////////////////// @@ -352,6 +356,10 @@ rendering::ScenePtr RenderUtil::Scene() const void RenderUtil::SetEventManager(EventManager &_eventMgr) { this->dataPtr->eventManager = &_eventMgr; + this->dataPtr->UpdateGUIECMConn = + _eventMgr.Connect( + std::bind(&RenderUtilPrivate::RemoveRenderingEntities, this->dataPtr.get(), + std::placeholders::_1, std::placeholders::_2)); } ////////////////////////////////////////////////// @@ -1145,7 +1153,9 @@ void RenderUtilPrivate::CreateRenderingEntities( this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); - this->eventManager->Emit(_entity, "None", 0); + if (this->eventManager) + this->eventManager->Emit( + _entity, "None", 0); return true; }); @@ -1175,8 +1185,9 @@ void RenderUtilPrivate::CreateRenderingEntities( if (!found) { this->newModels.push_back(tupleTemp); - std::cerr << "Emit new Model " << _entity << '\n'; - this->eventManager->Emit(_entity, _name->Data(), _parent->Data()); + if (this->eventManager) + this->eventManager->Emit( + _entity, _name->Data(), _parent->Data()); } return true; }); @@ -1202,8 +1213,12 @@ void RenderUtilPrivate::CreateRenderingEntities( break; } } - if (!found) + if (!found){ this->newLinks.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _name->Data(), _parent->Data()); + } // used for collsions this->modelToLinkEntities[_parent->Data()].push_back(_entity); @@ -1283,8 +1298,12 @@ void RenderUtilPrivate::CreateRenderingEntities( break; } } - if (!found) + if (!found){ this->newVisuals.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _name->Data(), _parent->Data()); + } return true; }); @@ -1306,7 +1325,12 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) + { this->newActors.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _actor->Data().Name(), _parent->Data()); + } return true; }); @@ -1327,8 +1351,12 @@ void RenderUtilPrivate::CreateRenderingEntities( break; } } - if (!found) + if (!found){ this->newLights.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _light->Data().Name(), _parent->Data()); + } return true; }); @@ -1367,7 +1395,12 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) + { this->newParticleEmitters.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _emitter->Data().name(), _parent->Data()); + } return true; }); @@ -1444,7 +1477,9 @@ void RenderUtilPrivate::CreateRenderingEntities( this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); - this->eventManager->Emit(_entity, "None", 0); + if (this->eventManager) + this->eventManager->Emit( + _entity, "None", 0); return true; }); @@ -1474,7 +1509,9 @@ void RenderUtilPrivate::CreateRenderingEntities( { this->newModels.push_back(tupleTemp); std::cerr << "Emit new Model2 " << _entity << '\n'; - this->eventManager->Emit(_entity, _name->Data(), _parent->Data()); + if (this->eventManager) + this->eventManager->Emit( + _entity, _name->Data(), _parent->Data()); } return true; }); @@ -1501,7 +1538,12 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) + { this->newLinks.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _name->Data(), _parent->Data()); + } // used for collsions this->modelToLinkEntities[_parent->Data()].push_back(_entity); @@ -1582,7 +1624,12 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) + { this->newVisuals.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _name->Data(), _parent->Data()); + } return true; }); @@ -1604,7 +1651,12 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) + { this->newActors.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _actor->Data().Name(), _parent->Data()); + } return true; }); @@ -1626,7 +1678,12 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) + { this->newLights.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _light->Data().Name(), _parent->Data()); + } return true; }); @@ -1665,7 +1722,12 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) + { this->newParticleEmitters.push_back(tupleTemp); + if (this->eventManager) + this->eventManager->Emit( + _entity, _emitter->Data().name(), _parent->Data()); + } return true; }); @@ -1884,7 +1946,8 @@ void RenderUtilPrivate::RemoveRenderingEntities( { this->removeEntities[_entity] = _info.iterations; this->modelToLinkEntities.erase(_entity); - this->eventManager->Emit(_entity); + if (this->eventManager) + this->eventManager->Emit(_entity); return true; }); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 97c1bf9d91..e9110f04f8 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -77,6 +77,7 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/Events.hh" // Components #include "ignition/gazebo/components/AngularAcceleration.hh" @@ -173,7 +174,8 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Remove physics entities if they are removed from the ECM /// \param[in] _ecm Constant reference to ECM. - public: void RemovePhysicsEntities(const EntityComponentManager &_ecm); + public: void RemovePhysicsEntities(const EntityComponentManager &_ecm, + const UpdateInfo &_info); /// \brief Update physics from components /// \param[in] _ecm Mutable reference to ECM. @@ -439,6 +441,9 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief A map between collision entity ids in the ECM to FreeGroup Entities /// in ign-physics. public: EntityFreeGroupMap entityFreeGroupMap; + + /// \brief Pointer to the event manager + public: EventManager *eventManager{nullptr}; }; ////////////////////////////////////////////////// @@ -450,8 +455,11 @@ Physics::Physics() : System(), dataPtr(std::make_unique()) void Physics::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) + EventManager &_eventMgr) { + + this->dataPtr->eventManager = &_eventMgr; + std::string pluginLib; // 1. Engine from component (from command line / ServerConfig) @@ -589,7 +597,7 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) // Entities scheduled to be removed should be removed from physics after the // simulation step. Otherwise, since the to-be-removed entity still shows up // in the ECM::Each the UpdatePhysics and UpdateSim calls will have an error - this->dataPtr->RemovePhysicsEntities(_ecm); + this->dataPtr->RemovePhysicsEntities(_ecm, _info); } } @@ -1119,7 +1127,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm, + const UpdateInfo &_info) { // Assume the world will not be erased // Only removing models is supported by ign-physics right now so we only @@ -1132,6 +1141,9 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) [&](const Entity &_entity, const components::Model * /* _model */) -> bool { + if (this->eventManager) + this->eventManager->Emit(_ecm, _info); + // Remove model if found if (auto modelPtrPhys = this->entityModelMap.Get(_entity)) { diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index fbc12bbec4..6793627b70 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -417,6 +417,8 @@ void Sensors::SetSameProcess(bool _sameProcess) if (this->dataPtr->sameProcess) { + this->dataPtr->renderUtil.SetEventManager(*this->dataPtr->eventManager); + this->dataPtr->renderConn = this->dataPtr->eventManager->Connect( std::bind(&SensorsPrivate::Render, this->dataPtr.get())); } @@ -468,8 +470,6 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->stopConn = _eventMgr.Connect( std::bind(&SensorsPrivate::Stop, this->dataPtr.get())); - this->dataPtr->renderUtil.SetEventManager(_eventMgr); - // Kick off worker thread this->dataPtr->Run(); } From 5521b407e41376643d2064a79643f52673c73f4b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 9 Jun 2021 10:27:16 +0200 Subject: [PATCH 11/35] Added feedback Signed-off-by: ahcorde --- include/ignition/gazebo/Events.hh | 42 +-- src/SimulationRunner.cc | 8 + src/gui/GuiRunner.cc | 21 +- src/gui/plugins/entity_tree/EntityTree.cc | 264 +++++------------- src/gui/plugins/entity_tree/EntityTree.hh | 3 - src/rendering/RenderUtil.cc | 323 +++++++++++----------- src/systems/physics/Physics.cc | 20 +- 7 files changed, 257 insertions(+), 424 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 33933d20e7..6938716e66 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -78,42 +78,12 @@ namespace ignition ignition::common::EventT; /// \brief Event used to emit a render event when running in one process. - /// This will allow to emit a signal to remove an entity, this event is - /// used for example in the entitytree. The ECM is udpated at 30Hz in - /// GUI thread, which means it will miss some additions or removals - /// This event will allow us to remove entities indenpendly from the - /// update rate. - /// - /// For example: - /// \code - /// eventManager.Emit(_entity); - /// \endcode - using RemoveFromECM = - ignition::common::EventT; - - /// \brief Event used to emit a render event when running in one process. - /// This will allow to emit a signal to add an entity, this event is - /// used for example in the entitytree. The ECM is udpated at 30Hz in - /// GUI thread, which means it will miss some additions or removals - /// This event will allow us to add entities indenpendly from the - /// update rate. - /// - /// For example: - /// \code - /// eventManager.Emit( - /// _entity, _name, _parent); - /// \endcode - using AddToECM = - ignition::common::EventT; - - /// \brief Event used to emit a render event when running in one process. - /// Same remove event are lost because of the rate when running in the - /// same process without sensors. This event is launched in the physics - /// system plugin to remove entities in the renderUtil - using UpdateGUIECM = common::EventT; + /// Some remove events are lost because of the rate when running in the + /// same process without sensors. This event is launched in the Simulation + /// runner when there is any new entity or entity marked to be removed + /// to remove/add entities in the renderUtil + using UpdateGUIThread = + ignition::common::EventT; } } // namespace events } // namespace gazebo diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 9e05256377..dcd8eac063 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -524,6 +524,14 @@ void SimulationRunner::UpdateSystems() IGN_PROFILE("Update"); for (auto& system : this->systemsUpdate) system->Update(this->currentInfo, this->entityCompMgr); + + if (serverConfig.SameProcessAsGUI()){ + if (this->entityCompMgr.HasNewEntities() || + this->entityCompMgr.HasEntitiesMarkedForRemoval()) + { + this->eventMgr.Emit(); + } + } } { diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 7472ada6b2..00e646ad66 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -26,6 +26,7 @@ #include "ignition/gazebo/components/components.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Events.hh" #include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/GuiRunner.hh" #include "ignition/gazebo/gui/GuiSystem.hh" @@ -38,11 +39,21 @@ class ignition::gazebo::GuiRunner::Implementation { public: explicit Implementation(gazebo::EntityComponentManager &_ecm, gazebo::EventManager &_eventMgr) - : ecm(_ecm), eventMgr(_eventMgr){} + : ecm(_ecm), eventMgr(_eventMgr) + { + this->UpdatePluginsConn = + _eventMgr.Connect( + std::bind(&Implementation::UpdatePluginsEvent, this)); + } /// \brief Update the plugins. public: void UpdatePlugins(); + /// \brief + void UpdatePluginsEvent(); + + public: ignition::common::ConnectionPtr UpdatePluginsConn; + /// \brief Entity-component manager. public: gazebo::EntityComponentManager &ecm; @@ -79,8 +90,6 @@ GuiRunner::GuiRunner(const std::string &_worldName, this->dataPtr->sameProcess = _sameProcess; this->setProperty("worldName", QString::fromStdString(_worldName)); - this->setProperty("sameProcess", - QString::fromStdString(std::to_string(_sameProcess))); auto win = gui::App()->findChild(); auto winWorldNames = win->property("worldNames").toStringList(); @@ -233,3 +242,9 @@ void GuiRunner::Implementation::UpdatePlugins() } this->ecm.ClearRemovedComponents(); } + +void GuiRunner::Implementation::UpdatePluginsEvent() +{ + std::lock_guard lock(this->updateMutex); + this->UpdatePlugins(); +} diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 88b391803f..983e524e51 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -45,13 +45,6 @@ namespace ignition::gazebo { - struct AddNewEntity - { - unsigned int entity; - std::string name; - unsigned int parent; - }; - class EntityTreePrivate { /// \brief Model holding all the current entities in the world. @@ -60,36 +53,6 @@ namespace ignition::gazebo /// \brief True if initialized public: bool initialized{false}; - /// \brief Event Manager - public: EventManager *eventManager{nullptr}; - - /// \brief is the simulation running in the same process - public: bool sameProcess{false}; - - /// \brief is any sensor available in the simulation - public: bool enableSensors{false}; - - /// \brief Connection to events::removeFromECM event, used to remove - /// entities. - public: ignition::common::ConnectionPtr removeFromECMConn; - - /// \brief Connection to events::removeFromECMConn event, used to add - /// entities. - public: ignition::common::ConnectionPtr addToECMConn; - - /// \brief vector with entities to add to the entity tree. - public: std::vector entitiesToAdd; - - /// \brief method to handle the removeFromECM event - public: void removeEntityEvent(unsigned int _entity); - - /// \brief method to handle the addToECM event - /// \param[in] _entity entity to add - /// \param[in] _name name of the entity - /// \param[in] _parentEntity parant of the entity - public: void addEntityEvent(unsigned int _entity, std::string _name, - unsigned int _parentEntity); - /// \brief World entity public: Entity worldEntity{kNullEntity}; }; @@ -170,6 +133,12 @@ void TreeModel::AddEntity(unsigned int _entity, const QString &_entityName, return; } + auto itemExists = this->entityItems.find(_entity); + if (itemExists != this->entityItems.end()) + { + return; + } + // New entity item auto entityItem = new QStandardItem(_entityName); entityItem->setData(_entityName, this->roleNames().key("entityName")); @@ -312,43 +281,6 @@ EntityTree::EntityTree() ///////////////////////////////////////////////// EntityTree::~EntityTree() = default; -void EntityTreePrivate::removeEntityEvent(unsigned int _entity) -{ - QMetaObject::invokeMethod(&this->treeModel, "RemoveEntity", - Qt::QueuedConnection, - Q_ARG(unsigned int, _entity)); -} - -void EntityTreePrivate::addEntityEvent(unsigned int _entity, std::string _name, unsigned int _parentEntity) -{ - entitiesToAdd.push_back(AddNewEntity {_entity, _name, _parentEntity}); -} - -///////////////////////////////////////////////// -void EntityTree::Configure(EventManager &_eventMgr, bool _sameProcess) -{ - if (this->dataPtr->eventManager) - return; - - this->dataPtr->eventManager = &_eventMgr; - this->dataPtr->sameProcess = _sameProcess; - - if (this->dataPtr->sameProcess) - { - this->dataPtr->removeFromECMConn = - _eventMgr.Connect( - std::bind(&EntityTreePrivate::removeEntityEvent, this->dataPtr.get(), - std::placeholders::_1)); - - this->dataPtr->addToECMConn = - _eventMgr.Connect( - std::bind(&EntityTreePrivate::addEntityEvent, this->dataPtr.get(), - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3)); - } -} - ///////////////////////////////////////////////// void EntityTree::LoadConfig(const tinyxml2::XMLElement *) { @@ -363,149 +295,87 @@ void EntityTree::LoadConfig(const tinyxml2::XMLElement *) void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) { IGN_PROFILE("EntityTree::Update"); - if (this->dataPtr->sameProcess) + // Treat all pre-existent entities as new at startup + if (!this->dataPtr->initialized) { - for (auto &entityToAdd: this->dataPtr->entitiesToAdd) + _ecm.Each( + [&](const Entity &_entity, + const components::Name *_name)->bool { - if (!this->dataPtr->initialized) + auto worldComp = _ecm.Component(_entity); + if (worldComp) { - auto worldComp = _ecm.Component(entityToAdd.entity); - if (worldComp) - { - this->dataPtr->worldEntity = entityToAdd.entity; - continue; - } + this->dataPtr->worldEntity = _entity; - // Parent - Entity parentEntity{kNullEntity}; + // Skipping the world for now to keep the tree shallow + return true; + } - auto parentComp = - _ecm.Component(entityToAdd.entity); - if (parentComp) - { - parentEntity = parentComp->Data(); - } + // Parent + Entity parentEntity{kNullEntity}; - // World children are top-level - if (this->dataPtr->worldEntity != kNullEntity && - parentEntity == this->dataPtr->worldEntity) - { - parentEntity = kNullEntity; - } - - QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", - Qt::QueuedConnection, - Q_ARG(unsigned int, entityToAdd.entity), - Q_ARG(QString, QString::fromStdString(entityToAdd.name)), - Q_ARG(unsigned int, parentEntity), - Q_ARG(QString, entityType(entityToAdd.entity, _ecm))); - if (this->dataPtr->worldEntity != kNullEntity) - this->dataPtr->initialized = true; + auto parentComp = _ecm.Component(_entity); + if (parentComp) + { + parentEntity = parentComp->Data(); } - else{ - auto parentEntity = entityToAdd.parent; - // World children are top-level - if (this->dataPtr->worldEntity != kNullEntity && - parentEntity == this->dataPtr->worldEntity) - { - parentEntity = kNullEntity; - } - - QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", - Qt::QueuedConnection, - Q_ARG(unsigned int, entityToAdd.entity), - Q_ARG(QString, QString::fromStdString(entityToAdd.name)), - Q_ARG(unsigned int, parentEntity), - Q_ARG(QString, entityType(entityToAdd.entity, _ecm))); + // World children are top-level + if (this->dataPtr->worldEntity != kNullEntity && + parentEntity == this->dataPtr->worldEntity) + { + parentEntity = kNullEntity; } - } - this->dataPtr->entitiesToAdd.clear(); + + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, _entity), + Q_ARG(QString, QString::fromStdString(_name->Data())), + Q_ARG(unsigned int, parentEntity), + Q_ARG(QString, entityType(_entity, _ecm))); + return true; + }); + + if (this->dataPtr->worldEntity != kNullEntity) + this->dataPtr->initialized = true; } else { - // Treat all pre-existent entities as new at startup - if (!this->dataPtr->initialized) + // Requiring a parent entity because we're not adding the world, which is + // parentless, to the tree + _ecm.EachNew( + [&](const Entity &_entity, + const components::Name *_name, + const components::ParentEntity *_parentEntity)->bool { - _ecm.Each( - [&](const Entity &_entity, - const components::Name *_name)->bool - { - auto worldComp = _ecm.Component(_entity); - if (worldComp) - { - this->dataPtr->worldEntity = _entity; - - // Skipping the world for now to keep the tree shallow - return true; - } - - // Parent - Entity parentEntity{kNullEntity}; - - auto parentComp = _ecm.Component(_entity); - if (parentComp) - { - parentEntity = parentComp->Data(); - } - - // World children are top-level - if (this->dataPtr->worldEntity != kNullEntity && - parentEntity == this->dataPtr->worldEntity) - { - parentEntity = kNullEntity; - } - - QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", - Qt::QueuedConnection, - Q_ARG(unsigned int, _entity), - Q_ARG(QString, QString::fromStdString(_name->Data())), - Q_ARG(unsigned int, parentEntity), - Q_ARG(QString, entityType(_entity, _ecm))); - return true; - }); + auto parentEntity = _parentEntity->Data(); - if (this->dataPtr->worldEntity != kNullEntity) - this->dataPtr->initialized = true; - } - else - { - // Requiring a parent entity because we're not adding the world, which is - // parentless, to the tree - _ecm.EachNew( - [&](const Entity &_entity, - const components::Name *_name, - const components::ParentEntity *_parentEntity)->bool + // World children are top-level + if (this->dataPtr->worldEntity != kNullEntity && + parentEntity == this->dataPtr->worldEntity) { - auto parentEntity = _parentEntity->Data(); - - // World children are top-level - if (this->dataPtr->worldEntity != kNullEntity && - parentEntity == this->dataPtr->worldEntity) - { - parentEntity = kNullEntity; - } - - QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", - Qt::QueuedConnection, - Q_ARG(unsigned int, _entity), - Q_ARG(QString, QString::fromStdString(_name->Data())), - Q_ARG(unsigned int, parentEntity), - Q_ARG(QString, entityType(_entity, _ecm))); - return true; - }); - } + parentEntity = kNullEntity; + } - _ecm.EachRemoved( - [&](const Entity &_entity, - const components::Name *)->bool - { - QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, _entity)); + Q_ARG(unsigned int, _entity), + Q_ARG(QString, QString::fromStdString(_name->Data())), + Q_ARG(unsigned int, parentEntity), + Q_ARG(QString, entityType(_entity, _ecm))); return true; }); } + + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::Name *)->bool + { + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", + Qt::QueuedConnection, + Q_ARG(unsigned int, _entity)); + return true; + }); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/entity_tree/EntityTree.hh b/src/gui/plugins/entity_tree/EntityTree.hh index 1f0824ad57..4f882de9f7 100644 --- a/src/gui/plugins/entity_tree/EntityTree.hh +++ b/src/gui/plugins/entity_tree/EntityTree.hh @@ -120,9 +120,6 @@ namespace gazebo // Documentation inherited public: void Update(const UpdateInfo &, EntityComponentManager &) override; - // Documentation inherited - public: void Configure(EventManager &_eventMgr, bool _sameProcess) override; - /// \brief Callback when an entity has been selected. This should be /// called from QML. /// \param[in] _entity Entity being selected. diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 0b838084f7..e34f4a262e 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -333,10 +333,6 @@ class ignition::gazebo::RenderUtilPrivate /// \brief Event Manager public: EventManager *eventManager{nullptr}; - - /// \brief Connection to events::UpdateGUIECMConn event, used to remove - /// entities. - public: ignition::common::ConnectionPtr UpdateGUIECMConn; }; ////////////////////////////////////////////////// @@ -356,10 +352,6 @@ rendering::ScenePtr RenderUtil::Scene() const void RenderUtil::SetEventManager(EventManager &_eventMgr) { this->dataPtr->eventManager = &_eventMgr; - this->dataPtr->UpdateGUIECMConn = - _eventMgr.Connect( - std::bind(&RenderUtilPrivate::RemoveRenderingEntities, this->dataPtr.get(), - std::placeholders::_1, std::placeholders::_2)); } ////////////////////////////////////////////////// @@ -1153,9 +1145,6 @@ void RenderUtilPrivate::CreateRenderingEntities( this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); - if (this->eventManager) - this->eventManager->Emit( - _entity, "None", 0); return true; }); @@ -1171,23 +1160,24 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), - _info.iterations); - bool found = false; - for (auto & data: this->newModels) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), + _info.iterations); + bool found = false; + for (auto & data: this->newModels) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newModels.push_back(tupleTemp); } - } - if (!found) - { - this->newModels.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _name->Data(), _parent->Data()); } return true; }); @@ -1203,21 +1193,22 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Link link; link.SetName(_name->Data()); link.SetRawPose(_pose->Data()); - auto tupleTemp = std::make_tuple(_entity, link, _parent->Data()); - bool found = false; - for (auto & data: this->newLinks) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, link, _parent->Data()); + bool found = false; + for (auto & data: this->newLinks) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found){ + this->newLinks.push_back(tupleTemp); } - } - if (!found){ - this->newLinks.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _name->Data(), _parent->Data()); } // used for collsions @@ -1287,24 +1278,23 @@ void RenderUtilPrivate::CreateRenderingEntities( std::string(heatSignature->Data())); } } - - auto tupleTemp = std::make_tuple(_entity, visual, _parent->Data()); - bool found = false; - for (auto & data: this->newVisuals) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, visual, _parent->Data()); + bool found = false; + for (auto & data: this->newVisuals) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found){ + this->newVisuals.push_back(tupleTemp); } } - if (!found){ - this->newVisuals.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _name->Data(), _parent->Data()); - } - return true; }); @@ -1314,24 +1304,24 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::Actor *_actor, const components::ParentEntity *_parent) -> bool { - auto tupleTemp = std::make_tuple(_entity, _actor->Data(), _parent->Data()); - bool found = false; - for (auto & data: this->newActors) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, _actor->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newActors) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newActors.push_back(tupleTemp); } } - if (!found) - { - this->newActors.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _actor->Data().Name(), _parent->Data()); - } - return true; }); @@ -1341,23 +1331,23 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::Light *_light, const components::ParentEntity *_parent) -> bool { - auto tupleTemp = std::make_tuple(_entity, _light->Data(), _parent->Data()); - bool found = false; - for (auto & data: this->newLights) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, _light->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newLights) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found){ + this->newLights.push_back(tupleTemp); } } - if (!found){ - this->newLights.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _light->Data().Name(), _parent->Data()); - } - return true; }); @@ -1397,9 +1387,6 @@ void RenderUtilPrivate::CreateRenderingEntities( if (!found) { this->newParticleEmitters.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _emitter->Data().name(), _parent->Data()); } return true; @@ -1477,9 +1464,6 @@ void RenderUtilPrivate::CreateRenderingEntities( this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); - if (this->eventManager) - this->eventManager->Emit( - _entity, "None", 0); return true; }); @@ -1494,24 +1478,25 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), - _info.iterations); - bool found = false; - for (auto & data: this->newModels) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), + _info.iterations); + bool found = false; + for (auto & data: this->newModels) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newModels.push_back(tupleTemp); + std::cerr << "Emit new Model2 " << _entity << '\n'; } - } - if (!found) - { - this->newModels.push_back(tupleTemp); - std::cerr << "Emit new Model2 " << _entity << '\n'; - if (this->eventManager) - this->eventManager->Emit( - _entity, _name->Data(), _parent->Data()); } return true; }); @@ -1527,22 +1512,23 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Link link; link.SetName(_name->Data()); link.SetRawPose(_pose->Data()); - auto tupleTemp = std::make_tuple(_entity, link, _parent->Data()); - bool found = false; - for (auto & data: this->newLinks) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, link, _parent->Data()); + bool found = false; + for (auto & data: this->newLinks) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newLinks.push_back(tupleTemp); } - } - if (!found) - { - this->newLinks.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _name->Data(), _parent->Data()); } // used for collsions @@ -1612,23 +1598,23 @@ void RenderUtilPrivate::CreateRenderingEntities( std::string(heatSignature->Data())); } } - - auto tupleTemp = std::make_tuple(_entity, visual, _parent->Data()); - bool found = false; - for (auto & data: this->newVisuals) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, visual, _parent->Data()); + bool found = false; + for (auto & data: this->newVisuals) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newVisuals.push_back(tupleTemp); } - } - if (!found) - { - this->newVisuals.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _name->Data(), _parent->Data()); } return true; @@ -1640,22 +1626,23 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::Actor *_actor, const components::ParentEntity *_parent) -> bool { - auto tupleTemp = std::make_tuple(_entity, _actor->Data(), _parent->Data()); - bool found = false; - for (auto & data: this->newActors) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, _actor->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newActors) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newActors.push_back(tupleTemp); } - } - if (!found) - { - this->newActors.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _actor->Data().Name(), _parent->Data()); } return true; @@ -1667,24 +1654,24 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::Light *_light, const components::ParentEntity *_parent) -> bool { - auto tupleTemp = std::make_tuple(_entity, _light->Data(), _parent->Data()); - bool found = false; - for (auto & data: this->newLights) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, _light->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newLights) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newLights.push_back(tupleTemp); } } - if (!found) - { - this->newLights.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _light->Data().Name(), _parent->Data()); - } - return true; }); @@ -1711,24 +1698,24 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::ParticleEmitter *_emitter, const components::ParentEntity *_parent) -> bool { - auto tupleTemp = std::make_tuple(_entity, _emitter->Data(), _parent->Data()); - bool found = false; - for (auto & data: this->newParticleEmitters) + auto node = this->sceneManager.NodeById(_entity); + if (!node) { - if (std::get<0>(data) == _entity) + auto tupleTemp = std::make_tuple(_entity, _emitter->Data(), _parent->Data()); + bool found = false; + for (auto & data: this->newParticleEmitters) { - found = true; - break; + if (std::get<0>(data) == _entity) + { + found = true; + break; + } + } + if (!found) + { + this->newParticleEmitters.push_back(tupleTemp); } } - if (!found) - { - this->newParticleEmitters.push_back(tupleTemp); - if (this->eventManager) - this->eventManager->Emit( - _entity, _emitter->Data().name(), _parent->Data()); - } - return true; }); @@ -1946,8 +1933,6 @@ void RenderUtilPrivate::RemoveRenderingEntities( { this->removeEntities[_entity] = _info.iterations; this->modelToLinkEntities.erase(_entity); - if (this->eventManager) - this->eventManager->Emit(_entity); return true; }); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index e9110f04f8..97c1bf9d91 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -77,7 +77,6 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Util.hh" -#include "ignition/gazebo/Events.hh" // Components #include "ignition/gazebo/components/AngularAcceleration.hh" @@ -174,8 +173,7 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Remove physics entities if they are removed from the ECM /// \param[in] _ecm Constant reference to ECM. - public: void RemovePhysicsEntities(const EntityComponentManager &_ecm, - const UpdateInfo &_info); + public: void RemovePhysicsEntities(const EntityComponentManager &_ecm); /// \brief Update physics from components /// \param[in] _ecm Mutable reference to ECM. @@ -441,9 +439,6 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief A map between collision entity ids in the ECM to FreeGroup Entities /// in ign-physics. public: EntityFreeGroupMap entityFreeGroupMap; - - /// \brief Pointer to the event manager - public: EventManager *eventManager{nullptr}; }; ////////////////////////////////////////////////// @@ -455,11 +450,8 @@ Physics::Physics() : System(), dataPtr(std::make_unique()) void Physics::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, - EventManager &_eventMgr) + EventManager &/*_eventMgr*/) { - - this->dataPtr->eventManager = &_eventMgr; - std::string pluginLib; // 1. Engine from component (from command line / ServerConfig) @@ -597,7 +589,7 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) // Entities scheduled to be removed should be removed from physics after the // simulation step. Otherwise, since the to-be-removed entity still shows up // in the ECM::Each the UpdatePhysics and UpdateSim calls will have an error - this->dataPtr->RemovePhysicsEntities(_ecm, _info); + this->dataPtr->RemovePhysicsEntities(_ecm); } } @@ -1127,8 +1119,7 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm, - const UpdateInfo &_info) +void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) { // Assume the world will not be erased // Only removing models is supported by ign-physics right now so we only @@ -1141,9 +1132,6 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm, [&](const Entity &_entity, const components::Model * /* _model */) -> bool { - if (this->eventManager) - this->eventManager->Emit(_ecm, _info); - // Remove model if found if (auto modelPtrPhys = this->entityModelMap.Get(_entity)) { From b7251f48b73a0c5979a3c7a0766ce49744fde5d0 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 10 Jun 2021 09:43:19 +0200 Subject: [PATCH 12/35] added more feedback Signed-off-by: ahcorde --- include/ignition/gazebo/Events.hh | 4 ++-- include/ignition/gazebo/Server.hh | 4 ++-- include/ignition/gazebo/System.hh | 3 ++- src/Server.cc | 4 ++-- src/ServerConfig.cc | 2 +- src/SimulationRunner.cc | 6 +----- src/gui/GuiRunner.cc | 19 ++++++++++++++----- .../plugins/scene_manager/GzSceneManager.cc | 4 ++++ src/ign.cc | 4 ++-- src/rendering/RenderUtil.cc | 1 - 10 files changed, 30 insertions(+), 21 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 6938716e66..144b16d7e8 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -82,8 +82,8 @@ namespace ignition /// same process without sensors. This event is launched in the Simulation /// runner when there is any new entity or entity marked to be removed /// to remove/add entities in the renderUtil - using UpdateGUIThread = - ignition::common::EventT; + using UpdateSystems = + ignition::common::EventT; } } // namespace events } // namespace gazebo diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 737f4874de..31bbc49de3 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -118,11 +118,11 @@ namespace ignition /// \brief Get the Entity Component Manager reference /// \return The Entity Component Manager reference - public: EntityComponentManager &GetEntityComponentManager(); + public: EntityComponentManager &SharedEntityComponentManager(); /// \brief Get the Event Manager reference /// \return The Event Manager reference - public: EventManager &GetEventManager(); + public: EventManager &SharedEventManager(); /// \brief Set the update period. The update period is the wall-clock time /// between ECS updates. diff --git a/include/ignition/gazebo/System.hh b/include/ignition/gazebo/System.hh index d012430402..1ec7946c14 100644 --- a/include/ignition/gazebo/System.hh +++ b/include/ignition/gazebo/System.hh @@ -99,7 +99,8 @@ namespace ignition EntityComponentManager &_ecm, EventManager &_eventMgr) = 0; - /// \brief + /// \brief Set the system plugins if the simulation is running in the + /// same process /// \param[in] _sameProcess True if the server and client are running in /// the same process, False otherwise. public: virtual void SetSameProcess(bool /*_sameProcess*/){}; diff --git a/src/Server.cc b/src/Server.cc index 2e5377f164..ca3d64859c 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -206,7 +206,7 @@ Server::Server(const ServerConfig &_config) ///////////////////////////////////////////////// Server::~Server() = default; -EntityComponentManager &Server::GetEntityComponentManager() +EntityComponentManager &Server::SharedEntityComponentManager() { for (std::unique_ptr &runner : this->dataPtr->simRunners) { @@ -214,7 +214,7 @@ EntityComponentManager &Server::GetEntityComponentManager() } } -EventManager &Server::GetEventManager() +EventManager &Server::SharedEventManager() { for (std::unique_ptr &runner : this->dataPtr->simRunners) { diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index f3640eb25f..235e0b1a97 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -305,7 +305,7 @@ class ignition::gazebo::ServerConfigPrivate /// \brief Boolean to define if the server and gui run in the same process /// True gui and server will run in the same process, False otherwise - public: bool sameProcessAsGUI; + public: bool sameProcessAsGUI = false; }; ////////////////////////////////////////////////// diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index dcd8eac063..79df0b6e87 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -526,11 +526,7 @@ void SimulationRunner::UpdateSystems() system->Update(this->currentInfo, this->entityCompMgr); if (serverConfig.SameProcessAsGUI()){ - if (this->entityCompMgr.HasNewEntities() || - this->entityCompMgr.HasEntitiesMarkedForRemoval()) - { - this->eventMgr.Emit(); - } + this->eventMgr.Emit(); } } diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 00e646ad66..1f5ac24a73 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -41,9 +41,6 @@ class ignition::gazebo::GuiRunner::Implementation gazebo::EventManager &_eventMgr) : ecm(_ecm), eventMgr(_eventMgr) { - this->UpdatePluginsConn = - _eventMgr.Connect( - std::bind(&Implementation::UpdatePluginsEvent, this)); } /// \brief Update the plugins. @@ -60,7 +57,8 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief Event manager. public: gazebo::EventManager &eventMgr; - public: bool sameProcess; + /// \brief Is the GUI and server running in the same process? + public: bool sameProcess = false; /// \brief Transport node. public: transport::Node node{}; @@ -89,6 +87,13 @@ GuiRunner::GuiRunner(const std::string &_worldName, { this->dataPtr->sameProcess = _sameProcess; + if (this->dataPtr->sameProcess) + { + this->dataPtr->UpdatePluginsConn = + _eventMgr.Connect( + std::bind(&Implementation::UpdatePluginsEvent, this->dataPtr.get())); + } + this->setProperty("worldName", QString::fromStdString(_worldName)); auto win = gui::App()->findChild(); @@ -246,5 +251,9 @@ void GuiRunner::Implementation::UpdatePlugins() void GuiRunner::Implementation::UpdatePluginsEvent() { std::lock_guard lock(this->updateMutex); - this->UpdatePlugins(); + if (this->ecm.HasNewEntities() || + this->ecm.HasEntitiesMarkedForRemoval()) + { + this->UpdatePlugins(); + } } diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 626bbb30a6..a03e36ce91 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -122,6 +122,10 @@ void GzSceneManager::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("GzSceneManager::Update"); + + // When we are running with the same process + // * has sensors - sensors system calls RenderUtil::Update* + // * no sensors - GzSceneManager calls RenderUtil::Update* if (this->dataPtr->emitFirstRender && (!this->dataPtr->sameProcess || !this->dataPtr->enableSensors)) { diff --git a/src/ign.cc b/src/ign.cc index 6413bf550c..2a574b21e8 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -403,8 +403,8 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, // Run the server server.Run(false, _iterations, _run == 0); - auto &sharedEcm = server.GetEntityComponentManager(); - auto &sharedEventManager = server.GetEventManager(); + auto &sharedEcm = server.SharedEntityComponentManager(); + auto &sharedEventManager = server.SharedEventManager(); // argc and argv are going to be passed to a QApplication. The Qt // documentation has a warning about these: diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index e34f4a262e..a2f5d25753 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1495,7 +1495,6 @@ void RenderUtilPrivate::CreateRenderingEntities( if (!found) { this->newModels.push_back(tupleTemp); - std::cerr << "Emit new Model2 " << _entity << '\n'; } } return true; From 25ea384ba2a9e20f95070eba551d72212d3016e9 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 10 Jun 2021 10:24:12 +0200 Subject: [PATCH 13/35] make linters happy Signed-off-by: ahcorde --- include/ignition/gazebo/System.hh | 2 +- include/ignition/gazebo/gui/GuiSystem.hh | 3 +- src/gui/Gui.cc | 6 +- src/gui/GuiRunner.cc | 2 +- src/ign.cc | 12 +- src/rendering/RenderUtil.cc | 327 +++++++++++------------ src/rendering/SceneManager.cc | 2 - src/systems/sensors/Sensors.cc | 9 +- 8 files changed, 183 insertions(+), 180 deletions(-) diff --git a/include/ignition/gazebo/System.hh b/include/ignition/gazebo/System.hh index 1ec7946c14..3a11af3dfe 100644 --- a/include/ignition/gazebo/System.hh +++ b/include/ignition/gazebo/System.hh @@ -103,7 +103,7 @@ namespace ignition /// same process /// \param[in] _sameProcess True if the server and client are running in /// the same process, False otherwise. - public: virtual void SetSameProcess(bool /*_sameProcess*/){}; + public: virtual void SetSameProcess(bool /*_sameProcess*/){} }; /// \class ISystemPreUpdate ISystem.hh ignition/gazebo/System.hh diff --git a/include/ignition/gazebo/gui/GuiSystem.hh b/include/ignition/gazebo/gui/GuiSystem.hh index 5c601968d7..141b737f69 100644 --- a/include/ignition/gazebo/gui/GuiSystem.hh +++ b/include/ignition/gazebo/gui/GuiSystem.hh @@ -55,7 +55,8 @@ namespace gazebo public: virtual void Update(const UpdateInfo &/*_info*/, EntityComponentManager &/*_ecm*/){} - public: virtual void Configure(EventManager &/*_event*/, bool /*sameProcess*/){}; + public: virtual void Configure( + EventManager &/*_event*/, bool /*sameProcess*/){} }; } } diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 71f37efce5..243aba5433 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -182,7 +182,8 @@ std::unique_ptr createGui( # pragma warning(push) # pragma warning(disable: 4996) #endif - auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0), sharedEcm, sharedEventManager, sameProcess); + auto runner = new ignition::gazebo::GuiRunner( + worldsMsg.data(0), sharedEcm, sharedEventManager, sameProcess); #ifndef _WIN32 # pragma GCC diagnostic pop #else @@ -246,7 +247,8 @@ std::unique_ptr createGui( # pragma warning(push) # pragma warning(disable: 4996) #endif - auto runner = new ignition::gazebo::GuiRunner(worldName, sharedEcm, sharedEventManager, sameProcess); + auto runner = new ignition::gazebo::GuiRunner( + worldName, sharedEcm, sharedEventManager, sameProcess); #ifndef _WIN32 # pragma GCC diagnostic pop #else diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 1f5ac24a73..2b54feedc5 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -194,7 +194,7 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) // load. This plugin will set again the eventMgr and the sameProcess // state. auto plugins = gui::App()->findChildren(); - for (auto &p: plugins) + for (auto &p : plugins) { p->Configure(this->dataPtr->eventMgr, this->dataPtr->sameProcess); } diff --git a/src/ign.cc b/src/ign.cc index 2a574b21e8..6211b706be 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -413,11 +413,12 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, // greater than zero and argv must contain at least one valid character // string." int argc = 1; - // Converting a string literal to char * is forbidden as of C++11. It can only - // be converted to a const char *. The const cast is here to prevent a warning - // since we do need to pass a char* to runGui + // Converting a string literal to char * is forbidden as of C++11. + // It can only be converted to a const char *. The const cast is here to + // prevent a warning since we do need to pass a char* to runGui char *argv = const_cast("ign-gazebo-gui"); - return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig, sharedEcm, sharedEventManager, true); + return ignition::gazebo::gui::runGui( + argc, &argv, _guiConfig, sharedEcm, sharedEventManager, true); } ignerr << "Unable to create server config\n"; @@ -441,5 +442,6 @@ extern "C" int runGui(const char *_guiConfig) char *argv = const_cast("ign-gazebo-gui"); ignition::gazebo::v6::EntityComponentManager guiEcm; ignition::gazebo::v6::EventManager guiEventEcm; - return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig, guiEcm, guiEventEcm, false); + return ignition::gazebo::gui::runGui( + argc, &argv, _guiConfig, guiEcm, guiEventEcm, false); } diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index a2f5d25753..34ee35f5aa 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -108,6 +108,11 @@ class ignition::gazebo::RenderUtilPrivate /// \param[in] _ecm The entity-component manager public: void UpdateRenderingEntities(const EntityComponentManager &_ecm); + /// \brief Helper function to add sensors in the CreateRenderingEntities + /// method + /// \param[in] _ecm The entity-component manager + public: void AddSensors(const EntityComponentManager &_ecm); + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1102,10 +1107,8 @@ void RenderUtil::Update() } ////////////////////////////////////////////////// -void RenderUtilPrivate::CreateRenderingEntities( - const EntityComponentManager &_ecm, const UpdateInfo &_info) +void RenderUtilPrivate::AddSensors(const EntityComponentManager &_ecm) { - IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); auto addNewSensor = [&_ecm, this](Entity _entity, const sdf::Sensor &_sdfData, Entity _parent, const std::string &_topicSuffix) @@ -1130,6 +1133,133 @@ void RenderUtilPrivate::CreateRenderingEntities( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; + if (!this->initialized) + { + if (this->enableSensors) + { + // Create cameras + _ecm.Each( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _camera->Data(), _parent->Data(), + cameraSuffix); + return true; + }); + + // Create depth cameras + _ecm.Each( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), + depthCameraSuffix); + return true; + }); + + // Create rgbd cameras + _ecm.Each( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), + rgbdCameraSuffix); + return true; + }); + + // Create gpu lidar + _ecm.Each( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); + return true; + }); + + // Create thermal camera + _ecm.Each( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), + thermalCameraSuffix); + return true; + }); + } + } + else + { + if (this->enableSensors) + { + // Create cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _camera->Data(), _parent->Data(), + cameraSuffix); + return true; + }); + + // Create depth cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), + depthCameraSuffix); + return true; + }); + + // Create RGBD cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), + rgbdCameraSuffix); + return true; + }); + + // Create gpu lidar + _ecm.EachNew( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); + return true; + }); + + // Create thermal camera + _ecm.EachNew( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), + thermalCameraSuffix); + return true; + }); + } + } +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::CreateRenderingEntities( + const EntityComponentManager &_ecm, const UpdateInfo &_info) +{ + IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); // Treat all pre-existent entities as new at startup // TODO(anyone) refactor Each and EachNew below to reduce duplicate code if (!this->initialized) @@ -1148,7 +1278,6 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); - _ecm.Each( [&](const Entity &_entity, @@ -1166,7 +1295,7 @@ void RenderUtilPrivate::CreateRenderingEntities( auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), _info.iterations); bool found = false; - for (auto & data: this->newModels) + for (auto & data : this->newModels) { if (std::get<0>(data) == _entity) { @@ -1175,9 +1304,7 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newModels.push_back(tupleTemp); - } } return true; }); @@ -1198,7 +1325,7 @@ void RenderUtilPrivate::CreateRenderingEntities( { auto tupleTemp = std::make_tuple(_entity, link, _parent->Data()); bool found = false; - for (auto & data: this->newLinks) + for (auto & data : this->newLinks) { if (std::get<0>(data) == _entity) { @@ -1206,11 +1333,9 @@ void RenderUtilPrivate::CreateRenderingEntities( break; } } - if (!found){ + if (!found) this->newLinks.push_back(tupleTemp); - } } - // used for collsions this->modelToLinkEntities[_parent->Data()].push_back(_entity); return true; @@ -1283,7 +1408,7 @@ void RenderUtilPrivate::CreateRenderingEntities( { auto tupleTemp = std::make_tuple(_entity, visual, _parent->Data()); bool found = false; - for (auto & data: this->newVisuals) + for (auto & data : this->newVisuals) { if (std::get<0>(data) == _entity) { @@ -1291,9 +1416,8 @@ void RenderUtilPrivate::CreateRenderingEntities( break; } } - if (!found){ + if (!found) this->newVisuals.push_back(tupleTemp); - } } return true; }); @@ -1307,9 +1431,10 @@ void RenderUtilPrivate::CreateRenderingEntities( auto node = this->sceneManager.NodeById(_entity); if (!node) { - auto tupleTemp = std::make_tuple(_entity, _actor->Data(), _parent->Data()); + auto tupleTemp = std::make_tuple( + _entity, _actor->Data(), _parent->Data()); bool found = false; - for (auto & data: this->newActors) + for (auto & data : this->newActors) { if (std::get<0>(data) == _entity) { @@ -1318,9 +1443,7 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newActors.push_back(tupleTemp); - } } return true; }); @@ -1334,9 +1457,10 @@ void RenderUtilPrivate::CreateRenderingEntities( auto node = this->sceneManager.NodeById(_entity); if (!node) { - auto tupleTemp = std::make_tuple(_entity, _light->Data(), _parent->Data()); + auto tupleTemp = std::make_tuple( + _entity, _light->Data(), _parent->Data()); bool found = false; - for (auto & data: this->newLights) + for (auto & data : this->newLights) { if (std::get<0>(data) == _entity) { @@ -1344,9 +1468,8 @@ void RenderUtilPrivate::CreateRenderingEntities( break; } } - if (!found){ + if (!found) this->newLights.push_back(tupleTemp); - } } return true; }); @@ -1374,9 +1497,10 @@ void RenderUtilPrivate::CreateRenderingEntities( const components::ParticleEmitter *_emitter, const components::ParentEntity *_parent) -> bool { - auto tupleTemp = std::make_tuple(_entity, _emitter->Data(), _parent->Data()); + auto tupleTemp = std::make_tuple( + _entity, _emitter->Data(), _parent->Data()); bool found = false; - for (auto & data: this->newParticleEmitters) + for (auto & data : this->newParticleEmitters) { if (std::get<0>(data) == _entity) { @@ -1385,70 +1509,11 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newParticleEmitters.push_back(tupleTemp); - } - return true; }); - if (this->enableSensors) - { - // Create cameras - _ecm.Each( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.Each( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create rgbd cameras - _ecm.Each( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.Each( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.Each( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } + this->AddSensors(_ecm); this->initialized = true; } else @@ -1484,7 +1549,7 @@ void RenderUtilPrivate::CreateRenderingEntities( auto tupleTemp = std::make_tuple(_entity, model, _parent->Data(), _info.iterations); bool found = false; - for (auto & data: this->newModels) + for (auto & data : this->newModels) { if (std::get<0>(data) == _entity) { @@ -1493,9 +1558,7 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newModels.push_back(tupleTemp); - } } return true; }); @@ -1516,7 +1579,7 @@ void RenderUtilPrivate::CreateRenderingEntities( { auto tupleTemp = std::make_tuple(_entity, link, _parent->Data()); bool found = false; - for (auto & data: this->newLinks) + for (auto & data : this->newLinks) { if (std::get<0>(data) == _entity) { @@ -1525,11 +1588,8 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newLinks.push_back(tupleTemp); - } } - // used for collsions this->modelToLinkEntities[_parent->Data()].push_back(_entity); return true; @@ -1602,7 +1662,7 @@ void RenderUtilPrivate::CreateRenderingEntities( { auto tupleTemp = std::make_tuple(_entity, visual, _parent->Data()); bool found = false; - for (auto & data: this->newVisuals) + for (auto & data : this->newVisuals) { if (std::get<0>(data) == _entity) { @@ -1611,11 +1671,8 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newVisuals.push_back(tupleTemp); - } } - return true; }); @@ -1628,9 +1685,10 @@ void RenderUtilPrivate::CreateRenderingEntities( auto node = this->sceneManager.NodeById(_entity); if (!node) { - auto tupleTemp = std::make_tuple(_entity, _actor->Data(), _parent->Data()); + auto tupleTemp = std::make_tuple( + _entity, _actor->Data(), _parent->Data()); bool found = false; - for (auto & data: this->newActors) + for (auto & data : this->newActors) { if (std::get<0>(data) == _entity) { @@ -1639,11 +1697,8 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newActors.push_back(tupleTemp); - } } - return true; }); @@ -1656,9 +1711,10 @@ void RenderUtilPrivate::CreateRenderingEntities( auto node = this->sceneManager.NodeById(_entity); if (!node) { - auto tupleTemp = std::make_tuple(_entity, _light->Data(), _parent->Data()); + auto tupleTemp = std::make_tuple( + _entity, _light->Data(), _parent->Data()); bool found = false; - for (auto & data: this->newLights) + for (auto & data : this->newLights) { if (std::get<0>(data) == _entity) { @@ -1667,9 +1723,7 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newLights.push_back(tupleTemp); - } } return true; }); @@ -1700,9 +1754,10 @@ void RenderUtilPrivate::CreateRenderingEntities( auto node = this->sceneManager.NodeById(_entity); if (!node) { - auto tupleTemp = std::make_tuple(_entity, _emitter->Data(), _parent->Data()); + auto tupleTemp = std::make_tuple( + _entity, _emitter->Data(), _parent->Data()); bool found = false; - for (auto & data: this->newParticleEmitters) + for (auto & data : this->newParticleEmitters) { if (std::get<0>(data) == _entity) { @@ -1711,70 +1766,12 @@ void RenderUtilPrivate::CreateRenderingEntities( } } if (!found) - { this->newParticleEmitters.push_back(tupleTemp); - } } return true; }); - if (this->enableSensors) - { - // Create cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create RGBD cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.EachNew( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.EachNew( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } + this->AddSensors(_ecm); } } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 474044e547..c515550780 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -175,7 +175,6 @@ rendering::VisualPtr SceneManager::CreateModel(Entity _id, if (this->dataPtr->scene->HasVisualName(name)) { - ignerr << "Visual: [" << name << " - " << _id << "] already exists" << std::endl; auto vis = this->dataPtr->scene->VisualByName(name); this->dataPtr->visuals[_id] = vis; return vis; @@ -1111,7 +1110,6 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, if (this->dataPtr->scene->HasLightName(name)) { - ignerr << "Visual: [" << name << " - " << _id << "] already exists" << std::endl; auto l = this->dataPtr->scene->LightByName(name); this->dataPtr->lights[_id] = l; return l; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 6793627b70..72ac86a0bb 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -419,7 +419,8 @@ void Sensors::SetSameProcess(bool _sameProcess) { this->dataPtr->renderUtil.SetEventManager(*this->dataPtr->eventManager); - this->dataPtr->renderConn = this->dataPtr->eventManager->Connect( + this->dataPtr->renderConn = + this->dataPtr->eventManager->Connect( std::bind(&SensorsPrivate::Render, this->dataPtr.get())); } } @@ -511,10 +512,12 @@ void Sensors::PostUpdate(const UpdateInfo &_info, // This will allow to load first the scene3D if (this->dataPtr->sameProcess) { - if (rendering::isEngineLoaded("ogre") || rendering::isEngineLoaded("ogre2")) + if (rendering::isEngineLoaded("ogre") || + rendering::isEngineLoaded("ogre2")) { igndbg << "Initialization needed" << std::endl; - this->dataPtr->eventManager->Emit(true); + this->dataPtr->eventManager->Emit + (true); this->dataPtr->doInit = true; this->dataPtr->renderCv.notify_one(); } From ce10db12f8f7b440531987d8df4f43723d404ba0 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 11 Jun 2021 13:15:09 +0200 Subject: [PATCH 14/35] Fixed tests Signed-off-by: ahcorde --- src/gui/Gui_TEST.cc | 5 ++++- .../JointPositionController_TEST.cc | 12 +++++++++--- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/gui/Gui_TEST.cc b/src/gui/Gui_TEST.cc index 7c61a211dc..c8b8e7fdb4 100644 --- a/src/gui/Gui_TEST.cc +++ b/src/gui/Gui_TEST.cc @@ -84,7 +84,10 @@ TEST(GuiTest, IGN_UTILS_TEST_DISABLED_ON_MAC(PathManager)) node.Advertise("/gazebo/resource_paths/get", pathsCb); igndbg << "Paths advertised" << std::endl; - auto app = ignition::gazebo::gui::createGui(gg_argc, gg_argv, nullptr); + ignition::gazebo::EntityComponentManager ecm; + ignition::gazebo::EventManager eventMgr; + auto app = ignition::gazebo::gui::createGui( + gg_argc, gg_argv, nullptr, ecm, eventMgr, false); EXPECT_NE(nullptr, app); igndbg << "GUI created" << std::endl; diff --git a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc index 66ceb93015..945bd0f82f 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc @@ -70,7 +70,9 @@ TEST_F(JointPositionControllerGui, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) // Create GUI runner to handle gazebo::gui plugins // TODO(anyone) Remove deprecation guard once GuiRunner becomes private IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - auto runner = new gazebo::GuiRunner("test"); + ignition::gazebo::EntityComponentManager ecm; + ignition::gazebo::EventManager eventMgr; + auto runner = new gazebo::GuiRunner("test", ecm, eventMgr, false); IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION runner->connect(app.get(), &gui::Application::PluginAdded, runner, &gazebo::GuiRunner::OnPluginAdded); @@ -148,7 +150,9 @@ TEST_F(JointPositionControllerGui, // Create GUI runner to handle gazebo::gui plugins // TODO(anyone) Remove deprecation guard once GuiRunner becomes private IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - auto runner = new gazebo::GuiRunner("test"); + ignition::gazebo::EntityComponentManager ecm2; + ignition::gazebo::EventManager eventMgr; + auto runner = new gazebo::GuiRunner("test", ecm2, eventMgr, false); IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION runner->connect(app.get(), &gui::Application::PluginAdded, runner, &gazebo::GuiRunner::OnPluginAdded); @@ -198,14 +202,16 @@ TEST_F(JointPositionControllerGui, plugin->SetModelEntity(1); // Request state again, do it in separate thread so we can call processEvents + bool isRequestDone = false; std::thread waiting([&]() { runner->RequestState(); + isRequestDone = true; }); int sleep = 0; int maxSleep = 30; - while (plugin->ModelName() != "model_name" && sleep < maxSleep) + while (sleep < maxSleep && !isRequestDone) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); QCoreApplication::processEvents(); From 7a5084315c0317a34c02a4c39440a22d72b72551 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 21 Jun 2021 10:54:17 +0200 Subject: [PATCH 15/35] Added feedback Signed-off-by: ahcorde --- include/ignition/gazebo/Events.hh | 7 +++---- include/ignition/gazebo/gui/Gui.hh | 10 ++++++++-- include/ignition/gazebo/gui/GuiRunner.hh | 7 +++++-- include/ignition/gazebo/gui/GuiSystem.hh | 5 +++++ src/Server.cc | 4 ++-- src/SimulationRunner.cc | 3 ++- src/gui/Gui.cc | 15 +++++---------- src/gui/GuiRunner.cc | 7 ++++--- src/gui/plugins/CMakeLists.txt | 2 +- src/gui/plugins/entity_tree/EntityTree.cc | 3 +-- src/gui/plugins/scene_manager/GzSceneManager.cc | 9 ++++++++- src/ign.cc | 5 +++-- src/ign.hh | 8 +++----- src/rendering/RenderUtil.cc | 11 ++++++++--- src/rendering/SceneManager.cc | 1 + src/systems/sensors/Sensors.cc | 3 ++- 16 files changed, 61 insertions(+), 39 deletions(-) diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 144b16d7e8..f9e4a0dae0 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -23,7 +23,6 @@ #include "ignition/gazebo/config.hh" #include "ignition/gazebo/Entity.hh" -#include "ignition/gazebo/EntityComponentManager.hh" namespace ignition { @@ -61,10 +60,10 @@ namespace ignition struct LoadPluginsTag>; /// \brief Event used to emit a render event when running in one process. - /// This is required because we have two RenderUtils classes when there + /// This is required because we have two RenderUtil classes when there /// is a render sensor in the scene (camera, depth sensor, etc). - /// We could only have one thread updating the renderscene, with this - /// signal we are able to call grom the GzSceneManager the render calls + /// We could only have one thread updating the render scene, with this + /// signal we are able to call from the GzSceneManager the render calls /// required by the sensor /// /// For example: diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index ed9989cdbd..e82a148017 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -42,12 +42,15 @@ namespace gui /// \param[in] _argv Command line arguments (Used when running without /// ign-tools. Set to the name of the application if using ign-tools) /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default + /// \param[in] _ecm Entity-component manager. + /// \param[in] _eventMgr Event manager + /// \param[in] _sameProcess is server and gui running in the same process ? /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, char **_argv, const char *_guiConfig, EntityComponentManager &_ecm, EventManager &_eventMgr, - bool sameProcess); + bool _sameProcess); /// \brief Create a Gazebo GUI application /// \param[in] _argc Number of command line arguments (Used when running @@ -58,6 +61,9 @@ namespace gui /// ign-tools. Set to the name of the application if using ign-tools) /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _eventMgr Event manager + /// \param[in] _sameProcess is server and gui running in the same process ? /// \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 @@ -70,7 +76,7 @@ namespace gui int &_argc, char **_argv, const char *_guiConfig, EntityComponentManager &_ecm, EventManager &_eventMgr, - bool sameProcess, + bool _sameProcess, const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); } // namespace gui diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index ede6a18651..b31b16d8c7 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/include/ignition/gazebo/gui/GuiRunner.hh @@ -45,11 +45,14 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \brief Constructor /// \param[in] _worldName World name. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _eventMgr Event manager + /// \param[in] _sameProcess is server and gui running in the same process ? /// \todo Move to src/gui on v6. public: explicit IGN_DEPRECATED(5.0) GuiRunner( const std::string &_worldName, EntityComponentManager &_ecm, - EventManager &_event, - bool sameProcess); + EventManager &_eventMgr, + bool _sameProcess); /// \brief Destructor public: ~GuiRunner() override; diff --git a/include/ignition/gazebo/gui/GuiSystem.hh b/include/ignition/gazebo/gui/GuiSystem.hh index 141b737f69..f1200da7c7 100644 --- a/include/ignition/gazebo/gui/GuiSystem.hh +++ b/include/ignition/gazebo/gui/GuiSystem.hh @@ -55,6 +55,11 @@ namespace gazebo public: virtual void Update(const UpdateInfo &/*_info*/, EntityComponentManager &/*_ecm*/){} + /// \brief When running in the same process GUI and server we need to + /// configure the gui system plugins with the event manager to listen + /// ecm updates and set if we are running in the same process + /// \param[in] _ecm Mutable event manager + /// \param[in] _sameProcess is server and gui running in the same process ? public: virtual void Configure( EventManager &/*_event*/, bool /*sameProcess*/){} }; diff --git a/src/Server.cc b/src/Server.cc index ca3d64859c..7e3610e4e8 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -208,7 +208,7 @@ Server::~Server() = default; EntityComponentManager &Server::SharedEntityComponentManager() { - for (std::unique_ptr &runner : this->dataPtr->simRunners) + for (auto &runner : this->dataPtr->simRunners) { return runner->EntityCompMgr(); } @@ -216,7 +216,7 @@ EntityComponentManager &Server::SharedEntityComponentManager() EventManager &Server::SharedEventManager() { - for (std::unique_ptr &runner : this->dataPtr->simRunners) + for (auto &runner : this->dataPtr->simRunners) { return runner->EventMgr(); } diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 6ce8d6d133..d27b005ad4 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -537,7 +537,8 @@ void SimulationRunner::UpdateSystems() for (auto& system : this->systemsUpdate) system->Update(this->currentInfo, this->entityCompMgr); - if (serverConfig.SameProcessAsGUI()){ + if (serverConfig.SameProcessAsGUI()) + { this->eventMgr.Emit(); } } diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 243aba5433..5c8af778f9 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -44,12 +44,9 @@ namespace gui std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, EntityComponentManager &_ecm, EventManager &_eventMgr, - bool sameProcess, const char *_defaultGuiConfig, + bool _sameProcess, const char *_defaultGuiConfig, bool _loadPluginsFromSdf) { - auto &sharedEcm = _ecm; - auto &sharedEventManager = _eventMgr; - ignition::common::SignalHandler sigHandler; bool sigKilled = false; sigHandler.AddCallback([&](const int /*_sig*/) @@ -183,7 +180,7 @@ std::unique_ptr createGui( # pragma warning(disable: 4996) #endif auto runner = new ignition::gazebo::GuiRunner( - worldsMsg.data(0), sharedEcm, sharedEventManager, sameProcess); + worldsMsg.data(0), _ecm, _eventMgr, _sameProcess); #ifndef _WIN32 # pragma GCC diagnostic pop #else @@ -248,7 +245,7 @@ std::unique_ptr createGui( # pragma warning(disable: 4996) #endif auto runner = new ignition::gazebo::GuiRunner( - worldName, sharedEcm, sharedEventManager, sameProcess); + worldName, _ecm, _eventMgr, _sameProcess); #ifndef _WIN32 # pragma GCC diagnostic pop #else @@ -326,12 +323,10 @@ std::unique_ptr createGui( ////////////////////////////////////////////////// int runGui(int &_argc, char **_argv, const char *_guiConfig, - EntityComponentManager &_ecm, EventManager &_eventMgr, bool sameProcess) + EntityComponentManager &_ecm, EventManager &_eventMgr, bool _sameProcess) { - auto &sharedEcm = _ecm; - auto &sharedEventManager = _eventMgr; auto app = gazebo::gui::createGui(_argc, _argv, _guiConfig, - sharedEcm, sharedEventManager, sameProcess); + _ecm, _eventMgr, _sameProcess); if (nullptr != app) { // Run main window. diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 2b54feedc5..32211c96ea 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -49,6 +49,7 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief void UpdatePluginsEvent(); + /// \brief Connection to the UpdatePlugins event. public: ignition::common::ConnectionPtr UpdatePluginsConn; /// \brief Entity-component manager. @@ -190,9 +191,9 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) return; } - // The call above always return the same plugin, which the first that was - // load. This plugin will set again the eventMgr and the sameProcess - // state. + // The call above always returns the same plugin, which is the first plugin + // that was loaded. This plugin will set again the eventMgr and the + // sameProcess state. auto plugins = gui::App()->findChildren(); for (auto &p : plugins) { diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 3db223c25f..3c9fc4a92f 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -127,7 +127,7 @@ add_subdirectory(lights) add_subdirectory(playback_scrubber) add_subdirectory(plotting) add_subdirectory(resource_spawner) -# add_subdirectory(scene3d) +add_subdirectory(scene3d) add_subdirectory(scene_manager) add_subdirectory(shapes) add_subdirectory(tape_measure) diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 983e524e51..8314539fa4 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -133,8 +133,7 @@ void TreeModel::AddEntity(unsigned int _entity, const QString &_entityName, return; } - auto itemExists = this->entityItems.find(_entity); - if (itemExists != this->entityItems.end()) + if (this->entityItems.find(_entity) != this->entityItems.end()) { return; } diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index a03e36ce91..d0ab75bc87 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -48,6 +48,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief This method is used to connect with the event /// events::EnableSensors. It will set if the simulation is running any /// sensors + /// \param[in] _enable True if the sensors thread is enabled, false + /// otherwise public: void EnableSensors(bool _enable); //// \brief Pointer to the rendering scene @@ -85,7 +87,12 @@ GzSceneManager::GzSceneManager() } ///////////////////////////////////////////////// -GzSceneManager::~GzSceneManager() = default; +GzSceneManager::~GzSceneManager() +{ + this->dataPtr->eventManager = nullptr; + this->dataPtr->enableSensorsConn = nullptr; + this->dataPtr = nullptr; +} ///////////////////////////////////////////////// void GzSceneManager::Configure(EventManager &_eventMgr, bool _sameProcess) diff --git a/src/ign.cc b/src/ign.cc index 6211b706be..b4d2f957a4 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -375,8 +375,9 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, return 0; } - igndbg << "Shutting down ign-gazebo-server" << std::endl; - return 0; + ignerr << "Something was wrong configuring the server. " << + "Shutting down ign-gazebo-server" << std::endl; + return -1; } ////////////////////////////////////////////////// diff --git a/src/ign.hh b/src/ign.hh index 2d807615d2..d60bc0654b 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -54,7 +54,7 @@ extern "C" const char *worldInstallDir(); /// \param[in] _file Path to file being loaded /// \param[in] _recordTopics Colon separated list of topics to record. Leave /// null to record the default topics. -/// \return 0 if successful, 1 if not. +/// \return 0 if successful, -1 if not. extern "C" int runServer(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, const char *_networkRole, int _networkSecondaries, int _record, @@ -66,7 +66,7 @@ extern "C" int runServer(const char *_sdfString, /// \brief External hook to run simulation GUI. /// \param[in] _guiConfig Path to Ignition GUI configuration file. -/// \return 0 if successful, 1 if not. +/// \return 0 if successful, -1 if not. extern "C" int runGui(const char *_guiConfig); /// \brief External hook to find or download a fuel world provided a URL. @@ -97,7 +97,7 @@ extern "C" const char *findFuelResource( /// \param[in] _recordTopics Colon separated list of topics to record. Leave /// null to record the default topics. /// \param[in] _guiConfig Path to Ignition GUI configuration file. -/// \return 0 if successful, 1 if not. +/// \return 0 if successful, -1 if not. extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, @@ -105,6 +105,4 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, const char *_playback, const char *_physicsEngine, const char *_renderEngineServer, const char *_renderEngineGui, const char *_file, const char *_recordTopics, const char *_guiConfig); - - #endif diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 326dac2992..e0e78b809f 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -358,10 +358,16 @@ class ignition::gazebo::RenderUtilPrivate public: std::unordered_map> thermalCameraData; - /// \brief Event Manager + /// \brief Event Manager public: EventManager *eventManager{nullptr}; + + /// \brief when running in the same process we should initialize the scenes + /// one by one otherwise a race condition will happen + static std::mutex mutexInit; }; +std::mutex ignition::gazebo::RenderUtilPrivate::mutexInit; + ////////////////////////////////////////////////// RenderUtil::RenderUtil() : dataPtr(std::make_unique()) { @@ -2152,14 +2158,13 @@ void RenderUtilPrivate::RemoveRenderingEntities( }); } -static std::mutex mutexInit; ///////////////////////////////////////////////// void RenderUtil::Init() { // Already initialized if (nullptr != this->dataPtr->scene) return; - std::lock_guard lock(mutexInit); + std::lock_guard lock(this->dataPtr->mutexInit); ignition::common::SystemPaths pluginPath; pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 3bb3686880..3288ffedd2 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -175,6 +175,7 @@ rendering::VisualPtr SceneManager::CreateModel(Entity _id, if (this->dataPtr->scene->HasVisualName(name)) { + ignerr << "Visual: [" << name << "] already exists" << std::endl; auto vis = this->dataPtr->scene->VisualByName(name); this->dataPtr->visuals[_id] = vis; return vis; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 72ac86a0bb..ea0148e3c0 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -139,6 +139,7 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Pointer to the event manager public: EventManager *eventManager{nullptr}; + /// \brief Is the GUI and server running in the same process? public: bool sameProcess = false; /// \brief Wait for initialization to happen @@ -181,7 +182,7 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Stop the rendering thread public: void Stop(); - /// \brief Stop the rendering thread + /// \brief Start the rendering thread public: void Render(); }; From 5f94fe2e910d1355fb2f6675f7eeb6ab572b4543 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 21 Jun 2021 12:46:15 -0700 Subject: [PATCH 16/35] Update to latest Signed-off-by: Louise Poubel --- src/gui/gui.config | 2 +- src/gui/plugins/scene_manager/GzSceneManager.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gui/gui.config b/src/gui/gui.config index bb2b9f7e42..67f4de172e 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -26,7 +26,7 @@ - + 3D View false diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 7491687688..61c86a21d2 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -103,7 +103,7 @@ void GzSceneManagerPrivate::OnRender() { if (nullptr == this->scene) { - this->scene = rendering::someInitializedScene(); + this->scene = rendering::sceneFromFirstRenderEngine(); if (nullptr == this->scene) return; From 9962dbf826132a6d777792ecf2715d51b09e0786 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 22 Jun 2021 23:27:18 +0200 Subject: [PATCH 17/35] Same process bool as a component and minor fixes Signed-off-by: ahcorde --- include/ignition/gazebo/ServerConfig.hh | 2 +- include/ignition/gazebo/System.hh | 6 --- .../ignition/gazebo/components/SameProcess.hh | 43 +++++++++++++++++++ src/LevelManager.cc | 5 +++ src/ServerConfig.cc | 2 +- src/SimulationRunner.cc | 1 - src/gui/plugins/entity_tree/EntityTree.cc | 1 - src/systems/sensors/Sensors.cc | 35 ++++++++------- src/systems/sensors/Sensors.hh | 3 -- 9 files changed, 69 insertions(+), 29 deletions(-) create mode 100644 include/ignition/gazebo/components/SameProcess.hh diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 9a6ce7224f..61cdebe59d 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -178,7 +178,7 @@ namespace ignition /// \brief Set if the server and GUI should run in the same process. /// \param[in] _sameProcessAsGUI True if the server and GUI will run in /// the same process, False otherwise - public: void SetSameProcessAsGUI(const bool &_sameProcessAsGUI); + public: void SetSameProcessAsGUI(bool _sameProcessAsGUI); /// \brief Get if the server and GUI are running in the same process /// \return True if the server and GUI will run in diff --git a/include/ignition/gazebo/System.hh b/include/ignition/gazebo/System.hh index 3a11af3dfe..467815f202 100644 --- a/include/ignition/gazebo/System.hh +++ b/include/ignition/gazebo/System.hh @@ -98,12 +98,6 @@ namespace ignition const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &_eventMgr) = 0; - - /// \brief Set the system plugins if the simulation is running in the - /// same process - /// \param[in] _sameProcess True if the server and client are running in - /// the same process, False otherwise. - public: virtual void SetSameProcess(bool /*_sameProcess*/){} }; /// \class ISystemPreUpdate ISystem.hh ignition/gazebo/System.hh diff --git a/include/ignition/gazebo/components/SameProcess.hh b/include/ignition/gazebo/components/SameProcess.hh new file mode 100644 index 0000000000..a38e377259 --- /dev/null +++ b/include/ignition/gazebo/components/SameProcess.hh @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_COMPONENTS_SAMEPROCESS_HH_ +#define IGNITION_GAZEBO_COMPONENTS_SAMEPROCESS_HH_ + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component used to hold if the server and gui are running in the + /// same process property. + using SameProcess = Component; + + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SameProcess", + SameProcess) +} +} +} +} + +#endif diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 74e2f71020..7fd1220fd7 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -51,6 +51,7 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" +#include "ignition/gazebo/components/SameProcess.hh" #include "ignition/gazebo/components/Scene.hh" #include "ignition/gazebo/components/Wind.hh" #include "ignition/gazebo/components/World.hh" @@ -126,6 +127,10 @@ void LevelManager::ReadLevelPerformerInfo() components::RenderEngineGuiPlugin( this->runner->serverConfig.RenderEngineGui())); + this->runner->entityCompMgr.CreateComponent(this->worldEntity, + components::SameProcess( + this->runner->serverConfig.SameProcessAsGUI())); + auto worldElem = this->runner->sdfWorld->Element(); // Create Wind diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 235e0b1a97..a3cc530a22 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -352,7 +352,7 @@ std::string ServerConfig::SdfString() const } ////////////////////////////////////////////////// -void ServerConfig::SetSameProcessAsGUI(const bool &_sameProcessAsGUI) +void ServerConfig::SetSameProcessAsGUI(bool _sameProcessAsGUI) { this->dataPtr->sameProcessAsGUI = _sameProcessAsGUI; } diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index d27b005ad4..8db9451f67 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -850,7 +850,6 @@ void SimulationRunner::LoadPlugin(const Entity _entity, systemConfig->Configure(_entity, _sdf, this->entityCompMgr, this->eventMgr); - systemConfig->SetSameProcess(serverConfig.SameProcessAsGUI()); } this->AddSystem(system.value()); diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 8314539fa4..e438861dbf 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -26,7 +26,6 @@ #include #include -#include "ignition/gazebo/Events.hh" #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index ea0148e3c0..48aacb2e4a 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -43,6 +43,7 @@ #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/RgbdCamera.hh" +#include "ignition/gazebo/components/SameProcess.hh" #include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Events.hh" @@ -412,20 +413,6 @@ Sensors::~Sensors() this->dataPtr->Stop(); } -void Sensors::SetSameProcess(bool _sameProcess) -{ - this->dataPtr->sameProcess = _sameProcess; - - if (this->dataPtr->sameProcess) - { - this->dataPtr->renderUtil.SetEventManager(*this->dataPtr->eventManager); - - this->dataPtr->renderConn = - this->dataPtr->eventManager->Connect( - std::bind(&SensorsPrivate::Render, this->dataPtr.get())); - } -} - ////////////////////////////////////////////////// void Sensors::Configure(const Entity &/*_id*/, const std::shared_ptr &_sdf, @@ -465,6 +452,23 @@ void Sensors::Configure(const Entity &/*_id*/, { this->dataPtr->renderUtil.SetEngineName(renderEngineServerComp->Data()); } + + // Check if the server and gui are running in the same process + auto sameProcessComp = + _ecm.Component(worldEntity); + if (sameProcessComp) + { + this->dataPtr->sameProcess = sameProcessComp->Data(); + + if (this->dataPtr->sameProcess) + { + this->dataPtr->renderUtil.SetEventManager(*this->dataPtr->eventManager); + + this->dataPtr->renderConn = + _eventMgr.Connect( + std::bind(&SensorsPrivate::Render, this->dataPtr.get())); + } + } } this->dataPtr->eventManager = &_eventMgr; @@ -513,8 +517,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, // This will allow to load first the scene3D if (this->dataPtr->sameProcess) { - if (rendering::isEngineLoaded("ogre") || - rendering::isEngineLoaded("ogre2")) + if (rendering::loadedEngines().size()) { igndbg << "Initialization needed" << std::endl; this->dataPtr->eventManager->Emit diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index e936d57415..3f988fa146 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -56,9 +56,6 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; - // Documentation inherited - public: void SetSameProcess(bool _sameProcess) final; - // Documentation inherited public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) final; From 84956cf51f01414b16527aa6c8b0b0a98535feb8 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 23 Jun 2021 10:20:26 +0200 Subject: [PATCH 18/35] feedback Signed-off-by: ahcorde --- include/ignition/gazebo/Server.hh | 8 ++++++-- src/Server.cc | 15 +++++++++------ 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 31bbc49de3..58bf17c1c3 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -117,12 +117,16 @@ namespace ignition public: ~Server(); /// \brief Get the Entity Component Manager reference + /// \param[in] _worldIndex Index of the world in the simrunner /// \return The Entity Component Manager reference - public: EntityComponentManager &SharedEntityComponentManager(); + public: EntityComponentManager &SharedEntityComponentManager( + const unsigned int _worldIndex = 0); /// \brief Get the Event Manager reference + /// \param[in] _worldIndex Index of the world in the simrunner /// \return The Event Manager reference - public: EventManager &SharedEventManager(); + public: EventManager &SharedEventManager( + const unsigned int _worldIndex = 0); /// \brief Set the update period. The update period is the wall-clock time /// between ECS updates. diff --git a/src/Server.cc b/src/Server.cc index 7e3610e4e8..5bceb26ba5 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -206,19 +206,22 @@ Server::Server(const ServerConfig &_config) ///////////////////////////////////////////////// Server::~Server() = default; -EntityComponentManager &Server::SharedEntityComponentManager() +///////////////////////////////////////////////// +EntityComponentManager &Server::SharedEntityComponentManager( + const unsigned int _worldIndex) { - for (auto &runner : this->dataPtr->simRunners) + if (this->dataPtr->simRunners.size() > _worldIndex) { - return runner->EntityCompMgr(); + return this->dataPtr->simRunners[_worldIndex]->EntityCompMgr(); } } -EventManager &Server::SharedEventManager() +///////////////////////////////////////////////// +EventManager &Server::SharedEventManager(const unsigned int _worldIndex) { - for (auto &runner : this->dataPtr->simRunners) + if (this->dataPtr->simRunners.size() > _worldIndex) { - return runner->EventMgr(); + return this->dataPtr->simRunners[_worldIndex]->EventMgr(); } } From 51c838257c96a190c8db0600afa7313e4eb69111 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 24 Jun 2021 19:43:42 +0200 Subject: [PATCH 19/35] Use std::optional with SharedEntityComponentManager and SharedEventManager Signed-off-by: ahcorde --- include/ignition/gazebo/Server.hh | 4 ++-- src/Server.cc | 11 +++++++---- src/ign.cc | 17 ++++++++++++++--- 3 files changed, 23 insertions(+), 9 deletions(-) diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 58bf17c1c3..0601648577 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -119,13 +119,13 @@ namespace ignition /// \brief Get the Entity Component Manager reference /// \param[in] _worldIndex Index of the world in the simrunner /// \return The Entity Component Manager reference - public: EntityComponentManager &SharedEntityComponentManager( + public: std::optional> SharedEntityComponentManager( const unsigned int _worldIndex = 0); /// \brief Get the Event Manager reference /// \param[in] _worldIndex Index of the world in the simrunner /// \return The Event Manager reference - public: EventManager &SharedEventManager( + public: std::optional> SharedEventManager( const unsigned int _worldIndex = 0); /// \brief Set the update period. The update period is the wall-clock time diff --git a/src/Server.cc b/src/Server.cc index 5bceb26ba5..a9d57afdc2 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -207,22 +208,24 @@ Server::Server(const ServerConfig &_config) Server::~Server() = default; ///////////////////////////////////////////////// -EntityComponentManager &Server::SharedEntityComponentManager( +std::optional> Server::SharedEntityComponentManager( const unsigned int _worldIndex) { if (this->dataPtr->simRunners.size() > _worldIndex) { - return this->dataPtr->simRunners[_worldIndex]->EntityCompMgr(); + return std::reference_wrapper(this->dataPtr->simRunners[_worldIndex]->EntityCompMgr()); } + return std::nullopt; } ///////////////////////////////////////////////// -EventManager &Server::SharedEventManager(const unsigned int _worldIndex) +std::optional> Server::SharedEventManager(const unsigned int _worldIndex) { if (this->dataPtr->simRunners.size() > _worldIndex) { - return this->dataPtr->simRunners[_worldIndex]->EventMgr(); + return std::reference_wrapper(this->dataPtr->simRunners[_worldIndex]->EventMgr()); } + return std::nullopt; } ///////////////////////////////////////////////// diff --git a/src/ign.cc b/src/ign.cc index b4d2f957a4..17967dd8a7 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -404,8 +404,19 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, // Run the server server.Run(false, _iterations, _run == 0); - auto &sharedEcm = server.SharedEntityComponentManager(); - auto &sharedEventManager = server.SharedEventManager(); + auto sharedEcm = server.SharedEntityComponentManager(); + auto sharedEventManager = server.SharedEventManager(); + + if (!sharedEcm) + { + ignerr << "Unable to get a shared ECM\n"; + return -1; + } + if (!sharedEventManager) + { + ignerr << "Unable to get a shared Event Manager\n"; + return -1; + } // argc and argv are going to be passed to a QApplication. The Qt // documentation has a warning about these: @@ -419,7 +430,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, // prevent a warning since we do need to pass a char* to runGui char *argv = const_cast("ign-gazebo-gui"); return ignition::gazebo::gui::runGui( - argc, &argv, _guiConfig, sharedEcm, sharedEventManager, true); + argc, &argv, _guiConfig, (*sharedEcm).get(), (*sharedEventManager).get(), true); } ignerr << "Unable to create server config\n"; From 195c1e417895072ca6f04e284d6aaeac6965d093 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 24 Jun 2021 19:57:43 +0200 Subject: [PATCH 20/35] tick-tock runGui and createGui Signed-off-by: ahcorde --- include/ignition/gazebo/gui/Gui.hh | 45 ++++++++++++++++++++++++++---- src/gui/Gui.cc | 20 +++++++++++++ 2 files changed, 60 insertions(+), 5 deletions(-) diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index e82a148017..14f956d7b4 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -34,6 +34,19 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace gui { + /// \brief Run GUI application + /// \param[in] _argc Number of command line arguments (Used when running + /// without ign-tools. Set to 1 if using ign-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 + /// ign-tools. Set to the name of the application if using ign-tools) + /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default + /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. + IGNITION_GAZEBO_GUI_VISIBLE int IGN_DEPRECATED(6) runGui(int &_argc, + char **_argv, + const char *_guiConfig); + /// \brief Run GUI application /// \param[in] _argc Number of command line arguments (Used when running /// without ign-tools. Set to 1 if using ign-tools). Note: The object @@ -46,11 +59,33 @@ namespace gui /// \param[in] _eventMgr Event manager /// \param[in] _sameProcess is server and gui running in the same process ? /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. - IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, char **_argv, - const char *_guiConfig, - EntityComponentManager &_ecm, - EventManager &_eventMgr, - bool _sameProcess); + IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, + char **_argv, + const char *_guiConfig, + EntityComponentManager &_ecm, + EventManager &_eventMgr, + bool _sameProcess); + + /// \brief Create a Gazebo GUI application + /// \param[in] _argc Number of command line arguments (Used when running + /// without ign-tools. Set to 1 if using ign-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 + /// ign-tools. Set to the name of the application if using ign-tools) + /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default + /// configuration from IGN_HOMEDIR/.ignition/gazebo/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 + /// IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. + /// \param[in] _loadPluginsFromSdf If true, plugins specified in the world + /// SDFormat file will get loaded. + IGNITION_GAZEBO_GUI_VISIBLE + std::unique_ptr IGN_DEPRECATED(6) createGui( + int &_argc, char **_argv, const char *_guiConfig, + const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); /// \brief Create a Gazebo GUI application /// \param[in] _argc Number of command line arguments (Used when running diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index ea5c04e92d..284edc350b 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -40,6 +40,18 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace gui { +////////////////////////////////////////////////// +std::unique_ptr createGui( + int &_argc, char **_argv, const char *_guiConfig, + const char *_defaultGuiConfig, + bool _loadPluginsFromSdf) +{ + EntityComponentManager ecm; + EventManager eventMgr; + return createGui(_argc, _argv, _guiConfig, ecm, eventMgr, + false, _defaultGuiConfig, _loadPluginsFromSdf); +} + ////////////////////////////////////////////////// std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, @@ -295,6 +307,14 @@ std::unique_ptr createGui( return app; } +////////////////////////////////////////////////// +int runGui(int &_argc, char **_argv, const char *_guiConfig) +{ + EntityComponentManager ecm; + EventManager eventMgr; + return runGui(_argc, _argv, _guiConfig, ecm, eventMgr, false); +} + ////////////////////////////////////////////////// int runGui(int &_argc, char **_argv, const char *_guiConfig, EntityComponentManager &_ecm, EventManager &_eventMgr, bool _sameProcess) From 47a87eb14d204a7590a218497aac1d88dab594d6 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 24 Jun 2021 22:50:33 +0200 Subject: [PATCH 21/35] make linters happy Signed-off-by: ahcorde --- include/ignition/gazebo/Server.hh | 8 ++--- include/ignition/gazebo/gui/Gui.hh | 55 +++++++++++++++--------------- src/Server.cc | 13 ++++--- src/ign.cc | 3 +- 4 files changed, 42 insertions(+), 37 deletions(-) diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 0601648577..d396167f95 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -119,14 +119,14 @@ namespace ignition /// \brief Get the Entity Component Manager reference /// \param[in] _worldIndex Index of the world in the simrunner /// \return The Entity Component Manager reference - public: std::optional> SharedEntityComponentManager( - const unsigned int _worldIndex = 0); + public: std::optional> + SharedEntityComponentManager(const unsigned int _worldIndex = 0); /// \brief Get the Event Manager reference /// \param[in] _worldIndex Index of the world in the simrunner /// \return The Event Manager reference - public: std::optional> SharedEventManager( - const unsigned int _worldIndex = 0); + public: std::optional> + SharedEventManager(const unsigned int _worldIndex = 0); /// \brief Set the update period. The update period is the wall-clock time /// between ECS updates. diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index 14f956d7b4..9150ad488f 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -37,8 +37,8 @@ namespace gui /// \brief Run GUI application /// \param[in] _argc Number of command line arguments (Used when running /// without ign-tools. Set to 1 if using ign-tools). Note: The object - /// referenced by this variable must continue to exist for the lifetime of the - /// application. + /// referenced by this variable must continue to exist for the lifetime of + /// the application. /// \param[in] _argv Command line arguments (Used when running without /// ign-tools. Set to the name of the application if using ign-tools) /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default @@ -50,8 +50,8 @@ namespace gui /// \brief Run GUI application /// \param[in] _argc Number of command line arguments (Used when running /// without ign-tools. Set to 1 if using ign-tools). Note: The object - /// referenced by this variable must continue to exist for the lifetime of the - /// application. + /// referenced by this variable must continue to exist for the lifetime of + /// the application. /// \param[in] _argv Command line arguments (Used when running without /// ign-tools. Set to the name of the application if using ign-tools) /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default @@ -66,26 +66,26 @@ namespace gui EventManager &_eventMgr, bool _sameProcess); - /// \brief Create a Gazebo GUI application - /// \param[in] _argc Number of command line arguments (Used when running - /// without ign-tools. Set to 1 if using ign-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 - /// ign-tools. Set to the name of the application if using ign-tools) - /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default - /// configuration from IGN_HOMEDIR/.ignition/gazebo/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 - /// IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. - /// \param[in] _loadPluginsFromSdf If true, plugins specified in the world - /// SDFormat file will get loaded. - IGNITION_GAZEBO_GUI_VISIBLE - std::unique_ptr IGN_DEPRECATED(6) createGui( - int &_argc, char **_argv, const char *_guiConfig, - const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); + /// \brief Create a Gazebo GUI application + /// \param[in] _argc Number of command line arguments (Used when running + /// without ign-tools. Set to 1 if using ign-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 + /// ign-tools. Set to the name of the application if using ign-tools) + /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default + /// configuration from IGN_HOMEDIR/.ignition/gazebo/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 + /// IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. + /// \param[in] _loadPluginsFromSdf If true, plugins specified in the world + /// SDFormat file will get loaded. + IGNITION_GAZEBO_GUI_VISIBLE + std::unique_ptr IGN_DEPRECATED(6) createGui( + int &_argc, char **_argv, const char *_guiConfig, + const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); /// \brief Create a Gazebo GUI application /// \param[in] _argc Number of command line arguments (Used when running @@ -101,8 +101,8 @@ namespace gui /// \param[in] _sameProcess is server and gui running in the same process ? /// \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 + /// configuration file will be loaded. If this argument is a nullptr or if + /// the file does not exist, the default configuration from /// IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. /// \param[in] _loadPluginsFromSdf If true, plugins specified in the world /// SDFormat file will get loaded. @@ -112,7 +112,8 @@ namespace gui EntityComponentManager &_ecm, EventManager &_eventMgr, bool _sameProcess, - const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); + const char *_defaultGuiConfig = nullptr, + bool _loadPluginsFromSdf = true); } // namespace gui } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE diff --git a/src/Server.cc b/src/Server.cc index a9d57afdc2..a694e478df 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -208,22 +208,25 @@ Server::Server(const ServerConfig &_config) Server::~Server() = default; ///////////////////////////////////////////////// -std::optional> Server::SharedEntityComponentManager( - const unsigned int _worldIndex) +std::optional> + Server::SharedEntityComponentManager(const unsigned int _worldIndex) { if (this->dataPtr->simRunners.size() > _worldIndex) { - return std::reference_wrapper(this->dataPtr->simRunners[_worldIndex]->EntityCompMgr()); + return std::reference_wrapper( + this->dataPtr->simRunners[_worldIndex]->EntityCompMgr()); } return std::nullopt; } ///////////////////////////////////////////////// -std::optional> Server::SharedEventManager(const unsigned int _worldIndex) +std::optional> + Server::SharedEventManager(const unsigned int _worldIndex) { if (this->dataPtr->simRunners.size() > _worldIndex) { - return std::reference_wrapper(this->dataPtr->simRunners[_worldIndex]->EventMgr()); + return std::reference_wrapper( + this->dataPtr->simRunners[_worldIndex]->EventMgr()); } return std::nullopt; } diff --git a/src/ign.cc b/src/ign.cc index 17967dd8a7..067a723b6c 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -430,7 +430,8 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, // prevent a warning since we do need to pass a char* to runGui char *argv = const_cast("ign-gazebo-gui"); return ignition::gazebo::gui::runGui( - argc, &argv, _guiConfig, (*sharedEcm).get(), (*sharedEventManager).get(), true); + argc, &argv, _guiConfig, (*sharedEcm).get(), + (*sharedEventManager).get(), true); } ignerr << "Unable to create server config\n"; From dc02d76d13359ad5097d74c0da0d2017e3fb6dbf Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 29 Jun 2021 20:14:37 +0200 Subject: [PATCH 22/35] Using a worker pool to update plugins Signed-off-by: ahcorde --- src/SimulationRunner.cc | 8 ++++---- src/gui/GuiRunner.cc | 19 +++++++++++++++++-- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 8db9451f67..4c587008aa 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -536,11 +536,11 @@ void SimulationRunner::UpdateSystems() IGN_PROFILE("Update"); for (auto& system : this->systemsUpdate) system->Update(this->currentInfo, this->entityCompMgr); + } - if (serverConfig.SameProcessAsGUI()) - { - this->eventMgr.Emit(); - } + if (serverConfig.SameProcessAsGUI()) + { + this->eventMgr.Emit(); } { diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 91415a019e..ae8fbe2f4c 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -47,9 +48,12 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief Update the plugins. public: void UpdatePlugins(); - /// \brief + /// \brief This method will be executed when a UpdatePlugins event is received. void UpdatePluginsEvent(); + /// \brief This method will update the plugins in one of the workers + public: void UpdatePluginsFromEvent(); + /// \brief Connection to the UpdatePlugins event. public: ignition::common::ConnectionPtr UpdatePluginsConn; @@ -79,6 +83,9 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief The plugin update thread.. public: std::thread updateThread; + + /// \brief A pool of worker threads. + public: common::WorkerPool pool; }; ///////////////////////////////////////////////// @@ -250,12 +257,20 @@ void GuiRunner::Implementation::UpdatePlugins() this->ecm.ClearRemovedComponents(); } +///////////////////////////////////////////////// +void GuiRunner::Implementation::UpdatePluginsFromEvent() +{ + std::lock_guard lock(this->updateMutex); + UpdatePlugins(); +} + +///////////////////////////////////////////////// void GuiRunner::Implementation::UpdatePluginsEvent() { std::lock_guard lock(this->updateMutex); if (this->ecm.HasNewEntities() || this->ecm.HasEntitiesMarkedForRemoval()) { - this->UpdatePlugins(); + pool.AddWork(std::bind(&GuiRunner::Implementation::UpdatePluginsFromEvent, this)); } } From 7f63aa68ff48c8bcef293a9299fbfc56f8f2d31e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 29 Jun 2021 22:00:51 +0200 Subject: [PATCH 23/35] make linters happy Signed-off-by: ahcorde --- src/gui/GuiRunner.cc | 6 ++++-- src/gui/gui.config | 17 +++++++++++++++++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index ae8fbe2f4c..4cffb8e088 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -48,7 +48,8 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief Update the plugins. public: void UpdatePlugins(); - /// \brief This method will be executed when a UpdatePlugins event is received. + /// \brief This method will be executed when a UpdatePlugins event is + /// received. void UpdatePluginsEvent(); /// \brief This method will update the plugins in one of the workers @@ -271,6 +272,7 @@ void GuiRunner::Implementation::UpdatePluginsEvent() if (this->ecm.HasNewEntities() || this->ecm.HasEntitiesMarkedForRemoval()) { - pool.AddWork(std::bind(&GuiRunner::Implementation::UpdatePluginsFromEvent, this)); + pool.AddWork(std::bind( + &GuiRunner::Implementation::UpdatePluginsFromEvent, this)); } } diff --git a/src/gui/gui.config b/src/gui/gui.config index 67f4de172e..bf7c7338a7 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -40,6 +40,23 @@ 6 0 6 0 0.5 3.14 + + + + + + Camera tracking + false + docked + + + From 7e4a5f5b14068684931e03c7fcd68163b8186832 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 2 Jul 2021 13:52:30 +0200 Subject: [PATCH 24/35] Restored CreateRenderingEntities in RenderUtils.cc Signed-off-by: ahcorde --- src/rendering/RenderUtil.cc | 252 +++++++++++++++++------------------- 1 file changed, 117 insertions(+), 135 deletions(-) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 0046e47b38..07c9d645c7 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -109,11 +109,6 @@ class ignition::gazebo::RenderUtilPrivate /// \param[in] _ecm The entity-component manager public: void UpdateRenderingEntities(const EntityComponentManager &_ecm); - /// \brief Helper function to add sensors in the CreateRenderingEntities - /// method - /// \param[in] _ecm The entity-component manager - public: void AddSensors(const EntityComponentManager &_ecm); - /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1236,8 +1231,10 @@ void RenderUtil::Update() } ////////////////////////////////////////////////// -void RenderUtilPrivate::AddSensors(const EntityComponentManager &_ecm) +void RenderUtilPrivate::CreateRenderingEntities( + const EntityComponentManager &_ecm, const UpdateInfo &_info) { + IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); auto addNewSensor = [&_ecm, this](Entity _entity, const sdf::Sensor &_sdfData, Entity _parent, const std::string &_topicSuffix) @@ -1262,133 +1259,6 @@ void RenderUtilPrivate::AddSensors(const EntityComponentManager &_ecm) const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; - if (!this->initialized) - { - if (this->enableSensors) - { - // Create cameras - _ecm.Each( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.Each( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create rgbd cameras - _ecm.Each( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.Each( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.Each( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } - } - else - { - if (this->enableSensors) - { - // Create cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create RGBD cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.EachNew( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.EachNew( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } - } -} - -////////////////////////////////////////////////// -void RenderUtilPrivate::CreateRenderingEntities( - const EntityComponentManager &_ecm, const UpdateInfo &_info) -{ - IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); // Treat all pre-existent entities as new at startup // TODO(anyone) refactor Each and EachNew below to reduce duplicate code if (!this->initialized) @@ -1644,7 +1514,63 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); - this->AddSensors(_ecm); + if (this->enableSensors) + { + // Create cameras + _ecm.Each( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _camera->Data(), _parent->Data(), + cameraSuffix); + return true; + }); + + // Create depth cameras + _ecm.Each( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), + depthCameraSuffix); + return true; + }); + + // Create rgbd cameras + _ecm.Each( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), + rgbdCameraSuffix); + return true; + }); + + // Create gpu lidar + _ecm.Each( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); + return true; + }); + + // Create thermal camera + _ecm.Each( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), + thermalCameraSuffix); + return true; + }); + } this->initialized = true; } else @@ -1904,7 +1830,63 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); - this->AddSensors(_ecm); + if (this->enableSensors) + { + // Create cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _camera->Data(), _parent->Data(), + cameraSuffix); + return true; + }); + + // Create depth cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), + depthCameraSuffix); + return true; + }); + + // Create RGBD cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), + rgbdCameraSuffix); + return true; + }); + + // Create gpu lidar + _ecm.EachNew( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); + return true; + }); + + // Create thermal camera + _ecm.EachNew( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), + thermalCameraSuffix); + return true; + }); + } } } From 5c0c29fe8b06ec317d219b93bf652e5a1e00248e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 2 Jul 2021 15:55:58 +0200 Subject: [PATCH 25/35] make linters happy Signed-off-by: ahcorde --- src/rendering/RenderUtil.cc | 255 +++++++++++++++++++----------------- 1 file changed, 135 insertions(+), 120 deletions(-) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 30a7ded79b..ce38c287a0 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -109,6 +109,11 @@ class ignition::gazebo::RenderUtilPrivate /// \param[in] _ecm The entity-component manager public: void UpdateRenderingEntities(const EntityComponentManager &_ecm); + /// \brief Helper function to add sensors in the CreateRenderingEntities + /// method + /// \param[in] _ecm The entity-component manager + public: void AddSensors(const EntityComponentManager &_ecm); + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1034,12 +1039,8 @@ void RenderUtil::Update() this->dataPtr->UpdateThermalCamera(thermalCameraData); } - -////////////////////////////////////////////////// -void RenderUtilPrivate::CreateRenderingEntities( - const EntityComponentManager &_ecm, const UpdateInfo &_info) +void RenderUtilPrivate::AddSensors(const EntityComponentManager &_ecm) { - IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); auto addNewSensor = [&_ecm, this](Entity _entity, const sdf::Sensor &_sdfData, Entity _parent, const std::string &_topicSuffix) @@ -1064,6 +1065,133 @@ void RenderUtilPrivate::CreateRenderingEntities( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; + if (!this->initialized) + { + if (this->enableSensors) + { + // Create cameras + _ecm.Each( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _camera->Data(), _parent->Data(), + cameraSuffix); + return true; + }); + + // Create depth cameras + _ecm.Each( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), + depthCameraSuffix); + return true; + }); + + // Create rgbd cameras + _ecm.Each( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), + rgbdCameraSuffix); + return true; + }); + + // Create gpu lidar + _ecm.Each( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); + return true; + }); + + // Create thermal camera + _ecm.Each( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), + thermalCameraSuffix); + return true; + }); + } + } + else + { + if (this->enableSensors) + { + // Create cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _camera->Data(), _parent->Data(), + cameraSuffix); + return true; + }); + + // Create depth cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), + depthCameraSuffix); + return true; + }); + + // Create RGBD cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), + rgbdCameraSuffix); + return true; + }); + + // Create gpu lidar + _ecm.EachNew( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); + return true; + }); + + // Create thermal camera + _ecm.EachNew( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), + thermalCameraSuffix); + return true; + }); + } + } +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::CreateRenderingEntities( + const EntityComponentManager &_ecm, const UpdateInfo &_info) +{ + IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); // Treat all pre-existent entities as new at startup // TODO(anyone) refactor Each and EachNew below to reduce duplicate code if (!this->initialized) @@ -1319,63 +1447,7 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); - if (this->enableSensors) - { - // Create cameras - _ecm.Each( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.Each( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create rgbd cameras - _ecm.Each( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.Each( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.Each( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } + this->AddSensors(_ecm); this->initialized = true; } else @@ -1634,64 +1706,7 @@ void RenderUtilPrivate::CreateRenderingEntities( } return true; }); - - if (this->enableSensors) - { - // Create cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create RGBD cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.EachNew( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.EachNew( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } + this->AddSensors(_ecm); } } From 247e827829c0a28ef49938233f399daf4f0af653 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 22 Jul 2021 13:33:28 +0200 Subject: [PATCH 26/35] Added feedback Signed-off-by: ahcorde --- include/ignition/gazebo/Server.hh | 4 +- .../ignition/gazebo/components/SameProcess.hh | 2 +- include/ignition/gazebo/gui/Gui.hh | 6 +-- include/ignition/gazebo/gui/GuiSystem.hh | 2 +- src/Server.cc | 4 +- src/SimulationRunner.cc | 2 +- src/gui/GuiRunner.cc | 4 +- src/gui/GuiRunner.hh | 5 ++- src/gui/gui.config | 45 ------------------- .../plugins/scene_manager/GzSceneManager.cc | 2 +- src/rendering/RenderUtil.cc | 2 + src/rendering/SceneManager.cc | 5 ++- src/systems/sensors/Sensors.cc | 12 ++--- 13 files changed, 24 insertions(+), 71 deletions(-) diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index d396167f95..af40cd92bc 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -120,13 +120,13 @@ namespace ignition /// \param[in] _worldIndex Index of the world in the simrunner /// \return The Entity Component Manager reference public: std::optional> - SharedEntityComponentManager(const unsigned int _worldIndex = 0); + SharedEntityComponentManager(const unsigned int _worldIndex = 0) const; /// \brief Get the Event Manager reference /// \param[in] _worldIndex Index of the world in the simrunner /// \return The Event Manager reference public: std::optional> - SharedEventManager(const unsigned int _worldIndex = 0); + SharedEventManager(const unsigned int _worldIndex = 0) const; /// \brief Set the update period. The update period is the wall-clock time /// between ECS updates. diff --git a/include/ignition/gazebo/components/SameProcess.hh b/include/ignition/gazebo/components/SameProcess.hh index a38e377259..96a75b642d 100644 --- a/include/ignition/gazebo/components/SameProcess.hh +++ b/include/ignition/gazebo/components/SameProcess.hh @@ -30,7 +30,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { /// \brief A component used to hold if the server and gui are running in the - /// same process property. + /// same process. using SameProcess = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SameProcess", diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index 9150ad488f..3f7b62128b 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -55,10 +55,10 @@ namespace gui /// \param[in] _argv Command line arguments (Used when running without /// ign-tools. Set to the name of the application if using ign-tools) /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default + /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. /// \param[in] _ecm Entity-component manager. /// \param[in] _eventMgr Event manager - /// \param[in] _sameProcess is server and gui running in the same process ? - /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. + /// \param[in] _sameProcess Are the server and gui running in the same process? IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, char **_argv, const char *_guiConfig, @@ -98,7 +98,7 @@ namespace gui /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. /// \param[in] _ecm Entity-component manager. /// \param[in] _eventMgr Event manager - /// \param[in] _sameProcess is server and gui running in the same process ? + /// \param[in] _sameProcess are the server and gui running in the same process ? /// \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 diff --git a/include/ignition/gazebo/gui/GuiSystem.hh b/include/ignition/gazebo/gui/GuiSystem.hh index f1200da7c7..4d121a5860 100644 --- a/include/ignition/gazebo/gui/GuiSystem.hh +++ b/include/ignition/gazebo/gui/GuiSystem.hh @@ -59,7 +59,7 @@ namespace gazebo /// configure the gui system plugins with the event manager to listen /// ecm updates and set if we are running in the same process /// \param[in] _ecm Mutable event manager - /// \param[in] _sameProcess is server and gui running in the same process ? + /// \param[in] _sameProcess are the server and gui running in the same process ? public: virtual void Configure( EventManager &/*_event*/, bool /*sameProcess*/){} }; diff --git a/src/Server.cc b/src/Server.cc index a694e478df..8e070ef45d 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -209,7 +209,7 @@ Server::~Server() = default; ///////////////////////////////////////////////// std::optional> - Server::SharedEntityComponentManager(const unsigned int _worldIndex) + Server::SharedEntityComponentManager(const unsigned int _worldIndex) const { if (this->dataPtr->simRunners.size() > _worldIndex) { @@ -221,7 +221,7 @@ std::optional> ///////////////////////////////////////////////// std::optional> - Server::SharedEventManager(const unsigned int _worldIndex) + Server::SharedEventManager(const unsigned int _worldIndex) const { if (this->dataPtr->simRunners.size() > _worldIndex) { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 4c587008aa..644c416e7f 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -538,7 +538,7 @@ void SimulationRunner::UpdateSystems() system->Update(this->currentInfo, this->entityCompMgr); } - if (serverConfig.SameProcessAsGUI()) + if (this->serverConfig.SameProcessAsGUI()) { this->eventMgr.Emit(); } diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 4cffb8e088..11a4542673 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -39,7 +39,7 @@ using namespace gazebo; ///////////////////////////////////////////////// class ignition::gazebo::GuiRunner::Implementation { - public: explicit Implementation(gazebo::EntityComponentManager &_ecm, + public: Implementation(gazebo::EntityComponentManager &_ecm, gazebo::EventManager &_eventMgr) : ecm(_ecm), eventMgr(_eventMgr) { @@ -52,7 +52,7 @@ class ignition::gazebo::GuiRunner::Implementation /// received. void UpdatePluginsEvent(); - /// \brief This method will update the plugins in one of the workers + /// \brief This method will update the plugins in one of the worker threads public: void UpdatePluginsFromEvent(); /// \brief Connection to the UpdatePlugins event. diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh index d698bb17bd..f91a9e0e30 100644 --- a/src/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -47,12 +47,13 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \param[in] _worldName World name. /// \param[in] _ecm Entity-component manager. /// \param[in] _eventMgr Event manager - /// \param[in] _sameProcess is server and gui running in the same process ? + /// \param[in] _sameProcess are the server and gui running in the same process ? /// \todo Move to src/gui on v6. - public: explicit GuiRunner( + public: GuiRunner( const std::string &_worldName, EntityComponentManager &_ecm, EventManager &_eventMgr, bool _sameProcess); + /// \brief Destructor public: ~GuiRunner() override; diff --git a/src/gui/gui.config b/src/gui/gui.config index 668d91544b..0d2af5ad65 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -41,23 +41,6 @@ 6 0 6 0 0.5 3.14 - - - - - - Camera tracking - false - docked - - - @@ -147,34 +130,6 @@ - - - - false - 0 - 50 - 250 - 50 - floating - false - #777777 - - - - - - - false - 250 - 50 - 50 - 50 - floating - false - #777777 - - - diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index d0ab75bc87..a65c0b1699 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -61,7 +61,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Is the simulation running the GUI and server in the same process public: bool sameProcess{false}; - /// \brief is the sensors system plugin running ? + /// \brief is the sensors system plugin running? public: bool enableSensors{false}; /// \brief did the first render event occur? diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index ce38c287a0..604d427ec7 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1039,6 +1039,8 @@ void RenderUtil::Update() this->dataPtr->UpdateThermalCamera(thermalCameraData); } + +////////////////////////////////////////////////// void RenderUtilPrivate::AddSensors(const EntityComponentManager &_ecm) { auto addNewSensor = [&_ecm, this](Entity _entity, const sdf::Sensor &_sdfData, diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 769f8b2b94..03feb3691c 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -1084,11 +1084,12 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, if (!this->dataPtr->scene) return rendering::LightPtr(); - if (this->dataPtr->lights.find(_id) != this->dataPtr->lights.end()) + auto lightFind = this->dataPtr->lights.find(_id); + if ( lightFind != this->dataPtr->lights.end()) { ignerr << "Light with Id: [" << _id << "] already exists in the scene" << std::endl; - return this->dataPtr->lights.find(_id)->second; + return lightFind->second; } rendering::VisualPtr parent; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 48aacb2e4a..1f49045195 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -268,7 +268,7 @@ void SensorsPrivate::RunOnce() IGN_PROFILE("PreRender"); this->eventManager->Emit(); // Update the scene graph manually to improve performance - // We only need to do this once per frame It is important to call + // We only need to do this once per frame. It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true // so we don't waste cycles doing one scene graph update per sensor this->scene->PreRender(); @@ -321,13 +321,7 @@ void SensorsPrivate::Run() ////////////////////////////////////////////////// void SensorsPrivate::Render() { - if (!this->sameProcess) - return; - - if (!this->running) - return; - - if (!this->scene) + if (!this->sameProcess || !this->running || !this->scene) return; IGN_PROFILE("SensorsPrivate::RunOnce"); @@ -340,7 +334,7 @@ void SensorsPrivate::Render() IGN_PROFILE("PreRender"); this->eventManager->Emit(); // Update the scene graph manually to improve performance - // We only need to do this once per frame It is important to call + // We only need to do this once per frame. It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true // so we don't waste cycles doing one scene graph update per sensor this->scene->PreRender(); From 2cad0b592d34c72d1ce50b2ad73f35648f79364d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 22 Jul 2021 14:54:18 +0200 Subject: [PATCH 27/35] make linters happy Signed-off-by: ahcorde --- include/ignition/gazebo/gui/Gui.hh | 6 ++++-- include/ignition/gazebo/gui/GuiSystem.hh | 3 ++- src/gui/GuiRunner.hh | 3 ++- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index 3f7b62128b..d2504c7467 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -58,7 +58,8 @@ namespace gui /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. /// \param[in] _ecm Entity-component manager. /// \param[in] _eventMgr Event manager - /// \param[in] _sameProcess Are the server and gui running in the same process? + /// \param[in] _sameProcess Are the server and gui running in the same + /// process? IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, char **_argv, const char *_guiConfig, @@ -98,7 +99,8 @@ namespace gui /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. /// \param[in] _ecm Entity-component manager. /// \param[in] _eventMgr Event manager - /// \param[in] _sameProcess are the server and gui running in the same process ? + /// \param[in] _sameProcess are the server and gui running in the same + /// process ? /// \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 diff --git a/include/ignition/gazebo/gui/GuiSystem.hh b/include/ignition/gazebo/gui/GuiSystem.hh index 4d121a5860..d273e6c0ee 100644 --- a/include/ignition/gazebo/gui/GuiSystem.hh +++ b/include/ignition/gazebo/gui/GuiSystem.hh @@ -59,7 +59,8 @@ namespace gazebo /// configure the gui system plugins with the event manager to listen /// ecm updates and set if we are running in the same process /// \param[in] _ecm Mutable event manager - /// \param[in] _sameProcess are the server and gui running in the same process ? + /// \param[in] _sameProcess are the server and gui running in the same + /// process ? public: virtual void Configure( EventManager &/*_event*/, bool /*sameProcess*/){} }; diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh index f91a9e0e30..cbe519f0a7 100644 --- a/src/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -47,7 +47,8 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \param[in] _worldName World name. /// \param[in] _ecm Entity-component manager. /// \param[in] _eventMgr Event manager - /// \param[in] _sameProcess are the server and gui running in the same process ? + /// \param[in] _sameProcess are the server and gui running in the same + /// process ? /// \todo Move to src/gui on v6. public: GuiRunner( const std::string &_worldName, EntityComponentManager &_ecm, From fbceade64d8e2530f8cb31054134e716ef1ccca6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 9 Aug 2021 15:08:37 -0700 Subject: [PATCH 28/35] Don't make it official yet Signed-off-by: Louise Poubel --- examples/worlds/minimal_scene.sdf | 452 ++++++++++++++++++++++++++++++ src/gui/gui.config | 16 +- 2 files changed, 453 insertions(+), 15 deletions(-) create mode 100644 examples/worlds/minimal_scene.sdf diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf new file mode 100644 index 0000000000..6720d2aa85 --- /dev/null +++ b/examples/worlds/minimal_scene.sdf @@ -0,0 +1,452 @@ + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/buoyancy/control + /world/buoyancy/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/buoyancy/stats + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + docked + + + + + + + false + docked + + + + + + + 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 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 0.1458 + 0 + 0 + 0.1458 + 0 + 0.125 + + 1.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 0 -3.0 0.5 0 0 0 + + + + 0.074154 + 0 + 0 + 0.074154 + 0 + 0.018769 + + 1.0 + + + + + 0.2 + 0.6 + + + + + + + 0.2 + 0.6 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + + 0 3.0 0.5 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + + + + diff --git a/src/gui/gui.config b/src/gui/gui.config index 71c16f48f3..137c55e648 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -27,7 +27,7 @@ - + 3D View false @@ -41,20 +41,6 @@ 6 0 6 0 0.5 3.14 - - - - - - - false - 5 - 5 - floating - false - - - From 21e1b7b9c923742e200e22ea0cfe651c824d80d1 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 9 Aug 2021 15:22:13 -0700 Subject: [PATCH 29/35] alphabetize Signed-off-by: Louise Poubel --- src/gui/plugins/scene_manager/GzSceneManager.cc | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 61c86a21d2..891f9920b4 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -18,19 +18,16 @@ #include "GzSceneManager.hh" #include - +#include +#include #include - #include #include - -#include -#include #include +#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" namespace ignition From 8eeb5afcf0de2c1f9fa48d1b9ad2b64fc67a354c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 10 Aug 2021 19:04:48 +0200 Subject: [PATCH 30/35] Fixed tests Signed-off-by: ahcorde --- .../JointPositionController_TEST.cc | 4 ++-- src/gui/plugins/plot_3d/Plot3D_TEST.cc | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc index 41e694c4ed..0b25cf7fb5 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc @@ -224,8 +224,8 @@ TEST_F(JointPositionControllerGui, isRequestDone = true; }); - int sleep = 0; - int maxSleep = 30; + sleep = 0; + maxSleep = 30; while (sleep < maxSleep && !isRequestDone) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc index cd74822ee0..c40db1133c 100644 --- a/src/gui/plugins/plot_3d/Plot3D_TEST.cc +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -72,8 +72,12 @@ TEST_F(Plot3D, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); // Create GUI runner to handle gazebo::gui plugins + ignition::gazebo::EntityComponentManager ecm; + ignition::gazebo::EventManager eventMgr; IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - auto runner = new gazebo::GuiRunner("test"); + auto runner = new gazebo::GuiRunner("test", ecm, eventMgr, false); + runner->connect(app.get(), &gui::Application::PluginAdded, + runner, &gazebo::GuiRunner::OnPluginAdded); IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION runner->setParent(gui::App()); From af0c3f0776487099ab938e9a6c4d4b0062910e16 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 12 Aug 2021 21:18:17 +0200 Subject: [PATCH 31/35] Windows compile fix Signed-off-by: ahcorde --- src/ign.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ign.hh b/src/ign.hh index d60bc0654b..e3510f69cd 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -98,7 +98,7 @@ extern "C" const char *findFuelResource( /// null to record the default topics. /// \param[in] _guiConfig Path to Ignition GUI configuration file. /// \return 0 if successful, -1 if not. -extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, +extern "C" int runCombined(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, int _recordResources, int _logOverwrite, int _logCompress, From e58339fa0a09775c3c7d811b9a144795179ff976 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 Aug 2021 00:43:48 +0200 Subject: [PATCH 32/35] same process running on Scene3D Signed-off-by: ahcorde --- examples/worlds/minimal_scene.sdf | 21 ++++- src/gui/plugins/scene3d/Scene3D.cc | 141 ++++++++++++++++++++++++++++- src/gui/plugins/scene3d/Scene3D.hh | 42 +++++++++ src/systems/sensors/Sensors.cc | 9 ++ 4 files changed, 206 insertions(+), 7 deletions(-) diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf index 8dc69f2bbf..b6e0ae5b8b 100644 --- a/examples/worlds/minimal_scene.sdf +++ b/examples/worlds/minimal_scene.sdf @@ -26,6 +26,23 @@ Missing for parity with GzScene3D: --> + + 0.001 + 1.0 + + + + + + + ogre2 + @@ -118,9 +135,6 @@ Missing for parity with GzScene3D: true true true - /world/buoyancy/control - /world/buoyancy/stats - @@ -144,7 +158,6 @@ Missing for parity with GzScene3D: true true true - /world/buoyancy/stats diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index f64b711aae..94fad56c27 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -67,6 +67,7 @@ #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Events.hh" #include "ignition/gazebo/gui/GuiEvents.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" @@ -327,6 +328,18 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief The scale values by which to snap the object. public: math::Vector3d scaleSnap = math::Vector3d::One; + + /// \brief Event Manager + public: EventManager *eventManager{nullptr}; + + /// \brief Is the simulation running the GUI and server in the same process + public: bool sameProcess{false}; + + /// \brief is the sensors system plugin running? + public: bool enableSensors{false}; + + /// \brief did the first render event occur? + public: bool emitFirstRender{false}; }; /// \brief Qt and Ogre rendering is happening in different threads @@ -434,6 +447,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Private data class for Scene3D class Scene3DPrivate { + /// \brief This method is used to connect with the event + /// events::EnableSensors. It will set if the simulation is running any + /// sensors + /// \param[in] _enable True if the sensors thread is enabled, false + /// otherwise + public: void EnableSensors(bool _enable); + /// \brief Transport node public: transport::Node node; @@ -503,6 +523,21 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Camera view control service public: std::string cameraViewControlService; + + /// \brief Event Manager + public: EventManager *eventManager{nullptr}; + + /// \brief Is the simulation running the GUI and server in the same process + public: bool sameProcess{false}; + + /// \brief is the sensors system plugin running? + public: bool enableSensors{false}; + + /// \brief did the first render event occur? + public: bool emitFirstRender{false}; + + /// \brief Track connection to "EnableSensors" Event + public: ignition::common::ConnectionPtr enableSensorsConn{nullptr}; }; } } @@ -648,7 +683,12 @@ void IgnRenderer::Render(RenderSync *_renderSync) // update the scene this->dataPtr->renderUtil.SetTransformActive( this->dataPtr->transformControl.Active()); - this->dataPtr->renderUtil.Update(); + + if (this->dataPtr->emitFirstRender && + (!this->dataPtr->sameProcess || !this->dataPtr->enableSensors)) + { + this->dataPtr->renderUtil.Update(); + } // view control this->HandleMouseEvent(); @@ -2330,6 +2370,36 @@ void IgnRenderer::SetFollowPGain(double _gain) this->dataPtr->followPGain = _gain; } +///////////////////////////////////////////////// +void IgnRenderer::SetSameProcess(bool _sameProcess) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->sameProcess = _sameProcess; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetEmitFirstRender(bool _emitFirstRender) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->emitFirstRender = _emitFirstRender; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetEnableSensors(bool _enableSensors) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->enableSensors = _enableSensors; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetEventManager(EventManager &_eventMgr) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->eventManager = &_eventMgr; + if (this->dataPtr->eventManager != nullptr) + this->dataPtr->renderUtil.SetEventManager(_eventMgr); +} + ///////////////////////////////////////////////// void IgnRenderer::SetFollowWorldFrame(bool _worldFrame) { @@ -3183,8 +3253,12 @@ void Scene3D::Update(const UpdateInfo &_info, msgs::Pose poseMsg = msgs::Convert(renderWindow->CameraPose()); this->dataPtr->cameraPosePub.Publish(poseMsg); } - this->dataPtr->renderUtil->UpdateECM(_info, _ecm); - this->dataPtr->renderUtil->UpdateFromECM(_info, _ecm); + if (this->dataPtr->emitFirstRender && + (!this->dataPtr->sameProcess || !this->dataPtr->enableSensors)) + { + this->dataPtr->renderUtil->UpdateECM(_info, _ecm); + this->dataPtr->renderUtil->UpdateFromECM(_info, _ecm); + } // check if video recording is enabled and if we need to lock step // ECM updates with GUI rendering during video recording @@ -3197,6 +3271,27 @@ void Scene3D::Update(const UpdateInfo &_info, } } +///////////////////////////////////////////////// +void Scene3D::Configure(EventManager &_eventMgr, bool _sameProcess) +{ + if (this->dataPtr->eventManager) + return; + + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->SetSameProcess(_sameProcess); + + this->dataPtr->eventManager = &_eventMgr; + this->dataPtr->sameProcess = _sameProcess; + + if (_sameProcess) + { + this->dataPtr->enableSensorsConn = + _eventMgr.Connect( + std::bind(&Scene3DPrivate::EnableSensors, this->dataPtr.get(), + std::placeholders::_1)); + } +} + ///////////////////////////////////////////////// bool Scene3D::OnTransformMode(const msgs::StringMsg &_msg, msgs::Boolean &_res) @@ -3549,11 +3644,27 @@ bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) dropdownMenuEnabledEvent->MenuEnabled()); } } + else if (_event->type() == ignition::gui::events::Render::kType) + { + if (this->dataPtr->sameProcess) + { + this->dataPtr->eventManager->Emit(); + } + this->dataPtr->emitFirstRender = true; + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->SetEmitFirstRender(true); + } // Standard event processing return QObject::eventFilter(_obj, _event); } +///////////////////////////////////////////////// +void Scene3DPrivate::EnableSensors(bool _enable) +{ + this->enableSensors = _enable; +} + ///////////////////////////////////////////////// void RenderWindowItem::UpdateSelectedEntity(Entity _entity, bool _sendEvent) @@ -3672,6 +3783,30 @@ void RenderWindowItem::SetFollowPGain(double _gain) this->dataPtr->renderThread->ignRenderer.SetFollowPGain(_gain); } +///////////////////////////////////////////////// +void RenderWindowItem::SetSameProcess(bool _sameProcess) +{ + this->dataPtr->renderThread->ignRenderer.SetSameProcess(_sameProcess); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetEmitFirstRender(bool _emitFirstRender) +{ + this->dataPtr->renderThread->ignRenderer.SetEmitFirstRender(_emitFirstRender); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetEnableSensors(bool _enableSensors) +{ + this->dataPtr->renderThread->ignRenderer.SetEnableSensors(_enableSensors); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetEventManager(EventManager &_eventMgr) +{ + this->dataPtr->renderThread->ignRenderer.SetEventManager(_eventMgr); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetFollowWorldFrame(bool _worldFrame) { diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index caf21a8102..9208bd86fb 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -40,6 +40,7 @@ #include +#include #include #include "ignition/gui/qt.h" @@ -93,6 +94,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) override; + // Documentation inherited + public: void Configure(EventManager &_eventMgr, bool _sameProcess) override; + /// \brief Callback when receives a drop event. /// \param[in] _drop Dropped string. /// \param[in] _mouseX x coordinate of mouse position. @@ -337,6 +341,25 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _offset Camera follow offset position. public: void SetFollowOffset(const math::Vector3d &_offset); + /// \brief Set if the server and GUI should run in the same process. + /// \param[in]_sameProcessAsGUI True if the server and GUI will run in + /// the same process, False otherwise + public: void SetSameProcess(bool _sameProcess); + + /// \brief Set if the first render event is emitted + /// \param[in] _emitFirstRender True if the first render event is emitted, + /// false otherwise. + public: void SetEmitFirstRender(bool _emitFirstRender); + + /// \brief Set if there is any sensor available + /// \param[in] _enableSensors True if there is any sensor available, + /// false otherwise. + public: void SetEnableSensors(bool _enableSensors); + + /// brief Set the event Manager + /// \param[in] _eventMgr Reference to the event Manager + public: void SetEventManager(EventManager &_eventMgr); + /// \brief Get the target which the user camera is following /// \return Target being followed public: std::string FollowTarget() const; @@ -699,6 +722,25 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _gain Camera follow p gain. public: void SetFollowPGain(double _gain); + /// \brief Set if the server and GUI should run in the same process. + /// \param[in]_sameProcessAsGUI True if the server and GUI will run in + /// the same process, False otherwise + public: void SetSameProcess(bool _sameProcess); + + /// \brief Set if the first render event is emitted + /// \param[in] _emitFirstRender True if the first render event is emitted, + /// false otherwise. + public: void SetEmitFirstRender(bool _emitFirstRender); + + /// \brief Set if there is any sensor available + /// \param[in] _enableSensors True if there is any sensor available, + /// false otherwise. + public: void SetEnableSensors(bool _enableSensors); + + /// brief Set the event Manager + /// \param[in] _eventMgr Reference to the event Manager + public: void SetEventManager(EventManager &_eventMgr); + /// \brief True to set the camera to follow the target in world frame, /// false to follow in target's local frame /// \param[in] _gain Camera follow p gain. diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index fae79f61e8..f13e8034c9 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -356,6 +356,15 @@ void SensorsPrivate::Render() this->sensorManager.RunOnce(this->updateTime); this->eventManager->Emit(); } + { + IGN_PROFILE("PostRender"); + // Update the scene graph manually to improve performance + // We only need to do this once per frame It is important to call + // sensors::RenderingSensor::SetManualSceneUpdate and set it to true + // so we don't waste cycles doing one scene graph update per sensor + this->scene->PostRender(); + this->eventManager->Emit(); + } } ////////////////////////////////////////////////// From e1e1c6fc20b4f99697ea30aac55ef6a7f2ad4da9 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 Aug 2021 09:37:46 +0200 Subject: [PATCH 33/35] try to fix Windows build Signed-off-by: ahcorde --- src/ign.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ign.cc b/src/ign.cc index 067a723b6c..d9fed3a1c5 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -350,7 +350,7 @@ int createServerConfig(ignition::gazebo::ServerConfig &_serverConfig, } ////////////////////////////////////////////////// -extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, +extern "C" int runServer(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, int _recordResources, int _logOverwrite, int _logCompress, @@ -381,7 +381,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, } ////////////////////////////////////////////////// -extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, +extern "C" int runCombined(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, int _recordResources, int _logOverwrite, int _logCompress, From d6d1f8ae90a429900537380c4daf23d4aa70cbed Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 13 Aug 2021 18:58:27 -0700 Subject: [PATCH 34/35] small cleanup tweaks, also fix OnPluginUpdate Signed-off-by: Louise Poubel --- src/gui/GuiRunner.cc | 25 ++++--- src/gui/gui.config | 28 ++++++++ src/gui/plugins/scene3d/Scene3D.cc | 4 ++ .../plugins/scene_manager/GzSceneManager.cc | 4 ++ src/ign.cc | 69 +++++++++---------- 5 files changed, 85 insertions(+), 45 deletions(-) diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index d2e58d3df1..3cead26879 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -131,7 +131,6 @@ GuiRunner::GuiRunner(const std::string &_worldName, this->RequestState(); // Periodically update the plugins - // \todo(anyone) Move the global variables to GuiRunner::Implementation on v5 this->dataPtr->running = true; this->dataPtr->updateThread = std::thread([&]() { @@ -190,17 +189,23 @@ void GuiRunner::RequestState() } ///////////////////////////////////////////////// -void GuiRunner::OnPluginAdded(const QString &) +void GuiRunner::OnPluginAdded(const QString &_objectName) { - // The call above always returns the same plugin, which is the first plugin - // that was loaded. This plugin will set again the eventMgr and the - // sameProcess state. - auto plugins = gui::App()->findChildren(); - for (auto &p : plugins) + auto plugin = gui::App()->PluginByName(_objectName.toStdString()); + if (!plugin) { - p->Configure(this->dataPtr->eventMgr, this->dataPtr->sameProcess); + ignerr << "Failed to get plugin [" << _objectName.toStdString() + << "]" << std::endl; + return; } - this->RequestState(); + + auto guiSystem = dynamic_cast(plugin.get()); + + // Do nothing for pure ign-gui plugins + if (!guiSystem) + return; + + guiSystem->Configure(this->dataPtr->eventMgr, this->dataPtr->sameProcess); } ///////////////////////////////////////////////// @@ -254,7 +259,7 @@ void GuiRunner::Implementation::UpdatePlugins() void GuiRunner::Implementation::UpdatePluginsFromEvent() { std::lock_guard lock(this->updateMutex); - UpdatePlugins(); + this->UpdatePlugins(); } ///////////////////////////////////////////////// diff --git a/src/gui/gui.config b/src/gui/gui.config index 0fe0c798be..137c55e648 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -116,6 +116,34 @@ + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index ac4d4f5d39..871445f8cb 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -3279,7 +3279,11 @@ void Scene3D::Update(const UpdateInfo &_info, void Scene3D::Configure(EventManager &_eventMgr, bool _sameProcess) { if (this->dataPtr->eventManager) + { + ignerr << "Already have event manager, configure was called multiple times." + << std::endl; return; + } auto renderWindow = this->PluginItem()->findChild(); renderWindow->SetSameProcess(_sameProcess); diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index d6d7686c6a..4af0dd81fd 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -95,7 +95,11 @@ GzSceneManager::~GzSceneManager() void GzSceneManager::Configure(EventManager &_eventMgr, bool _sameProcess) { if (this->dataPtr->eventManager) + { + ignerr << "Already have event manager, configure was called multiple times." + << std::endl; return; + } this->dataPtr->eventManager = &_eventMgr; this->dataPtr->sameProcess = _sameProcess; diff --git a/src/ign.cc b/src/ign.cc index d9fed3a1c5..50f9319517 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -31,7 +31,6 @@ #include "ignition/gazebo/config.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/ServerConfig.hh" -#include "SimulationRunner.hh" #include "ignition/gazebo/gui/Gui.hh" @@ -116,6 +115,7 @@ extern "C" const char *findFuelResource( } ////////////////////////////////////////////////// +// Populate _serverConfig with the values from the other inputs. int createServerConfig(ignition::gazebo::ServerConfig &_serverConfig, const char *_sdfString, float _hz, int _levels, const char *_networkRole, @@ -391,52 +391,51 @@ extern "C" int runCombined(const char *_sdfString, { ignition::gazebo::ServerConfig serverConfig; - if (createServerConfig(serverConfig, + if (!createServerConfig(serverConfig, _sdfString, _hz, _levels, _networkRole, _networkSecondaries, _record, _recordPath, _recordResources, _logOverwrite, _logCompress, _playback, _physicsEngine, _renderEngineServer, _renderEngineGui, _file, _recordTopics, true) == 0) { - // Create the Gazebo server - ignition::gazebo::Server server(serverConfig); + ignerr << "Unable to create server config\n"; + return -1; + } - // Run the server - server.Run(false, _iterations, _run == 0); + // Create the Gazebo server + ignition::gazebo::Server server(serverConfig); - auto sharedEcm = server.SharedEntityComponentManager(); - auto sharedEventManager = server.SharedEventManager(); + auto sharedEcm = server.SharedEntityComponentManager(); + auto sharedEventManager = server.SharedEventManager(); - if (!sharedEcm) - { - ignerr << "Unable to get a shared ECM\n"; - return -1; - } - if (!sharedEventManager) - { - ignerr << "Unable to get a shared Event Manager\n"; - return -1; - } - - // argc and argv are going to be passed to a QApplication. The Qt - // documentation has a warning about these: - // "Warning: The data referred to by argc and argv must stay valid for the - // entire lifetime of the QApplication object. In addition, argc must be - // greater than zero and argv must contain at least one valid character - // string." - int argc = 1; - // Converting a string literal to char * is forbidden as of C++11. - // It can only be converted to a const char *. The const cast is here to - // prevent a warning since we do need to pass a char* to runGui - char *argv = const_cast("ign-gazebo-gui"); - return ignition::gazebo::gui::runGui( - argc, &argv, _guiConfig, (*sharedEcm).get(), - (*sharedEventManager).get(), true); + if (!sharedEcm) + { + ignerr << "Unable to get a shared ECM\n"; + return -1; + } + if (!sharedEventManager) + { + ignerr << "Unable to get a shared Event Manager\n"; + return -1; } - ignerr << "Unable to create server config\n"; + // Run the server + server.Run(false, _iterations, _run == 0); - return -1; + // argc and argv are going to be passed to a QApplication. The Qt + // documentation has a warning about these: + // "Warning: The data referred to by argc and argv must stay valid for the + // entire lifetime of the QApplication object. In addition, argc must be + // greater than zero and argv must contain at least one valid character + // string." + int argc = 1; + // Converting a string literal to char * is forbidden as of C++11. + // It can only be converted to a const char *. The const cast is here to + // prevent a warning since we do need to pass a char* to runGui + char *argv = const_cast("ign-gazebo-gui"); + return ignition::gazebo::gui::runGui( + argc, &argv, _guiConfig, (*sharedEcm).get(), + (*sharedEventManager).get(), true); } ////////////////////////////////////////////////// From 449e6e4575fd64431d6e8b5e1d2788cd4df8cd32 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 21 Sep 2021 23:03:27 +0200 Subject: [PATCH 35/35] make linters happy Signed-off-by: ahcorde --- src/ign.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ign.cc b/src/ign.cc index 30f0e5e911..0db4b7b887 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -400,7 +400,7 @@ extern "C" int runCombined(const char *_sdfString, _networkSecondaries, _record, _recordPath, _recordResources, _logOverwrite, _logCompress, _playback, _physicsEngine, _renderEngineServer, - _renderEngineGui, _file, _recordTopics, true,_headless) == 0) + _renderEngineGui, _file, _recordTopics, true, _headless) == 0) { ignerr << "Unable to create server config\n"; return -1;