823 lines
24 KiB
C++
823 lines
24 KiB
C++
/*
|
|
* Copyright (C) 2012 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.
|
|
*
|
|
*/
|
|
|
|
#ifdef _WIN32
|
|
// Ensure that Winsock2.h is included before Windows.h, which can get
|
|
// pulled in by anybody (e.g., Boost).
|
|
#include <Winsock2.h>
|
|
#define snprintf _snprintf
|
|
#endif
|
|
|
|
#include <stdio.h>
|
|
#include <signal.h>
|
|
#include <mutex>
|
|
#include <boost/algorithm/string.hpp>
|
|
#include <boost/filesystem.hpp>
|
|
#include <boost/lexical_cast.hpp>
|
|
#include <boost/program_options.hpp>
|
|
|
|
#include <sdf/sdf.hh>
|
|
|
|
#include <ignition/math/Rand.hh>
|
|
|
|
#include "gazebo/gazebo.hh"
|
|
#include "gazebo/transport/transport.hh"
|
|
|
|
#include "gazebo/util/LogRecord.hh"
|
|
#include "gazebo/util/LogPlay.hh"
|
|
#include "gazebo/common/ModelDatabase.hh"
|
|
#include "gazebo/common/Exception.hh"
|
|
#include "gazebo/common/Plugin.hh"
|
|
#include "gazebo/common/CommonIface.hh"
|
|
#include "gazebo/common/Console.hh"
|
|
#include "gazebo/common/Events.hh"
|
|
|
|
#include "gazebo/msgs/msgs.hh"
|
|
|
|
#include "gazebo/sensors/SensorsIface.hh"
|
|
|
|
#include "gazebo/physics/PhysicsFactory.hh"
|
|
#include "gazebo/physics/PhysicsIface.hh"
|
|
#include "gazebo/physics/PresetManager.hh"
|
|
#include "gazebo/physics/World.hh"
|
|
#include "gazebo/physics/Base.hh"
|
|
|
|
#include "gazebo/Master.hh"
|
|
#include "gazebo/Server.hh"
|
|
|
|
namespace po = boost::program_options;
|
|
using namespace gazebo;
|
|
|
|
namespace gazebo
|
|
{
|
|
struct ServerPrivate
|
|
{
|
|
/// \brief Boolean used to stop the server.
|
|
static bool stop;
|
|
|
|
/// \brief Communication node.
|
|
transport::NodePtr node;
|
|
|
|
/// \brief Subscribe to server control messages.
|
|
transport::SubscriberPtr serverSub;
|
|
|
|
/// \brief Publisher for world modifications.
|
|
transport::PublisherPtr worldModPub;
|
|
|
|
/// \brief Mutex to protect controlMsgs.
|
|
std::mutex receiveMutex;
|
|
|
|
/// \brief List of received control messages.
|
|
std::list<msgs::ServerControl> controlMsgs;
|
|
|
|
/// \brief Command line params that are passed to various Gazebo objects.
|
|
gazebo::common::StrStr_M params;
|
|
|
|
/// \brief Boost program options variable map.
|
|
boost::program_options::variables_map vm;
|
|
|
|
/// \brief True when initialized.
|
|
bool initialized;
|
|
|
|
/// \brief Save argc for access by system plugins.
|
|
int systemPluginsArgc;
|
|
|
|
/// \brief Save argv for access by system plugins.
|
|
char **systemPluginsArgv;
|
|
};
|
|
}
|
|
|
|
bool ServerPrivate::stop = true;
|
|
|
|
/////////////////////////////////////////////////
|
|
Server::Server()
|
|
: dataPtr(new ServerPrivate())
|
|
{
|
|
this->dataPtr->initialized = false;
|
|
this->dataPtr->systemPluginsArgc = 0;
|
|
this->dataPtr->systemPluginsArgv = NULL;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
Server::~Server()
|
|
{
|
|
fflush(stdout);
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::PrintUsage()
|
|
{
|
|
std::cerr << "gzserver -- Run the Gazebo server.\n\n";
|
|
std::cerr << "`gzserver` [options] <world_file>\n\n";
|
|
std::cerr << "Gazebo server runs simulation and handles commandline "
|
|
<< "options, starts a Master, runs World update and sensor generation "
|
|
<< "loops.\n\n";
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
bool Server::ParseArgs(int _argc, char **_argv)
|
|
{
|
|
// Save a copy of argc and argv for consumption by system plugins
|
|
this->dataPtr->systemPluginsArgc = _argc;
|
|
this->dataPtr->systemPluginsArgv = new char*[_argc];
|
|
for (int i = 0; i < _argc; ++i)
|
|
{
|
|
int argvLen = strlen(_argv[i]) + 1;
|
|
this->dataPtr->systemPluginsArgv[i] = new char[argvLen];
|
|
snprintf(this->dataPtr->systemPluginsArgv[i], argvLen, "%s", _argv[i]);
|
|
}
|
|
|
|
po::options_description visibleDesc("Options");
|
|
visibleDesc.add_options()
|
|
("version,v", "Output version information.")
|
|
("verbose", "Increase the messages written to the terminal.")
|
|
("help,h", "Produce this help message.")
|
|
("pause,u", "Start the server in a paused state.")
|
|
("physics,e", po::value<std::string>(),
|
|
"Specify a physics engine (ode|bullet|dart|simbody).")
|
|
("play,p", po::value<std::string>(), "Play a log file.")
|
|
("record,r", "Record state data.")
|
|
("record_encoding", po::value<std::string>()->default_value("zlib"),
|
|
"Compression encoding format for log data (zlib|bz2|txt).")
|
|
("record_path", po::value<std::string>()->default_value(""),
|
|
"Absolute path in which to store state data")
|
|
("record_period", po::value<double>()->default_value(-1),
|
|
"Recording period (seconds).")
|
|
("record_filter", po::value<std::string>()->default_value(""),
|
|
"Recording filter (supports wildcard and regular expression).")
|
|
("record_resources", "Recording with model meshes and materials.")
|
|
("seed", po::value<double>(), "Start with a given random number seed.")
|
|
("iters", po::value<unsigned int>(), "Number of iterations to simulate.")
|
|
("minimal_comms", "Reduce the TCP/IP traffic output by gzserver")
|
|
("server-plugin,s", po::value<std::vector<std::string> >(),
|
|
"Load a plugin.")
|
|
("profile,o", po::value<std::string>(),
|
|
"Physics preset profile name from the options in the world file.");
|
|
|
|
po::options_description hiddenDesc("Hidden options");
|
|
hiddenDesc.add_options()
|
|
// This is a bit of a hack. The server assumes the last item on the
|
|
// command (if present) is a world file. A problem arises with:
|
|
// gazebo -g <some_gui_plugin.so>
|
|
// Without this hidden option, the server would try to load
|
|
// <some_gui_plugin.so> as a world file.
|
|
("gui-plugin,g", po::value<std::vector<std::string> >(),
|
|
"Gui plugin ignored.")
|
|
("world_file", po::value<std::string>(), "SDF world to load.")
|
|
("pass_through", po::value<std::vector<std::string> >(),
|
|
"not used, passed through to system plugins.");
|
|
|
|
po::options_description desc("Options");
|
|
desc.add(visibleDesc).add(hiddenDesc);
|
|
|
|
po::positional_options_description positionalDesc;
|
|
positionalDesc.add("world_file", 1).add("pass_through", -1);
|
|
|
|
try
|
|
{
|
|
po::store(po::command_line_parser(_argc, _argv).options(desc).positional(
|
|
positionalDesc).allow_unregistered().run(), this->dataPtr->vm);
|
|
|
|
po::notify(this->dataPtr->vm);
|
|
}
|
|
catch(boost::exception &_e)
|
|
{
|
|
std::cerr << "Error. Invalid arguments\n";
|
|
// NOTE: boost::diagnostic_information(_e) breaks lucid
|
|
// std::cerr << boost::diagnostic_information(_e) << "\n";
|
|
return false;
|
|
}
|
|
|
|
if (this->dataPtr->vm.count("version"))
|
|
{
|
|
std::cout << GAZEBO_VERSION_HEADER << std::endl;
|
|
return false;
|
|
}
|
|
|
|
if (this->dataPtr->vm.count("help"))
|
|
{
|
|
this->PrintUsage();
|
|
std::cerr << visibleDesc << "\n";
|
|
return false;
|
|
}
|
|
|
|
if (this->dataPtr->vm.count("verbose"))
|
|
{
|
|
gazebo::printVersion();
|
|
gazebo::common::Console::SetQuiet(false);
|
|
}
|
|
|
|
if (this->dataPtr->vm.count("minimal_comms"))
|
|
gazebo::transport::setMinimalComms(true);
|
|
else
|
|
gazebo::transport::setMinimalComms(false);
|
|
|
|
// Set the random number seed if present on the command line.
|
|
if (this->dataPtr->vm.count("seed"))
|
|
{
|
|
try
|
|
{
|
|
math::Rand::SetSeed(this->dataPtr->vm["seed"].as<double>());
|
|
ignition::math::Rand::Seed(this->dataPtr->vm["seed"].as<double>());
|
|
}
|
|
catch(boost::bad_any_cast &_e)
|
|
{
|
|
gzerr << "Unable to set random number seed. Must supply a number.\n";
|
|
}
|
|
}
|
|
|
|
/// Load all the plugins specified on the command line
|
|
if (this->dataPtr->vm.count("server-plugin"))
|
|
{
|
|
std::vector<std::string> pp =
|
|
this->dataPtr->vm["server-plugin"].as<std::vector<std::string> >();
|
|
|
|
for (std::vector<std::string>::iterator iter = pp.begin();
|
|
iter != pp.end(); ++iter)
|
|
{
|
|
gazebo::addPlugin(*iter);
|
|
}
|
|
}
|
|
|
|
// Set the parameter to record a log file
|
|
if (this->dataPtr->vm.count("record"))
|
|
{
|
|
this->dataPtr->params["record"] =
|
|
this->dataPtr->vm["record_path"].as<std::string>();
|
|
this->dataPtr->params["record_encoding"] =
|
|
this->dataPtr->vm["record_encoding"].as<std::string>();
|
|
if (this->dataPtr->vm.count("record_resources"))
|
|
this->dataPtr->params["record_resources"] = "true";
|
|
}
|
|
|
|
if (this->dataPtr->vm.count("iters"))
|
|
{
|
|
try
|
|
{
|
|
this->dataPtr->params["iterations"] = boost::lexical_cast<std::string>(
|
|
this->dataPtr->vm["iters"].as<unsigned int>());
|
|
}
|
|
catch(...)
|
|
{
|
|
this->dataPtr->params["iterations"] = "0";
|
|
gzerr << "Unable to set iterations of [" <<
|
|
this->dataPtr->vm["iters"].as<unsigned int>() << "]\n";
|
|
}
|
|
}
|
|
|
|
if (this->dataPtr->vm.count("pause"))
|
|
this->dataPtr->params["pause"] = "true";
|
|
else
|
|
this->dataPtr->params["pause"] = "false";
|
|
|
|
if (!this->PreLoad())
|
|
{
|
|
gzerr << "Unable to load gazebo\n";
|
|
return false;
|
|
}
|
|
|
|
// The following "if" block must be processed directly before
|
|
// this->dataPtr->ProcessPrarams.
|
|
//
|
|
// Set the parameter to playback a log file. The log file contains the
|
|
// world description, so don't try to read the world file from the
|
|
// command line.
|
|
if (this->dataPtr->vm.count("play"))
|
|
{
|
|
// Load the log file
|
|
util::LogPlay::Instance()->Open(
|
|
this->dataPtr->vm["play"].as<std::string>());
|
|
|
|
gzmsg << "\nLog playback:\n"
|
|
<< " Log Version: "
|
|
<< util::LogPlay::Instance()->LogVersion() << "\n"
|
|
<< " Gazebo Version: "
|
|
<< util::LogPlay::Instance()->GazeboVersion() << "\n"
|
|
<< " Random Seed: "
|
|
<< util::LogPlay::Instance()->RandSeed() << "\n"
|
|
<< " Log Start Time: "
|
|
<< util::LogPlay::Instance()->LogStartTime() << "\n"
|
|
<< " Log End Time: "
|
|
<< util::LogPlay::Instance()->LogEndTime() << "\n";
|
|
|
|
// Get the SDF world description from the log file
|
|
std::string sdfString;
|
|
util::LogPlay::Instance()->Step(sdfString);
|
|
|
|
// Load the server
|
|
if (!this->LoadString(sdfString))
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
// Get the world file name from the command line, or use "empty.world"
|
|
// if no world file is specified.
|
|
std::string configFilename = "worlds/empty.world";
|
|
if (this->dataPtr->vm.count("world_file"))
|
|
configFilename = this->dataPtr->vm["world_file"].as<std::string>();
|
|
|
|
// Get the physics engine name specified from the command line, or use ""
|
|
// if no physics engine is specified.
|
|
std::string physics;
|
|
if (this->dataPtr->vm.count("physics"))
|
|
physics = this->dataPtr->vm["physics"].as<std::string>();
|
|
|
|
// Load the server
|
|
if (!this->LoadFile(configFilename, physics))
|
|
{
|
|
gzwarn << "Falling back on worlds/empty.world\n";
|
|
if (!this->LoadFile("worlds/empty.world", physics))
|
|
return false;
|
|
}
|
|
|
|
if (this->dataPtr->vm.count("profile"))
|
|
{
|
|
std::string profileName = this->dataPtr->vm["profile"].as<std::string>();
|
|
if (physics::get_world()->GetPresetManager()->HasProfile(profileName))
|
|
{
|
|
physics::get_world()->GetPresetManager()->CurrentProfile(profileName);
|
|
gzmsg << "Setting physics profile to [" << profileName << "]."
|
|
<< std::endl;
|
|
}
|
|
else
|
|
{
|
|
gzerr << "Specified profile [" << profileName << "] was not found."
|
|
<< std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
this->ProcessParams();
|
|
|
|
return true;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
bool Server::GetInitialized() const
|
|
{
|
|
return !this->dataPtr->stop && this->dataPtr->initialized;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
bool Server::LoadFile(const std::string &_filename,
|
|
const std::string &_physics)
|
|
{
|
|
// Quick test for a valid file
|
|
FILE *test = fopen(common::find_file(_filename).c_str(), "r");
|
|
if (!test)
|
|
{
|
|
gzerr << "Could not open file[" << _filename << "]\n";
|
|
return false;
|
|
}
|
|
fclose(test);
|
|
|
|
// Load the world file
|
|
sdf::SDFPtr sdf(new sdf::SDF);
|
|
if (!sdf::init(sdf))
|
|
{
|
|
gzerr << "Unable to initialize sdf\n";
|
|
return false;
|
|
}
|
|
|
|
if (!sdf::readFile(common::find_file(_filename), sdf))
|
|
{
|
|
gzerr << "Unable to read sdf file[" << _filename << "]\n";
|
|
return false;
|
|
}
|
|
|
|
return this->LoadImpl(sdf->Root(), _physics);
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
bool Server::LoadString(const std::string &_sdfString)
|
|
{
|
|
// Load the world file
|
|
sdf::SDFPtr sdf(new sdf::SDF);
|
|
if (!sdf::init(sdf))
|
|
{
|
|
gzerr << "Unable to initialize sdf\n";
|
|
return false;
|
|
}
|
|
|
|
if (!sdf::readString(_sdfString, sdf))
|
|
{
|
|
gzerr << "Unable to read SDF string[" << _sdfString << "]\n";
|
|
return false;
|
|
}
|
|
|
|
return this->LoadImpl(sdf->Root());
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
bool Server::PreLoad()
|
|
{
|
|
// setup gazebo
|
|
return gazebo::setupServer(this->dataPtr->systemPluginsArgc,
|
|
this->dataPtr->systemPluginsArgv);
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
bool Server::LoadImpl(sdf::ElementPtr _elem,
|
|
const std::string &_physics)
|
|
{
|
|
// If a physics engine is specified,
|
|
if (_physics.length())
|
|
{
|
|
// Check if physics engine name is valid
|
|
// This must be done after physics::load();
|
|
if (!physics::PhysicsFactory::IsRegistered(_physics))
|
|
{
|
|
gzerr << "Unregistered physics engine [" << _physics
|
|
<< "], the default will be used instead.\n";
|
|
}
|
|
// Try inserting physics engine name if one is given
|
|
else if (_elem->HasElement("world") &&
|
|
_elem->GetElement("world")->HasElement("physics"))
|
|
{
|
|
_elem->GetElement("world")->GetElement("physics")
|
|
->GetAttribute("type")->Set(_physics);
|
|
}
|
|
else
|
|
{
|
|
gzerr << "Cannot set physics engine: <world> does not have <physics>\n";
|
|
}
|
|
}
|
|
|
|
sdf::ElementPtr worldElem = _elem->GetElement("world");
|
|
if (worldElem)
|
|
{
|
|
physics::WorldPtr world = physics::create_world();
|
|
|
|
// Create the world
|
|
try
|
|
{
|
|
physics::load_world(world, worldElem);
|
|
}
|
|
catch(common::Exception &e)
|
|
{
|
|
gzthrow("Failed to load the World\n" << e);
|
|
}
|
|
}
|
|
|
|
//////////////////// AIRC begin ////////////////////
|
|
// Added by zenglei@2018-12-26
|
|
// Read the parallel parameter configuration in world file
|
|
// if(_elem->GetElement("world")->HasElement("parallel"))
|
|
// {
|
|
// sdf::ElementPtr eventElem = _elem->GetElement("world")->GetElement("parallel");
|
|
// event::Events::worldUpdateBegin.SetThreadInfo(eventElem->Get<int>("method"),eventElem->Get<int>("numbers_of_thread"), eventElem->Get<int>("size_of_block"), eventElem->Get<int>("type"));
|
|
|
|
// //if(6 == eventElem->Get<int>("method"))
|
|
// // event::Events::worldUpdateBegin.InitThreadPool(eventElem->Get<int>("numbers_of_thread"));
|
|
// // std::cout<<"=======numbers_of_thread: "<<eventElem->Get<int>("numbers_of_thread")<<"\t size_of_block: "<<eventElem->Get<int>("size_of_block")<<"============="<<std::endl;
|
|
// }
|
|
//////////////////// AIRC end ////////////////////
|
|
|
|
this->dataPtr->node = transport::NodePtr(new transport::Node());
|
|
this->dataPtr->node->Init("/gazebo");
|
|
this->dataPtr->serverSub =
|
|
this->dataPtr->node->Subscribe("/gazebo/server/control",
|
|
&Server::OnControl, this);
|
|
|
|
this->dataPtr->worldModPub =
|
|
this->dataPtr->node->Advertise<msgs::WorldModify>("/gazebo/world/modify");
|
|
|
|
common::Time waitTime(1, 0);
|
|
int waitCount = 0;
|
|
int maxWaitCount = 10;
|
|
|
|
// Wait for namespaces.
|
|
while (!gazebo::transport::waitForNamespaces(waitTime) &&
|
|
(waitCount++) < maxWaitCount)
|
|
{
|
|
gzwarn << "Waited " << waitTime.Double() << "seconds for namespaces.\n";
|
|
}
|
|
|
|
if (waitCount >= maxWaitCount)
|
|
{
|
|
gzerr << "Waited " << (waitTime * waitCount).Double()
|
|
<< " seconds for namespaces. Giving up.\n";
|
|
}
|
|
|
|
physics::init_worlds();
|
|
this->dataPtr->stop = false;
|
|
|
|
return true;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::SigInt(int)
|
|
{
|
|
ServerPrivate::stop = true;
|
|
|
|
// Signal to plugins/etc that a shutdown event has occured
|
|
event::Events::sigInt();
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::Stop()
|
|
{
|
|
this->dataPtr->stop = true;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::Fini()
|
|
{
|
|
this->Stop();
|
|
gazebo::shutdown();
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::Run()
|
|
{
|
|
#ifndef _WIN32
|
|
// Now that we're about to run, install a signal handler to allow for
|
|
// graceful shutdown on Ctrl-C.
|
|
struct sigaction sigact;
|
|
sigact.sa_flags = 0;
|
|
sigact.sa_handler = Server::SigInt;
|
|
if (sigemptyset(&sigact.sa_mask) != 0)
|
|
std::cerr << "sigemptyset failed while setting up for SIGINT" << std::endl;
|
|
if (sigaction(SIGINT, &sigact, NULL))
|
|
std::cerr << "sigaction(2) failed while setting up for SIGINT" << std::endl;
|
|
#endif
|
|
|
|
if (this->dataPtr->stop)
|
|
return;
|
|
|
|
// Make sure the sensors are updated once before running the world.
|
|
// This makes sure plugins get loaded properly.
|
|
sensors::run_once(true);
|
|
|
|
// Run the sensor threads
|
|
sensors::run_threads();
|
|
|
|
unsigned int iterations = 0;
|
|
common::StrStr_M::iterator piter = this->dataPtr->params.find("iterations");
|
|
if (piter != this->dataPtr->params.end())
|
|
{
|
|
try
|
|
{
|
|
iterations = boost::lexical_cast<unsigned int>(piter->second);
|
|
}
|
|
catch(...)
|
|
{
|
|
iterations = 0;
|
|
gzerr << "Unable to cast iterations[" << piter->second << "] "
|
|
<< "to unsigned integer\n";
|
|
}
|
|
}
|
|
|
|
// Run each world. Each world starts a new thread
|
|
physics::run_worlds(iterations);
|
|
|
|
this->dataPtr->initialized = true;
|
|
|
|
// Update the sensors.
|
|
while (!this->dataPtr->stop && physics::worlds_running())
|
|
{
|
|
this->ProcessControlMsgs();
|
|
sensors::run_once();
|
|
common::Time::MSleep(1);
|
|
}
|
|
|
|
// Shutdown gazebo
|
|
gazebo::shutdown();
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::ProcessParams()
|
|
{
|
|
common::StrStr_M::const_iterator iter;
|
|
for (iter = this->dataPtr->params.begin();
|
|
iter != this->dataPtr->params.end();
|
|
++iter)
|
|
{
|
|
if (iter->first == "pause")
|
|
{
|
|
bool p = false;
|
|
try
|
|
{
|
|
p = boost::lexical_cast<bool>(iter->second);
|
|
}
|
|
catch(...)
|
|
{
|
|
// Unable to convert via lexical_cast, so try "true/false" string
|
|
std::string str = iter->second;
|
|
boost::to_lower(str);
|
|
|
|
if (str == "true")
|
|
p = true;
|
|
else if (str == "false")
|
|
p = false;
|
|
else
|
|
gzerr << "Invalid param value[" << iter->first << ":"
|
|
<< iter->second << "]\n";
|
|
}
|
|
|
|
physics::pause_worlds(p);
|
|
}
|
|
else if (iter->first == "record")
|
|
{
|
|
util::LogRecordParams params;
|
|
|
|
params.encoding = this->dataPtr->params["record_encoding"];
|
|
params.path = iter->second;
|
|
params.period = this->dataPtr->vm["record_period"].as<double>();
|
|
params.filter = this->dataPtr->vm["record_filter"].as<std::string>();
|
|
// TODO Remove call to SetRecordResources function and update to use
|
|
// params.record_resources instead.
|
|
util::LogRecord::Instance()->SetRecordResources(
|
|
this->dataPtr->params.count("record_resources") > 0);
|
|
util::LogRecord::Instance()->Start(params);
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::SetParams(const common::StrStr_M &_params)
|
|
{
|
|
common::StrStr_M::const_iterator iter;
|
|
for (iter = _params.begin(); iter != _params.end(); ++iter)
|
|
this->dataPtr->params[iter->first] = iter->second;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::OnControl(ConstServerControlPtr &_msg)
|
|
{
|
|
std::unique_lock<std::mutex> lock(this->dataPtr->receiveMutex);
|
|
this->dataPtr->controlMsgs.push_back(*_msg);
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void Server::ProcessControlMsgs()
|
|
{
|
|
std::list<msgs::ServerControl>::iterator iter;
|
|
for (iter = this->dataPtr->controlMsgs.begin();
|
|
iter != this->dataPtr->controlMsgs.end(); ++iter)
|
|
{
|
|
if ((*iter).has_clone() && (*iter).clone())
|
|
{
|
|
bool success = true;
|
|
std::string host;
|
|
std::string port;
|
|
physics::WorldPtr world;
|
|
|
|
// Get the world's name to be cloned.
|
|
std::string worldName = "";
|
|
if ((*iter).has_save_world_name())
|
|
worldName = (*iter).save_world_name();
|
|
|
|
// Get the world pointer.
|
|
try
|
|
{
|
|
world = physics::get_world(worldName);
|
|
}
|
|
catch(const common::Exception &)
|
|
{
|
|
gzwarn << "Unable to clone a server. Unknown world ["
|
|
<< (*iter).save_world_name() << "]" << std::endl;
|
|
success = false;
|
|
}
|
|
|
|
// Check if the message contains a port for the new server.
|
|
if ((*iter).has_new_port())
|
|
port = boost::lexical_cast<std::string>((*iter).new_port());
|
|
else
|
|
{
|
|
gzwarn << "Unable to clone a server. Port is missing" << std::endl;
|
|
success = false;
|
|
}
|
|
|
|
if (success)
|
|
{
|
|
// world should not be NULL at this point.
|
|
GZ_ASSERT(world, "NULL world pointer");
|
|
|
|
// Save the world's state in a temporary file (clone.<PORT>.world).
|
|
boost::filesystem::path tmpDir =
|
|
boost::filesystem::temp_directory_path();
|
|
boost::filesystem::path worldFilename = "clone." + port + ".world";
|
|
boost::filesystem::path worldPath = tmpDir / worldFilename;
|
|
world->Save(worldPath.string());
|
|
|
|
// Get the hostname from the current server's master.
|
|
unsigned int unused;
|
|
transport::get_master_uri(host, unused);
|
|
|
|
// Command to be executed for cloning the server. The new server will
|
|
// load the world file /tmp/clone.<PORT>.world
|
|
std::string cmd = "GAZEBO_MASTER_URI=http://" + host + ":" + port +
|
|
" gzserver " + worldPath.string() + " &";
|
|
|
|
// Spawn a new gzserver process and load the saved world.
|
|
if (std::system(cmd.c_str()) == 0)
|
|
{
|
|
gzlog << "Cloning world [" << worldName << "]. "
|
|
<< "Connect to the server by typing:\n\tGAZEBO_MASTER_URI="
|
|
<< "http://" << host << ":" << port << " gzclient" << std::endl;
|
|
}
|
|
else
|
|
{
|
|
gzerr << "Unable to clone a simulation running the following command:"
|
|
<< std::endl << "\t[" << cmd << "]" << std::endl;
|
|
success = false;
|
|
}
|
|
}
|
|
|
|
// Notify the result.
|
|
msgs::WorldModify worldMsg;
|
|
worldMsg.set_world_name(worldName);
|
|
worldMsg.set_cloned(success);
|
|
if (success)
|
|
worldMsg.set_cloned_uri("http://" + host + ":" + port);
|
|
this->dataPtr->worldModPub->Publish(worldMsg);
|
|
}
|
|
else if ((*iter).has_save_world_name())
|
|
{
|
|
physics::WorldPtr world = physics::get_world((*iter).save_world_name());
|
|
if ((*iter).has_save_filename())
|
|
world->Save((*iter).save_filename());
|
|
else
|
|
gzerr << "No filename specified.\n";
|
|
}
|
|
else if ((*iter).has_new_world() && (*iter).new_world())
|
|
{
|
|
this->OpenWorld("worlds/empty.world");
|
|
}
|
|
else if ((*iter).has_open_filename())
|
|
{
|
|
this->OpenWorld((*iter).open_filename());
|
|
}
|
|
else if ((*iter).has_stop() && (*iter).stop())
|
|
{
|
|
this->Stop();
|
|
}
|
|
}
|
|
this->dataPtr->controlMsgs.clear();
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
bool Server::OpenWorld(const std::string & /*_filename*/)
|
|
{
|
|
gzerr << "Open World is not implemented\n";
|
|
return false;
|
|
/*
|
|
sdf::SDFPtr sdf(new sdf::SDF);
|
|
if (!sdf::init(sdf))
|
|
{
|
|
gzerr << "Unable to initialize sdf\n";
|
|
return false;
|
|
}
|
|
|
|
if (!sdf::readFile(_filename, sdf))
|
|
{
|
|
gzerr << "Unable to read sdf file[" << _filename << "]\n";
|
|
return false;
|
|
}
|
|
|
|
msgs::WorldModify worldMsg;
|
|
worldMsg.set_world_name("default");
|
|
worldMsg.set_remove(true);
|
|
this->worldModPub->Publish(worldMsg);
|
|
|
|
physics::stop_worlds();
|
|
|
|
physics::remove_worlds();
|
|
|
|
sensors::remove_sensors();
|
|
|
|
gazebo::transport::clear_buffers();
|
|
|
|
sdf::ElementPtr worldElem = sdf->Root()->GetElement("world");
|
|
|
|
physics::WorldPtr world = physics::create_world();
|
|
|
|
physics::load_world(world, worldElem);
|
|
|
|
physics::init_world(world);
|
|
|
|
physics::run_world(world);
|
|
|
|
worldMsg.set_world_name("default");
|
|
worldMsg.set_remove(false);
|
|
worldMsg.set_create(true);
|
|
this->worldModPub->Publish(worldMsg);
|
|
return true;
|
|
*/
|
|
}
|