diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf index b9fcdc3050..28b27cdfbd 100644 --- a/examples/worlds/minimal_scene.sdf +++ b/examples/worlds/minimal_scene.sdf @@ -27,6 +27,23 @@ Missing for parity with GzScene3D: --> + + 0.001 + 1.0 + + + + + + + ogre2 + @@ -132,7 +149,6 @@ Missing for parity with GzScene3D: true true true - diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 672c264c03..6ee6e880c3 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -58,6 +58,31 @@ namespace ignition /// the entity, which may contain multiple `` tags. using LoadPlugins = common::EventT; + + /// \brief Event used to emit a render event when running in one process. + /// 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 render scene, with this + /// signal we are able to call from 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. + /// 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 UpdateSystems = + ignition::common::EventT; } } // namespace events } // namespace gazebo diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index f67bf6ff1c..4556de7c56 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -116,6 +116,18 @@ namespace ignition /// \brief Destructor 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: std::optional> + 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) const; + /// \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 a975cc7bc4..daea3a6fea 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(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); diff --git a/include/ignition/gazebo/components/SameProcess.hh b/include/ignition/gazebo/components/SameProcess.hh new file mode 100644 index 0000000000..96a75b642d --- /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. + using SameProcess = Component; + + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SameProcess", + SameProcess) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index e23f99cfea..d2504c7467 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -22,6 +22,8 @@ #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -35,20 +37,41 @@ 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 /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. - IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, char **_argv, + 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 + /// 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] _ecm Entity-component manager. + /// \param[in] _eventMgr Event manager + /// \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, + 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. + /// 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 @@ -61,9 +84,38 @@ namespace gui /// \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] _ecm Entity-component manager. + /// \param[in] _eventMgr Event manager + /// \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 + /// 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 createGui( int &_argc, char **_argv, const char *_guiConfig, - const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); + EntityComponentManager &_ecm, + EventManager &_eventMgr, + bool _sameProcess, + const char *_defaultGuiConfig = nullptr, + bool _loadPluginsFromSdf = true); } // namespace gui } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE diff --git a/include/ignition/gazebo/gui/GuiSystem.hh b/include/ignition/gazebo/gui/GuiSystem.hh index 4d2a836869..d273e6c0ee 100644 --- a/include/ignition/gazebo/gui/GuiSystem.hh +++ b/include/ignition/gazebo/gui/GuiSystem.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -51,13 +52,17 @@ namespace gazebo /// \param[in] _info Current simulation information, such as time. /// \param[in] _ecm Mutable reference to the ECM, so the system can read /// and write entities and their components. - public: virtual void Update(const UpdateInfo &_info, - EntityComponentManager &_ecm) - { - // This will avoid many doxygen warnings - (void)_info; - (void)_ecm; - } + 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 are the server and gui running in the same + /// process ? + 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 e344b87038..ae21e26956 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(); + 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; diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 00f9e6c4ab..f01eb0bb69 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -52,6 +52,7 @@ #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" #include "ignition/gazebo/components/RenderEngineServerHeadless.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" @@ -156,6 +157,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/Server.cc b/src/Server.cc index 9b2a2b8448..e84bacc604 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -206,6 +207,30 @@ Server::Server(const ServerConfig &_config) ///////////////////////////////////////////////// Server::~Server() = default; +///////////////////////////////////////////////// +std::optional> + Server::SharedEntityComponentManager(const unsigned int _worldIndex) const +{ + if (this->dataPtr->simRunners.size() > _worldIndex) + { + return std::reference_wrapper( + this->dataPtr->simRunners[_worldIndex]->EntityCompMgr()); + } + return std::nullopt; +} + +///////////////////////////////////////////////// +std::optional> + Server::SharedEventManager(const unsigned int _worldIndex) const +{ + if (this->dataPtr->simRunners.size() > _worldIndex) + { + return std::reference_wrapper( + this->dataPtr->simRunners[_worldIndex]->EventMgr()); + } + return std::nullopt; +} + ///////////////////////////////////////////////// bool Server::Run(const bool _blocking, const uint64_t _iterations, const bool _paused) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index dacfc22246..b8d7cb0fd4 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -236,6 +236,7 @@ class ignition::gazebo::ServerConfigPrivate networkSecondaries(_cfg->networkSecondaries), seed(_cfg->seed), logRecordTopics(_cfg->logRecordTopics), + sameProcessAsGUI(_cfg->sameProcessAsGUI), isHeadlessRendering(_cfg->isHeadlessRendering) { } // \brief The SDF file that the server should load @@ -298,6 +299,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 = false; + /// \brief is the headless mode active. public: bool isHeadlessRendering; }; @@ -345,6 +350,18 @@ std::string ServerConfig::SdfString() const return this->dataPtr->sdfString; } +////////////////////////////////////////////////// +void ServerConfig::SetSameProcessAsGUI(bool _sameProcessAsGUI) +{ + this->dataPtr->sameProcessAsGUI = _sameProcessAsGUI; +} + +////////////////////////////////////////////////// +bool ServerConfig::SameProcessAsGUI() const +{ + return this->dataPtr->sameProcessAsGUI; +} + ////////////////////////////////////////////////// void ServerConfig::SetUpdateRate(const double &_hz) { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 250d0f2fb2..97e9b65203 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -570,6 +570,11 @@ void SimulationRunner::UpdateSystems() system->Update(this->currentInfo, this->entityCompMgr); } + if (this->serverConfig.SameProcessAsGUI()) + { + this->eventMgr.Emit(); + } + { IGN_PROFILE("PostUpdate"); this->entityCompMgr.LockAddingEntitiesToViews(true); @@ -1210,7 +1215,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 36754d3388..383e2908ac 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -300,7 +300,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 9c1d68ba01..782a52d5bb 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"\ @@ -215,6 +217,7 @@ class Cmd 'log-compress' => 0, 'playback' => '', 'run' => 0, + 'same-process' => 0, 'server' => 0, 'verbose' => '1', 'gui_config' => '', @@ -258,6 +261,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 *, int)' + # 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'], options['headless-rendering']) + # 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 2776c31ced..284edc350b 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -43,7 +43,21 @@ namespace gui ////////////////////////////////////////////////// std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, - const char *_defaultGuiConfig, bool _loadPluginsFromSdf) + 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, + EntityComponentManager &_ecm, EventManager &_eventMgr, + bool _sameProcess, const char *_defaultGuiConfig, + bool _loadPluginsFromSdf) { ignition::common::SignalHandler sigHandler; bool sigKilled = false; @@ -169,7 +183,10 @@ std::unique_ptr createGui( // TODO(anyone) Most of ign-gazebo's transport API includes the world name, // which makes it complicated to mix configurations across worlds. // We could have a way to use world-agnostic topics like Gazebo-classic's ~ - auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0)); + auto runner = new ignition::gazebo::GuiRunner( + worldsMsg.data(0), _ecm, _eventMgr, _sameProcess); + runner->connect(app.get(), &ignition::gui::Application::PluginAdded, runner, + &ignition::gazebo::GuiRunner::OnPluginAdded); ++runnerCount; runner->setParent(ignition::gui::App()); @@ -218,7 +235,10 @@ std::unique_ptr createGui( } // GUI runner - auto runner = new ignition::gazebo::GuiRunner(worldName); + auto runner = new ignition::gazebo::GuiRunner( + worldName, _ecm, _eventMgr, _sameProcess); + runner->connect(app.get(), &ignition::gui::Application::PluginAdded, + runner, &ignition::gazebo::GuiRunner::OnPluginAdded); runner->setParent(ignition::gui::App()); ++runnerCount; @@ -290,7 +310,17 @@ std::unique_ptr createGui( ////////////////////////////////////////////////// int runGui(int &_argc, char **_argv, const char *_guiConfig) { - auto app = gazebo::gui::createGui(_argc, _argv, _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) +{ + auto app = gazebo::gui::createGui(_argc, _argv, _guiConfig, + _ecm, _eventMgr, _sameProcess); if (nullptr != app) { // Run main window. diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 50c328dd8a..3cead26879 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -26,6 +27,8 @@ #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/GuiSystem.hh" #include "GuiRunner.hh" @@ -36,11 +39,33 @@ using namespace gazebo; ///////////////////////////////////////////////// class ignition::gazebo::GuiRunner::Implementation { + public: Implementation(gazebo::EntityComponentManager &_ecm, + gazebo::EventManager &_eventMgr) + : ecm(_ecm), eventMgr(_eventMgr) + { + } + /// \brief Update the plugins. public: void UpdatePlugins(); + /// \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 worker threads + public: void UpdatePluginsFromEvent(); + + /// \brief Connection to the UpdatePlugins event. + public: ignition::common::ConnectionPtr UpdatePluginsConn; + /// \brief Entity-component manager. - public: gazebo::EntityComponentManager ecm; + public: gazebo::EntityComponentManager &ecm; + + /// \brief Event manager. + public: gazebo::EventManager &eventMgr; + + /// \brief Is the GUI and server running in the same process? + public: bool sameProcess = false; /// \brief Transport node. public: transport::Node node{}; @@ -59,12 +84,26 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief The plugin update thread.. public: std::thread updateThread; + + /// \brief A pool of worker threads. + public: common::WorkerPool pool; }; ///////////////////////////////////////////////// -GuiRunner::GuiRunner(const std::string &_worldName) - : dataPtr(utils::MakeUniqueImpl()) +GuiRunner::GuiRunner(const std::string &_worldName, + EntityComponentManager &_ecm, gazebo::EventManager &_eventMgr, + bool _sameProcess) + : dataPtr(utils::MakeUniqueImpl(_ecm, _eventMgr)) { + 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(); @@ -92,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([&]() { @@ -151,10 +189,23 @@ void GuiRunner::RequestState() } ///////////////////////////////////////////////// -void GuiRunner::OnPluginAdded(const QString &) +void GuiRunner::OnPluginAdded(const QString &_objectName) { - // This function used to call Update on the plugin, but that's no longer - // necessary. The function is left here for ABI compatibility. + auto plugin = gui::App()->PluginByName(_objectName.toStdString()); + if (!plugin) + { + ignerr << "Failed to get plugin [" << _objectName.toStdString() + << "]" << std::endl; + return; + } + + auto guiSystem = dynamic_cast(plugin.get()); + + // Do nothing for pure ign-gui plugins + if (!guiSystem) + return; + + guiSystem->Configure(this->dataPtr->eventMgr, this->dataPtr->sameProcess); } ///////////////////////////////////////////////// @@ -203,3 +254,22 @@ void GuiRunner::Implementation::UpdatePlugins() } this->ecm.ClearRemovedComponents(); } + +///////////////////////////////////////////////// +void GuiRunner::Implementation::UpdatePluginsFromEvent() +{ + std::lock_guard lock(this->updateMutex); + this->UpdatePlugins(); +} + +///////////////////////////////////////////////// +void GuiRunner::Implementation::UpdatePluginsEvent() +{ + std::lock_guard lock(this->updateMutex); + if (this->ecm.HasNewEntities() || + this->ecm.HasEntitiesMarkedForRemoval()) + { + pool.AddWork(std::bind( + &GuiRunner::Implementation::UpdatePluginsFromEvent, this)); + } +} diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh index 2fdeac2e4e..2b46d29411 100644 --- a/src/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -25,6 +25,8 @@ #include #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -41,7 +43,15 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \brief Constructor /// \param[in] _worldName World name. - public: explicit GuiRunner(const std::string &_worldName); + /// \param[in] _ecm Entity-component manager. + /// \param[in] _eventMgr Event manager + /// \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, + EventManager &_eventMgr, + bool _sameProcess); /// \brief Destructor public: ~GuiRunner() override; diff --git a/src/gui/Gui_TEST.cc b/src/gui/Gui_TEST.cc index 96bd3c952b..604b586d7e 100644 --- a/src/gui/Gui_TEST.cc +++ b/src/gui/Gui_TEST.cc @@ -90,7 +90,10 @@ TEST_F(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 19f2a0da85..4ca3579cd7 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc @@ -63,7 +63,11 @@ TEST_F(JointPositionControllerGui, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); // Create GUI runner to handle gazebo::gui plugins - auto runner = new gazebo::GuiRunner("test"); + ignition::gazebo::EntityComponentManager ecm; + ignition::gazebo::EventManager eventMgr; + auto runner = new gazebo::GuiRunner("test", ecm, eventMgr, false); + runner->connect(app.get(), &gui::Application::PluginAdded, + runner, &gazebo::GuiRunner::OnPluginAdded); runner->setParent(gui::App()); // Add plugin @@ -147,7 +151,12 @@ TEST_F(JointPositionControllerGui, app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); // Create GUI runner to handle gazebo::gui plugins - auto runner = new gazebo::GuiRunner("test"); + // TODO(anyone) Remove deprecation guard once GuiRunner becomes private + ignition::gazebo::EntityComponentManager ecm2; + ignition::gazebo::EventManager eventMgr; + auto runner = new gazebo::GuiRunner("test", ecm2, eventMgr, false); + runner->connect(app.get(), &gui::Application::PluginAdded, + runner, &gazebo::GuiRunner::OnPluginAdded); runner->setParent(gui::App()); // Load plugin @@ -204,13 +213,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; }); sleep = 0; - while (plugin->ModelName() != "model_name" && sleep < maxSleep) + maxSleep = 30; + while (sleep < maxSleep && !isRequestDone) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); QCoreApplication::processEvents(); diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc index 916a0609b3..039d8a73f2 100644 --- a/src/gui/plugins/plot_3d/Plot3D_TEST.cc +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -68,8 +68,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()); diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 32bd92ac91..f9ff3a19d8 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -68,6 +68,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" @@ -331,6 +332,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 @@ -438,6 +451,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; @@ -513,6 +533,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}; }; } } @@ -658,7 +693,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(); @@ -2374,6 +2414,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) { @@ -3234,8 +3304,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 @@ -3248,6 +3322,31 @@ 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); + + 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) @@ -3675,11 +3774,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) @@ -3804,6 +3919,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 28c15227be..c9645d8d81 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" @@ -101,6 +102,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. @@ -372,6 +376,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; @@ -738,6 +761,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] _worldFrame True to use the world frame. diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index f7ec59fbac..4af0dd81fd 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -26,6 +26,7 @@ #include #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Events.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" @@ -41,9 +42,31 @@ 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 + /// \param[in] _enable True if the sensors thread is enabled, false + /// otherwise + 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; }; @@ -61,7 +84,36 @@ 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) +{ + 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; + + this->dataPtr->renderUtil.SetEventManager(_eventMgr); + + if (_sameProcess) + { + this->dataPtr->enableSensorsConn = + _eventMgr.Connect( + std::bind(&GzSceneManagerPrivate::EnableSensors, this->dataPtr.get(), + std::placeholders::_1)); + } +} ///////////////////////////////////////////////// void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *) @@ -79,8 +131,15 @@ void GzSceneManager::Update(const UpdateInfo &_info, { IGN_PROFILE("GzSceneManager::Update"); - this->dataPtr->renderUtil.UpdateECM(_info, _ecm); - this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + // 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)) + { + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + } } ///////////////////////////////////////////////// @@ -89,12 +148,23 @@ bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) if (_event->type() == gui::events::Render::kType) { this->dataPtr->OnRender(); + if (this->dataPtr->sameProcess) + { + this->dataPtr->eventManager->Emit(); + } + this->dataPtr->emitFirstRender = true; } // Standard event processing return QObject::eventFilter(_obj, _event); } +///////////////////////////////////////////////// +void GzSceneManagerPrivate::EnableSensors(bool _enable) +{ + this->enableSensors = _enable; +} + ///////////////////////////////////////////////// void GzSceneManagerPrivate::OnRender() { @@ -107,7 +177,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/gui/plugins/scene_manager/GzSceneManager.hh b/src/gui/plugins/scene_manager/GzSceneManager.hh index ab0d5f1be0..3077ea740e 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, bool _sameProcess) override; + // Documentation inherited private: bool eventFilter(QObject *_obj, QEvent *_event) override; diff --git a/src/ign.cc b/src/ign.cc index 0036ad429b..0db4b7b887 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -115,19 +115,21 @@ extern "C" const char *findFuelResource( } ////////////////////////////////////////////////// -extern "C" int runServer(const char *_sdfString, - int _iterations, int _run, float _hz, int _levels, const char *_networkRole, +// 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, 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, int _headless) { - 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); @@ -149,8 +151,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) @@ -262,23 +264,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 @@ -287,31 +289,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) @@ -325,36 +327,119 @@ 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); } - serverConfig.SetHeadlessRendering(_headless); + _serverConfig.SetHeadlessRendering(_headless); 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" 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, int _headless) +{ + // Create the Gazebo server + ignition::gazebo::ServerConfig serverConfig; + + if (createServerConfig(serverConfig, + _sdfString, _hz, _levels, _networkRole, + _networkSecondaries, _record, _recordPath, + _recordResources, _logOverwrite, _logCompress, + _playback, _physicsEngine, _renderEngineServer, + _renderEngineGui, _file, _recordTopics, false, _headless) == 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; + } + + ignerr << "Something was wrong configuring the server. " << + "Shutting down ign-gazebo-server" << std::endl; + return -1; +} + +////////////////////////////////////////////////// +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, + const char *_playback, const char *_physicsEngine, + const char *_renderEngineServer, const char *_renderEngineGui, + const char *_file, const char *_recordTopics, const char *_guiConfig, + int _headless) +{ + ignition::gazebo::ServerConfig serverConfig; + + if (!createServerConfig(serverConfig, + _sdfString, _hz, _levels, _networkRole, + _networkSecondaries, _record, _recordPath, + _recordResources, _logOverwrite, _logCompress, + _playback, _physicsEngine, _renderEngineServer, + _renderEngineGui, _file, _recordTopics, true, _headless) == 0) + { + ignerr << "Unable to create server config\n"; + return -1; } // Create the Gazebo server ignition::gazebo::Server server(serverConfig); + 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; + } + // Run the server - server.Run(true, _iterations, _run == 0); + server.Run(false, _iterations, _run == 0); - igndbg << "Shutting down ign-gazebo-server" << std::endl; - return 0; + // 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); } ////////////////////////////////////////////////// @@ -371,5 +456,8 @@ 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; + ignition::gazebo::v6::EventManager guiEventEcm; + return ignition::gazebo::gui::runGui( + argc, &argv, _guiConfig, guiEcm, guiEventEcm, false); } diff --git a/src/ign.hh b/src/ign.hh index a935407590..d9ebafe3b1 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -67,7 +67,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. @@ -77,4 +77,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. +/// \param[in] _headless True if server rendering should run headless +/// \return 0 if successful, -1 if not. +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, + const char *_playback, const char *_physicsEngine, + const char *_renderEngineServer, const char *_renderEngineGui, + const char *_file, const char *_recordTopics, const char *_guiConfig, + int _headless); #endif diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 02d67c53c2..de2042fd7a 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -85,6 +85,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" @@ -115,6 +116,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}; @@ -443,6 +449,13 @@ class ignition::gazebo::RenderUtilPrivate public: std::unordered_map> thermalCameraData; + /// \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; + /// \brief A helper function that removes the sensor associated with an /// entity, if an associated sensor exists. This should be called in /// RenderUtil::Update. @@ -485,6 +498,8 @@ class ignition::gazebo::RenderUtilPrivate const std::unordered_map &_trajectoryPoses); }; +std::mutex ignition::gazebo::RenderUtilPrivate::mutexInit; + ////////////////////////////////////////////////// RenderUtil::RenderUtil() : dataPtr(std::make_unique()) { @@ -499,6 +514,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) @@ -1363,10 +1383,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) @@ -1391,6 +1409,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) @@ -1409,7 +1554,6 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); - _ecm.Each( [&](const Entity &_entity, @@ -1421,10 +1565,24 @@ 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)); - this->modelToModelEntities[_parent->Data()].push_back(_entity); + auto node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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); + this->modelToModelEntities[_parent->Data()].push_back(_entity); + } return true; }); @@ -1439,8 +1597,22 @@ 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 node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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); // used for joints @@ -1511,11 +1683,23 @@ void RenderUtilPrivate::CreateRenderingEntities( std::string(heatSignature->Data())); } } - - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); - - this->linkToVisualEntities[_parent->Data()].push_back(_entity); + auto node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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); + this->linkToVisualEntities[_parent->Data()].push_back(_entity); + } return true; }); @@ -1525,8 +1709,23 @@ 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 node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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; }); @@ -1536,8 +1735,23 @@ 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 node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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; }); @@ -1612,68 +1826,23 @@ 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; }); - 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 @@ -1703,10 +1872,24 @@ 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)); - this->modelToModelEntities[_parent->Data()].push_back(_entity); + auto node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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); + this->modelToModelEntities[_parent->Data()].push_back(_entity); + } return true; }); @@ -1721,8 +1904,22 @@ 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 node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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); // used for joints @@ -1793,11 +1990,23 @@ void RenderUtilPrivate::CreateRenderingEntities( std::string(heatSignature->Data())); } } - - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); - - this->linkToVisualEntities[_parent->Data()].push_back(_entity); + auto node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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); + this->linkToVisualEntities[_parent->Data()].push_back(_entity); + } return true; }); @@ -1807,8 +2016,23 @@ 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 node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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; }); @@ -1818,8 +2042,23 @@ 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 node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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; }); @@ -1894,68 +2133,26 @@ 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 node = this->sceneManager.NodeById(_entity); + if (!node) + { + 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; }); - - 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); } } @@ -2259,6 +2456,8 @@ void RenderUtil::Init() if (nullptr != this->dataPtr->scene) return; + std::lock_guard lock(this->dataPtr->mutexInit); + ignition::common::SystemPaths pluginPath; pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); rendering::setPluginPaths(pluginPath.PluginPaths()); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index ee19fae158..1413b40136 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -186,7 +186,9 @@ rendering::VisualPtr SceneManager::CreateModel(Entity _id, if (this->dataPtr->scene->HasVisualName(name)) { ignerr << "Visual: [" << name << "] already exists" << std::endl; - return rendering::VisualPtr(); + auto vis = this->dataPtr->scene->VisualByName(name); + this->dataPtr->visuals[_id] = vis; + return vis; } rendering::VisualPtr modelVis = this->dataPtr->scene->CreateVisual(name); @@ -214,7 +216,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; @@ -234,6 +238,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)); @@ -280,6 +290,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)); @@ -469,6 +485,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(); @@ -1082,11 +1100,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 rendering::LightPtr(); + return lightFind->second; } rendering::VisualPtr parent; @@ -1107,15 +1126,26 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, if (parent) name = parent->Name() + "::" + name; + if (this->dataPtr->scene->HasLightName(name)) + { + auto l = this->dataPtr->scene->LightByName(name); + this->dataPtr->lights[_id] = l; + return l; + } + rendering::LightPtr light; switch (_light.Type()) { 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()); @@ -1127,6 +1157,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 f859f5ba38..411bf71598 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -30,6 +30,7 @@ #include +#include #include #include #include @@ -46,6 +47,7 @@ #include "ignition/gazebo/components/RenderEngineServerHeadless.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" @@ -123,6 +125,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; @@ -139,6 +144,9 @@ 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 private: void WaitForInit(); @@ -178,6 +186,9 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Stop the rendering thread public: void Stop(); + + /// \brief Start the rendering thread + public: void Render(); }; ////////////////////////////////////////////////// @@ -227,8 +238,11 @@ void SensorsPrivate::RunOnce() IGN_PROFILE("SensorsPrivate::RunOnce"); { - IGN_PROFILE("Update"); - this->renderUtil.Update(); + if (!this->sameProcess) + { + IGN_PROFILE("Update"); + this->renderUtil.Update(); + } } @@ -253,35 +267,36 @@ 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(); - } - - { - // publish data - IGN_PROFILE("RunOnce"); - this->sensorManager.RunOnce(this->updateTime); - } + { + 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("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(); + { + // publish data + IGN_PROFILE("RunOnce"); + 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(); + } } - this->activeSensors.clear(); } - this->updateAvailable = false; lock.unlock(); this->renderCv.notify_one(); @@ -317,6 +332,45 @@ void SensorsPrivate::Run() this->renderThread = std::thread(&SensorsPrivate::RenderThread, this); } +////////////////////////////////////////////////// +void SensorsPrivate::Render() +{ + if (!this->sameProcess || !this->running || !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(); + } + { + 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(); + } +} + ////////////////////////////////////////////////// void SensorsPrivate::Stop() { @@ -416,6 +470,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())); + } + } + // Set headless mode if specified from command line auto renderEngineServerHeadlessComp = _ecm.Component(worldEntity); @@ -467,10 +538,26 @@ 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::loadedEngines().size()) + { + igndbg << "Initialization needed" << std::endl; + this->dataPtr->eventManager->Emit + (true); + 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)