38 #include <gazebo/gazebo.hh> 39 #include <gazebo/common/common.hh> 40 #include <gazebo/physics/physics.hh> 41 #include <gazebo/sensors/sensors.hh> 42 #include <gazebo/sensors/SensorsIface.hh> 43 #include <gazebo/sensors/SensorManager.hh> 44 #include <sdf/parser_urdf.hh> 45 #include <sdf/parser.hh> 51 #if GAZEBO_MAJOR_VERSION < 6 56 return strdup(_s.c_str());
61 inline bool setupServer(
const std::vector<std::string> &_args)
63 std::vector<char *> pointers(_args.size());
64 std::transform(_args.begin(), _args.end(), pointers.begin(),
66 pointers.push_back(0);
70 for (
size_t i = 0; i < pointers.size(); ++i)
88 std::cout <<
"\x1B[32m[[--- Gazebo Server ---]]\033[0m"<<
'\n';
93 std::vector<std::string> v;
94 for (
int i = 0; i < argc; i++)
100 std::vector<std::string> v;
101 for (
int i = 0; i < argc; i++)
102 v.push_back(argv[i]);
105 GazeboServer(std::vector<std::string> server_options,
const std::string& world_name =
"worlds/empty.world")
107 load(server_options,world_name);
109 GazeboServer(
const std::string& world_name,std::vector<std::string> server_options = {
"--verbose"})
111 load(server_options,world_name);
114 bool load(std::vector<std::string> server_options = {
"--verbose"},
const std::string& world_name =
"worlds/empty.world")
116 std::vector<std::string> plugins;
117 for (
size_t i = 0; i < server_options.size() - 1; i++)
119 std::string op(server_options[i]);
120 std::string next_op(server_options[i+1]);
122 if (op.find(
"verbose") != std::string::npos)
124 ::gazebo::printVersion();
125 ::gazebo::common::Console::SetQuiet(
false);
127 if (op.find(
"-s") != std::string::npos || op.find(
"--server-plugin") != std::string::npos)
129 plugins.push_back(next_op);
134 std::cout <<
"\x1B[32m[[--- Gazebo setup ---]]\033[0m"<<
'\n';
137 std::cout <<
"\x1B[32m Loading plugins\033[0m"<<
'\n';
138 for(
auto plugin : plugins)
140 ::gazebo::addPlugin(plugin);
141 std::cout <<
"\x1B[32m - " << plugin <<
"\033[0m"<<
'\n';
147 std::cerr <<
"[GazeboServer] Could not setup server" <<
'\n';
150 return bool(this->loadWorld(world_name));
153 ::gazebo::physics::WorldPtr
loadWorld(
const std::string& world_name)
155 auto world = ::gazebo::loadWorld(world_name);
158 std::cerr <<
"[GazeboServer] Could not load world with world file " << world_name <<
'\n';
162 world_begin_ = ::gazebo::event::Events::ConnectWorldUpdateBegin(std::bind(&GazeboServer::worldUpdateBegin,
this));
163 world_end_ = ::gazebo::event::Events::ConnectWorldUpdateEnd(std::bind(&GazeboServer::worldUpdateEnd,
this));
164 #if GAZEBO_MAJOR_VERSION > 8 165 ::gazebo::sensors::SensorManager::Instance()->Init();
166 ::gazebo::sensors::SensorManager::Instance()->RunThreads();
174 #if GAZEBO_MAJOR_VERSION > 8 175 world_->SetPhysicsEnabled(enable);
177 world_->EnablePhysicsEngine(enable);
184 return std::ifstream(file_path).good();
190 std::ifstream ifs(file_path);
191 out.assign((std::istreambuf_iterator<char>(ifs)),
192 (std::istreambuf_iterator<char>()));
197 const std::string& modelName)
322 #if GAZEBO_MAJOR_VERSION > 8 323 return world_->Physics()->GetMaxStepSize();
325 return world_->GetPhysicsEngine()->GetMaxStepSize();
332 ::gazebo::runWorld(world_, 1);
338 ::gazebo::runWorld(world_, 0);
343 ::gazebo::event::Events::sigInt.Signal();
344 std::cout <<
"\x1B[32m[[--- Stoping Simulation ---]]\033[0m"<<
'\n';
347 std::cout <<
"\x1B[32m[[--- Gazebo Shutdown... ---]]\033[0m"<<
'\n';
349 ::gazebo::shutdown();
353 , Eigen::Vector3d init_pos = Eigen::Vector3d::Zero()
354 , Eigen::Quaterniond init_rot = Eigen::Quaterniond::Identity()
355 , std::string model_name=
"")
357 std::string cmd =
"gz sdf -p " + urdf_url;
358 return insertModelFromSDFString(
custom_exec(cmd) );
362 , Eigen::Vector3d init_pos = Eigen::Vector3d::Zero()
363 , Eigen::Quaterniond init_rot = Eigen::Quaterniond::Identity()
364 , std::string model_name=
"")
367 doc.Parse(urdf_str.c_str());
368 return insertModelFromTinyXML(&doc,init_pos,init_rot,model_name);
374 #if GAZEBO_MAJOR_VERSION > 8 375 return world_->ModelCount();
377 return world_->GetModelCount();
384 std::vector<std::string> names;
386 #if GAZEBO_MAJOR_VERSION > 8 387 for(
auto model : world_->Models())
388 names.push_back(model->GetName());
390 for(
auto model : world_->GetModels())
391 names.push_back(model->GetName());
400 #if GAZEBO_MAJOR_VERSION > 8 401 auto model = world_->ModelByName( model_name );
403 auto model = world_->GetModel( model_name );
405 return static_cast<bool>(model);
411 #if GAZEBO_MAJOR_VERSION > 8 412 auto g = world_->Gravity();
414 auto g = world_->GetPhysicsEngine()->GetGravity();
416 gravity_vector_[0] = g[0];
417 gravity_vector_[1] = g[1];
418 gravity_vector_[2] = g[2];
419 return gravity_vector_;
424 std::vector<std::string> joint_names;
425 auto model = getModelByName(model_name);
428 for(
auto j : model->GetJoints())
429 joint_names.push_back(j->GetName());
433 ::gazebo::physics::Joint_V
getJointsFromNames(
const std::string& model_name,
const std::vector<std::string>& joint_names)
435 ::gazebo::physics::Joint_V jv;
436 auto model = getModelByName(model_name);
439 for(
auto n : joint_names)
441 auto joint = model->GetJoint(n);
451 #if GAZEBO_MAJOR_VERSION > 8 452 return world_->ModelByName(model_name);
454 return world_->GetModel(model_name);
464 callback_ = callback;
473 void assertWorldLoaded()
476 throw std::runtime_error(
"[GazeboServer] World is not loaded");
478 ::gazebo::physics::ModelPtr insertModelFromTinyXML(TiXmlDocument* doc
479 , Eigen::Vector3d init_pos = Eigen::Vector3d::Zero()
480 , Eigen::Quaterniond init_rot = Eigen::Quaterniond::Identity()
481 , std::string model_name=
"")
485 std::cerr <<
"[GazeboServer] Document provided is null" <<
'\n';
488 TiXmlElement* robotElement = doc->FirstChildElement(
"robot");
492 TiXmlPrinter printer;
493 printer.SetIndent(
" " );
494 doc->Accept( &printer );
495 std::cerr << printer.CStr() <<
'\n';
496 std::cerr <<
"[GazeboServer] Could not get the <robot> tag in the URDF " <<
'\n';
501 if(model_name.empty())
510 if(!setRobotNameToTinyXML(robotElement,model_name))
514 std::cout <<
"[GazeboServer] Trying to insert model \'" << model_name <<
"\'" << std::endl;
516 sdf::URDF2SDF urdf_to_sdf;
517 TiXmlDocument sdf_xml = urdf_to_sdf.InitModelDoc(doc);
528 TiXmlPrinter printer;
529 printer.SetIndent(
" " );
530 sdf_xml.Accept( &printer );
532 return insertModelFromSDFString(printer.CStr(),init_pos,init_rot,model_name);
535 ::gazebo::physics::ModelPtr insertModelFromSDFString(std::string sdf_string
536 , Eigen::Vector3d init_pos = Eigen::Vector3d::Zero()
537 , Eigen::Quaterniond init_rot = Eigen::Quaterniond::Identity()
538 , std::string model_name=
"")
541 _sdf.SetFromString(sdf_string);
546 _sdf.Root()->GetElement(
"model")->GetElement(
"pose")->Set<ignition::math::Pose3d>(init_pose);
548 if(model_name.empty())
550 model_name = _sdf.Root()->GetElement(
"model")->GetAttribute(
"name")->GetAsString();
553 std::cout <<
"[GazeboServer] Converted to sdf :\n" << _sdf.ToString() <<
'\n';
555 world_->InsertModelSDF(_sdf);
557 std::atomic<bool> do_exit(
false);
558 auto th = std::thread([&]()
565 std::this_thread::sleep_for(std::chrono::seconds(1));
568 std::cerr <<
"[GazeboServer] \x1B[33m\n\nYou are seeing this message because gazebo does not know where to find the meshes for the \'" << model_name <<
"\' model\n\e[0m" << std::endl
569 <<
"\x1B[33mTo make it work, you need to append the path to the dir containing the meshes to the GAZEBO_MODEL_PATH env variable\n, one level above the package.xml. Example :\n\e[0m" << std::endl
570 <<
"\x1B[33mIf the meshes for the model \'" << model_name <<
"\' are located at /path/to/" << model_name <<
"_description\n\e[0m" << std::endl
571 <<
"\x1B[33mThen append \'/path/to/\' to GAZEBO_MODEL_PATH with export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/path/to/\n\e[0m" << std::endl
572 <<
"\x1B[33mIf you are using ROS (and rospack find " << model_name <<
"_description works), just add this to your ~/.bashrc :\n\nexport GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$ROS_PACKAGE_PATH\n\e[0m" << std::endl
573 <<
"\x1B[33mIf you are NOT using ROS, add also :\n\nsource /usr/share/gazebo/setup.sh\n\e[0m" << std::endl;
577 for (
size_t i = 0; i < max_trials; i++)
579 std::cout <<
"[GazeboServer] Runing the world (" << i+1 <<
"/" << max_trials <<
") ... " << std::endl;
581 ::gazebo::runWorld(world_,1);
583 std::cout <<
"[GazeboServer] Now verifying if the model is correctly loaded..." << std::endl;
585 ::gazebo::physics::ModelPtr model = getModelByName(model_name);
591 std::cout <<
"[GazeboServer] Model '" << model_name <<
"' successfully loaded" << std::endl;
592 countExistingSensors();
595 std::cout <<
"[GazeboServer] Model '" << model_name <<
"' yet loaded" << std::endl;
596 std::this_thread::sleep_for(std::chrono::milliseconds(500));
608 if (robotElement->Attribute(
"name"))
610 model_name = robotElement->Attribute(
"name");
614 std::cerr <<
"[GazeboServer] Could not get the robot tag in the URDF " <<
'\n';
620 std::cerr <<
"[GazeboServer] Provided robot element is invalid" <<
'\n';
624 if (model_name.empty())
626 std::cerr <<
"[GazeboServer] Robot name is empty" <<
'\n';
632 bool setRobotNameToTinyXML(TiXmlElement* robotElement, std::string& model_name)
638 robotElement->RemoveAttribute(
"name");
639 robotElement->SetAttribute(
"name", model_name);
643 void worldUpdateBegin()
645 #if GAZEBO_MAJOR_VERSION > 8 646 ::gazebo::sensors::SensorManager::Instance()->Update();
648 int tmp_sensor_count = 0;
649 for(
auto model : world_->GetModels())
650 tmp_sensor_count += model->GetSensorCount();
654 if(tmp_sensor_count > n_sensors_)
656 std::cerr <<
"[GazeboServer] Loading " << tmp_sensor_count - n_sensors_ <<
" sensors\n";
657 if (!::gazebo::sensors::load())
659 std::cerr <<
"[GazeboServer] Unable to load sensors\n";
662 if (!::gazebo::sensors::init())
664 std::cerr <<
"[GazeboServer] Unable to initialize sensors\n";
667 ::gazebo::sensors::run_once(
true);
668 ::gazebo::sensors::run_threads();
669 n_sensors_ = tmp_sensor_count;
672 n_sensors_ = tmp_sensor_count;
676 ::gazebo::sensors::run_once();
680 void worldUpdateEnd()
683 callback_(getIterations(),getSimTime(),getDt());
686 double getIterations()
688 #if GAZEBO_MAJOR_VERSION > 8 689 return world_->Iterations();
691 return world_->GetIterations();
698 #if GAZEBO_MAJOR_VERSION > 8 699 return world_->SimTime().Double();
701 return world_->GetSimTime().Double();
705 void countExistingSensors()
708 #if GAZEBO_MAJOR_VERSION > 8 709 for(
auto model : world_->Models())
710 n_sensors_ += model->GetSensorCount();
712 for(
auto model : world_->GetModels())
713 n_sensors_ += model->GetSensorCount();
717 std::function<void(uint32_t,double,double)> callback_;
718 ::gazebo::physics::WorldPtr world_;
719 ::gazebo::event::ConnectionPtr world_begin_;
720 ::gazebo::event::ConnectionPtr world_end_;
721 Eigen::Vector3d gravity_vector_;
::gazebo::physics::Joint_V getJointsFromNames(const std::string &model_name, const std::vector< std::string > &joint_names)
Definition: GazeboServer.h:433
bool fileExists(const std::string &file_path)
Definition: GazeboServer.h:182
void runOnce()
Definition: GazeboServer.h:329
GazeboServer()
Definition: GazeboServer.h:86
Definition: GazeboServer.h:59
::gazebo::physics::ModelPtr insertModelFromURDFString(const std::string &urdf_str, Eigen::Vector3d init_pos=Eigen::Vector3d::Zero(), Eigen::Quaterniond init_rot=Eigen::Quaterniond::Identity(), std::string model_name="")
Definition: GazeboServer.h:361
bool enablePhysicsEngine(bool enable)
Definition: GazeboServer.h:171
bool setupServer(const std::vector< std::string > &_args)
Definition: GazeboServer.h:61
GazeboServer(std::vector< std::string > server_options, const std::string &world_name="worlds/empty.world")
Definition: GazeboServer.h:105
void shutdown()
Definition: GazeboServer.h:341
static bool getRobotNameFromTinyXML(TiXmlDocument *doc, std::string &model_name)
Definition: RobotModel.cc:34
void executeAfterWorldUpdate(std::function< void(uint32_t, double, double)> callback)
Definition: GazeboServer.h:462
std::vector< std::string > getModelJointNames(const std::string &model_name)
Definition: GazeboServer.h:422
void run()
Definition: GazeboServer.h:335
std::string custom_exec(const std::string &_cmd)
Definition: Utils.h:115
GazeboServer(int argc, char const *argv[])
Definition: GazeboServer.h:98
double getDt()
Definition: GazeboServer.h:319
GazeboServer(const std::string &world_name, std::vector< std::string > server_options={"--verbose"})
Definition: GazeboServer.h:109
bool spawnModel(const std::string &instanceName, const std::string &modelName)
Definition: GazeboServer.h:196
virtual ~GazeboServer()
Definition: GazeboServer.h:467
int getModelCount()
Definition: GazeboServer.h:371
bool load(std::vector< std::string > server_options={"--verbose"}, const std::string &world_name="worlds/empty.world")
Definition: GazeboServer.h:114
Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
Definition: Utils.h:69
const Eigen::Vector3d & getGravity()
Definition: GazeboServer.h:408
char * operator()(const std::string &_s)
Definition: GazeboServer.h:54
::gazebo::physics::WorldPtr loadWorld(const std::string &world_name)
Definition: GazeboServer.h:153
std::string fileToString(const std::string &file_path)
Definition: GazeboServer.h:187
::gazebo::physics::WorldPtr getWorld()
Definition: GazeboServer.h:457
std::vector< std::string > getModelNames()
Definition: GazeboServer.h:381
GazeboServer(int argc, char *argv[])
Definition: GazeboServer.h:91
Definition: CartesianAccelerationPID.h:44
::gazebo::physics::ModelPtr insertModelFromURDFFile(const std::string &urdf_url, Eigen::Vector3d init_pos=Eigen::Vector3d::Zero(), Eigen::Quaterniond init_rot=Eigen::Quaterniond::Identity(), std::string model_name="")
Definition: GazeboServer.h:352
::gazebo::physics::ModelPtr getModelByName(const std::string &model_name)
Definition: GazeboServer.h:448
Eigen::Quaterniond convQuat(const ignition::math::Quaterniond &_quat)
Definition: Utils.h:84
Definition: GazeboServer.h:52
bool modelExists(const std::string &model_name)
Definition: GazeboServer.h:397
Definition: GazeboServer.h:83