gazebo_ros_api_plugin.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 /* Desc: External interfaces for Gazebo
19  * Author: Nate Koenig, John Hsu, Dave Coleman
20  * Date: Jun 10 2013
21  */
22 
23 #include <gazebo/common/Events.hh>
24 #include <gazebo/gazebo_config.h>
26 
27 namespace gazebo
28 {
29 
31  plugin_loaded_(false),
32  stop_(false),
33  pub_link_states_connection_count_(0),
34  pub_model_states_connection_count_(0),
35  pub_performance_metrics_connection_count_(0),
36  physics_reconfigure_initialized_(false),
37  pub_clock_frequency_(0),
38  world_created_(false)
39 {
40  robot_namespace_.clear();
41 }
42 
44 {
45  ROS_DEBUG_STREAM_NAMED("api_plugin","GazeboRosApiPlugin Deconstructor start");
46 
47  // Unload the sigint event
48  sigint_event_.reset();
49  ROS_DEBUG_STREAM_NAMED("api_plugin","After sigint_event unload");
50 
51  // Don't attempt to unload this plugin if it was never loaded in the Load() function
52  if(!plugin_loaded_)
53  {
54  ROS_DEBUG_STREAM_NAMED("api_plugin","Deconstructor skipped because never loaded");
55  return;
56  }
57 
58  // Disconnect slots
60  wrench_update_event_.reset();
61  force_update_event_.reset();
62  time_update_event_.reset();
63  ROS_DEBUG_STREAM_NAMED("api_plugin","Slots disconnected");
64 
65  if (pub_link_states_connection_count_ > 0) // disconnect if there are subscribers on exit
66  pub_link_states_event_.reset();
67  if (pub_model_states_connection_count_ > 0) // disconnect if there are subscribers on exit
69  ROS_DEBUG_STREAM_NAMED("api_plugin","Disconnected World Updates");
70 
71  // Stop the multi threaded ROS spinner
72  async_ros_spin_->stop();
73  ROS_DEBUG_STREAM_NAMED("api_plugin","Async ROS Spin Stopped");
74 
75  // Shutdown the ROS node
76  nh_->shutdown();
77  ROS_DEBUG_STREAM_NAMED("api_plugin","Node Handle Shutdown");
78 
79  // Shutdown ROS queue
81  ROS_DEBUG_STREAM_NAMED("api_plugin","Callback Queue Joined");
82 
83  // Physics Dynamic Reconfigure
85  ROS_DEBUG_STREAM_NAMED("api_plugin","Physics reconfigure joined");
86 
87  // Delete Force and Wrench Jobs
88  lock_.lock();
89  for (std::vector<GazeboRosApiPlugin::ForceJointJob*>::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();)
90  {
91  delete (*iter);
92  iter = force_joint_jobs_.erase(iter);
93  }
94  force_joint_jobs_.clear();
95  ROS_DEBUG_STREAM_NAMED("api_plugin","ForceJointJobs deleted");
96  for (std::vector<GazeboRosApiPlugin::WrenchBodyJob*>::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();)
97  {
98  delete (*iter);
99  iter = wrench_body_jobs_.erase(iter);
100  }
101  wrench_body_jobs_.clear();
102  lock_.unlock();
103  ROS_DEBUG_STREAM_NAMED("api_plugin","WrenchBodyJobs deleted");
104 
105  ROS_DEBUG_STREAM_NAMED("api_plugin","Unloaded");
106 }
107 
109 {
110  ROS_DEBUG_STREAM_NAMED("api_plugin","shutdownSignal() recieved");
111  stop_ = true;
112 }
113 
114 void GazeboRosApiPlugin::Load(int argc, char** argv)
115 {
116  ROS_DEBUG_STREAM_NAMED("api_plugin","Load");
117 
118  // connect to sigint event
119  sigint_event_ = gazebo::event::Events::ConnectSigInt(boost::bind(&GazeboRosApiPlugin::shutdownSignal,this));
120 
121  // setup ros related
122  if (!ros::isInitialized())
123  ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler);
124  else
125  ROS_ERROR_NAMED("api_plugin", "Something other than this gazebo_ros_api plugin started ros::init(...), command line arguments may not be parsed properly.");
126 
127  // check if the ros master is available - required
128  while(!ros::master::check())
129  {
130  ROS_WARN_STREAM_NAMED("api_plugin","No ROS master - start roscore to continue...");
131  // wait 0.5 second
132  usleep(500*1000); // can't use ROS Time here b/c node handle is not yet initialized
133 
134  if(stop_)
135  {
136  ROS_WARN_STREAM_NAMED("api_plugin","Canceled loading Gazebo ROS API plugin by sigint event");
137  return;
138  }
139  }
140 
141  nh_.reset(new ros::NodeHandle("~")); // advertise topics and services in this node's namespace
142 
143  // Built-in multi-threaded ROS spinning
144  async_ros_spin_.reset(new ros::AsyncSpinner(0)); // will use a thread for each CPU core
145  async_ros_spin_->start();
146 
148  gazebo_callback_queue_thread_.reset(new boost::thread( &GazeboRosApiPlugin::gazeboQueueThread, this) );
149 
151  physics_reconfigure_thread_.reset(new boost::thread(boost::bind(&GazeboRosApiPlugin::physicsReconfigureThread, this)));
152 
153  // below needs the world to be created first
154  load_gazebo_ros_api_plugin_event_ = gazebo::event::Events::ConnectWorldCreated(boost::bind(&GazeboRosApiPlugin::loadGazeboRosApiPlugin,this,_1));
155 
156  plugin_loaded_ = true;
157  ROS_INFO_NAMED("api_plugin", "Finished loading Gazebo ROS API Plugin.");
158 }
159 
160 void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
161 {
162  // make sure things are only called once
163  lock_.lock();
164  if (world_created_)
165  {
166  lock_.unlock();
167  return;
168  }
169 
170  // set flag to true and load this plugin
171  world_created_ = true;
172  lock_.unlock();
173 
174  world_ = gazebo::physics::get_world(world_name);
175  if (!world_)
176  {
177  //ROS_ERROR_NAMED("api_plugin", "world name: [%s]",world->Name().c_str());
178  // connect helper function to signal for scheduling torque/forces, etc
179  ROS_FATAL_NAMED("api_plugin", "cannot load gazebo ros api server plugin, physics::get_world() fails to return world");
180  return;
181  }
182 
183  gazebonode_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
184  gazebonode_->Init(world_name);
185  //stat_sub_ = gazebonode_->Subscribe("~/world_stats", &GazeboRosApiPlugin::publishSimTime, this); // TODO: does not work in server plugin?
186  factory_pub_ = gazebonode_->Advertise<gazebo::msgs::Factory>("~/factory");
187  factory_light_pub_ = gazebonode_->Advertise<gazebo::msgs::Light>("~/factory/light");
188  light_modify_pub_ = gazebonode_->Advertise<gazebo::msgs::Light>("~/light/modify");
189  request_pub_ = gazebonode_->Advertise<gazebo::msgs::Request>("~/request");
190  response_sub_ = gazebonode_->Subscribe("~/response",&GazeboRosApiPlugin::onResponse, this);
191  // reset topic connection counts
195 
198 
199  // hooks for applying forces, publishing simtime on /clock
200  wrench_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::wrenchBodySchedulerSlot,this));
201  force_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::forceJointSchedulerSlot,this));
202  time_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishSimTime,this));
203 }
204 
205 void GazeboRosApiPlugin::onResponse(ConstResponsePtr &response)
206 {
207 
208 }
209 #ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
210 void GazeboRosApiPlugin::onPerformanceMetrics(const boost::shared_ptr<gazebo::msgs::PerformanceMetrics const> &msg)
211 {
212  gazebo_msgs::PerformanceMetrics msg_ros;
213  msg_ros.header.stamp = ros::Time::now();
214  msg_ros.real_time_factor = msg->real_time_factor();
215  for (auto sensor: msg->sensor())
216  {
217  gazebo_msgs::SensorPerformanceMetric sensor_msgs;
218  sensor_msgs.sim_update_rate = sensor.sim_update_rate();
219  sensor_msgs.real_update_rate = sensor.real_update_rate();
220  sensor_msgs.name = sensor.name();
221 
222  if (sensor.has_fps())
223  {
224  sensor_msgs.fps = sensor.fps();
225  }
226  else{
227  sensor_msgs.fps = -1;
228  }
229 
230  msg_ros.sensors.push_back(sensor_msgs);
231  }
232 
234 }
235 #endif
236 
238 {
239  static const double timeout = 0.001;
240  while (nh_->ok())
241  {
243  }
244 }
245 
247 {
248  // publish clock for simulated ros time
249  pub_clock_ = nh_->advertise<rosgraph_msgs::Clock>("/clock",10);
250 
251  // Advertise spawn services on the custom queue
252  std::string spawn_sdf_model_service_name("spawn_sdf_model");
253  ros::AdvertiseServiceOptions spawn_sdf_model_aso =
254  ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
255  spawn_sdf_model_service_name,
256  boost::bind(&GazeboRosApiPlugin::spawnSDFModel,this,_1,_2),
258  spawn_sdf_model_service_ = nh_->advertiseService(spawn_sdf_model_aso);
259 
260  // Advertise spawn services on the custom queue
261  std::string spawn_urdf_model_service_name("spawn_urdf_model");
262  ros::AdvertiseServiceOptions spawn_urdf_model_aso =
263  ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
264  spawn_urdf_model_service_name,
265  boost::bind(&GazeboRosApiPlugin::spawnURDFModel,this,_1,_2),
267  spawn_urdf_model_service_ = nh_->advertiseService(spawn_urdf_model_aso);
268 
269  // Advertise delete services on the custom queue
270  std::string delete_model_service_name("delete_model");
271  ros::AdvertiseServiceOptions delete_aso =
272  ros::AdvertiseServiceOptions::create<gazebo_msgs::DeleteModel>(
273  delete_model_service_name,
274  boost::bind(&GazeboRosApiPlugin::deleteModel,this,_1,_2),
276  delete_model_service_ = nh_->advertiseService(delete_aso);
277 
278  // Advertise delete service for lights on the custom queue
279  std::string delete_light_service_name("delete_light");
280  ros::AdvertiseServiceOptions delete_light_aso =
281  ros::AdvertiseServiceOptions::create<gazebo_msgs::DeleteLight>(
282  delete_light_service_name,
283  boost::bind(&GazeboRosApiPlugin::deleteLight,this,_1,_2),
285  delete_light_service_ = nh_->advertiseService(delete_light_aso);
286 
287  // Advertise more services on the custom queue
288  std::string get_model_properties_service_name("get_model_properties");
289  ros::AdvertiseServiceOptions get_model_properties_aso =
290  ros::AdvertiseServiceOptions::create<gazebo_msgs::GetModelProperties>(
291  get_model_properties_service_name,
292  boost::bind(&GazeboRosApiPlugin::getModelProperties,this,_1,_2),
294  get_model_properties_service_ = nh_->advertiseService(get_model_properties_aso);
295 
296  // Advertise more services on the custom queue
297  std::string get_model_state_service_name("get_model_state");
298  ros::AdvertiseServiceOptions get_model_state_aso =
299  ros::AdvertiseServiceOptions::create<gazebo_msgs::GetModelState>(
300  get_model_state_service_name,
301  boost::bind(&GazeboRosApiPlugin::getModelState,this,_1,_2),
303  get_model_state_service_ = nh_->advertiseService(get_model_state_aso);
304 
305  // Advertise more services on the custom queue
306  std::string get_world_properties_service_name("get_world_properties");
307  ros::AdvertiseServiceOptions get_world_properties_aso =
308  ros::AdvertiseServiceOptions::create<gazebo_msgs::GetWorldProperties>(
309  get_world_properties_service_name,
310  boost::bind(&GazeboRosApiPlugin::getWorldProperties,this,_1,_2),
312  get_world_properties_service_ = nh_->advertiseService(get_world_properties_aso);
313 
314  // Advertise more services on the custom queue
315  std::string get_joint_properties_service_name("get_joint_properties");
316  ros::AdvertiseServiceOptions get_joint_properties_aso =
317  ros::AdvertiseServiceOptions::create<gazebo_msgs::GetJointProperties>(
318  get_joint_properties_service_name,
319  boost::bind(&GazeboRosApiPlugin::getJointProperties,this,_1,_2),
321  get_joint_properties_service_ = nh_->advertiseService(get_joint_properties_aso);
322 
323  // Advertise more services on the custom queue
324  std::string get_link_properties_service_name("get_link_properties");
325  ros::AdvertiseServiceOptions get_link_properties_aso =
326  ros::AdvertiseServiceOptions::create<gazebo_msgs::GetLinkProperties>(
327  get_link_properties_service_name,
328  boost::bind(&GazeboRosApiPlugin::getLinkProperties,this,_1,_2),
330  get_link_properties_service_ = nh_->advertiseService(get_link_properties_aso);
331 
332  // Advertise more services on the custom queue
333  std::string get_link_state_service_name("get_link_state");
334  ros::AdvertiseServiceOptions get_link_state_aso =
335  ros::AdvertiseServiceOptions::create<gazebo_msgs::GetLinkState>(
336  get_link_state_service_name,
337  boost::bind(&GazeboRosApiPlugin::getLinkState,this,_1,_2),
339  get_link_state_service_ = nh_->advertiseService(get_link_state_aso);
340 
341  // Advertise more services on the custom queue
342  std::string get_light_properties_service_name("get_light_properties");
343  ros::AdvertiseServiceOptions get_light_properties_aso =
344  ros::AdvertiseServiceOptions::create<gazebo_msgs::GetLightProperties>(
345  get_light_properties_service_name,
346  boost::bind(&GazeboRosApiPlugin::getLightProperties,this,_1,_2),
348  get_light_properties_service_ = nh_->advertiseService(get_light_properties_aso);
349 
350  // Advertise more services on the custom queue
351  std::string set_light_properties_service_name("set_light_properties");
352  ros::AdvertiseServiceOptions set_light_properties_aso =
353  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetLightProperties>(
354  set_light_properties_service_name,
355  boost::bind(&GazeboRosApiPlugin::setLightProperties,this,_1,_2),
357  set_light_properties_service_ = nh_->advertiseService(set_light_properties_aso);
358 
359  // Advertise more services on the custom queue
360  std::string get_physics_properties_service_name("get_physics_properties");
361  ros::AdvertiseServiceOptions get_physics_properties_aso =
362  ros::AdvertiseServiceOptions::create<gazebo_msgs::GetPhysicsProperties>(
363  get_physics_properties_service_name,
364  boost::bind(&GazeboRosApiPlugin::getPhysicsProperties,this,_1,_2),
366  get_physics_properties_service_ = nh_->advertiseService(get_physics_properties_aso);
367 
368  // publish complete link states in world frame
369  ros::AdvertiseOptions pub_link_states_ao =
370  ros::AdvertiseOptions::create<gazebo_msgs::LinkStates>(
371  "link_states",10,
372  boost::bind(&GazeboRosApiPlugin::onLinkStatesConnect,this),
375  pub_link_states_ = nh_->advertise(pub_link_states_ao);
376 
377  // publish complete model states in world frame
378  ros::AdvertiseOptions pub_model_states_ao =
379  ros::AdvertiseOptions::create<gazebo_msgs::ModelStates>(
380  "model_states",10,
381  boost::bind(&GazeboRosApiPlugin::onModelStatesConnect,this),
384  pub_model_states_ = nh_->advertise(pub_model_states_ao);
385 
386 #ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
387  // publish performance metrics
388  ros::AdvertiseOptions pub_performance_metrics_ao =
389  ros::AdvertiseOptions::create<gazebo_msgs::PerformanceMetrics>(
390  "performance_metrics",10,
391  boost::bind(&GazeboRosApiPlugin::onPerformanceMetricsConnect,this),
392  boost::bind(&GazeboRosApiPlugin::onPerformanceMetricsDisconnect,this),
394  pub_performance_metrics_ = nh_->advertise(pub_performance_metrics_ao);
395 #endif
396 
397  // Advertise more services on the custom queue
398  std::string set_link_properties_service_name("set_link_properties");
399  ros::AdvertiseServiceOptions set_link_properties_aso =
400  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetLinkProperties>(
401  set_link_properties_service_name,
402  boost::bind(&GazeboRosApiPlugin::setLinkProperties,this,_1,_2),
404  set_link_properties_service_ = nh_->advertiseService(set_link_properties_aso);
405 
406  // Advertise more services on the custom queue
407  std::string set_physics_properties_service_name("set_physics_properties");
408  ros::AdvertiseServiceOptions set_physics_properties_aso =
409  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetPhysicsProperties>(
410  set_physics_properties_service_name,
411  boost::bind(&GazeboRosApiPlugin::setPhysicsProperties,this,_1,_2),
413  set_physics_properties_service_ = nh_->advertiseService(set_physics_properties_aso);
414 
415  // Advertise more services on the custom queue
416  std::string set_model_state_service_name("set_model_state");
417  ros::AdvertiseServiceOptions set_model_state_aso =
418  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetModelState>(
419  set_model_state_service_name,
420  boost::bind(&GazeboRosApiPlugin::setModelState,this,_1,_2),
422  set_model_state_service_ = nh_->advertiseService(set_model_state_aso);
423 
424  // Advertise more services on the custom queue
425  std::string set_model_configuration_service_name("set_model_configuration");
426  ros::AdvertiseServiceOptions set_model_configuration_aso =
427  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetModelConfiguration>(
428  set_model_configuration_service_name,
429  boost::bind(&GazeboRosApiPlugin::setModelConfiguration,this,_1,_2),
431  set_model_configuration_service_ = nh_->advertiseService(set_model_configuration_aso);
432 
433  // Advertise more services on the custom queue
434  std::string set_joint_properties_service_name("set_joint_properties");
435  ros::AdvertiseServiceOptions set_joint_properties_aso =
436  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetJointProperties>(
437  set_joint_properties_service_name,
438  boost::bind(&GazeboRosApiPlugin::setJointProperties,this,_1,_2),
440  set_joint_properties_service_ = nh_->advertiseService(set_joint_properties_aso);
441 
442  // Advertise more services on the custom queue
443  std::string set_link_state_service_name("set_link_state");
444  ros::AdvertiseServiceOptions set_link_state_aso =
445  ros::AdvertiseServiceOptions::create<gazebo_msgs::SetLinkState>(
446  set_link_state_service_name,
447  boost::bind(&GazeboRosApiPlugin::setLinkState,this,_1,_2),
449  set_link_state_service_ = nh_->advertiseService(set_link_state_aso);
450 
451  // Advertise topic on custom queue
452  // topic callback version for set_link_state
453  ros::SubscribeOptions link_state_so =
454  ros::SubscribeOptions::create<gazebo_msgs::LinkState>(
455  "set_link_state",10,
456  boost::bind( &GazeboRosApiPlugin::updateLinkState,this,_1),
458  set_link_state_topic_ = nh_->subscribe(link_state_so);
459 
460  // topic callback version for set_model_state
461  ros::SubscribeOptions model_state_so =
462  ros::SubscribeOptions::create<gazebo_msgs::ModelState>(
463  "set_model_state",10,
464  boost::bind( &GazeboRosApiPlugin::updateModelState,this,_1),
466  set_model_state_topic_ = nh_->subscribe(model_state_so);
467 
468  // Advertise more services on the custom queue
469  std::string pause_physics_service_name("pause_physics");
470  ros::AdvertiseServiceOptions pause_physics_aso =
471  ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
472  pause_physics_service_name,
473  boost::bind(&GazeboRosApiPlugin::pausePhysics,this,_1,_2),
475  pause_physics_service_ = nh_->advertiseService(pause_physics_aso);
476 
477  // Advertise more services on the custom queue
478  std::string unpause_physics_service_name("unpause_physics");
479  ros::AdvertiseServiceOptions unpause_physics_aso =
480  ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
481  unpause_physics_service_name,
482  boost::bind(&GazeboRosApiPlugin::unpausePhysics,this,_1,_2),
484  unpause_physics_service_ = nh_->advertiseService(unpause_physics_aso);
485 
486  // Advertise more services on the custom queue
487  std::string apply_body_wrench_service_name("apply_body_wrench");
488  ros::AdvertiseServiceOptions apply_body_wrench_aso =
489  ros::AdvertiseServiceOptions::create<gazebo_msgs::ApplyBodyWrench>(
490  apply_body_wrench_service_name,
491  boost::bind(&GazeboRosApiPlugin::applyBodyWrench,this,_1,_2),
493  apply_body_wrench_service_ = nh_->advertiseService(apply_body_wrench_aso);
494 
495  // Advertise more services on the custom queue
496  std::string apply_joint_effort_service_name("apply_joint_effort");
497  ros::AdvertiseServiceOptions apply_joint_effort_aso =
498  ros::AdvertiseServiceOptions::create<gazebo_msgs::ApplyJointEffort>(
499  apply_joint_effort_service_name,
500  boost::bind(&GazeboRosApiPlugin::applyJointEffort,this,_1,_2),
502  apply_joint_effort_service_ = nh_->advertiseService(apply_joint_effort_aso);
503 
504  // Advertise more services on the custom queue
505  std::string clear_joint_forces_service_name("clear_joint_forces");
506  ros::AdvertiseServiceOptions clear_joint_forces_aso =
507  ros::AdvertiseServiceOptions::create<gazebo_msgs::JointRequest>(
508  clear_joint_forces_service_name,
509  boost::bind(&GazeboRosApiPlugin::clearJointForces,this,_1,_2),
511  clear_joint_forces_service_ = nh_->advertiseService(clear_joint_forces_aso);
512 
513  // Advertise more services on the custom queue
514  std::string clear_body_wrenches_service_name("clear_body_wrenches");
515  ros::AdvertiseServiceOptions clear_body_wrenches_aso =
516  ros::AdvertiseServiceOptions::create<gazebo_msgs::BodyRequest>(
517  clear_body_wrenches_service_name,
518  boost::bind(&GazeboRosApiPlugin::clearBodyWrenches,this,_1,_2),
520  clear_body_wrenches_service_ = nh_->advertiseService(clear_body_wrenches_aso);
521 
522  // Advertise more services on the custom queue
523  std::string reset_simulation_service_name("reset_simulation");
524  ros::AdvertiseServiceOptions reset_simulation_aso =
525  ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
526  reset_simulation_service_name,
527  boost::bind(&GazeboRosApiPlugin::resetSimulation,this,_1,_2),
529  reset_simulation_service_ = nh_->advertiseService(reset_simulation_aso);
530 
531  // Advertise more services on the custom queue
532  std::string reset_world_service_name("reset_world");
533  ros::AdvertiseServiceOptions reset_world_aso =
534  ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
535  reset_world_service_name,
536  boost::bind(&GazeboRosApiPlugin::resetWorld,this,_1,_2),
538  reset_world_service_ = nh_->advertiseService(reset_world_aso);
539 
540 
541  // set param for use_sim_time if not set by user already
542  if(!(nh_->hasParam("/use_sim_time")))
543  nh_->setParam("/use_sim_time", true);
544 
545  // todo: contemplate setting environment variable ROBOT=sim here???
546  nh_->getParam("pub_clock_frequency", pub_clock_frequency_);
547 #if GAZEBO_MAJOR_VERSION >= 8
548  last_pub_clock_time_ = world_->SimTime();
549 #else
550  last_pub_clock_time_ = world_->GetSimTime();
551 #endif
552 }
553 
555 {
557  if (pub_link_states_connection_count_ == 1) // connect on first subscriber
558  pub_link_states_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishLinkStates,this));
559 }
560 
562 {
564  if (pub_model_states_connection_count_ == 1) // connect on first subscriber
565  pub_model_states_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishModelStates,this));
566 }
567 
568 #ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
569 void GazeboRosApiPlugin::onPerformanceMetricsConnect()
570 {
572  if (pub_performance_metrics_connection_count_ == 1) // connect on first subscriber
573  {
574  performance_metric_sub_ = gazebonode_->Subscribe("/gazebo/performance_metrics",
575  &GazeboRosApiPlugin::onPerformanceMetrics, this);
576  }
577 }
578 
579 void GazeboRosApiPlugin::onPerformanceMetricsDisconnect()
580 {
582  if (pub_performance_metrics_connection_count_ <= 0) // disconnect with no subscribers
583  {
584  performance_metric_sub_.reset();
585  if (pub_performance_metrics_connection_count_ < 0) // should not be possible
586  ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_performance_metrics_ in gazebo_ros.cpp? something weird");
587  }
588 }
589 #endif
590 
592 {
594  if (pub_link_states_connection_count_ <= 0) // disconnect with no subscribers
595  {
596  pub_link_states_event_.reset();
597  if (pub_link_states_connection_count_ < 0) // should not be possible
598  ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_link_states_ in gazebo_ros.cpp? something weird");
599  }
600 }
601 
603 {
605  if (pub_model_states_connection_count_ <= 0) // disconnect with no subscribers
606  {
607  pub_model_states_event_.reset();
608  if (pub_model_states_connection_count_ < 0) // should not be possible
609  ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_model_states_ in gazebo_ros.cpp? something weird");
610  }
611 }
612 
613 bool GazeboRosApiPlugin::spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,
614  gazebo_msgs::SpawnModel::Response &res)
615 {
616  // get namespace for the corresponding model plugins
617  robot_namespace_ = req.robot_namespace;
618 
619  // incoming entity string
620  std::string model_xml = req.model_xml;
621 
622  if (!isURDF(model_xml))
623  {
624  ROS_ERROR_NAMED("api_plugin", "SpawnModel: Failure - entity format is invalid.");
625  res.success = false;
626  res.status_message = "SpawnModel: Failure - entity format is invalid.";
627  return false;
628  }
629 
633  {
634  std::string open_bracket("<?");
635  std::string close_bracket("?>");
636  size_t pos1 = model_xml.find(open_bracket,0);
637  size_t pos2 = model_xml.find(close_bracket,0);
638  if (pos1 != std::string::npos && pos2 != std::string::npos)
639  model_xml.replace(pos1,pos2-pos1+2,std::string(""));
640  }
641 
642  // Remove comments from URDF
643  {
644  std::string open_comment("<!--");
645  std::string close_comment("-->");
646  size_t pos1;
647  while((pos1 = model_xml.find(open_comment,0)) != std::string::npos){
648  size_t pos2 = model_xml.find(close_comment,0);
649  if (pos2 != std::string::npos)
650  model_xml.replace(pos1,pos2-pos1+3,std::string(""));
651  }
652  }
653 
654  // Now, replace package://xxx with the full path to the package
655  {
656  std::string package_prefix("package://");
657  size_t pos1 = model_xml.find(package_prefix,0);
658  while (pos1 != std::string::npos)
659  {
660  size_t pos2 = model_xml.find("/", pos1+10);
661  //ROS_DEBUG_NAMED("api_plugin", " pos %d %d",(int)pos1, (int)pos2);
662  if (pos2 == std::string::npos || pos1 >= pos2)
663  {
664  ROS_ERROR_NAMED("api_plugin", "Malformed package name?");
665  break;
666  }
667 
668  std::string package_name = model_xml.substr(pos1+10,pos2-pos1-10);
669  //ROS_DEBUG_NAMED("api_plugin", "package name [%s]", package_name.c_str());
670  std::string package_path = ros::package::getPath(package_name);
671  if (package_path.empty())
672  {
673  ROS_FATAL_NAMED("api_plugin", "Package[%s] does not have a path",package_name.c_str());
674  res.success = false;
675  res.status_message = "urdf reference package name does not exist: " + package_name;
676  return false;
677  }
678  ROS_DEBUG_ONCE_NAMED("api_plugin", "Package name [%s] has path [%s]", package_name.c_str(), package_path.c_str());
679 
680  model_xml.replace(pos1,(pos2-pos1),package_path);
681  pos1 = model_xml.find(package_prefix, pos1);
682  }
683  }
684  // ROS_DEBUG_NAMED("api_plugin", "Model XML\n\n%s\n\n ",model_xml.c_str());
685 
686  req.model_xml = model_xml;
687 
688  // Model is now considered convert to SDF
689  return spawnSDFModel(req,res);
690 }
691 
692 bool GazeboRosApiPlugin::spawnSDFModel(gazebo_msgs::SpawnModel::Request &req,
693  gazebo_msgs::SpawnModel::Response &res)
694 {
695  // incoming entity name
696  std::string model_name = req.model_name;
697 
698  // get namespace for the corresponding model plugins
699  robot_namespace_ = req.robot_namespace;
700 
701  // get initial pose of model
702  ignition::math::Vector3d initial_xyz(req.initial_pose.position.x,req.initial_pose.position.y,req.initial_pose.position.z);
703  // get initial roll pitch yaw (fixed frame transform)
704  ignition::math::Quaterniond initial_q(req.initial_pose.orientation.w,req.initial_pose.orientation.x,req.initial_pose.orientation.y,req.initial_pose.orientation.z);
705 
706  // refernce frame for initial pose definition, modify initial pose if defined
707 #if GAZEBO_MAJOR_VERSION >= 8
708  gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame);
709 #else
710  gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame);
711 #endif
712  if (frame)
713  {
714  // convert to relative pose
715 #if GAZEBO_MAJOR_VERSION >= 8
716  ignition::math::Pose3d frame_pose = frame->WorldPose();
717 #else
718  ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign();
719 #endif
720  initial_xyz = frame_pose.Pos() + frame_pose.Rot().RotateVector(initial_xyz);
721  initial_q = frame_pose.Rot() * initial_q;
722  }
723 
725  else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map")
726  {
727  ROS_DEBUG_NAMED("api_plugin", "SpawnModel: reference_frame is empty/world/map, using inertial frame");
728  }
729  else
730  {
731  res.success = false;
732  res.status_message = "SpawnModel: reference reference_frame not found, did you forget to scope the link by model name?";
733  return true;
734  }
735 
736  // incoming robot model string
737  std::string model_xml = req.model_xml;
738 
739  // store resulting Gazebo Model XML to be sent to spawn queue
740  // get incoming string containg either an URDF or a Gazebo Model XML
741  // grab from parameter server if necessary convert to SDF if necessary
742  stripXmlDeclaration(model_xml);
743 
744  // put string in TiXmlDocument for manipulation
745  TiXmlDocument gazebo_model_xml;
746  gazebo_model_xml.Parse(model_xml.c_str());
747 
748  // optional model manipulations: update initial pose && replace model name
749  if (isSDF(model_xml))
750  {
751  updateSDFAttributes(gazebo_model_xml, model_name, initial_xyz, initial_q);
752 
753  // Walk recursively through the entire SDF, locate plugin tags and
754  // add robotNamespace as a child with the correct namespace
755  if (!this->robot_namespace_.empty())
756  {
757  // Get root element for SDF
758  // TODO: implement the spawning also with <light></light> and <model></model>
759  TiXmlNode* model_tixml = gazebo_model_xml.FirstChild("sdf");
760  model_tixml = (!model_tixml) ?
761  gazebo_model_xml.FirstChild("gazebo") : model_tixml;
762  if (model_tixml)
763  {
764  walkChildAddRobotNamespace(model_tixml);
765  }
766  else
767  {
768  ROS_WARN_NAMED("api_plugin", "Unable to add robot namespace to xml");
769  }
770  }
771  }
772  else if (isURDF(model_xml))
773  {
774  updateURDFModelPose(gazebo_model_xml, initial_xyz, initial_q);
775  updateURDFName(gazebo_model_xml, model_name);
776 
777  // Walk recursively through the entire URDF, locate plugin tags and
778  // add robotNamespace as a child with the correct namespace
779  if (!this->robot_namespace_.empty())
780  {
781  // Get root element for URDF
782  TiXmlNode* model_tixml = gazebo_model_xml.FirstChild("robot");
783  if (model_tixml)
784  {
785  walkChildAddRobotNamespace(model_tixml);
786  }
787  else
788  {
789  ROS_WARN_NAMED("api_plugin", "Unable to add robot namespace to xml");
790  }
791  }
792  }
793  else
794  {
795  ROS_ERROR_NAMED("api_plugin", "GazeboRosApiPlugin SpawnModel Failure: input xml format not recognized");
796  res.success = false;
797  res.status_message = "GazeboRosApiPlugin SpawnModel Failure: input model_xml not SDF or URDF, or cannot be converted to Gazebo compatible format.";
798  return true;
799  }
800 
801  // do spawning check if spawn worked, return response
802  return spawnAndConform(gazebo_model_xml, model_name, res);
803 }
804 
805 bool GazeboRosApiPlugin::deleteModel(gazebo_msgs::DeleteModel::Request &req,
806  gazebo_msgs::DeleteModel::Response &res)
807 {
808  // clear forces, etc for the body in question
809 #if GAZEBO_MAJOR_VERSION >= 8
810  gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name);
811 #else
812  gazebo::physics::ModelPtr model = world_->GetModel(req.model_name);
813 #endif
814  if (!model)
815  {
816  ROS_ERROR_NAMED("api_plugin", "DeleteModel: model [%s] does not exist",req.model_name.c_str());
817  res.success = false;
818  res.status_message = "DeleteModel: model does not exist";
819  return true;
820  }
821 
822  // delete wrench jobs on bodies
823  for (unsigned int i = 0 ; i < model->GetChildCount(); i ++)
824  {
825  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(i));
826  if (body)
827  {
828  // look for it in jobs, delete body wrench jobs
829  clearBodyWrenches(body->GetScopedName());
830  }
831  }
832 
833  // delete force jobs on joints
834  gazebo::physics::Joint_V joints = model->GetJoints();
835  for (unsigned int i=0;i< joints.size(); i++)
836  {
837  // look for it in jobs, delete joint force jobs
838  clearJointForces(joints[i]->GetName());
839  }
840 
841  // send delete model request
842  gazebo::msgs::Request *msg = gazebo::msgs::CreateRequest("entity_delete",req.model_name);
843  request_pub_->Publish(*msg,true);
844  delete msg;
845  msg = nullptr;
846 
847  ros::Duration model_spawn_timeout(60.0);
848  ros::Time timeout = ros::Time::now() + model_spawn_timeout;
849  // wait and verify that model is deleted
850  while (true)
851  {
852  if (ros::Time::now() > timeout)
853  {
854  res.success = false;
855  res.status_message = "DeleteModel: Model pushed to delete queue, but delete service timed out waiting for model to disappear from simulation";
856  return true;
857  }
858  {
859  //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex());
860 #if GAZEBO_MAJOR_VERSION >= 8
861  if (!world_->ModelByName(req.model_name)) break;
862 #else
863  if (!world_->GetModel(req.model_name)) break;
864 #endif
865  }
866  ROS_DEBUG_NAMED("api_plugin", "Waiting for model deletion (%s)",req.model_name.c_str());
867  usleep(1000);
868  }
869 
870  // set result
871  res.success = true;
872  res.status_message = "DeleteModel: successfully deleted model";
873  return true;
874 }
875 
876 bool GazeboRosApiPlugin::deleteLight(gazebo_msgs::DeleteLight::Request &req,
877  gazebo_msgs::DeleteLight::Response &res)
878 {
879 #if GAZEBO_MAJOR_VERSION >= 8
880  gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name);
881 #else
882  gazebo::physics::LightPtr phy_light = world_->Light(req.light_name);
883 #endif
884 
885  if (phy_light == NULL)
886  {
887  res.success = false;
888  res.status_message = "DeleteLight: Requested light " + req.light_name + " not found!";
889  }
890  else
891  {
892  gazebo::msgs::Request* msg = gazebo::msgs::CreateRequest("entity_delete", req.light_name);
893  request_pub_->Publish(*msg, true);
894  delete msg;
895  msg = nullptr;
896 
897  res.success = false;
898 
899  for (int i = 0; i < 100; i++)
900  {
901 #if GAZEBO_MAJOR_VERSION >= 8
902  phy_light = world_->LightByName(req.light_name);
903 #else
904  phy_light = world_->Light(req.light_name);
905 #endif
906  if (phy_light == NULL)
907  {
908  res.success = true;
909  res.status_message = "DeleteLight: " + req.light_name + " successfully deleted";
910  return true;
911  }
912  // Check every 100ms
913  usleep(100000);
914  }
915  }
916 
917  res.status_message = "DeleteLight: Timeout reached while removing light \"" + req.light_name
918  + "\"";
919 
920  return true;
921 }
922 
923 bool GazeboRosApiPlugin::getModelState(gazebo_msgs::GetModelState::Request &req,
924  gazebo_msgs::GetModelState::Response &res)
925 {
926 #if GAZEBO_MAJOR_VERSION >= 8
927  gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name);
928  gazebo::physics::EntityPtr frame = world_->EntityByName(req.relative_entity_name);
929 #else
930  gazebo::physics::ModelPtr model = world_->GetModel(req.model_name);
931  gazebo::physics::EntityPtr frame = world_->GetEntity(req.relative_entity_name);
932 #endif
933  if (!model)
934  {
935  ROS_ERROR_NAMED("api_plugin", "GetModelState: model [%s] does not exist",req.model_name.c_str());
936  res.success = false;
937  res.status_message = "GetModelState: model does not exist";
938  return true;
939  }
940  else
941  {
947  {
948  std::map<std::string, unsigned int>::iterator it = access_count_get_model_state_.find(req.model_name);
949  if(it == access_count_get_model_state_.end())
950  {
951  access_count_get_model_state_.insert( std::pair<std::string, unsigned int>(req.model_name, 1) );
952  res.header.seq = 1;
953  }
954  else
955  {
956  it->second++;
957  res.header.seq = it->second;
958  }
959  res.header.stamp = ros::Time::now();
960  res.header.frame_id = req.relative_entity_name;
961  }
962  // get model pose
963  // get model twist
964 #if GAZEBO_MAJOR_VERSION >= 8
965  ignition::math::Pose3d model_pose = model->WorldPose();
966  ignition::math::Vector3d model_linear_vel = model->WorldLinearVel();
967  ignition::math::Vector3d model_angular_vel = model->WorldAngularVel();
968 #else
969  ignition::math::Pose3d model_pose = model->GetWorldPose().Ign();
970  ignition::math::Vector3d model_linear_vel = model->GetWorldLinearVel().Ign();
971  ignition::math::Vector3d model_angular_vel = model->GetWorldAngularVel().Ign();
972 #endif
973  ignition::math::Vector3d model_pos = model_pose.Pos();
974  ignition::math::Quaterniond model_rot = model_pose.Rot();
975 
976 
977 
978  if (frame)
979  {
980  // convert to relative pose, rates
981 #if GAZEBO_MAJOR_VERSION >= 8
982  ignition::math::Pose3d frame_pose = frame->WorldPose();
983  ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame
984  ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame
985 #else
986  ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign();
987  ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign(); // get velocity in gazebo frame
988  ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign(); // get velocity in gazebo frame
989 #endif
990  ignition::math::Pose3d model_rel_pose = model_pose - frame_pose;
991  model_pos = model_rel_pose.Pos();
992  model_rot = model_rel_pose.Rot();
993 
994  model_linear_vel = frame_pose.Rot().RotateVectorReverse(model_linear_vel - frame_vpos);
995  model_angular_vel = frame_pose.Rot().RotateVectorReverse(model_angular_vel - frame_veul);
996  }
998  else if (req.relative_entity_name == "" || req.relative_entity_name == "world" || req.relative_entity_name == "map" || req.relative_entity_name == "/map")
999  {
1000  ROS_DEBUG_NAMED("api_plugin", "GetModelState: relative_entity_name is empty/world/map, using inertial frame");
1001  }
1002  else
1003  {
1004  res.success = false;
1005  res.status_message = "GetModelState: reference relative_entity_name not found, did you forget to scope the body by model name?";
1006  return true;
1007  }
1008 
1009  // fill in response
1010  res.pose.position.x = model_pos.X();
1011  res.pose.position.y = model_pos.Y();
1012  res.pose.position.z = model_pos.Z();
1013  res.pose.orientation.w = model_rot.W();
1014  res.pose.orientation.x = model_rot.X();
1015  res.pose.orientation.y = model_rot.Y();
1016  res.pose.orientation.z = model_rot.Z();
1017 
1018  res.twist.linear.x = model_linear_vel.X();
1019  res.twist.linear.y = model_linear_vel.Y();
1020  res.twist.linear.z = model_linear_vel.Z();
1021  res.twist.angular.x = model_angular_vel.X();
1022  res.twist.angular.y = model_angular_vel.Y();
1023  res.twist.angular.z = model_angular_vel.Z();
1024 
1025  res.success = true;
1026  res.status_message = "GetModelState: got properties";
1027  return true;
1028  }
1029  return true;
1030 }
1031 
1032 bool GazeboRosApiPlugin::getModelProperties(gazebo_msgs::GetModelProperties::Request &req,
1033  gazebo_msgs::GetModelProperties::Response &res)
1034 {
1035 #if GAZEBO_MAJOR_VERSION >= 8
1036  gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name);
1037 #else
1038  gazebo::physics::ModelPtr model = world_->GetModel(req.model_name);
1039 #endif
1040  if (!model)
1041  {
1042  ROS_ERROR_NAMED("api_plugin", "GetModelProperties: model [%s] does not exist",req.model_name.c_str());
1043  res.success = false;
1044  res.status_message = "GetModelProperties: model does not exist";
1045  return true;
1046  }
1047  else
1048  {
1049  // get model parent name
1050  gazebo::physics::ModelPtr parent_model = boost::dynamic_pointer_cast<gazebo::physics::Model>(model->GetParent());
1051  if (parent_model) res.parent_model_name = parent_model->GetName();
1052 
1053  // get list of child bodies, geoms
1054  res.body_names.clear();
1055  res.geom_names.clear();
1056  for (unsigned int i = 0 ; i < model->GetChildCount(); i ++)
1057  {
1058  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(i));
1059  if (body)
1060  {
1061  res.body_names.push_back(body->GetName());
1062  // get list of geoms
1063  for (unsigned int j = 0; j < body->GetChildCount() ; j++)
1064  {
1065  gazebo::physics::CollisionPtr geom = boost::dynamic_pointer_cast<gazebo::physics::Collision>(body->GetChild(j));
1066  if (geom)
1067  res.geom_names.push_back(geom->GetName());
1068  }
1069  }
1070  }
1071 
1072  // get list of joints
1073  res.joint_names.clear();
1074 
1075  gazebo::physics::Joint_V joints = model->GetJoints();
1076  for (unsigned int i=0;i< joints.size(); i++)
1077  res.joint_names.push_back( joints[i]->GetName() );
1078 
1079  // get children model names
1080  res.child_model_names.clear();
1081  for (unsigned int j = 0; j < model->GetChildCount(); j++)
1082  {
1083  gazebo::physics::ModelPtr child_model = boost::dynamic_pointer_cast<gazebo::physics::Model>(model->GetChild(j));
1084  if (child_model)
1085  res.child_model_names.push_back(child_model->GetName() );
1086  }
1087 
1088  // is model static
1089  res.is_static = model->IsStatic();
1090 
1091  res.success = true;
1092  res.status_message = "GetModelProperties: got properties";
1093  return true;
1094  }
1095  return true;
1096 }
1097 
1098 bool GazeboRosApiPlugin::getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req,
1099  gazebo_msgs::GetWorldProperties::Response &res)
1100 {
1101  res.model_names.clear();
1102 #if GAZEBO_MAJOR_VERSION >= 8
1103  res.sim_time = world_->SimTime().Double();
1104  for (unsigned int i = 0; i < world_->ModelCount(); i ++)
1105  res.model_names.push_back(world_->ModelByIndex(i)->GetName());
1106 #else
1107  res.sim_time = world_->GetSimTime().Double();
1108  for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
1109  res.model_names.push_back(world_->GetModel(i)->GetName());
1110 #endif
1111  gzerr << "disablign rendering has not been implemented, rendering is always enabled\n";
1112  res.rendering_enabled = true; //world->GetRenderEngineEnabled();
1113  res.success = true;
1114  res.status_message = "GetWorldProperties: got properties";
1115  return true;
1116 }
1117 
1118 bool GazeboRosApiPlugin::getJointProperties(gazebo_msgs::GetJointProperties::Request &req,
1119  gazebo_msgs::GetJointProperties::Response &res)
1120 {
1121  gazebo::physics::JointPtr joint;
1122 #if GAZEBO_MAJOR_VERSION >= 8
1123  for (unsigned int i = 0; i < world_->ModelCount(); i ++)
1124  {
1125  joint = world_->ModelByIndex(i)->GetJoint(req.joint_name);
1126 #else
1127  for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
1128  {
1129  joint = world_->GetModel(i)->GetJoint(req.joint_name);
1130 #endif
1131  if (joint) break;
1132  }
1133 
1134  if (!joint)
1135  {
1136  res.success = false;
1137  res.status_message = "GetJointProperties: joint not found";
1138  return true;
1139  }
1140  else
1141  {
1143  res.type = res.REVOLUTE;
1144 
1145  res.damping.clear(); // to be added to gazebo
1146  //res.damping.push_back(joint->GetDamping(0));
1147 
1148  res.position.clear();
1149 #if GAZEBO_MAJOR_VERSION >= 8
1150  res.position.push_back(joint->Position(0));
1151 #else
1152  res.position.push_back(joint->GetAngle(0).Radian());
1153 #endif
1154 
1155  res.rate.clear(); // use GetVelocity(i)
1156  res.rate.push_back(joint->GetVelocity(0));
1157 
1158  res.success = true;
1159  res.status_message = "GetJointProperties: got properties";
1160  return true;
1161  }
1162 }
1163 
1164 bool GazeboRosApiPlugin::getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req,
1165  gazebo_msgs::GetLinkProperties::Response &res)
1166 {
1167 #if GAZEBO_MAJOR_VERSION >= 8
1168  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_name));
1169 #else
1170  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name));
1171 #endif
1172  if (!body)
1173  {
1174  res.success = false;
1175  res.status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?";
1176  return true;
1177  }
1178  else
1179  {
1181  res.gravity_mode = body->GetGravityMode();
1182 
1183  gazebo::physics::InertialPtr inertia = body->GetInertial();
1184 
1185 #if GAZEBO_MAJOR_VERSION >= 8
1186  res.mass = body->GetInertial()->Mass();
1187 
1188  res.ixx = inertia->IXX();
1189  res.iyy = inertia->IYY();
1190  res.izz = inertia->IZZ();
1191  res.ixy = inertia->IXY();
1192  res.ixz = inertia->IXZ();
1193  res.iyz = inertia->IYZ();
1194 
1195  ignition::math::Vector3d com = body->GetInertial()->CoG();
1196 #else
1197  res.mass = body->GetInertial()->GetMass();
1198 
1199  res.ixx = inertia->GetIXX();
1200  res.iyy = inertia->GetIYY();
1201  res.izz = inertia->GetIZZ();
1202  res.ixy = inertia->GetIXY();
1203  res.ixz = inertia->GetIXZ();
1204  res.iyz = inertia->GetIYZ();
1205 
1206  ignition::math::Vector3d com = body->GetInertial()->GetCoG().Ign();
1207 #endif
1208  res.com.position.x = com.X();
1209  res.com.position.y = com.Y();
1210  res.com.position.z = com.Z();
1211  res.com.orientation.x = 0; // @todo: gazebo do not support rotated inertia yet
1212  res.com.orientation.y = 0;
1213  res.com.orientation.z = 0;
1214  res.com.orientation.w = 1;
1215 
1216  res.success = true;
1217  res.status_message = "GetLinkProperties: got properties";
1218  return true;
1219  }
1220 }
1221 
1222 bool GazeboRosApiPlugin::getLinkState(gazebo_msgs::GetLinkState::Request &req,
1223  gazebo_msgs::GetLinkState::Response &res)
1224 {
1225 #if GAZEBO_MAJOR_VERSION >= 8
1226  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_name));
1227  gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame);
1228 #else
1229  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name));
1230  gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame);
1231 #endif
1232 
1233  if (!body)
1234  {
1235  res.success = false;
1236  res.status_message = "GetLinkState: link not found, did you forget to scope the link by model name?";
1237  return true;
1238  }
1239 
1240  // get body pose
1241  // Get inertial rates
1242 #if GAZEBO_MAJOR_VERSION >= 8
1243  ignition::math::Pose3d body_pose = body->WorldPose();
1244  ignition::math::Vector3d body_vpos = body->WorldLinearVel(); // get velocity in gazebo frame
1245  ignition::math::Vector3d body_veul = body->WorldAngularVel(); // get velocity in gazebo frame
1246 #else
1247  ignition::math::Pose3d body_pose = body->GetWorldPose().Ign();
1248  ignition::math::Vector3d body_vpos = body->GetWorldLinearVel().Ign(); // get velocity in gazebo frame
1249  ignition::math::Vector3d body_veul = body->GetWorldAngularVel().Ign(); // get velocity in gazebo frame
1250 #endif
1251 
1252  if (frame)
1253  {
1254  // convert to relative pose, rates
1255 #if GAZEBO_MAJOR_VERSION >= 8
1256  ignition::math::Pose3d frame_pose = frame->WorldPose();
1257  ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame
1258  ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame
1259 #else
1260  ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign();
1261  ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign(); // get velocity in gazebo frame
1262  ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign(); // get velocity in gazebo frame
1263 #endif
1264  body_pose = body_pose - frame_pose;
1265 
1266  body_vpos = frame_pose.Rot().RotateVectorReverse(body_vpos - frame_vpos);
1267  body_veul = frame_pose.Rot().RotateVectorReverse(body_veul - frame_veul);
1268  }
1270  else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map")
1271  {
1272  ROS_DEBUG_NAMED("api_plugin", "GetLinkState: reference_frame is empty/world/map, using inertial frame");
1273  }
1274  else
1275  {
1276  res.success = false;
1277  res.status_message = "GetLinkState: reference reference_frame not found, did you forget to scope the link by model name?";
1278  return true;
1279  }
1280 
1281  res.link_state.link_name = req.link_name;
1282  res.link_state.pose.position.x = body_pose.Pos().X();
1283  res.link_state.pose.position.y = body_pose.Pos().Y();
1284  res.link_state.pose.position.z = body_pose.Pos().Z();
1285  res.link_state.pose.orientation.x = body_pose.Rot().X();
1286  res.link_state.pose.orientation.y = body_pose.Rot().Y();
1287  res.link_state.pose.orientation.z = body_pose.Rot().Z();
1288  res.link_state.pose.orientation.w = body_pose.Rot().W();
1289  res.link_state.twist.linear.x = body_vpos.X();
1290  res.link_state.twist.linear.y = body_vpos.Y();
1291  res.link_state.twist.linear.z = body_vpos.Z();
1292  res.link_state.twist.angular.x = body_veul.X();
1293  res.link_state.twist.angular.y = body_veul.Y();
1294  res.link_state.twist.angular.z = body_veul.Z();
1295  res.link_state.reference_frame = req.reference_frame;
1296 
1297  res.success = true;
1298  res.status_message = "GetLinkState: got state";
1299  return true;
1300 }
1301 
1302 bool GazeboRosApiPlugin::getLightProperties(gazebo_msgs::GetLightProperties::Request &req,
1303  gazebo_msgs::GetLightProperties::Response &res)
1304 {
1305 #if GAZEBO_MAJOR_VERSION >= 8
1306  gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name);
1307 #else
1308  gazebo::physics::LightPtr phy_light = world_->Light(req.light_name);
1309 #endif
1310 
1311  if (phy_light == NULL)
1312  {
1313  res.success = false;
1314  res.status_message = "getLightProperties: Requested light " + req.light_name + " not found!";
1315  }
1316  else
1317  {
1318  gazebo::msgs::Light light;
1319  phy_light->FillMsg(light);
1320 
1321  res.diffuse.r = light.diffuse().r();
1322  res.diffuse.g = light.diffuse().g();
1323  res.diffuse.b = light.diffuse().b();
1324  res.diffuse.a = light.diffuse().a();
1325 
1326  res.attenuation_constant = light.attenuation_constant();
1327  res.attenuation_linear = light.attenuation_linear();
1328  res.attenuation_quadratic = light.attenuation_quadratic();
1329 
1330  res.success = true;
1331  }
1332 
1333  return true;
1334 }
1335 
1336 bool GazeboRosApiPlugin::setLightProperties(gazebo_msgs::SetLightProperties::Request &req,
1337  gazebo_msgs::SetLightProperties::Response &res)
1338 {
1339 #if GAZEBO_MAJOR_VERSION >= 8
1340  gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name);
1341 #else
1342  gazebo::physics::LightPtr phy_light = world_->Light(req.light_name);
1343 #endif
1344 
1345  if (phy_light == NULL)
1346  {
1347  res.success = false;
1348  res.status_message = "setLightProperties: Requested light " + req.light_name + " not found!";
1349  }
1350  else
1351  {
1352  gazebo::msgs::Light light;
1353 
1354  phy_light->FillMsg(light);
1355 
1356  light.mutable_diffuse()->set_r(req.diffuse.r);
1357  light.mutable_diffuse()->set_g(req.diffuse.g);
1358  light.mutable_diffuse()->set_b(req.diffuse.b);
1359  light.mutable_diffuse()->set_a(req.diffuse.a);
1360 
1361  light.set_attenuation_constant(req.attenuation_constant);
1362  light.set_attenuation_linear(req.attenuation_linear);
1363  light.set_attenuation_quadratic(req.attenuation_quadratic);
1364 
1365  light_modify_pub_->Publish(light, true);
1366 
1367  res.success = true;
1368  }
1369 
1370  return true;
1371 }
1372 
1373 bool GazeboRosApiPlugin::setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req,
1374  gazebo_msgs::SetLinkProperties::Response &res)
1375 {
1376 #if GAZEBO_MAJOR_VERSION >= 8
1377  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_name));
1378 #else
1379  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name));
1380 #endif
1381  if (!body)
1382  {
1383  res.success = false;
1384  res.status_message = "SetLinkProperties: link not found, did you forget to scope the link by model name?";
1385  return true;
1386  }
1387  else
1388  {
1389  gazebo::physics::InertialPtr mass = body->GetInertial();
1390  // @todo: FIXME: add inertia matrix rotation to Gazebo
1391  // mass.SetInertiaRotation(ignition::math::Quaterniondion(req.com.orientation.w,res.com.orientation.x,req.com.orientation.y req.com.orientation.z));
1392  mass->SetCoG(ignition::math::Vector3d(req.com.position.x,req.com.position.y,req.com.position.z));
1393  mass->SetInertiaMatrix(req.ixx,req.iyy,req.izz,req.ixy,req.ixz,req.iyz);
1394  mass->SetMass(req.mass);
1395  body->SetGravityMode(req.gravity_mode);
1396  // @todo: mass change unverified
1397  res.success = true;
1398  res.status_message = "SetLinkProperties: properties set";
1399  return true;
1400  }
1401 }
1402 
1403 bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req,
1404  gazebo_msgs::SetPhysicsProperties::Response &res)
1405 {
1406  // pause simulation if requested
1407  bool is_paused = world_->IsPaused();
1408  world_->SetPaused(true);
1409  world_->SetGravity(ignition::math::Vector3d(req.gravity.x,req.gravity.y,req.gravity.z));
1410 
1411  // supported updates
1412 #if GAZEBO_MAJOR_VERSION >= 8
1413  gazebo::physics::PhysicsEnginePtr pe = (world_->Physics());
1414 #else
1415  gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine());
1416 #endif
1417  pe->SetMaxStepSize(req.time_step);
1418  pe->SetRealTimeUpdateRate(req.max_update_rate);
1419 
1420  if (pe->GetType() == "ode")
1421  {
1422  // stuff only works in ODE right now
1423  pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies);
1424  pe->SetParam("precon_iters", int(req.ode_config.sor_pgs_precon_iters));
1425  pe->SetParam("iters", int(req.ode_config.sor_pgs_iters));
1426  pe->SetParam("sor", req.ode_config.sor_pgs_w);
1427  pe->SetParam("cfm", req.ode_config.cfm);
1428  pe->SetParam("erp", req.ode_config.erp);
1429  pe->SetParam("contact_surface_layer",
1430  req.ode_config.contact_surface_layer);
1431  pe->SetParam("contact_max_correcting_vel",
1432  req.ode_config.contact_max_correcting_vel);
1433  pe->SetParam("max_contacts", int(req.ode_config.max_contacts));
1434 
1435  world_->SetPaused(is_paused);
1436 
1437  res.success = true;
1438  res.status_message = "physics engine updated";
1439  }
1440  else
1441  {
1443  ROS_ERROR_NAMED("api_plugin", "ROS set_physics_properties service call does not yet support physics engine [%s].", pe->GetType().c_str());
1444  res.success = false;
1445  res.status_message = "Physics engine [" + pe->GetType() + "]: set_physics_properties not supported.";
1446  }
1447  return res.success;
1448 }
1449 
1450 bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req,
1451  gazebo_msgs::GetPhysicsProperties::Response &res)
1452 {
1453  // supported updates
1454 #if GAZEBO_MAJOR_VERSION >= 8
1455  gazebo::physics::PhysicsEnginePtr pe = (world_->Physics());
1456 #else
1457  gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine());
1458 #endif
1459  res.time_step = pe->GetMaxStepSize();
1460  res.pause = world_->IsPaused();
1461  res.max_update_rate = pe->GetRealTimeUpdateRate();
1462  ignition::math::Vector3d gravity = world_->Gravity();
1463  res.gravity.x = gravity.X();
1464  res.gravity.y = gravity.Y();
1465  res.gravity.z = gravity.Z();
1466 
1467  // stuff only works in ODE right now
1468  if (pe->GetType() == "ode")
1469  {
1470  res.ode_config.auto_disable_bodies =
1471  pe->GetAutoDisableFlag();
1472  res.ode_config.sor_pgs_precon_iters = boost::any_cast<int>(
1473  pe->GetParam("precon_iters"));
1474  res.ode_config.sor_pgs_iters = boost::any_cast<int>(
1475  pe->GetParam("iters"));
1476  res.ode_config.sor_pgs_w = boost::any_cast<double>(
1477  pe->GetParam("sor"));
1478  res.ode_config.contact_surface_layer = boost::any_cast<double>(
1479  pe->GetParam("contact_surface_layer"));
1480  res.ode_config.contact_max_correcting_vel = boost::any_cast<double>(
1481  pe->GetParam("contact_max_correcting_vel"));
1482  res.ode_config.cfm = boost::any_cast<double>(
1483  pe->GetParam("cfm"));
1484  res.ode_config.erp = boost::any_cast<double>(
1485  pe->GetParam("erp"));
1486  res.ode_config.max_contacts = boost::any_cast<int>(
1487  pe->GetParam("max_contacts"));
1488 
1489  res.success = true;
1490  res.status_message = "GetPhysicsProperties: got properties";
1491  }
1492  else
1493  {
1495  ROS_ERROR_NAMED("api_plugin", "ROS get_physics_properties service call does not yet support physics engine [%s].", pe->GetType().c_str());
1496  res.success = false;
1497  res.status_message = "Physics engine [" + pe->GetType() + "]: get_physics_properties not supported.";
1498  }
1499  return res.success;
1500 }
1501 
1502 bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Request &req,
1503  gazebo_msgs::SetJointProperties::Response &res)
1504 {
1506  gazebo::physics::JointPtr joint;
1507 #if GAZEBO_MAJOR_VERSION >= 8
1508  for (unsigned int i = 0; i < world_->ModelCount(); i ++)
1509  {
1510  joint = world_->ModelByIndex(i)->GetJoint(req.joint_name);
1511 #else
1512  for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
1513  {
1514  joint = world_->GetModel(i)->GetJoint(req.joint_name);
1515 #endif
1516  if (joint) break;
1517  }
1518 
1519  if (!joint)
1520  {
1521  res.success = false;
1522  res.status_message = "SetJointProperties: joint not found";
1523  return true;
1524  }
1525  else
1526  {
1527  for(unsigned int i=0;i< req.ode_joint_config.damping.size();i++)
1528  joint->SetDamping(i,req.ode_joint_config.damping[i]);
1529  for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++)
1530  joint->SetParam("hi_stop",i,req.ode_joint_config.hiStop[i]);
1531  for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++)
1532  joint->SetParam("lo_stop",i,req.ode_joint_config.loStop[i]);
1533  for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++)
1534  joint->SetParam("erp",i,req.ode_joint_config.erp[i]);
1535  for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++)
1536  joint->SetParam("cfm",i,req.ode_joint_config.cfm[i]);
1537  for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++)
1538  joint->SetParam("stop_erp",i,req.ode_joint_config.stop_erp[i]);
1539  for(unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++)
1540  joint->SetParam("stop_cfm",i,req.ode_joint_config.stop_cfm[i]);
1541  for(unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++)
1542  joint->SetParam("fudge_factor",i,req.ode_joint_config.fudge_factor[i]);
1543  for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++)
1544  joint->SetParam("fmax",i,req.ode_joint_config.fmax[i]);
1545  for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++)
1546  joint->SetParam("vel",i,req.ode_joint_config.vel[i]);
1547 
1548  res.success = true;
1549  res.status_message = "SetJointProperties: properties set";
1550  return true;
1551  }
1552 }
1553 
1554 bool GazeboRosApiPlugin::setModelState(gazebo_msgs::SetModelState::Request &req,
1555  gazebo_msgs::SetModelState::Response &res)
1556 {
1557  ignition::math::Vector3d target_pos(req.model_state.pose.position.x,req.model_state.pose.position.y,req.model_state.pose.position.z);
1558  ignition::math::Quaterniond target_rot(req.model_state.pose.orientation.w,req.model_state.pose.orientation.x,req.model_state.pose.orientation.y,req.model_state.pose.orientation.z);
1559  target_rot.Normalize(); // eliminates invalid rotation (0, 0, 0, 0)
1560  ignition::math::Pose3d target_pose(target_pos,target_rot);
1561  ignition::math::Vector3d target_pos_dot(req.model_state.twist.linear.x,req.model_state.twist.linear.y,req.model_state.twist.linear.z);
1562  ignition::math::Vector3d target_rot_dot(req.model_state.twist.angular.x,req.model_state.twist.angular.y,req.model_state.twist.angular.z);
1563 
1564 #if GAZEBO_MAJOR_VERSION >= 8
1565  gazebo::physics::ModelPtr model = world_->ModelByName(req.model_state.model_name);
1566 #else
1567  gazebo::physics::ModelPtr model = world_->GetModel(req.model_state.model_name);
1568 #endif
1569  if (!model)
1570  {
1571  ROS_ERROR_NAMED("api_plugin", "Updating ModelState: model [%s] does not exist",req.model_state.model_name.c_str());
1572  res.success = false;
1573  res.status_message = "SetModelState: model does not exist";
1574  return true;
1575  }
1576  else
1577  {
1578 #if GAZEBO_MAJOR_VERSION >= 8
1579  gazebo::physics::EntityPtr relative_entity = world_->EntityByName(req.model_state.reference_frame);
1580 #else
1581  gazebo::physics::EntityPtr relative_entity = world_->GetEntity(req.model_state.reference_frame);
1582 #endif
1583  if (relative_entity)
1584  {
1585 #if GAZEBO_MAJOR_VERSION >= 8
1586  ignition::math::Pose3d frame_pose = relative_entity->WorldPose(); // - myBody->GetCoMPose();
1587 #else
1588  ignition::math::Pose3d frame_pose = relative_entity->GetWorldPose().Ign(); // - myBody->GetCoMPose();
1589 #endif
1590 
1591  target_pose = target_pose + frame_pose;
1592 
1593  // Velocities should be commanded in the requested reference
1594  // frame, so we need to translate them to the world frame
1595  target_pos_dot = frame_pose.Rot().RotateVector(target_pos_dot);
1596  target_rot_dot = frame_pose.Rot().RotateVector(target_rot_dot);
1597  }
1599  else if (req.model_state.reference_frame == "" || req.model_state.reference_frame == "world" || req.model_state.reference_frame == "map" || req.model_state.reference_frame == "/map" )
1600  {
1601  ROS_DEBUG_NAMED("api_plugin", "Updating ModelState: reference frame is empty/world/map, usig inertial frame");
1602  }
1603  else
1604  {
1605  ROS_ERROR_NAMED("api_plugin", "Updating ModelState: for model[%s], specified reference frame entity [%s] does not exist",
1606  req.model_state.model_name.c_str(),req.model_state.reference_frame.c_str());
1607  res.success = false;
1608  res.status_message = "SetModelState: specified reference frame entity does not exist";
1609  return true;
1610  }
1611 
1612  //ROS_ERROR_NAMED("api_plugin", "target state: %f %f %f",target_pose.Pos().X(),target_pose.Pos().Y(),target_pose.Pos().Z());
1613  bool is_paused = world_->IsPaused();
1614  world_->SetPaused(true);
1615  model->SetWorldPose(target_pose);
1616  world_->SetPaused(is_paused);
1617  //ignition::math::Pose3d p3d = model->WorldPose();
1618  //ROS_ERROR_NAMED("api_plugin", "model updated state: %f %f %f",p3d.Pos().X(),p3d.Pos().Y(),p3d.Pos().Z());
1619 
1620  // set model velocity
1621  model->SetLinearVel(target_pos_dot);
1622  model->SetAngularVel(target_rot_dot);
1623 
1624  res.success = true;
1625  res.status_message = "SetModelState: set model state done";
1626  return true;
1627  }
1628 }
1629 
1630 void GazeboRosApiPlugin::updateModelState(const gazebo_msgs::ModelState::ConstPtr& model_state)
1631 {
1632  gazebo_msgs::SetModelState::Response res;
1633  gazebo_msgs::SetModelState::Request req;
1634  req.model_state = *model_state;
1635  /*bool success =*/ setModelState(req,res);
1636 }
1637 
1638 bool GazeboRosApiPlugin::applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,
1639  gazebo_msgs::ApplyJointEffort::Response &res)
1640 {
1641  gazebo::physics::JointPtr joint;
1642 #if GAZEBO_MAJOR_VERSION >= 8
1643  for (unsigned int i = 0; i < world_->ModelCount(); i ++)
1644  {
1645  joint = world_->ModelByIndex(i)->GetJoint(req.joint_name);
1646 #else
1647  for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
1648  {
1649  joint = world_->GetModel(i)->GetJoint(req.joint_name);
1650 #endif
1651  if (joint)
1652  {
1654  fjj->joint = joint;
1655  fjj->force = req.effort;
1656  fjj->start_time = req.start_time;
1657 #if GAZEBO_MAJOR_VERSION >= 8
1658  if (fjj->start_time < ros::Time(world_->SimTime().Double()))
1659  fjj->start_time = ros::Time(world_->SimTime().Double());
1660 #else
1661  if (fjj->start_time < ros::Time(world_->GetSimTime().Double()))
1662  fjj->start_time = ros::Time(world_->GetSimTime().Double());
1663 #endif
1664  fjj->duration = req.duration;
1665  lock_.lock();
1666  force_joint_jobs_.push_back(fjj);
1667  lock_.unlock();
1668 
1669  res.success = true;
1670  res.status_message = "ApplyJointEffort: effort set";
1671  return true;
1672  }
1673  }
1674 
1675  res.success = false;
1676  res.status_message = "ApplyJointEffort: joint not found";
1677  return true;
1678 }
1679 
1680 bool GazeboRosApiPlugin::resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
1681 {
1682  world_->Reset();
1683  return true;
1684 }
1685 
1686 bool GazeboRosApiPlugin::resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
1687 {
1688  world_->ResetEntities(gazebo::physics::Base::MODEL);
1689  return true;
1690 }
1691 
1692 bool GazeboRosApiPlugin::pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
1693 {
1694  world_->SetPaused(true);
1695  return true;
1696 }
1697 
1698 bool GazeboRosApiPlugin::unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
1699 {
1700  world_->SetPaused(false);
1701  return true;
1702 }
1703 
1704 bool GazeboRosApiPlugin::clearJointForces(gazebo_msgs::JointRequest::Request &req,
1705  gazebo_msgs::JointRequest::Response &res)
1706 {
1707  return clearJointForces(req.joint_name);
1708 }
1709 bool GazeboRosApiPlugin::clearJointForces(std::string joint_name)
1710 {
1711  bool search = true;
1712  lock_.lock();
1713  while(search)
1714  {
1715  search = false;
1716  for (std::vector<GazeboRosApiPlugin::ForceJointJob*>::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();++iter)
1717  {
1718  if ((*iter)->joint->GetName() == joint_name)
1719  {
1720  // found one, search through again
1721  search = true;
1722  delete (*iter);
1723  force_joint_jobs_.erase(iter);
1724  break;
1725  }
1726  }
1727  }
1728  lock_.unlock();
1729  return true;
1730 }
1731 
1732 bool GazeboRosApiPlugin::clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req,
1733  gazebo_msgs::BodyRequest::Response &res)
1734 {
1735  return clearBodyWrenches(req.body_name);
1736 }
1737 bool GazeboRosApiPlugin::clearBodyWrenches(std::string body_name)
1738 {
1739  bool search = true;
1740  lock_.lock();
1741  while(search)
1742  {
1743  search = false;
1744  for (std::vector<GazeboRosApiPlugin::WrenchBodyJob*>::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();++iter)
1745  {
1746  //ROS_ERROR_NAMED("api_plugin", "search %s %s",(*iter)->body->GetScopedName().c_str(), body_name.c_str());
1747  if ((*iter)->body->GetScopedName() == body_name)
1748  {
1749  // found one, search through again
1750  search = true;
1751  delete (*iter);
1752  wrench_body_jobs_.erase(iter);
1753  break;
1754  }
1755  }
1756  }
1757  lock_.unlock();
1758  return true;
1759 }
1760 
1761 bool GazeboRosApiPlugin::setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req,
1762  gazebo_msgs::SetModelConfiguration::Response &res)
1763 {
1764  std::string gazebo_model_name = req.model_name;
1765 
1766  // search for model with name
1767 #if GAZEBO_MAJOR_VERSION >= 8
1768  gazebo::physics::ModelPtr gazebo_model = world_->ModelByName(req.model_name);
1769 #else
1770  gazebo::physics::ModelPtr gazebo_model = world_->GetModel(req.model_name);
1771 #endif
1772  if (!gazebo_model)
1773  {
1774  ROS_ERROR_NAMED("api_plugin", "SetModelConfiguration: model [%s] does not exist",gazebo_model_name.c_str());
1775  res.success = false;
1776  res.status_message = "SetModelConfiguration: model does not exist";
1777  return true;
1778  }
1779 
1780  if (req.joint_names.size() == req.joint_positions.size())
1781  {
1782  std::map<std::string, double> joint_position_map;
1783  for (unsigned int i = 0; i < req.joint_names.size(); i++)
1784  {
1785  joint_position_map[req.joint_names[i]] = req.joint_positions[i];
1786  }
1787 
1788  // make the service call to pause gazebo
1789  bool is_paused = world_->IsPaused();
1790  if (!is_paused) world_->SetPaused(true);
1791 
1792  gazebo_model->SetJointPositions(joint_position_map);
1793 
1794  // resume paused state before this call
1795  world_->SetPaused(is_paused);
1796 
1797  res.success = true;
1798  res.status_message = "SetModelConfiguration: success";
1799  return true;
1800  }
1801  else
1802  {
1803  res.success = false;
1804  res.status_message = "SetModelConfiguration: joint name and position list have different lengths";
1805  return true;
1806  }
1807 }
1808 
1809 bool GazeboRosApiPlugin::setLinkState(gazebo_msgs::SetLinkState::Request &req,
1810  gazebo_msgs::SetLinkState::Response &res)
1811 {
1812 #if GAZEBO_MAJOR_VERSION >= 8
1813  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_state.link_name));
1814  gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.link_state.reference_frame));
1815 #else
1816  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_state.link_name));
1817  gazebo::physics::EntityPtr frame = world_->GetEntity(req.link_state.reference_frame);
1818 #endif
1819  if (!body)
1820  {
1821  ROS_ERROR_NAMED("api_plugin", "Updating LinkState: link [%s] does not exist",req.link_state.link_name.c_str());
1822  res.success = false;
1823  res.status_message = "SetLinkState: link does not exist";
1824  return true;
1825  }
1826 
1828  // get reference frame (body/model(link)) pose and
1829  // transform target pose to absolute world frame
1830  ignition::math::Vector3d target_pos(req.link_state.pose.position.x,req.link_state.pose.position.y,req.link_state.pose.position.z);
1831  ignition::math::Quaterniond target_rot(req.link_state.pose.orientation.w,req.link_state.pose.orientation.x,req.link_state.pose.orientation.y,req.link_state.pose.orientation.z);
1832  ignition::math::Pose3d target_pose(target_pos,target_rot);
1833  ignition::math::Vector3d target_linear_vel(req.link_state.twist.linear.x,req.link_state.twist.linear.y,req.link_state.twist.linear.z);
1834  ignition::math::Vector3d target_angular_vel(req.link_state.twist.angular.x,req.link_state.twist.angular.y,req.link_state.twist.angular.z);
1835 
1836  if (frame)
1837  {
1838 #if GAZEBO_MAJOR_VERSION >= 8
1839  ignition::math::Pose3d frame_pose = frame->WorldPose(); // - myBody->GetCoMPose();
1840  ignition::math::Vector3d frame_linear_vel = frame->WorldLinearVel();
1841  ignition::math::Vector3d frame_angular_vel = frame->WorldAngularVel();
1842 #else
1843  ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); // - myBody->GetCoMPose();
1844  ignition::math::Vector3d frame_linear_vel = frame->GetWorldLinearVel().Ign();
1845  ignition::math::Vector3d frame_angular_vel = frame->GetWorldAngularVel().Ign();
1846 #endif
1847  ignition::math::Vector3d frame_pos = frame_pose.Pos();
1848  ignition::math::Quaterniond frame_rot = frame_pose.Rot();
1849 
1850  //std::cout << " debug : " << frame->GetName() << " : " << frame_pose << " : " << target_pose << std::endl;
1851  target_pose = target_pose + frame_pose;
1852 
1853  target_linear_vel -= frame_linear_vel;
1854  target_angular_vel -= frame_angular_vel;
1855  }
1856  else if (req.link_state.reference_frame == "" || req.link_state.reference_frame == "world" || req.link_state.reference_frame == "map" || req.link_state.reference_frame == "/map")
1857  {
1858  ROS_INFO_NAMED("api_plugin", "Updating LinkState: reference_frame is empty/world/map, using inertial frame");
1859  }
1860  else
1861  {
1862  ROS_ERROR_NAMED("api_plugin", "Updating LinkState: reference_frame is not a valid entity name");
1863  res.success = false;
1864  res.status_message = "SetLinkState: failed";
1865  return true;
1866  }
1867 
1868  //std::cout << " debug : " << target_pose << std::endl;
1869  //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex());
1870 
1871  bool is_paused = world_->IsPaused();
1872  if (!is_paused) world_->SetPaused(true);
1873  body->SetWorldPose(target_pose);
1874  world_->SetPaused(is_paused);
1875 
1876  // set body velocity to desired twist
1877  body->SetLinearVel(target_linear_vel);
1878  body->SetAngularVel(target_angular_vel);
1879 
1880  res.success = true;
1881  res.status_message = "SetLinkState: success";
1882  return true;
1883 }
1884 
1885 void GazeboRosApiPlugin::updateLinkState(const gazebo_msgs::LinkState::ConstPtr& link_state)
1886 {
1887  gazebo_msgs::SetLinkState::Request req;
1888  gazebo_msgs::SetLinkState::Response res;
1889  req.link_state = *link_state;
1890  /*bool success = */ setLinkState(req,res);
1891 }
1892 
1893 void GazeboRosApiPlugin::transformWrench( ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque,
1894  const ignition::math::Vector3d &reference_force,
1895  const ignition::math::Vector3d &reference_torque,
1896  const ignition::math::Pose3d &target_to_reference )
1897 {
1898  // rotate force into target frame
1899  target_force = target_to_reference.Rot().RotateVector(reference_force);
1900  // rotate torque into target frame
1901  target_torque = target_to_reference.Rot().RotateVector(reference_torque);
1902 
1903  // target force is the refence force rotated by the target->reference transform
1904  target_torque = target_torque + target_to_reference.Pos().Cross(target_force);
1905 }
1906 
1907 bool GazeboRosApiPlugin::applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req,
1908  gazebo_msgs::ApplyBodyWrench::Response &res)
1909 {
1910 #if GAZEBO_MAJOR_VERSION >= 8
1911  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->EntityByName(req.body_name));
1912  gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame);
1913 #else
1914  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.body_name));
1915  gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame);
1916 #endif
1917  if (!body)
1918  {
1919  ROS_ERROR_NAMED("api_plugin", "ApplyBodyWrench: body [%s] does not exist",req.body_name.c_str());
1920  res.success = false;
1921  res.status_message = "ApplyBodyWrench: body does not exist";
1922  return true;
1923  }
1924 
1925  // target wrench
1926  ignition::math::Vector3d reference_force(req.wrench.force.x,req.wrench.force.y,req.wrench.force.z);
1927  ignition::math::Vector3d reference_torque(req.wrench.torque.x,req.wrench.torque.y,req.wrench.torque.z);
1928  ignition::math::Vector3d reference_point(req.reference_point.x,req.reference_point.y,req.reference_point.z);
1929 
1930  ignition::math::Vector3d target_force;
1931  ignition::math::Vector3d target_torque;
1932 
1935  reference_torque = reference_torque + reference_point.Cross(reference_force);
1936 
1938  if (frame)
1939  {
1940  // get reference frame (body/model(body)) pose and
1941  // transform target pose to absolute world frame
1942  // @todo: need to modify wrench (target force and torque by frame)
1943  // transform wrench from reference_point in reference_frame
1944  // into the reference frame of the body
1945  // first, translate by reference point to the body frame
1946 #if GAZEBO_MAJOR_VERSION >= 8
1947  ignition::math::Pose3d framePose = frame->WorldPose();
1948 #else
1949  ignition::math::Pose3d framePose = frame->GetWorldPose().Ign();
1950 #endif
1951 #if GAZEBO_MAJOR_VERSION >= 8
1952  ignition::math::Pose3d bodyPose = body->WorldPose();
1953 #else
1954  ignition::math::Pose3d bodyPose = body->GetWorldPose().Ign();
1955 #endif
1956  ignition::math::Pose3d target_to_reference = framePose - bodyPose;
1957  ROS_DEBUG_NAMED("api_plugin", "reference frame for applied wrench: [%f %f %f, %f %f %f]-[%f %f %f, %f %f %f]=[%f %f %f, %f %f %f]",
1958  bodyPose.Pos().X(),
1959  bodyPose.Pos().Y(),
1960  bodyPose.Pos().Z(),
1961  bodyPose.Rot().Euler().X(),
1962  bodyPose.Rot().Euler().Y(),
1963  bodyPose.Rot().Euler().Z(),
1964  framePose.Pos().X(),
1965  framePose.Pos().Y(),
1966  framePose.Pos().Z(),
1967  framePose.Rot().Euler().X(),
1968  framePose.Rot().Euler().Y(),
1969  framePose.Rot().Euler().Z(),
1970  target_to_reference.Pos().X(),
1971  target_to_reference.Pos().Y(),
1972  target_to_reference.Pos().Z(),
1973  target_to_reference.Rot().Euler().X(),
1974  target_to_reference.Rot().Euler().Y(),
1975  target_to_reference.Rot().Euler().Z()
1976  );
1977  transformWrench(target_force, target_torque, reference_force, reference_torque, target_to_reference);
1978  ROS_ERROR_NAMED("api_plugin", "wrench defined as [%s]:[%f %f %f, %f %f %f] --> applied as [%s]:[%f %f %f, %f %f %f]",
1979  frame->GetName().c_str(),
1980  reference_force.X(),
1981  reference_force.Y(),
1982  reference_force.Z(),
1983  reference_torque.X(),
1984  reference_torque.Y(),
1985  reference_torque.Z(),
1986  body->GetName().c_str(),
1987  target_force.X(),
1988  target_force.Y(),
1989  target_force.Z(),
1990  target_torque.X(),
1991  target_torque.Y(),
1992  target_torque.Z()
1993  );
1994 
1995  }
1996  else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map")
1997  {
1998  ROS_INFO_NAMED("api_plugin", "ApplyBodyWrench: reference_frame is empty/world/map, using inertial frame, transferring from body relative to inertial frame");
1999  // FIXME: transfer to inertial frame
2000 #if GAZEBO_MAJOR_VERSION >= 8
2001  ignition::math::Pose3d target_to_reference = body->WorldPose();
2002 #else
2003  ignition::math::Pose3d target_to_reference = body->GetWorldPose().Ign();
2004 #endif
2005  target_force = reference_force;
2006  target_torque = reference_torque;
2007 
2008  }
2009  else
2010  {
2011  ROS_ERROR_NAMED("api_plugin", "ApplyBodyWrench: reference_frame is not a valid entity name");
2012  res.success = false;
2013  res.status_message = "ApplyBodyWrench: reference_frame not found";
2014  return true;
2015  }
2016 
2017  // apply wrench
2018  // schedule a job to do below at appropriate times:
2019  // body->SetForce(force)
2020  // body->SetTorque(torque)
2022  wej->body = body;
2023  wej->force = target_force;
2024  wej->torque = target_torque;
2025  wej->start_time = req.start_time;
2026 #if GAZEBO_MAJOR_VERSION >= 8
2027  if (wej->start_time < ros::Time(world_->SimTime().Double()))
2028  wej->start_time = ros::Time(world_->SimTime().Double());
2029 #else
2030  if (wej->start_time < ros::Time(world_->GetSimTime().Double()))
2031  wej->start_time = ros::Time(world_->GetSimTime().Double());
2032 #endif
2033  wej->duration = req.duration;
2034  lock_.lock();
2035  wrench_body_jobs_.push_back(wej);
2036  lock_.unlock();
2037 
2038  res.success = true;
2039  res.status_message = "";
2040  return true;
2041 }
2042 
2043 bool GazeboRosApiPlugin::isURDF(std::string model_xml)
2044 {
2045  TiXmlDocument doc_in;
2046  doc_in.Parse(model_xml.c_str());
2047  if (doc_in.FirstChild("robot"))
2048  return true;
2049  else
2050  return false;
2051 }
2052 
2053 bool GazeboRosApiPlugin::isSDF(std::string model_xml)
2054 {
2055  // FIXME: very crude check
2056  TiXmlDocument doc_in;
2057  doc_in.Parse(model_xml.c_str());
2058  if (doc_in.FirstChild("gazebo") ||
2059  doc_in.FirstChild("sdf")) // sdf
2060  return true;
2061  else
2062  return false;
2063 }
2064 
2066 {
2067  // MDMutex locks in case model is getting deleted, don't have to do this if we delete jobs first
2068  // boost::recursive_mutex::scoped_lock lock(*world->GetMDMutex());
2069  lock_.lock();
2070  for (std::vector<GazeboRosApiPlugin::WrenchBodyJob*>::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();)
2071  {
2072  // check times and apply wrench if necessary
2073 #if GAZEBO_MAJOR_VERSION >= 8
2074  ros::Time simTime = ros::Time(world_->SimTime().Double());
2075 #else
2076  ros::Time simTime = ros::Time(world_->GetSimTime().Double());
2077 #endif
2078  if (simTime >= (*iter)->start_time)
2079  if (simTime <= (*iter)->start_time+(*iter)->duration ||
2080  (*iter)->duration.toSec() < 0.0)
2081  {
2082  if ((*iter)->body) // if body exists
2083  {
2084  (*iter)->body->SetForce((*iter)->force);
2085  (*iter)->body->SetTorque((*iter)->torque);
2086  }
2087  else
2088  (*iter)->duration.fromSec(0.0); // mark for delete
2089  }
2090 
2091  if (simTime > (*iter)->start_time+(*iter)->duration &&
2092  (*iter)->duration.toSec() >= 0.0)
2093  {
2094  // remove from queue once expires
2095  delete (*iter);
2096  iter = wrench_body_jobs_.erase(iter);
2097  }
2098  else
2099  ++iter;
2100  }
2101  lock_.unlock();
2102 }
2103 
2105 {
2106  // MDMutex locks in case model is getting deleted, don't have to do this if we delete jobs first
2107  // boost::recursive_mutex::scoped_lock lock(*world->GetMDMutex());
2108  lock_.lock();
2109  for (std::vector<GazeboRosApiPlugin::ForceJointJob*>::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();)
2110  {
2111  // check times and apply force if necessary
2112 #if GAZEBO_MAJOR_VERSION >= 8
2113  ros::Time simTime = ros::Time(world_->SimTime().Double());
2114 #else
2115  ros::Time simTime = ros::Time(world_->GetSimTime().Double());
2116 #endif
2117  if (simTime >= (*iter)->start_time)
2118  if (simTime <= (*iter)->start_time+(*iter)->duration ||
2119  (*iter)->duration.toSec() < 0.0)
2120  {
2121  if ((*iter)->joint) // if joint exists
2122  (*iter)->joint->SetForce(0,(*iter)->force);
2123  else
2124  (*iter)->duration.fromSec(0.0); // mark for delete
2125  }
2126 
2127  if (simTime > (*iter)->start_time+(*iter)->duration &&
2128  (*iter)->duration.toSec() >= 0.0)
2129  {
2130  // remove from queue once expires
2131  iter = force_joint_jobs_.erase(iter);
2132  }
2133  else
2134  ++iter;
2135  }
2136  lock_.unlock();
2137 }
2138 
2140 {
2141  ROS_ERROR_NAMED("api_plugin", "CLOCK2");
2142 #if GAZEBO_MAJOR_VERSION >= 8
2143  gazebo::common::Time sim_time = world_->SimTime();
2144 #else
2145  gazebo::common::Time sim_time = world_->GetSimTime();
2146 #endif
2147  if (pub_clock_frequency_ > 0 && (sim_time - last_pub_clock_time_).Double() < 1.0/pub_clock_frequency_)
2148  return;
2149 
2150  gazebo::common::Time currentTime = gazebo::msgs::Convert( msg->sim_time() );
2151  rosgraph_msgs::Clock ros_time_;
2152  ros_time_.clock.fromSec(currentTime.Double());
2153  // publish time to ros
2154  last_pub_clock_time_ = sim_time;
2155  pub_clock_.publish(ros_time_);
2156 }
2158 {
2159 #if GAZEBO_MAJOR_VERSION >= 8
2160  gazebo::common::Time sim_time = world_->SimTime();
2161 #else
2162  gazebo::common::Time sim_time = world_->GetSimTime();
2163 #endif
2164  if (pub_clock_frequency_ > 0 && (sim_time - last_pub_clock_time_).Double() < 1.0/pub_clock_frequency_)
2165  return;
2166 
2167 #if GAZEBO_MAJOR_VERSION >= 8
2168  gazebo::common::Time currentTime = world_->SimTime();
2169 #else
2170  gazebo::common::Time currentTime = world_->GetSimTime();
2171 #endif
2172  rosgraph_msgs::Clock ros_time_;
2173  ros_time_.clock.fromSec(currentTime.Double());
2174  // publish time to ros
2175  last_pub_clock_time_ = sim_time;
2176  pub_clock_.publish(ros_time_);
2177 }
2178 
2180 {
2181  gazebo_msgs::LinkStates link_states;
2182 
2183  // fill link_states
2184 #if GAZEBO_MAJOR_VERSION >= 8
2185  for (unsigned int i = 0; i < world_->ModelCount(); i ++)
2186  {
2187  gazebo::physics::ModelPtr model = world_->ModelByIndex(i);
2188 #else
2189  for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
2190  {
2191  gazebo::physics::ModelPtr model = world_->GetModel(i);
2192 #endif
2193 
2194  for (unsigned int j = 0 ; j < model->GetChildCount(); j ++)
2195  {
2196  gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(j));
2197 
2198  if (body)
2199  {
2200  link_states.name.push_back(body->GetScopedName());
2201  geometry_msgs::Pose pose;
2202 #if GAZEBO_MAJOR_VERSION >= 8
2203  ignition::math::Pose3d body_pose = body->WorldPose(); // - myBody->GetCoMPose();
2204  ignition::math::Vector3d linear_vel = body->WorldLinearVel();
2205  ignition::math::Vector3d angular_vel = body->WorldAngularVel();
2206 #else
2207  ignition::math::Pose3d body_pose = body->GetWorldPose().Ign(); // - myBody->GetCoMPose();
2208  ignition::math::Vector3d linear_vel = body->GetWorldLinearVel().Ign();
2209  ignition::math::Vector3d angular_vel = body->GetWorldAngularVel().Ign();
2210 #endif
2211  ignition::math::Vector3d pos = body_pose.Pos();
2212  ignition::math::Quaterniond rot = body_pose.Rot();
2213  pose.position.x = pos.X();
2214  pose.position.y = pos.Y();
2215  pose.position.z = pos.Z();
2216  pose.orientation.w = rot.W();
2217  pose.orientation.x = rot.X();
2218  pose.orientation.y = rot.Y();
2219  pose.orientation.z = rot.Z();
2220  link_states.pose.push_back(pose);
2221  geometry_msgs::Twist twist;
2222  twist.linear.x = linear_vel.X();
2223  twist.linear.y = linear_vel.Y();
2224  twist.linear.z = linear_vel.Z();
2225  twist.angular.x = angular_vel.X();
2226  twist.angular.y = angular_vel.Y();
2227  twist.angular.z = angular_vel.Z();
2228  link_states.twist.push_back(twist);
2229  }
2230  }
2231  }
2232 
2233  pub_link_states_.publish(link_states);
2234 }
2235 
2237 {
2238  gazebo_msgs::ModelStates model_states;
2239 
2240  // fill model_states
2241 #if GAZEBO_MAJOR_VERSION >= 8
2242  for (unsigned int i = 0; i < world_->ModelCount(); i ++)
2243  {
2244  gazebo::physics::ModelPtr model = world_->ModelByIndex(i);
2245  ignition::math::Pose3d model_pose = model->WorldPose(); // - myBody->GetCoMPose();
2246  ignition::math::Vector3d linear_vel = model->WorldLinearVel();
2247  ignition::math::Vector3d angular_vel = model->WorldAngularVel();
2248 #else
2249  for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
2250  {
2251  gazebo::physics::ModelPtr model = world_->GetModel(i);
2252  ignition::math::Pose3d model_pose = model->GetWorldPose().Ign(); // - myBody->GetCoMPose();
2253  ignition::math::Vector3d linear_vel = model->GetWorldLinearVel().Ign();
2254  ignition::math::Vector3d angular_vel = model->GetWorldAngularVel().Ign();
2255 #endif
2256  ignition::math::Vector3d pos = model_pose.Pos();
2257  ignition::math::Quaterniond rot = model_pose.Rot();
2258  geometry_msgs::Pose pose;
2259  pose.position.x = pos.X();
2260  pose.position.y = pos.Y();
2261  pose.position.z = pos.Z();
2262  pose.orientation.w = rot.W();
2263  pose.orientation.x = rot.X();
2264  pose.orientation.y = rot.Y();
2265  pose.orientation.z = rot.Z();
2266  model_states.pose.push_back(pose);
2267  model_states.name.push_back(model->GetName());
2268  geometry_msgs::Twist twist;
2269  twist.linear.x = linear_vel.X();
2270  twist.linear.y = linear_vel.Y();
2271  twist.linear.z = linear_vel.Z();
2272  twist.angular.x = angular_vel.X();
2273  twist.angular.y = angular_vel.Y();
2274  twist.angular.z = angular_vel.Z();
2275  model_states.twist.push_back(twist);
2276  }
2277  pub_model_states_.publish(model_states);
2278 }
2279 
2280 void GazeboRosApiPlugin::physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level)
2281 {
2283  {
2284  gazebo_msgs::GetPhysicsProperties srv;
2286 
2287  config.time_step = srv.response.time_step;
2288  config.max_update_rate = srv.response.max_update_rate;
2289  config.gravity_x = srv.response.gravity.x;
2290  config.gravity_y = srv.response.gravity.y;
2291  config.gravity_z = srv.response.gravity.z;
2292  config.auto_disable_bodies = srv.response.ode_config.auto_disable_bodies;
2293  config.sor_pgs_precon_iters = srv.response.ode_config.sor_pgs_precon_iters;
2294  config.sor_pgs_iters = srv.response.ode_config.sor_pgs_iters;
2295  config.sor_pgs_rms_error_tol = srv.response.ode_config.sor_pgs_rms_error_tol;
2296  config.sor_pgs_w = srv.response.ode_config.sor_pgs_w;
2297  config.contact_surface_layer = srv.response.ode_config.contact_surface_layer;
2298  config.contact_max_correcting_vel = srv.response.ode_config.contact_max_correcting_vel;
2299  config.cfm = srv.response.ode_config.cfm;
2300  config.erp = srv.response.ode_config.erp;
2301  config.max_contacts = srv.response.ode_config.max_contacts;
2303  }
2304  else
2305  {
2306  bool changed = false;
2307  gazebo_msgs::GetPhysicsProperties srv;
2309 
2310  // check for changes
2311  if (config.time_step != srv.response.time_step) changed = true;
2312  if (config.max_update_rate != srv.response.max_update_rate) changed = true;
2313  if (config.gravity_x != srv.response.gravity.x) changed = true;
2314  if (config.gravity_y != srv.response.gravity.y) changed = true;
2315  if (config.gravity_z != srv.response.gravity.z) changed = true;
2316  if (config.auto_disable_bodies != srv.response.ode_config.auto_disable_bodies) changed = true;
2317  if ((uint32_t)config.sor_pgs_precon_iters != srv.response.ode_config.sor_pgs_precon_iters) changed = true;
2318  if ((uint32_t)config.sor_pgs_iters != srv.response.ode_config.sor_pgs_iters) changed = true;
2319  if (config.sor_pgs_rms_error_tol != srv.response.ode_config.sor_pgs_rms_error_tol) changed = true;
2320  if (config.sor_pgs_w != srv.response.ode_config.sor_pgs_w) changed = true;
2321  if (config.contact_surface_layer != srv.response.ode_config.contact_surface_layer) changed = true;
2322  if (config.contact_max_correcting_vel != srv.response.ode_config.contact_max_correcting_vel) changed = true;
2323  if (config.cfm != srv.response.ode_config.cfm) changed = true;
2324  if (config.erp != srv.response.ode_config.erp) changed = true;
2325  if ((uint32_t)config.max_contacts != srv.response.ode_config.max_contacts) changed = true;
2326 
2327  if (changed)
2328  {
2329  // pause simulation if requested
2330  gazebo_msgs::SetPhysicsProperties srv;
2331  srv.request.time_step = config.time_step ;
2332  srv.request.max_update_rate = config.max_update_rate ;
2333  srv.request.gravity.x = config.gravity_x ;
2334  srv.request.gravity.y = config.gravity_y ;
2335  srv.request.gravity.z = config.gravity_z ;
2336  srv.request.ode_config.auto_disable_bodies = config.auto_disable_bodies ;
2337  srv.request.ode_config.sor_pgs_precon_iters = config.sor_pgs_precon_iters ;
2338  srv.request.ode_config.sor_pgs_iters = config.sor_pgs_iters ;
2339  srv.request.ode_config.sor_pgs_rms_error_tol = config.sor_pgs_rms_error_tol ;
2340  srv.request.ode_config.sor_pgs_w = config.sor_pgs_w ;
2341  srv.request.ode_config.contact_surface_layer = config.contact_surface_layer ;
2342  srv.request.ode_config.contact_max_correcting_vel = config.contact_max_correcting_vel ;
2343  srv.request.ode_config.cfm = config.cfm ;
2344  srv.request.ode_config.erp = config.erp ;
2345  srv.request.ode_config.max_contacts = config.max_contacts ;
2347  ROS_INFO_NAMED("api_plugin", "physics dynamics reconfigure update complete");
2348  }
2349  ROS_INFO_NAMED("api_plugin", "physics dynamics reconfigure complete");
2350  }
2351 }
2352 
2354 {
2355  physics_reconfigure_set_client_ = nh_->serviceClient<gazebo_msgs::SetPhysicsProperties>("/gazebo/set_physics_properties");
2356  physics_reconfigure_get_client_ = nh_->serviceClient<gazebo_msgs::GetPhysicsProperties>("/gazebo/get_physics_properties");
2357 
2358  // Wait until the rest of this plugin is loaded and the services are being offered
2360  physics_reconfigure_get_client_.waitForExistence();
2361 
2362  physics_reconfigure_srv_.reset(new dynamic_reconfigure::Server<gazebo_ros::PhysicsConfig>());
2363 
2366 
2367  ROS_INFO_NAMED("api_plugin", "Physics dynamic reconfigure ready.");
2368 }
2369 
2370 void GazeboRosApiPlugin::stripXmlDeclaration(std::string &model_xml)
2371 {
2372  // incoming robot model string is a string containing a Gazebo Model XML
2376  std::string open_bracket("<?");
2377  std::string close_bracket("?>");
2378  size_t pos1 = model_xml.find(open_bracket,0);
2379  size_t pos2 = model_xml.find(close_bracket,0);
2380  if (pos1 != std::string::npos && pos2 != std::string::npos)
2381  model_xml.replace(pos1,pos2-pos1+2,std::string(""));
2382 }
2383 
2384 void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml,
2385  const std::string &model_name,
2386  const ignition::math::Vector3d &initial_xyz,
2387  const ignition::math::Quaterniond &initial_q)
2388 {
2389  // This function can handle both regular SDF files and <include> SDFs that are used with the
2390  // Gazebo Model Database
2391 
2392  TiXmlElement* pose_element; // This is used by both reguar and database SDFs
2393 
2394  // Check SDF for requires SDF element
2395  TiXmlElement* gazebo_tixml = gazebo_model_xml.FirstChildElement("sdf");
2396  if (!gazebo_tixml)
2397  {
2398  ROS_WARN_NAMED("api_plugin", "Could not find <sdf> element in sdf, so name and initial position cannot be applied");
2399  return;
2400  }
2401 
2402  // Check SDF for optional model element. May not have one
2403  TiXmlElement* model_tixml = gazebo_tixml->FirstChildElement("model");
2404  if (model_tixml)
2405  {
2406  // Update entity name
2407  if (model_tixml->Attribute("name") != NULL)
2408  {
2409  // removing old entity name
2410  model_tixml->RemoveAttribute("name");
2411  }
2412  // replace with user specified name
2413  model_tixml->SetAttribute("name",model_name);
2414  }
2415  else
2416  {
2417  // Check SDF for world element
2418  TiXmlElement* world_tixml = gazebo_tixml->FirstChildElement("world");
2419  if (!world_tixml)
2420  {
2421  ROS_WARN_NAMED("api_plugin", "Could not find <model> or <world> element in sdf, so name and initial position cannot be applied");
2422  return;
2423  }
2424  // If not <model> element, check SDF for required include element
2425  model_tixml = world_tixml->FirstChildElement("include");
2426  if (!model_tixml)
2427  {
2428  ROS_WARN_NAMED("api_plugin", "Could not find <include> element in sdf, so name and initial position cannot be applied");
2429  return;
2430  }
2431 
2432  // Check for name element
2433  TiXmlElement* name_tixml = model_tixml->FirstChildElement("name");
2434  if (!name_tixml)
2435  {
2436  // Create the name element
2437  name_tixml = new TiXmlElement("name");
2438  model_tixml->LinkEndChild(name_tixml);
2439  }
2440 
2441  // Set the text within the name element
2442  TiXmlText* text = new TiXmlText(model_name);
2443  name_tixml->LinkEndChild( text );
2444  }
2445 
2446 
2447  // Check for the pose element
2448  pose_element = model_tixml->FirstChildElement("pose");
2449  ignition::math::Pose3d model_pose;
2450 
2451  // Create the pose element if it doesn't exist
2452  // Remove it if it exists, since we are inserting a new one
2453  if (pose_element)
2454  {
2455  // save pose_element in ignition::math::Pose3d and remove child
2456  model_pose = this->parsePose(pose_element->GetText());
2457  model_tixml->RemoveChild(pose_element);
2458  }
2459 
2460  // Set and link the pose element after adding initial pose
2461  {
2462  // add pose_element Pose to initial pose
2463  ignition::math::Pose3d new_model_pose = model_pose + ignition::math::Pose3d(initial_xyz, initial_q);
2464 
2465  // Create the string of 6 numbers
2466  std::ostringstream pose_stream;
2467  ignition::math::Vector3d model_rpy = new_model_pose.Rot().Euler(); // convert to Euler angles for Gazebo XML
2468  pose_stream << new_model_pose.Pos().X() << " " << new_model_pose.Pos().Y() << " " << new_model_pose.Pos().Z() << " "
2469  << model_rpy.X() << " " << model_rpy.Y() << " " << model_rpy.Z();
2470 
2471  // Add value to pose element
2472  TiXmlText* text = new TiXmlText(pose_stream.str());
2473  TiXmlElement* new_pose_element = new TiXmlElement("pose");
2474  new_pose_element->LinkEndChild(text);
2475  model_tixml->LinkEndChild(new_pose_element);
2476  }
2477 }
2478 
2479 ignition::math::Pose3d GazeboRosApiPlugin::parsePose(const std::string &str)
2480 {
2481  std::vector<std::string> pieces;
2482  std::vector<double> vals;
2483 
2484  boost::split(pieces, str, boost::is_any_of(" "));
2485  for (unsigned int i = 0; i < pieces.size(); ++i)
2486  {
2487  if (pieces[i] != "")
2488  {
2489  try
2490  {
2491  vals.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
2492  }
2493  catch(boost::bad_lexical_cast &e)
2494  {
2495  sdferr << "xml key [" << str
2496  << "][" << i << "] value [" << pieces[i]
2497  << "] is not a valid double from a 3-tuple\n";
2498  return ignition::math::Pose3d();
2499  }
2500  }
2501  }
2502 
2503  if (vals.size() == 6)
2504  return ignition::math::Pose3d(vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]);
2505  else
2506  {
2507  ROS_ERROR_NAMED("api_plugin", "Beware: failed to parse string [%s] as ignition::math::Pose3d, returning zeros.", str.c_str());
2508  return ignition::math::Pose3d();
2509  }
2510 }
2511 
2512 ignition::math::Vector3d GazeboRosApiPlugin::parseVector3(const std::string &str)
2513 {
2514  std::vector<std::string> pieces;
2515  std::vector<double> vals;
2516 
2517  boost::split(pieces, str, boost::is_any_of(" "));
2518  for (unsigned int i = 0; i < pieces.size(); ++i)
2519  {
2520  if (pieces[i] != "")
2521  {
2522  try
2523  {
2524  vals.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
2525  }
2526  catch(boost::bad_lexical_cast &e)
2527  {
2528  sdferr << "xml key [" << str
2529  << "][" << i << "] value [" << pieces[i]
2530  << "] is not a valid double from a 3-tuple\n";
2531  return ignition::math::Vector3d();
2532  }
2533  }
2534  }
2535 
2536  if (vals.size() == 3)
2537  return ignition::math::Vector3d(vals[0], vals[1], vals[2]);
2538  else
2539  {
2540  ROS_ERROR_NAMED("api_plugin", "Beware: failed to parse string [%s] as ignition::math::Vector3d, returning zeros.", str.c_str());
2541  return ignition::math::Vector3d();
2542  }
2543 }
2544 
2545 void GazeboRosApiPlugin::updateURDFModelPose(TiXmlDocument &gazebo_model_xml,
2546  const ignition::math::Vector3d &initial_xyz,
2547  const ignition::math::Quaterniond &initial_q)
2548 {
2549  TiXmlElement* model_tixml = (gazebo_model_xml.FirstChildElement("robot"));
2550  if (model_tixml)
2551  {
2552  // replace initial pose of model
2553  // find first instance of xyz and rpy, replace with initial pose
2554  TiXmlElement* origin_key = model_tixml->FirstChildElement("origin");
2555 
2556  if (!origin_key)
2557  {
2558  origin_key = new TiXmlElement("origin");
2559  model_tixml->LinkEndChild(origin_key);
2560  }
2561 
2562  ignition::math::Vector3d xyz;
2563  ignition::math::Vector3d rpy;
2564  if (origin_key->Attribute("xyz"))
2565  {
2566  xyz = this->parseVector3(origin_key->Attribute("xyz"));
2567  origin_key->RemoveAttribute("xyz");
2568  }
2569  if (origin_key->Attribute("rpy"))
2570  {
2571  rpy = this->parseVector3(origin_key->Attribute("rpy"));
2572  origin_key->RemoveAttribute("rpy");
2573  }
2574 
2575  // add xyz, rpy to initial pose
2576  ignition::math::Pose3d model_pose = ignition::math::Pose3d(xyz, ignition::math::Quaterniond(rpy))
2577  + ignition::math::Pose3d(initial_xyz, initial_q);
2578 
2579  std::ostringstream xyz_stream;
2580  xyz_stream << model_pose.Pos().X() << " " << model_pose.Pos().Y() << " " << model_pose.Pos().Z();
2581 
2582  std::ostringstream rpy_stream;
2583  ignition::math::Vector3d model_rpy = model_pose.Rot().Euler(); // convert to Euler angles for Gazebo XML
2584  rpy_stream << model_rpy.X() << " " << model_rpy.Y() << " " << model_rpy.Z();
2585 
2586  origin_key->SetAttribute("xyz",xyz_stream.str());
2587  origin_key->SetAttribute("rpy",rpy_stream.str());
2588  }
2589  else
2590  ROS_WARN_NAMED("api_plugin", "Could not find <model> element in sdf, so name and initial position is not applied");
2591 }
2592 
2593 void GazeboRosApiPlugin::updateURDFName(TiXmlDocument &gazebo_model_xml, const std::string &model_name)
2594 {
2595  TiXmlElement* model_tixml = gazebo_model_xml.FirstChildElement("robot");
2596  // replace model name if one is specified by the user
2597  if (model_tixml)
2598  {
2599  if (model_tixml->Attribute("name") != NULL)
2600  {
2601  // removing old model name
2602  model_tixml->RemoveAttribute("name");
2603  }
2604  // replace with user specified name
2605  model_tixml->SetAttribute("name",model_name);
2606  }
2607  else
2608  ROS_WARN_NAMED("api_plugin", "Could not find <robot> element in URDF, name not replaced");
2609 }
2610 
2612 {
2613  TiXmlNode* child = 0;
2614  child = model_xml->IterateChildren(child);
2615  while (child != NULL)
2616  {
2617  if (child->Type() == TiXmlNode::TINYXML_ELEMENT &&
2618  child->ValueStr().compare(std::string("plugin")) == 0)
2619  {
2620  if (child->FirstChildElement("robotNamespace") == NULL)
2621  {
2622  TiXmlElement* child_elem = child->ToElement()->FirstChildElement("robotNamespace");
2623  while (child_elem)
2624  {
2625  child->ToElement()->RemoveChild(child_elem);
2626  child_elem = child->ToElement()->FirstChildElement("robotNamespace");
2627  }
2628  TiXmlElement* key = new TiXmlElement("robotNamespace");
2629  TiXmlText* val = new TiXmlText(robot_namespace_);
2630  key->LinkEndChild(val);
2631  child->ToElement()->LinkEndChild(key);
2632  }
2633  }
2635  child = model_xml->IterateChildren(child);
2636  }
2637 }
2638 
2639 bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, const std::string &model_name,
2640  gazebo_msgs::SpawnModel::Response &res)
2641 {
2642  std::string entity_type = gazebo_model_xml.RootElement()->FirstChild()->Value();
2643  // Convert the entity type to lower case
2644  std::transform(entity_type.begin(), entity_type.end(), entity_type.begin(), ::tolower);
2645 
2646  bool isLight = (entity_type == "light");
2647 
2648  // push to factory iface
2649  std::ostringstream stream;
2650  stream << gazebo_model_xml;
2651  std::string gazebo_model_xml_string = stream.str();
2652  ROS_DEBUG_NAMED("api_plugin.xml", "Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str());
2653 
2654  // publish to factory topic
2655  gazebo::msgs::Factory msg;
2656  gazebo::msgs::Init(msg, "spawn_model");
2657  msg.set_sdf( gazebo_model_xml_string );
2658 
2659  //ROS_ERROR_NAMED("api_plugin", "attempting to spawn model name [%s] [%s]", model_name.c_str(),gazebo_model_xml_string.c_str());
2660 
2661  // FIXME: should use entity_info or add lock to World::receiveMutex
2662  // looking for Model to see if it exists already
2663  gazebo::msgs::Request *entity_info_msg = gazebo::msgs::CreateRequest("entity_info", model_name);
2664  request_pub_->Publish(*entity_info_msg,true);
2665  delete entity_info_msg;
2666  entity_info_msg = nullptr;
2667  // todo: should wait for response response_sub_, check to see that if _msg->response == "nonexistant"
2668 
2669 #if GAZEBO_MAJOR_VERSION >= 8
2670  gazebo::physics::ModelPtr model = world_->ModelByName(model_name);
2671  gazebo::physics::LightPtr light = world_->LightByName(model_name);
2672 #else
2673  gazebo::physics::ModelPtr model = world_->GetModel(model_name);
2674  gazebo::physics::LightPtr light = world_->Light(model_name);
2675 #endif
2676  if ((isLight && light != NULL) || (model != NULL))
2677  {
2678  ROS_ERROR_NAMED("api_plugin", "SpawnModel: Failure - model name %s already exists.",model_name.c_str());
2679  res.success = false;
2680  res.status_message = "SpawnModel: Failure - entity already exists.";
2681  return true;
2682  }
2683 
2684  // for Gazebo 7 and up, use a different method to spawn lights
2685  if (isLight)
2686  {
2687  // Publish the light message to spawn the light (Gazebo 7 and up)
2688  sdf::SDF sdf_light;
2689  sdf_light.SetFromString(gazebo_model_xml_string);
2690  gazebo::msgs::Light msg = gazebo::msgs::LightFromSDF(sdf_light.Root()->GetElement("light"));
2691  msg.set_name(model_name);
2692  factory_light_pub_->Publish(msg);
2693  }
2694  else
2695  {
2696  // Publish the factory message
2697  factory_pub_->Publish(msg);
2698  }
2701 
2703  ros::Duration model_spawn_timeout(10.0);
2704  ros::Time timeout = ros::Time::now() + model_spawn_timeout;
2705 
2706  while (ros::ok())
2707  {
2708  if (ros::Time::now() > timeout)
2709  {
2710  res.success = false;
2711  res.status_message = "SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name " + model_name;
2712  return true;
2713  }
2714 
2715  {
2716  //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex());
2717 #if GAZEBO_MAJOR_VERSION >= 8
2718  if ((isLight && world_->LightByName(model_name) != NULL)
2719  || (world_->ModelByName(model_name) != NULL))
2720 #else
2721  if ((isLight && world_->Light(model_name) != NULL)
2722  || (world_->GetModel(model_name) != NULL))
2723 #endif
2724  break;
2725  }
2726 
2727  ROS_DEBUG_STREAM_ONCE_NAMED("api_plugin","Waiting for " << timeout - ros::Time::now()
2728  << " for entity " << model_name << " to spawn");
2729 
2730  usleep(2000);
2731  }
2732 
2733  // set result
2734  res.success = true;
2735  res.status_message = "SpawnModel: Successfully spawned entity";
2736  return true;
2737 }
2738 
2739 // Register this plugin with the simulator
2740 GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosApiPlugin)
2741 }
bool getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req, gazebo_msgs::GetLinkProperties::Response &res)
void shutdownSignal()
Detect if sig-int shutdown signal is recieved
#define ROS_DEBUG_ONCE_NAMED(name,...)
ros::ServiceServer reset_simulation_service_
bool getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req, gazebo_msgs::GetPhysicsProperties::Response &res)
ros::ServiceServer apply_body_wrench_service_
bool setJointProperties(gazebo_msgs::SetJointProperties::Request &req, gazebo_msgs::SetJointProperties::Response &res)
void updateURDFModelPose(TiXmlDocument &gazebo_model_xml, const ignition::math::Vector3d &initial_xyz, const ignition::math::Quaterniond &initial_q)
Update the model pose of the URDF file before sending to Gazebo.
bool setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req, gazebo_msgs::SetPhysicsProperties::Response &res)
gazebo::event::ConnectionPtr time_update_event_
bool setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req, gazebo_msgs::SetModelConfiguration::Response &res)
bool getModelState(gazebo_msgs::GetModelState::Request &req, gazebo_msgs::GetModelState::Response &res)
ROSCPP_DECL bool check()
ros::ServiceServer set_physics_properties_service_
void onLinkStatesDisconnect()
Callback for a subscriber disconnecting from LinkStates ros topic.
ros::ServiceServer get_model_properties_service_
#define ROS_INFO_NAMED(name,...)
ros::ServiceServer unpause_physics_service_
#define ROS_DEBUG_STREAM_NAMED(name, args)
bool setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req, gazebo_msgs::SetLinkProperties::Response &res)
boost::shared_ptr< dynamic_reconfigure::Server< gazebo_ros::PhysicsConfig > > physics_reconfigure_srv_
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
void loadGazeboRosApiPlugin(std::string world_name)
Connect to Gazebo via its plugin interface, get a pointer to the world, start events.
gazebo::common::Time last_pub_clock_time_
ros::ServiceServer spawn_urdf_model_service_
ros::ServiceServer get_light_properties_service_
dynamic_reconfigure::Server< gazebo_ros::PhysicsConfig >::CallbackType physics_reconfigure_callback_
bool isURDF(std::string model_xml)
utility for checking if string is in URDF format
boost::shared_ptr< ros::NodeHandle > nh_
void onLinkStatesConnect()
Callback for a subscriber connecting to LinkStates ros topic.
boost::shared_ptr< boost::thread > physics_reconfigure_thread_
ROSCPP_DECL bool isInitialized()
void physicsReconfigureThread()
waits for the rest of Gazebo to be ready before initializing the dynamic reconfigure services ...
bool setLinkState(gazebo_msgs::SetLinkState::Request &req, gazebo_msgs::SetLinkState::Response &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
gazebo::transport::PublisherPtr request_pub_
ros::ServiceServer clear_body_wrenches_service_
ignition::math::Pose3d parsePose(const std::string &str)
convert xml to Pose
void onModelStatesConnect()
Callback for a subscriber connecting to ModelStates ros topic.
ros::ServiceServer get_link_properties_service_
ros::ServiceServer set_model_configuration_service_
void advertiseServices()
advertise services
gazebo::physics::WorldPtr world_
ros::ServiceClient physics_reconfigure_set_client_
ros::ServiceServer delete_light_service_
bool resetSimulation(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ros::ServiceServer delete_model_service_
Time & fromSec(double t)
bool unpausePhysics(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ros::ServiceServer apply_joint_effort_service_
ros::ServiceServer clear_joint_forces_service_
std::vector< GazeboRosApiPlugin::ForceJointJob * > force_joint_jobs_
std::map< std::string, unsigned int > access_count_get_model_state_
index counters to count the accesses on models via GetModelState
#define ROS_DEBUG_NAMED(name,...)
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
void transformWrench(ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque, const ignition::math::Vector3d &reference_force, const ignition::math::Vector3d &reference_torque, const ignition::math::Pose3d &target_to_reference)
helper function for applyBodyWrench shift wrench from reference frame to target frame ...
ros::ServiceServer set_model_state_service_
ros::ServiceClient physics_reconfigure_get_client_
ros::ServiceServer reset_world_service_
bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req, gazebo_msgs::ApplyJointEffort::Response &res)
void updateSDFAttributes(TiXmlDocument &gazebo_model_xml, const std::string &model_name, const ignition::math::Vector3d &initial_xyz, const ignition::math::Quaterniond &initial_q)
Update the model name and pose of the SDF file before sending to Gazebo.
bool getJointProperties(gazebo_msgs::GetJointProperties::Request &req, gazebo_msgs::GetJointProperties::Response &res)
ros::ServiceServer get_joint_properties_service_
bool getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req, gazebo_msgs::GetWorldProperties::Response &res)
void updateModelState(const gazebo_msgs::ModelState::ConstPtr &model_state)
gazebo::event::ConnectionPtr sigint_event_
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
gazebo::event::ConnectionPtr pub_model_states_event_
bool deleteLight(gazebo_msgs::DeleteLight::Request &req, gazebo_msgs::DeleteLight::Response &res)
delete a given light by name
gazebo::event::ConnectionPtr wrench_update_event_
void Load(int argc, char **argv)
Gazebo-inherited load function.
gazebo::transport::PublisherPtr factory_light_pub_
bool pausePhysics(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ros::ServiceServer set_link_properties_service_
ros::ServiceServer set_light_properties_service_
bool clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req, gazebo_msgs::BodyRequest::Response &res)
gazebo::transport::SubscriberPtr response_sub_
ROSCPP_DECL bool ok()
bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req, gazebo_msgs::GetModelProperties::Response &res)
boost::shared_ptr< boost::thread > gazebo_callback_queue_thread_
gazebo::event::ConnectionPtr force_update_event_
bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req, gazebo_msgs::SpawnModel::Response &res)
Function for inserting a URDF into Gazebo from ROS Service Call.
void walkChildAddRobotNamespace(TiXmlNode *model_xml)
bool resetWorld(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
#define ROS_DEBUG_STREAM_ONCE_NAMED(name, args)
bool isSDF(std::string model_xml)
utility for checking if string is in SDF format
ROSLIB_DECL std::string getPath(const std::string &package_name)
gazebo::transport::SubscriberPtr performance_metric_sub_
boost::shared_ptr< ros::AsyncSpinner > async_ros_spin_
A plugin loaded within the gzserver on startup.
bool applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req, gazebo_msgs::ApplyBodyWrench::Response &res)
ros::ServiceServer get_model_state_service_
void onModelStatesDisconnect()
Callback for a subscriber disconnecting from ModelStates ros topic.
void physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level)
Used for the dynamic reconfigure callback function template.
bool clearJointForces(gazebo_msgs::JointRequest::Request &req, gazebo_msgs::JointRequest::Response &res)
ros::ServiceServer set_joint_properties_service_
gazebo::transport::PublisherPtr factory_pub_
gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_
bool spawnAndConform(TiXmlDocument &gazebo_model_xml, const std::string &model_name, gazebo_msgs::SpawnModel::Response &res)
ros::ServiceServer spawn_sdf_model_service_
ignition::math::Vector3d parseVector3(const std::string &str)
convert xml to Pose
bool getLightProperties(gazebo_msgs::GetLightProperties::Request &req, gazebo_msgs::GetLightProperties::Response &res)
ros::ServiceServer get_world_properties_service_
ros::ServiceServer set_link_state_service_
std::vector< GazeboRosApiPlugin::WrenchBodyJob * > wrench_body_jobs_
gazebo::event::ConnectionPtr pub_link_states_event_
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
static Time now()
ros::ServiceServer get_link_state_service_
void updateLinkState(const gazebo_msgs::LinkState::ConstPtr &link_state)
gazebo::transport::NodePtr gazebonode_
void gazeboQueueThread()
ros queue thread for this node
bool setLightProperties(gazebo_msgs::SetLightProperties::Request &req, gazebo_msgs::SetLightProperties::Response &res)
bool deleteModel(gazebo_msgs::DeleteModel::Request &req, gazebo_msgs::DeleteModel::Response &res)
delete model given name
bool spawnSDFModel(gazebo_msgs::SpawnModel::Request &req, gazebo_msgs::SpawnModel::Response &res)
Both SDFs and converted URDFs get sent to this function for further manipulation from a ROS Service c...
void onResponse(ConstResponsePtr &response)
Unused.
void stripXmlDeclaration(std::string &model_xml)
ROSCPP_DECL bool search(const std::string &ns, const std::string &key, std::string &result)
ros::ServiceServer pause_physics_service_
boost::shared_ptr< void > VoidPtr
bool getLinkState(gazebo_msgs::GetLinkState::Request &req, gazebo_msgs::GetLinkState::Response &res)
void updateURDFName(TiXmlDocument &gazebo_model_xml, const std::string &model_name)
Update the model name of the URDF file before sending to Gazebo.
ros::ServiceServer get_physics_properties_service_
bool setModelState(gazebo_msgs::SetModelState::Request &req, gazebo_msgs::SetModelState::Response &res)
gazebo::transport::PublisherPtr light_modify_pub_
#define ROS_WARN_STREAM_NAMED(name, args)


gazebo_ros
Author(s): John Hsu, Nate Koenig, Dave Coleman
autogenerated on Tue Apr 6 2021 02:19:48