/* * Copyright 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. **/#include <stdio.h>#include <signal.h>#include <boost/lexical_cast.hpp>#include <boost/algorithm/string.hpp>#include "gazebo/gazebo.hh"#include "gazebo/transport/transport.hh"#include "gazebo/common/LogRecord.hh"#include "gazebo/common/LogPlay.hh"#include "gazebo/common/Timer.hh"#include "gazebo/common/Exception.hh"#include "gazebo/common/Plugin.hh"#include "gazebo/common/Common.hh"#include "gazebo/sdf/sdf.hh"#include "gazebo/sensors/Sensors.hh"#include "gazebo/physics/Physics.hh"#include "gazebo/physics/World.hh"#include "gazebo/physics/Base.hh"#include "gazebo/Master.hh"#include "gazebo/Server.hh"usingnamespacegazebo;boolServer::stop=true;/////////////////////////////////////////////////Server::Server(){this->receiveMutex=newboost::mutex();gazebo::print_version();if(signal(SIGINT,Server::SigInt)==SIG_ERR)std::cerr<<"signal(2) failed while setting up for SIGINT"<<std::endl;}/////////////////////////////////////////////////Server::~Server(){fflush(stdout);deletethis->receiveMutex;deletethis->master;}/////////////////////////////////////////////////voidServer::PrintUsage(){std::cerr<<"Run the Gazebo server.\n\n"<<"Usage: gzserver [options] <world_file>\n\n";}/////////////////////////////////////////////////boolServer::ParseArgs(intargc,char**argv){// save a copy of argc and argv for consumption by system pluginsthis->systemPluginsArgc=argc;this->systemPluginsArgv=newchar*[argc];for(inti=0;i<argc;++i){intargv_len=strlen(argv[i]);this->systemPluginsArgv[i]=newchar[argv_len];for(intj=0;j<argv_len;++j)this->systemPluginsArgv[i][j]=argv[i][j];}po::options_descriptionv_desc("Allowed options");v_desc.add_options()("help,h","Produce this help message.")("pause,u","Start the server in a paused state.")("play,p",po::value<std::string>(),"Play a log file.")("record,r","Record state data to disk.")("seed",po::value<double>(),"Start with a given random number seed.")("server-plugin,s",po::value<std::vector<std::string>>(),"Load a plugin.");po::options_descriptionh_desc("Hidden options");h_desc.add_options()("world_file",po::value<std::string>(),"SDF world to load.");h_desc.add_options()("pass_through",po::value<std::vector<std::string>>(),"not used, passed through to system plugins.");po::options_descriptiondesc("Allowed options");desc.add(v_desc).add(h_desc);po::positional_options_descriptionp_desc;p_desc.add("world_file",1).add("pass_through",-1);try{po::store(po::command_line_parser(argc,argv).options(desc).positional(p_desc).allow_unregistered().run(),this->vm);po::notify(this->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";returnfalse;}// Set the random number seed if present on the command line.if(this->vm.count("seed")){try{math::Rand::SetSeed(this->vm["seed"].as<double>());}catch(boost::bad_any_cast&_e){gzerr<<"Unable to set random number seed. Must supply a number.\n";}}if(this->vm.count("help")){this->PrintUsage();std::cerr<<v_desc<<"\n";returnfalse;}/// Load all the plugins specified on the command lineif(this->vm.count("server-plugin")){std::vector<std::string>pp=this->vm["server-plugin"].as<std::vector<std::string>>();for(std::vector<std::string>::iteratoriter=pp.begin();iter!=pp.end();++iter){gazebo::add_plugin(*iter);}}// Set the parameter to record a log fileif(this->vm.count("record"))this->params["record"]="bz2";if(this->vm.count("pause"))this->params["pause"]="true";elsethis->params["pause"]="false";// The following "if" block must be processed directly before// this->ProcessPrarams.//// Set the parameter to playback a log file. The log file contains the// world description, so don't try to reead the world file from the// command line.if(this->vm.count("play")){// Load the log filecommon::LogPlay::Instance()->Open(this->vm["play"].as<std::string>());gzmsg<<"\nLog playback:\n"<<" Log Version: "<<common::LogPlay::Instance()->GetLogVersion()<<"\n"<<" Gazebo Version: "<<common::LogPlay::Instance()->GetGazeboVersion()<<"\n"<<" Random Seed: "<<common::LogPlay::Instance()->GetRandSeed()<<"\n";// Get the SDF world description from the log filestd::stringsdfString;common::LogPlay::Instance()->Step(sdfString);// Load the serverif(!this->LoadString(sdfString))returnfalse;}else{// Get the world file name from the command line, or use "empty.world"// if no world file is specified.std::stringconfigFilename="worlds/empty.world";if(this->vm.count("world_file"))configFilename=this->vm["world_file"].as<std::string>();// Load the serverif(!this->LoadFile(configFilename))returnfalse;}this->ProcessParams();this->Init();returntrue;}/////////////////////////////////////////////////boolServer::GetInitialized()const{return!this->stop&&!transport::is_stopped();}/////////////////////////////////////////////////boolServer::LoadFile(conststd::string&_filename){// Quick test for a valid fileFILE*test=fopen(common::find_file(_filename).c_str(),"r");if(!test){gzerr<<"Could not open file["<<_filename<<"]\n";returnfalse;}fclose(test);// Load the world filesdf::SDFPtrsdf(newsdf::SDF);if(!sdf::init(sdf)){gzerr<<"Unable to initialize sdf\n";returnfalse;}if(!sdf::readFile(_filename,sdf)){gzerr<<"Unable to read sdf file["<<_filename<<"]\n";returnfalse;}returnthis->LoadImpl(sdf->root);}/////////////////////////////////////////////////boolServer::LoadString(conststd::string&_sdfString){// Load the world filesdf::SDFPtrsdf(newsdf::SDF);if(!sdf::init(sdf)){gzerr<<"Unable to initialize sdf\n";returnfalse;}if(!sdf::readString(_sdfString,sdf)){gzerr<<"Unable to read SDF string["<<_sdfString<<"]\n";returnfalse;}returnthis->LoadImpl(sdf->root);}/////////////////////////////////////////////////boolServer::LoadImpl(sdf::ElementPtr_elem){std::stringhost="";unsignedintport=0;gazebo::transport::get_master_uri(host,port);this->master=newgazebo::Master();this->master->Init(port);this->master->RunThread();// Load gazebogazebo::load(this->systemPluginsArgc,this->systemPluginsArgv);/// Load the sensors librarysensors::load();/// Load the physics libraryphysics::load();sdf::ElementPtrworldElem=_elem->GetElement("world");if(worldElem){physics::WorldPtrworld=physics::create_world();// Create the worldtry{physics::load_world(world,worldElem);}catch(common::Exception&e){gzthrow("Failed to load the World\n"<<e);}}this->node=transport::NodePtr(newtransport::Node());this->node->Init("/gazebo");this->serverSub=this->node->Subscribe("/gazebo/server/control",&Server::OnControl,this);this->worldModPub=this->node->Advertise<msgs::WorldModify>("/gazebo/world/modify");// Run the gazebo, starts a new threadgazebo::run();returntrue;}/////////////////////////////////////////////////voidServer::Init(){gazebo::init();sensors::init();physics::init_worlds();this->stop=false;}/////////////////////////////////////////////////voidServer::SigInt(int){stop=true;}/////////////////////////////////////////////////voidServer::Stop(){this->stop=true;}/////////////////////////////////////////////////voidServer::Fini(){this->Stop();gazebo::fini();physics::fini();sensors::fini();if(this->master)this->master->Fini();deletethis->master;this->master=NULL;}/////////////////////////////////////////////////voidServer::Run(){if(this->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 each world. Each world starts a new threadphysics::run_worlds();// Update the sensors.while(!this->stop){this->ProcessControlMsgs();sensors::run_once();common::Time::MSleep(1);}// Stop all the worldsphysics::stop_worlds();sensors::stop();// Stop gazebogazebo::stop();// Stop the masterthis->master->Stop();}/////////////////////////////////////////////////voidServer::ProcessParams(){common::StrStr_M::const_iteratoriter;for(iter=this->params.begin();iter!=this->params.end();++iter){if(iter->first=="pause"){boolp=false;try{p=boost::lexical_cast<bool>(iter->second);}catch(...){// Unable to convert via lexical_cast, so try "true/false" stringstd::stringstr=iter->second;boost::to_lower(str);if(str=="true")p=true;elseif(str=="false")p=false;elsegzerr<<"Invalid param value["<<iter->first<<":"<<iter->second<<"]\n";}physics::pause_worlds(p);}elseif(iter->first=="record"){common::LogRecord::Instance()->Start(iter->second);}}}/////////////////////////////////////////////////voidServer::SetParams(constcommon::StrStr_M&_params){common::StrStr_M::const_iteratoriter;for(iter=_params.begin();iter!=_params.end();++iter)this->params[iter->first]=iter->second;}/////////////////////////////////////////////////voidServer::OnControl(ConstServerControlPtr&_msg){boost::mutex::scoped_locklock(*this->receiveMutex);this->controlMsgs.push_back(*_msg);}/////////////////////////////////////////////////voidServer::ProcessControlMsgs(){std::list<msgs::ServerControl>::iteratoriter;for(iter=this->controlMsgs.begin();iter!=this->controlMsgs.end();++iter){if((*iter).has_save_world_name()){physics::WorldPtrworld=physics::get_world((*iter).save_world_name());if((*iter).has_save_filename())world->Save((*iter).save_filename());elsegzerr<<"No filename specified.\n";}elseif((*iter).has_new_world()&&(*iter).new_world()){this->OpenWorld("worlds/empty.world");}elseif((*iter).has_open_filename()){this->OpenWorld((*iter).open_filename());}}this->controlMsgs.clear();}/////////////////////////////////////////////////boolServer::OpenWorld(conststd::string&_filename){sdf::SDFPtrsdf(newsdf::SDF);if(!sdf::init(sdf)){gzerr<<"Unable to initialize sdf\n";returnfalse;}if(!sdf::readFile(_filename,sdf)){gzerr<<"Unable to read sdf file["<<_filename<<"]\n";returnfalse;}msgs::WorldModifyworldMsg;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::ElementPtrworldElem=sdf->root->GetElement("world");physics::WorldPtrworld=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);returntrue;}