ORCA: Optimization-based framework for Robotic Control Applications
GazeboServer.h
Go to the documentation of this file.
1 //| This file is a part of the ORCA framework.
2 //|
3 //| Copyright 2018, Fuzzy Logic Robotics
4 //| Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
5 //|
6 //| Main contributor(s): Antoine Hoarau, Ryan Lober, and
7 //| Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
8 //|
9 //| ORCA is a whole-body reactive controller framework for robotics.
10 //|
11 //| This software is governed by the CeCILL-C license under French law and
12 //| abiding by the rules of distribution of free software. You can use,
13 //| modify and/ or redistribute the software under the terms of the CeCILL-C
14 //| license as circulated by CEA, CNRS and INRIA at the following URL
15 //| "http://www.cecill.info".
16 //|
17 //| As a counterpart to the access to the source code and rights to copy,
18 //| modify and redistribute granted by the license, users are provided only
19 //| with a limited warranty and the software's author, the holder of the
20 //| economic rights, and the successive licensors have only limited
21 //| liability.
22 //|
23 //| In this respect, the user's attention is drawn to the risks associated
24 //| with loading, using, modifying and/or developing or reproducing the
25 //| software by the user in light of its specific status of free software,
26 //| that may mean that it is complicated to manipulate, and that also
27 //| therefore means that it is reserved for developers and experienced
28 //| professionals having in-depth computer knowledge. Users are therefore
29 //| encouraged to load and test the software's suitability as regards their
30 //| requirements in conditions enabling the security of their systems and/or
31 //| data to be ensured and, more generally, to use and operate it in the
32 //| same conditions as regards security.
33 //|
34 //| The fact that you are presently reading this means that you have had
35 //| knowledge of the CeCILL-C license and that you accept its terms.
36 
37 #pragma once
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>
46 #include <sdf/sdf.hh>
47 #include <fstream>
48 #include <thread>
49 #include "orca/gazebo/Utils.h"
50 
51 #if GAZEBO_MAJOR_VERSION < 6
53 {
54  char *operator()(const std::string &_s)
55  {
56  return strdup(_s.c_str());
57  }
58 };
59 namespace gazebo
60 {
61  inline bool setupServer(const std::vector<std::string> &_args)
62  {
63  std::vector<char *> pointers(_args.size());
64  std::transform(_args.begin(), _args.end(), pointers.begin(),
66  pointers.push_back(0);
67  bool result = ::gazebo::setupServer(_args.size(), &pointers[0]);
68 
69  // Deallocate memory for the command line arguments alloocated with strdup.
70  for (size_t i = 0; i < pointers.size(); ++i)
71  free(pointers.at(i));
72 
73  return result;
74  }
75 }
76 #endif
77 
78 namespace orca
79 {
80 namespace gazebo
81 {
82 
84 {
85 public:
87  {
88  std::cout <<"\x1B[32m[[--- Gazebo Server ---]]\033[0m"<< '\n';
89  load();
90  }
91  GazeboServer(int argc, char * argv[])
92  {
93  std::vector<std::string> v;
94  for (int i = 0; i < argc; i++)
95  v.push_back(argv[i]);
96  load(v);
97  }
98  GazeboServer(int argc, char const * argv[])
99  {
100  std::vector<std::string> v;
101  for (int i = 0; i < argc; i++)
102  v.push_back(argv[i]);
103  load(v);
104  }
105  GazeboServer(std::vector<std::string> server_options,const std::string& world_name = "worlds/empty.world")
106  {
107  load(server_options,world_name);
108  }
109  GazeboServer(const std::string& world_name,std::vector<std::string> server_options = {"--verbose"})
110  {
111  load(server_options,world_name);
112  }
113 
114  bool load(std::vector<std::string> server_options = {"--verbose"},const std::string& world_name = "worlds/empty.world")
115  {
116  std::vector<std::string> plugins;
117  for (size_t i = 0; i < server_options.size() - 1; i++)
118  {
119  std::string op(server_options[i]);
120  std::string next_op(server_options[i+1]);
121 
122  if (op.find("verbose") != std::string::npos)
123  {
124  ::gazebo::printVersion();
125  ::gazebo::common::Console::SetQuiet(false);
126  }
127  if (op.find("-s") != std::string::npos || op.find("--server-plugin") != std::string::npos)
128  {
129  plugins.push_back(next_op);
130  i++;
131  }
132  }
133 
134  std::cout <<"\x1B[32m[[--- Gazebo setup ---]]\033[0m"<< '\n';
135  if(plugins.size())
136  {
137  std::cout <<"\x1B[32m Loading plugins\033[0m"<< '\n';
138  for(auto plugin : plugins)
139  {
140  ::gazebo::addPlugin(plugin);
141  std::cout <<"\x1B[32m - " << plugin << "\033[0m"<< '\n';
142  }
143  }
144 
145  if(!::gazebo::setupServer(server_options))
146  {
147  std::cerr << "[GazeboServer] Could not setup server" << '\n';
148  return false;
149  }
150  return bool(this->loadWorld(world_name));
151  }
152 
153  ::gazebo::physics::WorldPtr loadWorld(const std::string& world_name)
154  {
155  auto world = ::gazebo::loadWorld(world_name);
156  if(!world)
157  {
158  std::cerr << "[GazeboServer] Could not load world with world file " << world_name << '\n';
159  return nullptr;
160  }
161  world_ = world;
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();
167  #endif
168  return world_;
169  }
170 
171  bool enablePhysicsEngine(bool enable)
172  {
173  assertWorldLoaded();
174  #if GAZEBO_MAJOR_VERSION > 8
175  world_->SetPhysicsEnabled(enable);
176  #else
177  world_->EnablePhysicsEngine(enable);
178  #endif
179  return true;
180  }
181 
182  bool fileExists(const std::string& file_path)
183  {
184  return std::ifstream(file_path).good();
185  }
186 
187  std::string fileToString(const std::string& file_path)
188  {
189  std::string out;
190  std::ifstream ifs(file_path);
191  out.assign((std::istreambuf_iterator<char>(ifs)),
192  (std::istreambuf_iterator<char>()));
193  return out;
194  }
195 
196  bool spawnModel(const std::string& instanceName,
197  const std::string& modelName)
198  {
199  // assertWorldLoaded();
200  //
201  // //check if file exists
202  // const string path = ::gazebo::common::SystemPaths::Instance()->FindFileURI(
203  // modelName);
204  // if (path.empty()) {
205  // std::cout << "\x1B[32m[[--- Model " << modelName
206  // << " couldn't be found ---]]\033[0m" << std::endl;
207  // return false;
208  // }
209  //
210  // std::string model_str;
211  // std::string model_path;
212  //
213  // std::vector<std::string> files_to_try = {
214  // path + "/model.urdf")
215  // , path + "/model.sdf")
216  // , path + "/" + instanceName + ".urdf")
217  // , path + "/" + instanceName + ".sdf")
218  // }
219  //
220  // for(auto f : files_to_try)
221  // {
222  // if(fileExists(f))
223  // {
224  // model_str = fileToString(f);
225  // model_path = f;
226  // }
227  // }
228  //
229  // if (model_str.empty()) {
230  // std::cout << "\x1B[32m[[--- Model file " << model_path
231  // << " is empty ! ---]]\033[0m" << std::endl;
232  // return false;
233  // }
234  //
235  // TiXmlDocument gazebo_model_xml;
236  // gazebo_model_xml.Parse(model_xml.c_str());
237  //
238  // TiXmlElement* name_urdf = gazebo_model_xml.FirstChildElement("robot");
239  // TiXmlElement* name_sdf = gazebo_model_xml.FirstChildElement("model");
240  //
241  // if(name_urdf)
242  // {
243  // if (nameElement->Attribute("name") != NULL) {
244  // // removing old model name
245  // nameElement->RemoveAttribute("name");
246  // }
247  // // replace with user specified name
248  // nameElement->SetAttribute("name", instanceName);
249  // }
250  //
251  // if (!nameElement) {
252  // cout << "it's not an urdf check for sdf" << endl;
253  // nameElement = gazebo_model_xml.FirstChildElement("model");
254  // if (!nameElement) {
255  // std::cout
256  // << "\x1B[31m[[--- Can't be parsed: No <model> or <robot> tag found! ---]]\033[0m"
257  // << std::endl;
258  // return false;
259  // } else {
260  // // handle sdf
261  // sdf::SDF root;
262  // root.SetFromString(model_xml);
263  // #if GAZEBO_MAJOR_VERSION >= 6
264  // sdf::ElementPtr nameElementSDF = root.Root()->GetElement("model");
265  // #else
266  // sdf::ElementPtr nameElementSDF = root.root->GetElement("model");
267  // #endif
268  // nameElementSDF->GetAttribute("name")->SetFromString(instanceName);
269  // }
270  // } else {
271  // // handle urdf
272  //
273  // if (nameElement->Attribute("name") != NULL) {
274  // // removing old model name
275  // nameElement->RemoveAttribute("name");
276  // }
277  // // replace with user specified name
278  // nameElement->SetAttribute("name", instanceName);
279  // }
280  //
281  // // world->InsertModelFile(modelName);
282  // TiXmlPrinter printer;
283  // printer.SetIndent(" ");
284  // gazebo_model_xml.Accept(&printer);
285  //
286  // world_->InsertModelString(printer.CStr());
287  //
288  // gazebo::common::Time timeout((double) timeoutSec);
289  //
290  // auto modelDeployTimer(new gazebo::common::Timer());
291  //
292  // modelDeployTimer->Start();
293  // while (modelDeployTimer->GetRunning()) {
294  // if (modelDeployTimer->GetElapsed() > timeout) {
295  // gzerr
296  // << "SpawnModel: Model pushed to spawn queue, but spawn service timed out waiting for model to appear in simulation under the name "
297  // << instanceName << endl;
298  // modelDeployTimer->Stop();
299  // return false;
300  // }
301  //
302  // {
303  // #if GAZEBO_MAJOR_VERSION > 8
304  // auto model = world_->ModelByName(instanceName);
305  // #else
306  // auto model = world_->GetModel(instanceName);
307  // #endif
308  // if (model){
309  // modelDeployTimer->Stop();
310  // break;
311  // }
312  // }
313  // std::this_thread::sleep_for(std::chrono::milliseconds(500));
314  // }
315 
316  return false;
317  }
318 
319  double getDt()
320  {
321  assertWorldLoaded();
322  #if GAZEBO_MAJOR_VERSION > 8
323  return world_->Physics()->GetMaxStepSize();
324  #else
325  return world_->GetPhysicsEngine()->GetMaxStepSize();
326  #endif
327  }
328 
329  void runOnce()
330  {
331  assertWorldLoaded();
332  ::gazebo::runWorld(world_, 1);
333  }
334 
335  void run()
336  {
337  assertWorldLoaded();
338  ::gazebo::runWorld(world_, 0);
339  }
340 
341  void shutdown()
342  {
343  ::gazebo::event::Events::sigInt.Signal();
344  std::cout <<"\x1B[32m[[--- Stoping Simulation ---]]\033[0m"<< '\n';
345  if(world_)
346  world_->Fini();
347  std::cout <<"\x1B[32m[[--- Gazebo Shutdown... ---]]\033[0m"<< '\n';
348  //NOTE: This crashes as gazebo is running is a thread
349  ::gazebo::shutdown();
350  }
351 
352  ::gazebo::physics::ModelPtr insertModelFromURDFFile(const std::string& urdf_url
353  , Eigen::Vector3d init_pos = Eigen::Vector3d::Zero()
354  , Eigen::Quaterniond init_rot = Eigen::Quaterniond::Identity()
355  , std::string model_name="")
356  {
357  std::string cmd = "gz sdf -p " + urdf_url;
358  return insertModelFromSDFString( custom_exec(cmd) );
359  }
360 
361  ::gazebo::physics::ModelPtr insertModelFromURDFString(const std::string& urdf_str
362  , Eigen::Vector3d init_pos = Eigen::Vector3d::Zero()
363  , Eigen::Quaterniond init_rot = Eigen::Quaterniond::Identity()
364  , std::string model_name="")
365  {
366  TiXmlDocument doc;
367  doc.Parse(urdf_str.c_str());
368  return insertModelFromTinyXML(&doc,init_pos,init_rot,model_name);
369  }
370 
372  {
373  assertWorldLoaded();
374  #if GAZEBO_MAJOR_VERSION > 8
375  return world_->ModelCount();
376  #else
377  return world_->GetModelCount();
378  #endif
379  }
380 
381  std::vector<std::string> getModelNames()
382  {
383  assertWorldLoaded();
384  std::vector<std::string> names;
385 
386  #if GAZEBO_MAJOR_VERSION > 8
387  for(auto model : world_->Models())
388  names.push_back(model->GetName());
389  #else
390  for(auto model : world_->GetModels())
391  names.push_back(model->GetName());
392  #endif
393 
394  return names;
395  }
396 
397  bool modelExists(const std::string& model_name)
398  {
399  assertWorldLoaded();
400  #if GAZEBO_MAJOR_VERSION > 8
401  auto model = world_->ModelByName( model_name );
402  #else
403  auto model = world_->GetModel( model_name );
404  #endif
405  return static_cast<bool>(model);
406  }
407 
408  const Eigen::Vector3d& getGravity()
409  {
410  assertWorldLoaded();
411  #if GAZEBO_MAJOR_VERSION > 8
412  auto g = world_->Gravity();
413  #else
414  auto g = world_->GetPhysicsEngine()->GetGravity();
415  #endif
416  gravity_vector_[0] = g[0];
417  gravity_vector_[1] = g[1];
418  gravity_vector_[2] = g[2];
419  return gravity_vector_;
420  }
421 
422  std::vector<std::string> getModelJointNames(const std::string& model_name)
423  {
424  std::vector<std::string> joint_names;
425  auto model = getModelByName(model_name);
426  if(!model)
427  return joint_names;
428  for(auto j : model->GetJoints())
429  joint_names.push_back(j->GetName());
430  return joint_names;
431  }
432 
433  ::gazebo::physics::Joint_V getJointsFromNames(const std::string& model_name,const std::vector<std::string>& joint_names)
434  {
435  ::gazebo::physics::Joint_V jv;
436  auto model = getModelByName(model_name);
437  if(!model)
438  return jv;
439  for(auto n : joint_names)
440  {
441  auto joint = model->GetJoint(n);
442  if(joint)
443  jv.push_back(joint);
444  }
445  return jv;
446  }
447 
448  ::gazebo::physics::ModelPtr getModelByName(const std::string& model_name)
449  {
450  assertWorldLoaded();
451  #if GAZEBO_MAJOR_VERSION > 8
452  return world_->ModelByName(model_name);
453  #else
454  return world_->GetModel(model_name);
455  #endif
456  }
457  ::gazebo::physics::WorldPtr getWorld()
458  {
459  return world_;
460  }
461 
462  void executeAfterWorldUpdate(std::function<void(uint32_t,double,double)> callback)
463  {
464  callback_ = callback;
465  }
466 
467  virtual ~GazeboServer()
468  {
469 
470  }
471 
472 private:
473  void assertWorldLoaded()
474  {
475  if(!world_)
476  throw std::runtime_error("[GazeboServer] World is not loaded");
477  }
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="")
482  {
483  if(!doc)
484  {
485  std::cerr << "[GazeboServer] Document provided is null" << '\n';
486  return nullptr;
487  }
488  TiXmlElement* robotElement = doc->FirstChildElement("robot");
489  if(!robotElement)
490  {
491 
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';
497  return nullptr;
498  }
499 
500 
501  if(model_name.empty())
502  {
503  // Extract model name from URDF
504  if(!getRobotNameFromTinyXML(robotElement,model_name))
505  return nullptr;
506  }
507  else
508  {
509  // Set the robot name to user specified name
510  if(!setRobotNameToTinyXML(robotElement,model_name))
511  return nullptr;
512  }
513 
514  std::cout << "[GazeboServer] Trying to insert model \'" << model_name << "\'" << std::endl;
515 
516  sdf::URDF2SDF urdf_to_sdf;
517  TiXmlDocument sdf_xml = urdf_to_sdf.InitModelDoc(doc);
518  // NOTE : from parser_urdf.cc
519  // URDF is compatible with version 1.4. The automatic conversion script
520  // will up-convert URDF to SDF.
521  // sdf->SetAttribute("version", "1.4");
522  // I'm setting it to the current sdf version to avoid warnings
523  // NOTE 2 : Settings the version to 1.6 (kinetic+16.04) disallow the fixed joints
524  // to have any <origin rpy="" part, it is ignored for some reason
525  // FIXME : Find out how to convert URDF to sdf 1.6 (or ar least current version)
526  //sdf_xml.RootElement()->SetAttribute("version",sdf::SDF::Version());
527 
528  TiXmlPrinter printer;
529  printer.SetIndent( " " );
530  sdf_xml.Accept( &printer );
531 
532  return insertModelFromSDFString(printer.CStr(),init_pos,init_rot,model_name);
533  }
534 
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="")
539  {
540  sdf::SDF _sdf;
541  _sdf.SetFromString(sdf_string);
542 
543  // Set the initial position
544  ignition::math::Pose3d init_pose(convVec3(init_pos),convQuat(init_rot));
545  // WARNING: These elemts should always exist, so i'm not checking is they are null
546  _sdf.Root()->GetElement("model")->GetElement("pose")->Set<ignition::math::Pose3d>(init_pose);
547 
548  if(model_name.empty())
549  {
550  model_name = _sdf.Root()->GetElement("model")->GetAttribute("name")->GetAsString();
551  }
552 
553  std::cout << "[GazeboServer] Converted to sdf :\n" << _sdf.ToString() << '\n';
554 
555  world_->InsertModelSDF(_sdf);
556 
557  std::atomic<bool> do_exit(false);
558  auto th = std::thread([&]()
559  {
560  int i=10;
561  while(--i)
562  {
563  if(do_exit)
564  return;
565  std::this_thread::sleep_for(std::chrono::seconds(1));
566  }
567 
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;
574  });
575 
576  int max_trials = 10;
577  for (size_t i = 0; i < max_trials; i++)
578  {
579  std::cout << "[GazeboServer] Runing the world (" << i+1 << "/" << max_trials << ") ... " << std::endl;
580 
581  ::gazebo::runWorld(world_,1);
582 
583  std::cout << "[GazeboServer] Now verifying if the model is correctly loaded..." << std::endl;
584 
585  ::gazebo::physics::ModelPtr model = getModelByName(model_name);
586  if(model)
587  {
588  do_exit = true;
589  if(th.joinable())
590  th.join();
591  std::cout << "[GazeboServer] Model '" << model_name << "' successfully loaded" << std::endl;
592  countExistingSensors();
593  return model;
594  }
595  std::cout << "[GazeboServer] Model '" << model_name << "' yet loaded" << std::endl;
596  std::this_thread::sleep_for(std::chrono::milliseconds(500));
597  }
598  do_exit = true;
599  if(th.joinable())
600  th.join();
601  return nullptr;
602  }
603 
604  bool getRobotNameFromTinyXML(TiXmlElement* robotElement, std::string& model_name)
605  {
606  if(robotElement)
607  {
608  if (robotElement->Attribute("name"))
609  {
610  model_name = robotElement->Attribute("name");
611  }
612  else
613  {
614  std::cerr << "[GazeboServer] Could not get the robot tag in the URDF " << '\n';
615  return false;
616  }
617  }
618  else
619  {
620  std::cerr << "[GazeboServer] Provided robot element is invalid" << '\n';
621  return false;
622  }
623 
624  if (model_name.empty())
625  {
626  std::cerr << "[GazeboServer] Robot name is empty" << '\n';
627  return false;
628  }
629  return true;
630  }
631 
632  bool setRobotNameToTinyXML(TiXmlElement* robotElement, std::string& model_name)
633  {
634  std::string dummy;
635  if(!getRobotNameFromTinyXML(robotElement,dummy))
636  return false;
637 
638  robotElement->RemoveAttribute("name");
639  robotElement->SetAttribute("name", model_name);
640  return true;
641  }
642 
643  void worldUpdateBegin()
644  {
645  #if GAZEBO_MAJOR_VERSION > 8
646  ::gazebo::sensors::SensorManager::Instance()->Update();
647  #else
648  int tmp_sensor_count = 0;
649  for(auto model : world_->GetModels())
650  tmp_sensor_count += model->GetSensorCount();
651 
652  // FIXME: Find out a way to be notified when a sensor is replaced
653  // Here we only check the number of sensors
654  if(tmp_sensor_count > n_sensors_)
655  {
656  std::cerr << "[GazeboServer] Loading " << tmp_sensor_count - n_sensors_ << " sensors\n";
657  if (!::gazebo::sensors::load())
658  {
659  std::cerr << "[GazeboServer] Unable to load sensors\n";
660  break;
661  }
662  if (!::gazebo::sensors::init())
663  {
664  std::cerr << "[GazeboServer] Unable to initialize sensors\n";
665  break;
666  }
667  ::gazebo::sensors::run_once(true);
668  ::gazebo::sensors::run_threads();
669  n_sensors_ = tmp_sensor_count;
670  } else {
671  // NOTE: same number, we do nothing, less it means we removed a model
672  n_sensors_ = tmp_sensor_count;
673  }
674 
675  if(n_sensors_ > 0)
676  ::gazebo::sensors::run_once();
677  #endif
678  }
679 
680  void worldUpdateEnd()
681  {
682  if(callback_)
683  callback_(getIterations(),getSimTime(),getDt());
684  }
685 
686  double getIterations()
687  {
688  #if GAZEBO_MAJOR_VERSION > 8
689  return world_->Iterations();
690  #else
691  return world_->GetIterations();
692  #endif
693  }
694 
695  double getSimTime()
696  {
697  assertWorldLoaded();
698  #if GAZEBO_MAJOR_VERSION > 8
699  return world_->SimTime().Double();
700  #else
701  return world_->GetSimTime().Double();
702  #endif
703  }
704 
705  void countExistingSensors()
706  {
707  n_sensors_ = 0;
708  #if GAZEBO_MAJOR_VERSION > 8
709  for(auto model : world_->Models())
710  n_sensors_ += model->GetSensorCount();
711  #else
712  for(auto model : world_->GetModels())
713  n_sensors_ += model->GetSensorCount();
714  #endif
715  }
716 
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_;
722  int n_sensors_ = 0;
723 };
724 
725 } // namespace gazebo
726 } // namespace orca
::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