From 9bfff866e80cb93ea61b4e28bd12dce1379f295f Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 16 Oct 2020 11:41:58 -0700 Subject: [PATCH 1/5] Working on a combined gui and server Signed-off-by: Nate Koenig --- src/cmd/cmdgazebo.rb.in | 77 +++++---- src/gui/GuiRunner.cc | 5 +- src/gui/plugins/scene3d/Scene3D.cc | 1 + src/ign.cc | 259 +++++++++++++++++++++++++++++ src/ign.hh | 8 + 5 files changed, 320 insertions(+), 30 deletions(-) diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index ccbb88e03d..b239129dbf 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -418,6 +418,14 @@ class Cmd # 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 @@ -429,41 +437,52 @@ class Cmd # and gui. if options['server'] == 0 && options['gui'] == 0 - serverPid = Process.fork do - ENV['RMT_PORT'] = '1500' - Process.setpgid(0, 0) - Process.setproctitle('ign gazebo server') - Importer.runServer(parsed, options['iterations'], options['run'], + ENV['RMT_PORT'] = '1500' + 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(':')) - end - - guiPid = Process.fork do - ENV['RMT_PORT'] = '1501' - Process.setpgid(0, 0) - Process.setproctitle('ign gazebo gui') - Importer.runGui(options['gui_config']) - end - - Signal.trap("INT") { - self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0) - self.killProcess(serverPid, "Ignition Gazebo Server", 5.0) - return 1 - } - - # Wait for a child process to end - pid, status = Process.wait2 - - if pid == serverPid - self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0) - else - self.killProcess(serverPid, "Ignition Gazebo Server", 5.0) - end + options['file'], options['record-topics'].join(':'), + options['gui_config']) + + # serverPid = Process.fork do + # ENV['RMT_PORT'] = '1500' + # Process.setpgid(0, 0) + # Process.setproctitle('ign gazebo server') + # Importer.runServer(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(':')) + # end + + # guiPid = Process.fork do + # ENV['RMT_PORT'] = '1501' + # Process.setpgid(0, 0) + # Process.setproctitle('ign gazebo gui') + # Importer.runGui(options['gui_config']) + # end + + # Signal.trap("INT") { + # self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0) + # self.killProcess(serverPid, "Ignition Gazebo Server", 5.0) + # return 1 + # } + + # # Wait for a child process to end + # pid, status = Process.wait2 + + # if pid == serverPid + # self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0) + # else + # self.killProcess(serverPid, "Ignition Gazebo Server", 5.0) + # end # If the -s option was specified, then run only the server elsif options['server'] == 1 diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index a433cc7a4a..b92c27cfb4 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -50,7 +50,7 @@ GuiRunner::GuiRunner(const std::string &_worldName) igndbg << "Requesting initial state from [" << this->stateTopic << "]..." << std::endl; - this->RequestState(); + std::thread *requestThread(new std::thread([&](){this->RequestState();})); } ///////////////////////////////////////////////// @@ -59,6 +59,8 @@ GuiRunner::~GuiRunner() = default; ///////////////////////////////////////////////// void GuiRunner::RequestState() { + std::cout << "\n\nRequest State from[" << this->stateTopic << "]\n"; + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); this->node.Request(this->stateTopic, &GuiRunner::OnStateService, this); } @@ -80,6 +82,7 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) void GuiRunner::OnStateService(const msgs::SerializedStepMap &_res, const bool _result) { + std::cout << "\n\nOnStateService\n\n" << std::endl; if (!_result) { ignerr << "Service call failed for [" << this->stateTopic << "]" diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 053fcd003a..ded83f0751 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -2364,6 +2364,7 @@ void Scene3D::Update(const UpdateInfo &_info, auto worldEntity = _ecm.EntityByComponents(components::Name(this->dataPtr->worldName), components::World()); + std::cout << "\n\n\nWorldName[" << this->dataPtr->worldName << "]\n\n\n"; auto renderEngineGuiComp = _ecm.Component(worldEntity); if (renderEngineGuiComp && !renderEngineGuiComp->Data().empty()) diff --git a/src/ign.cc b/src/ign.cc index 681ec0e09c..b5c59fb6fd 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -373,3 +373,262 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runGui(const char *_guiConfig) char *argv = const_cast("ign-gazebo-gui"); return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig); } + +////////////////////////////////////////////////// +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; + + // Path for logs + std::string recordPathMod = serverConfig.LogRecordPath(); + + // Path for compressed log, used to check for duplicates + std::string cmpPath = std::string(recordPathMod); + if (!std::string(1, cmpPath.back()).compare(ignition::common::separator(""))) + { + // Remove the separator at end of path + cmpPath = cmpPath.substr(0, cmpPath.length() - 1); + } + cmpPath += ".zip"; + + // Initialize console log + if ((_recordPath != nullptr && std::strlen(_recordPath) > 0) || + _record > 0 || _recordResources > 0 || + (_recordTopics != nullptr && std::strlen(_recordTopics) > 0)) + { + if (_playback != nullptr && std::strlen(_playback) > 0) + { + ignerr << "Both record and playback are specified. Only specify one.\n"; + return -1; + } + + serverConfig.SetUseLogRecord(true); + serverConfig.SetLogRecordResources(_recordResources); + + // If a record path is specified + if (_recordPath != nullptr && std::strlen(_recordPath) > 0) + { + recordPathMod = std::string(_recordPath); + + // Update compressed file path to name of recording directory path + cmpPath = std::string(recordPathMod); + if (!std::string(1, cmpPath.back()).compare(ignition::common::separator( + ""))) + { + // Remove the separator at end of path + cmpPath = cmpPath.substr(0, cmpPath.length() - 1); + } + cmpPath += ".zip"; + + // Check if path or compressed file with same prefix exists + if (ignition::common::exists(recordPathMod) || + ignition::common::exists(cmpPath)) + { + // Overwrite if flag specified + if (_logOverwrite > 0) + { + bool recordMsg = false; + bool cmpMsg = false; + // Remove files before initializing console log files on top of them + if (ignition::common::exists(recordPathMod)) + { + recordMsg = true; + ignition::common::removeAll(recordPathMod); + } + if (ignition::common::exists(cmpPath)) + { + cmpMsg = true; + ignition::common::removeFile(cmpPath); + } + + // Create log file before printing any messages so they can be logged + ignLogInit(recordPathMod, "server_console.log"); + + if (recordMsg) + { + ignmsg << "Log path already exists on disk! Existing files will " + << "be overwritten." << std::endl; + ignmsg << "Removing existing path [" << recordPathMod << "]\n"; + } + if (cmpMsg) + { + if (_logCompress > 0) + { + ignwarn << "Compressed log path already exists on disk! Existing " + << "files will be overwritten." << std::endl; + } + ignmsg << "Removing existing compressed file [" << cmpPath << "]\n"; + } + } + // Otherwise rename to unique path + else + { + // Remove the separator at end of path + if (!std::string(1, recordPathMod.back()).compare( + ignition::common::separator(""))) + { + recordPathMod = recordPathMod.substr(0, recordPathMod.length() + - 1); + } + + std::string recordOrigPrefix = std::string(recordPathMod); + int count = 1; + + // Keep renaming until path does not exist for both directory and + // compressed file + while (ignition::common::exists(recordPathMod) || + ignition::common::exists(cmpPath)) + { + recordPathMod = recordOrigPrefix + "(" + std::to_string(count++) + + ")"; + + cmpPath = std::string(recordPathMod); + // Remove the separator at end of path + if (!std::string(1, cmpPath.back()).compare( + ignition::common::separator(""))) + { + cmpPath = cmpPath.substr(0, cmpPath.length() - 1); + } + cmpPath += ".zip"; + } + + ignLogInit(recordPathMod, "server_console.log"); + ignwarn << "Log path already exists on disk! " + << "Recording instead to [" << recordPathMod << "]" << std::endl; + if (_logCompress > 0) + { + ignwarn << "Compressed log path already exists on disk! " + << "Recording instead to [" << cmpPath << "]" << std::endl; + } + } + } + else + { + ignLogInit(recordPathMod, "server_console.log"); + } + // TODO(anyone) In Ignition-D, to be moved to outside and after this + // if-else statement, after all ignLogInit() calls have been finalized, + // so that in SDF will always be ignored in favor of logging both + // console logs and LogRecord recordings to common::ignLogDirectory(). + // In Blueprint and Citadel, LogRecord will record to if no + // --record-path is specified on command line. + serverConfig.SetLogRecordPath(recordPathMod); + serverConfig.SetLogIgnoreSdfPath(true); + } + // Empty record path specified. Use default. + else + { + // Create log file before printing any messages so they can be logged + ignLogInit(recordPathMod, "server_console.log"); + ignmsg << "Recording states to default path [" << recordPathMod << "]" + << std::endl; + + serverConfig.SetLogRecordPath(recordPathMod); + } + + std::vector topics = ignition::common::split( + _recordTopics, ":"); + for (const std::string &topic : topics) + { + serverConfig.AddLogRecordTopic(topic); + } + } + else + { + ignLogInit(serverConfig.LogRecordPath(), "server_console.log"); + } + + if (_logCompress > 0) + { + serverConfig.SetLogRecordCompressPath(cmpPath); + } + + ignmsg << "Ignition Gazebo Server v" << IGNITION_GAZEBO_VERSION_FULL + << std::endl; + + // Set the SDF string to user + if (_sdfString != nullptr && std::strlen(_sdfString) > 0) + { + if (!serverConfig.SetSdfString(_sdfString)) + { + ignerr << "Failed to set SDF string [" << _sdfString << "]" << std::endl; + return -1; + } + } + serverConfig.SetSdfFile(_file); + + // Set the update rate. + if (_hz > 0.0) + serverConfig.SetUpdateRate(_hz); + + // Set whether levels should be used. + if (_levels > 0) + { + ignmsg << "Using the level system\n"; + 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); + } + + if (_playback != nullptr && std::strlen(_playback) > 0) + { + if (_sdfString != nullptr && std::strlen(_sdfString) > 0) + { + ignerr << "Both an SDF file and playback flag are specified. " + << "Only specify one.\n"; + return -1; + } + else + { + ignmsg << "Playing back states" << _playback << std::endl; + serverConfig.SetLogPlaybackPath(ignition::common::absPath( + std::string(_playback))); + } + } + + if (_physicsEngine != nullptr && std::strlen(_physicsEngine) > 0) + { + serverConfig.SetPhysicsEngine(_physicsEngine); + } + + if (_renderEngineServer != nullptr && std::strlen(_renderEngineServer) > 0) + { + serverConfig.SetRenderEngineServer(_renderEngineServer); + } + + if (_renderEngineGui != nullptr && std::strlen(_renderEngineGui) > 0) + { + serverConfig.SetRenderEngineGui(_renderEngineGui); + } + + // Create the Gazebo server + ignition::gazebo::Server server(serverConfig); + + // Run the server + server.Run(false, _iterations, _run == 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"); + return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig); +} diff --git a/src/ign.hh b/src/ign.hh index 9f8aeca3dc..c87e855e26 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -76,4 +76,12 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runGui(const char *_guiConfig); extern "C" IGNITION_GAZEBO_VISIBLE const char *findFuelResource( char *_pathToResource); +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 From 8f3e3bddcd1df1c3268133c58db52009fdf9b642 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 16 Oct 2020 12:44:50 -0700 Subject: [PATCH 2/5] cleanup Signed-off-by: Nate Koenig --- src/cmd/cmdgazebo.rb.in | 36 ---- src/gui/GuiRunner.cc | 4 +- src/gui/plugins/scene3d/Scene3D.cc | 1 - src/ign.cc | 335 ++++++----------------------- 4 files changed, 68 insertions(+), 308 deletions(-) diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index b239129dbf..71278ce6fd 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -448,42 +448,6 @@ class Cmd options['file'], options['record-topics'].join(':'), options['gui_config']) - # serverPid = Process.fork do - # ENV['RMT_PORT'] = '1500' - # Process.setpgid(0, 0) - # Process.setproctitle('ign gazebo server') - # Importer.runServer(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(':')) - # end - - # guiPid = Process.fork do - # ENV['RMT_PORT'] = '1501' - # Process.setpgid(0, 0) - # Process.setproctitle('ign gazebo gui') - # Importer.runGui(options['gui_config']) - # end - - # Signal.trap("INT") { - # self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0) - # self.killProcess(serverPid, "Ignition Gazebo Server", 5.0) - # return 1 - # } - - # # Wait for a child process to end - # pid, status = Process.wait2 - - # if pid == serverPid - # self.killProcess(guiPid, "Ignition Gazebo GUI", 5.0) - # else - # self.killProcess(serverPid, "Ignition Gazebo Server", 5.0) - # end - # If the -s option was specified, then run only the server elsif options['server'] == 1 ENV['RMT_PORT'] = '1500' diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index b92c27cfb4..c5f9b5b750 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -59,7 +59,8 @@ GuiRunner::~GuiRunner() = default; ///////////////////////////////////////////////// void GuiRunner::RequestState() { - std::cout << "\n\nRequest State from[" << this->stateTopic << "]\n"; + // \todo: HACK warning. This sleep is here to prevent a deadlock in + // ignition-transport. std::this_thread::sleep_for(std::chrono::milliseconds(1000)); this->node.Request(this->stateTopic, &GuiRunner::OnStateService, this); } @@ -82,7 +83,6 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) void GuiRunner::OnStateService(const msgs::SerializedStepMap &_res, const bool _result) { - std::cout << "\n\nOnStateService\n\n" << std::endl; if (!_result) { ignerr << "Service call failed for [" << this->stateTopic << "]" diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index e5c2510ada..2674740104 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -2403,7 +2403,6 @@ void Scene3D::Update(const UpdateInfo &_info, auto worldEntity = _ecm.EntityByComponents(components::Name(this->dataPtr->worldName), components::World()); - std::cout << "\n\n\nWorldName[" << this->dataPtr->worldName << "]\n\n\n"; auto renderEngineGuiComp = _ecm.Component(worldEntity); if (renderEngineGuiComp && !renderEngineGuiComp->Data().empty()) diff --git a/src/ign.cc b/src/ign.cc index b5c59fb6fd..76ba1dfb70 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -109,18 +109,17 @@ extern "C" IGNITION_GAZEBO_VISIBLE const char *findFuelResource( } ////////////////////////////////////////////////// -extern "C" IGNITION_GAZEBO_VISIBLE 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) { - ignition::gazebo::ServerConfig serverConfig; - // Path for logs - std::string recordPathMod = serverConfig.LogRecordPath(); + std::string recordPathMod = _serverConfig.LogRecordPath(); // Path for compressed log, used to check for duplicates std::string cmpPath = std::string(recordPathMod); @@ -142,8 +141,8 @@ extern "C" IGNITION_GAZEBO_VISIBLE 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) @@ -252,8 +251,8 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, // console logs and LogRecord recordings to common::ignLogDirectory(). // In Blueprint and Citadel, LogRecord will record to if no // --record-path is specified on command line. - serverConfig.SetLogRecordPath(recordPathMod); - serverConfig.SetLogIgnoreSdfPath(true); + _serverConfig.SetLogRecordPath(recordPathMod); + _serverConfig.SetLogIgnoreSdfPath(true); } // Empty record path specified. Use default. else @@ -263,24 +262,24 @@ extern "C" IGNITION_GAZEBO_VISIBLE 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 @@ -289,31 +288,31 @@ extern "C" IGNITION_GAZEBO_VISIBLE 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) @@ -327,34 +326,58 @@ extern "C" IGNITION_GAZEBO_VISIBLE 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) == 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; + } + + ignerr << "Unable to create server config" << std::endl; + return -1; } ////////////////////////////////////////////////// @@ -385,250 +408,24 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runCombined(const char *_sdfString, { ignition::gazebo::ServerConfig serverConfig; - // Path for logs - std::string recordPathMod = serverConfig.LogRecordPath(); - - // Path for compressed log, used to check for duplicates - std::string cmpPath = std::string(recordPathMod); - if (!std::string(1, cmpPath.back()).compare(ignition::common::separator(""))) - { - // Remove the separator at end of path - cmpPath = cmpPath.substr(0, cmpPath.length() - 1); - } - cmpPath += ".zip"; - - // Initialize console log - if ((_recordPath != nullptr && std::strlen(_recordPath) > 0) || - _record > 0 || _recordResources > 0 || - (_recordTopics != nullptr && std::strlen(_recordTopics) > 0)) + if (createServerConfig(serverConfig, + _sdfString, _hz, _levels, _networkRole, + _networkSecondaries, _record, _recordPath, + _recordResources, _logOverwrite, _logCompress, + _playback, _physicsEngine, _renderEngineServer, + _renderEngineGui, _file, _recordTopics) == 0) { - if (_playback != nullptr && std::strlen(_playback) > 0) - { - ignerr << "Both record and playback are specified. Only specify one.\n"; - return -1; - } - - serverConfig.SetUseLogRecord(true); - serverConfig.SetLogRecordResources(_recordResources); - - // If a record path is specified - if (_recordPath != nullptr && std::strlen(_recordPath) > 0) - { - recordPathMod = std::string(_recordPath); + // Create the Gazebo server + ignition::gazebo::Server server(serverConfig); - // Update compressed file path to name of recording directory path - cmpPath = std::string(recordPathMod); - if (!std::string(1, cmpPath.back()).compare(ignition::common::separator( - ""))) - { - // Remove the separator at end of path - cmpPath = cmpPath.substr(0, cmpPath.length() - 1); - } - cmpPath += ".zip"; + // Run the server + server.Run(false, _iterations, _run == 0); - // Check if path or compressed file with same prefix exists - if (ignition::common::exists(recordPathMod) || - ignition::common::exists(cmpPath)) - { - // Overwrite if flag specified - if (_logOverwrite > 0) - { - bool recordMsg = false; - bool cmpMsg = false; - // Remove files before initializing console log files on top of them - if (ignition::common::exists(recordPathMod)) - { - recordMsg = true; - ignition::common::removeAll(recordPathMod); - } - if (ignition::common::exists(cmpPath)) - { - cmpMsg = true; - ignition::common::removeFile(cmpPath); - } - - // Create log file before printing any messages so they can be logged - ignLogInit(recordPathMod, "server_console.log"); - - if (recordMsg) - { - ignmsg << "Log path already exists on disk! Existing files will " - << "be overwritten." << std::endl; - ignmsg << "Removing existing path [" << recordPathMod << "]\n"; - } - if (cmpMsg) - { - if (_logCompress > 0) - { - ignwarn << "Compressed log path already exists on disk! Existing " - << "files will be overwritten." << std::endl; - } - ignmsg << "Removing existing compressed file [" << cmpPath << "]\n"; - } - } - // Otherwise rename to unique path - else - { - // Remove the separator at end of path - if (!std::string(1, recordPathMod.back()).compare( - ignition::common::separator(""))) - { - recordPathMod = recordPathMod.substr(0, recordPathMod.length() - - 1); - } - - std::string recordOrigPrefix = std::string(recordPathMod); - int count = 1; - - // Keep renaming until path does not exist for both directory and - // compressed file - while (ignition::common::exists(recordPathMod) || - ignition::common::exists(cmpPath)) - { - recordPathMod = recordOrigPrefix + "(" + std::to_string(count++) + - ")"; - - cmpPath = std::string(recordPathMod); - // Remove the separator at end of path - if (!std::string(1, cmpPath.back()).compare( - ignition::common::separator(""))) - { - cmpPath = cmpPath.substr(0, cmpPath.length() - 1); - } - cmpPath += ".zip"; - } - - ignLogInit(recordPathMod, "server_console.log"); - ignwarn << "Log path already exists on disk! " - << "Recording instead to [" << recordPathMod << "]" << std::endl; - if (_logCompress > 0) - { - ignwarn << "Compressed log path already exists on disk! " - << "Recording instead to [" << cmpPath << "]" << std::endl; - } - } - } - else - { - ignLogInit(recordPathMod, "server_console.log"); - } - // TODO(anyone) In Ignition-D, to be moved to outside and after this - // if-else statement, after all ignLogInit() calls have been finalized, - // so that in SDF will always be ignored in favor of logging both - // console logs and LogRecord recordings to common::ignLogDirectory(). - // In Blueprint and Citadel, LogRecord will record to if no - // --record-path is specified on command line. - serverConfig.SetLogRecordPath(recordPathMod); - serverConfig.SetLogIgnoreSdfPath(true); - } - // Empty record path specified. Use default. - else - { - // Create log file before printing any messages so they can be logged - ignLogInit(recordPathMod, "server_console.log"); - ignmsg << "Recording states to default path [" << recordPathMod << "]" - << std::endl; - - serverConfig.SetLogRecordPath(recordPathMod); - } - - std::vector topics = ignition::common::split( - _recordTopics, ":"); - for (const std::string &topic : topics) - { - serverConfig.AddLogRecordTopic(topic); - } - } - else - { - ignLogInit(serverConfig.LogRecordPath(), "server_console.log"); - } - - if (_logCompress > 0) - { - serverConfig.SetLogRecordCompressPath(cmpPath); - } - - ignmsg << "Ignition Gazebo Server v" << IGNITION_GAZEBO_VERSION_FULL - << std::endl; - - // Set the SDF string to user - if (_sdfString != nullptr && std::strlen(_sdfString) > 0) - { - if (!serverConfig.SetSdfString(_sdfString)) - { - ignerr << "Failed to set SDF string [" << _sdfString << "]" << std::endl; - return -1; - } + // Run the gui + return runGui(_guiConfig); } - serverConfig.SetSdfFile(_file); - - // Set the update rate. - if (_hz > 0.0) - serverConfig.SetUpdateRate(_hz); - // Set whether levels should be used. - if (_levels > 0) - { - ignmsg << "Using the level system\n"; - 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); - } + ignerr << "Unable to create server config\n"; - if (_playback != nullptr && std::strlen(_playback) > 0) - { - if (_sdfString != nullptr && std::strlen(_sdfString) > 0) - { - ignerr << "Both an SDF file and playback flag are specified. " - << "Only specify one.\n"; - return -1; - } - else - { - ignmsg << "Playing back states" << _playback << std::endl; - serverConfig.SetLogPlaybackPath(ignition::common::absPath( - std::string(_playback))); - } - } - - if (_physicsEngine != nullptr && std::strlen(_physicsEngine) > 0) - { - serverConfig.SetPhysicsEngine(_physicsEngine); - } - - if (_renderEngineServer != nullptr && std::strlen(_renderEngineServer) > 0) - { - serverConfig.SetRenderEngineServer(_renderEngineServer); - } - - if (_renderEngineGui != nullptr && std::strlen(_renderEngineGui) > 0) - { - serverConfig.SetRenderEngineGui(_renderEngineGui); - } - - // Create the Gazebo server - ignition::gazebo::Server server(serverConfig); - - // Run the server - server.Run(false, _iterations, _run == 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"); - return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig); + return -1; } From 8e1b09eb311bad417700c4882ecd4e9658e3f340 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 16 Oct 2020 12:49:37 -0700 Subject: [PATCH 3/5] More cleanup Signed-off-by: Nate Koenig --- src/gui/GuiRunner.cc | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index c5f9b5b750..39daaa2b2e 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -30,6 +30,8 @@ using namespace ignition; using namespace gazebo; +std::thread *gRequestThread = nullptr; + ///////////////////////////////////////////////// GuiRunner::GuiRunner(const std::string &_worldName) { @@ -50,11 +52,20 @@ GuiRunner::GuiRunner(const std::string &_worldName) igndbg << "Requesting initial state from [" << this->stateTopic << "]..." << std::endl; - std::thread *requestThread(new std::thread([&](){this->RequestState();})); + // \todo: HACK warning. This thread should be removed when the deadlock + // in ignition transport is resolved. See also the todo in RequestState. + gRequestThread = new std::thread([&](){this->RequestState();}); } ///////////////////////////////////////////////// -GuiRunner::~GuiRunner() = default; +GuiRunner::~GuiRunner() +{ + if (gRequestThread) + { + gRequestThread->join(); + delete gRequestThread; + } +} ///////////////////////////////////////////////// void GuiRunner::RequestState() From a325b944207760aa3d412aebab39e1e3dc0f60db Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 16 Oct 2020 12:52:05 -0700 Subject: [PATCH 4/5] Documentation Signed-off-by: Nate Koenig --- src/ign.hh | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/ign.hh b/src/ign.hh index c87e855e26..4e9902d7aa 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -76,6 +76,28 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runGui(const char *_guiConfig); extern "C" IGNITION_GAZEBO_VISIBLE 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, From e4c759d2bd1f06ff0ba428e662d073e6283f8680 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 16 Oct 2020 12:59:48 -0700 Subject: [PATCH 5/5] set process title Signed-off-by: Nate Koenig --- src/cmd/cmdgazebo.rb.in | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 71278ce6fd..71597590c0 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -438,6 +438,7 @@ class Cmd if options['server'] == 0 && options['gui'] == 0 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'], @@ -451,6 +452,7 @@ class Cmd # If the -s option was specified, then run only the server elsif options['server'] == 1 ENV['RMT_PORT'] = '1500' + Process.setproctitle('ign gazebo -s') Importer.runServer(parsed, options['iterations'], options['run'], options['hz'], options['levels'], options['network_role'], options['network_secondaries'], options['record'], @@ -462,6 +464,7 @@ class Cmd # Otherwise run the gui else options['gui'] ENV['RMT_PORT'] = '1501' + Process.setproctitle('ign gazebo -g') Importer.runGui(options['gui_config']) end rescue