gazebo_ros_api_plugin.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /* Desc: External interfaces for Gazebo
00019  * Author: Nate Koenig, John Hsu, Dave Coleman
00020  * Date: Jun 10 2013
00021  */
00022 
00023 #include <gazebo/common/Events.hh>
00024 #include <gazebo/gazebo_config.h>
00025 #include "gazebo_ros_api_plugin.h"
00026 
00027 namespace gazebo
00028 {
00029 
00030 GazeboRosApiPlugin::GazeboRosApiPlugin() :
00031   physics_reconfigure_initialized_(false),
00032   world_created_(false),
00033   stop_(false),
00034   plugin_loaded_(false)
00035 {
00036   robot_namespace_.clear();
00037 }
00038 
00039 GazeboRosApiPlugin::~GazeboRosApiPlugin()
00040 {
00041   ROS_DEBUG_STREAM_NAMED("api_plugin","GazeboRosApiPlugin Deconstructor start");
00042 
00043   // Unload the sigint event
00044   gazebo::event::Events::DisconnectSigInt(sigint_event_);
00045   ROS_DEBUG_STREAM_NAMED("api_plugin","After sigint_event unload");
00046 
00047   // Don't attempt to unload this plugin if it was never loaded in the Load() function
00048   if(!plugin_loaded_)
00049   {
00050     ROS_DEBUG_STREAM_NAMED("api_plugin","Deconstructor skipped because never loaded");
00051     return;
00052   }
00053 
00054   // Disconnect slots
00055   gazebo::event::Events::DisconnectWorldUpdateBegin(wrench_update_event_);
00056   gazebo::event::Events::DisconnectWorldUpdateBegin(force_update_event_);
00057   gazebo::event::Events::DisconnectWorldUpdateBegin(time_update_event_);
00058   ROS_DEBUG_STREAM_NAMED("api_plugin","Slots disconnected");
00059 
00060   if (pub_link_states_connection_count_ > 0) // disconnect if there are subscribers on exit
00061     gazebo::event::Events::DisconnectWorldUpdateBegin(pub_link_states_event_);
00062   if (pub_model_states_connection_count_ > 0) // disconnect if there are subscribers on exit
00063     gazebo::event::Events::DisconnectWorldUpdateBegin(pub_model_states_event_);
00064   ROS_DEBUG_STREAM_NAMED("api_plugin","Disconnected World Updates");
00065 
00066   // Stop the multi threaded ROS spinner
00067   async_ros_spin_->stop();
00068   ROS_DEBUG_STREAM_NAMED("api_plugin","Async ROS Spin Stopped");
00069 
00070   // Shutdown the ROS node
00071   nh_->shutdown();
00072   ROS_DEBUG_STREAM_NAMED("api_plugin","Node Handle Shutdown");
00073 
00074   // Shutdown ROS queue
00075   gazebo_callback_queue_thread_->join();
00076   ROS_DEBUG_STREAM_NAMED("api_plugin","Callback Queue Joined");
00077 
00078   // Physics Dynamic Reconfigure
00079   physics_reconfigure_thread_->join();
00080   ROS_DEBUG_STREAM_NAMED("api_plugin","Physics reconfigure joined");
00081 
00082   // Delete Force and Wrench Jobs
00083   lock_.lock();
00084   for (std::vector<GazeboRosApiPlugin::ForceJointJob*>::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();)
00085     delete (*iter);
00086   force_joint_jobs_.clear();
00087   ROS_DEBUG_STREAM_NAMED("api_plugin","ForceJointJobs deleted");
00088   for (std::vector<GazeboRosApiPlugin::WrenchBodyJob*>::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();)
00089     delete (*iter);
00090   wrench_body_jobs_.clear();
00091   lock_.unlock();
00092   ROS_DEBUG_STREAM_NAMED("api_plugin","WrenchBodyJobs deleted");
00093 
00094   ROS_DEBUG_STREAM_NAMED("api_plugin","Unloaded");
00095 }
00096 
00097 void GazeboRosApiPlugin::shutdownSignal()
00098 {
00099   ROS_DEBUG_STREAM_NAMED("api_plugin","shutdownSignal() recieved");
00100   stop_ = true;
00101 }
00102 
00103 void GazeboRosApiPlugin::Load(int argc, char** argv)
00104 {
00105   ROS_DEBUG_STREAM_NAMED("api_plugin","Load");
00106 
00107   // connect to sigint event
00108   sigint_event_ = gazebo::event::Events::ConnectSigInt(boost::bind(&GazeboRosApiPlugin::shutdownSignal,this));
00109 
00110   // setup ros related
00111   if (!ros::isInitialized())
00112     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler);
00113   else
00114     ROS_ERROR("Something other than this gazebo_ros_api plugin started ros::init(...), command line arguments may not be parsed properly.");
00115 
00116   // check if the ros master is available - required
00117   while(!ros::master::check())
00118   {
00119     ROS_WARN_STREAM_NAMED("api_plugin","No ROS master - start roscore to continue...");
00120     // wait 0.5 second
00121     usleep(500*1000); // can't use ROS Time here b/c node handle is not yet initialized
00122 
00123     if(stop_)
00124     {
00125       ROS_WARN_STREAM_NAMED("api_plugin","Canceled loading Gazebo ROS API plugin by sigint event");
00126       return;
00127     }
00128   }
00129 
00130   nh_.reset(new ros::NodeHandle("~")); // advertise topics and services in this node's namespace
00131 
00132   // Built-in multi-threaded ROS spinning
00133   async_ros_spin_.reset(new ros::AsyncSpinner(0)); // will use a thread for each CPU core
00134   async_ros_spin_->start();
00135 
00137   gazebo_callback_queue_thread_.reset(new boost::thread( &GazeboRosApiPlugin::gazeboQueueThread, this) );
00138 
00140   physics_reconfigure_thread_.reset(new boost::thread(boost::bind(&GazeboRosApiPlugin::physicsReconfigureThread, this)));
00141 
00142   // below needs the world to be created first
00143   load_gazebo_ros_api_plugin_event_ = gazebo::event::Events::ConnectWorldCreated(boost::bind(&GazeboRosApiPlugin::loadGazeboRosApiPlugin,this,_1));
00144 
00145   plugin_loaded_ = true;
00146   ROS_INFO("Finished loading Gazebo ROS API Plugin.");
00147 }
00148 
00149 void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
00150 {
00151   // make sure things are only called once
00152   gazebo::event::Events::DisconnectWorldCreated(load_gazebo_ros_api_plugin_event_);
00153   lock_.lock();
00154   if (world_created_)
00155   {
00156     lock_.unlock();
00157     return;
00158   }
00159 
00160   // set flag to true and load this plugin
00161   world_created_ = true;
00162   lock_.unlock();
00163 
00164   world_ = gazebo::physics::get_world(world_name);
00165   if (!world_)
00166   {
00167     //ROS_ERROR("world name: [%s]",world->GetName().c_str());
00168     // connect helper function to signal for scheduling torque/forces, etc
00169     ROS_FATAL("cannot load gazebo ros api server plugin, physics::get_world() fails to return world");
00170     return;
00171   }
00172 
00173   gazebonode_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
00174   gazebonode_->Init(world_name);
00175   //stat_sub_ = gazebonode_->Subscribe("~/world_stats", &GazeboRosApiPlugin::publishSimTime, this); // TODO: does not work in server plugin?
00176   factory_pub_ = gazebonode_->Advertise<gazebo::msgs::Factory>("~/factory");
00177   request_pub_ = gazebonode_->Advertise<gazebo::msgs::Request>("~/request");
00178   response_sub_ = gazebonode_->Subscribe("~/response",&GazeboRosApiPlugin::onResponse, this);
00179 
00180   // reset topic connection counts
00181   pub_link_states_connection_count_ = 0;
00182   pub_model_states_connection_count_ = 0;
00183 
00185   advertiseServices();
00186 
00187   // hooks for applying forces, publishing simtime on /clock
00188   wrench_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::wrenchBodySchedulerSlot,this));
00189   force_update_event_  = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::forceJointSchedulerSlot,this));
00190   time_update_event_   = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishSimTime,this));
00191 }
00192 
00193 void GazeboRosApiPlugin::onResponse(ConstResponsePtr &response)
00194 {
00195 
00196 }
00197 
00198 void GazeboRosApiPlugin::gazeboQueueThread()
00199 {
00200   static const double timeout = 0.001;
00201   while (nh_->ok())
00202   {
00203     gazebo_queue_.callAvailable(ros::WallDuration(timeout));
00204   }
00205 }
00206 
00207 void GazeboRosApiPlugin::advertiseServices()
00208 {
00209   // publish clock for simulated ros time
00210   pub_clock_ = nh_->advertise<rosgraph_msgs::Clock>("/clock",10);
00211 
00212   // Advertise spawn services on the custom queue - DEPRECATED IN HYDRO
00213   std::string spawn_gazebo_model_service_name("spawn_gazebo_model");
00214   ros::AdvertiseServiceOptions spawn_gazebo_model_aso =
00215     ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
00216                                                                   spawn_gazebo_model_service_name,
00217                                                                   boost::bind(&GazeboRosApiPlugin::spawnGazeboModel,this,_1,_2),
00218                                                                   ros::VoidPtr(), &gazebo_queue_);
00219   spawn_gazebo_model_service_ = nh_->advertiseService(spawn_gazebo_model_aso);
00220 
00221   // Advertise spawn services on the custom queue
00222   std::string spawn_sdf_model_service_name("spawn_sdf_model");
00223   ros::AdvertiseServiceOptions spawn_sdf_model_aso =
00224     ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
00225                                                                   spawn_sdf_model_service_name,
00226                                                                   boost::bind(&GazeboRosApiPlugin::spawnSDFModel,this,_1,_2),
00227                                                                   ros::VoidPtr(), &gazebo_queue_);
00228   spawn_sdf_model_service_ = nh_->advertiseService(spawn_sdf_model_aso);
00229 
00230   // Advertise spawn services on the custom queue
00231   std::string spawn_urdf_model_service_name("spawn_urdf_model");
00232   ros::AdvertiseServiceOptions spawn_urdf_model_aso =
00233     ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
00234                                                                   spawn_urdf_model_service_name,
00235                                                                   boost::bind(&GazeboRosApiPlugin::spawnURDFModel,this,_1,_2),
00236                                                                   ros::VoidPtr(), &gazebo_queue_);
00237   spawn_urdf_model_service_ = nh_->advertiseService(spawn_urdf_model_aso);
00238 
00239   // Advertise delete services on the custom queue
00240   std::string delete_model_service_name("delete_model");
00241   ros::AdvertiseServiceOptions delete_aso =
00242     ros::AdvertiseServiceOptions::create<gazebo_msgs::DeleteModel>(
00243                                                                    delete_model_service_name,
00244                                                                    boost::bind(&GazeboRosApiPlugin::deleteModel,this,_1,_2),
00245                                                                    ros::VoidPtr(), &gazebo_queue_);
00246   delete_model_service_ = nh_->advertiseService(delete_aso);
00247 
00248   // Advertise more services on the custom queue
00249   std::string get_model_properties_service_name("get_model_properties");
00250   ros::AdvertiseServiceOptions get_model_properties_aso =
00251     ros::AdvertiseServiceOptions::create<gazebo_msgs::GetModelProperties>(
00252                                                                           get_model_properties_service_name,
00253                                                                           boost::bind(&GazeboRosApiPlugin::getModelProperties,this,_1,_2),
00254                                                                           ros::VoidPtr(), &gazebo_queue_);
00255   get_model_properties_service_ = nh_->advertiseService(get_model_properties_aso);
00256 
00257   // Advertise more services on the custom queue
00258   std::string get_model_state_service_name("get_model_state");
00259   ros::AdvertiseServiceOptions get_model_state_aso =
00260     ros::AdvertiseServiceOptions::create<gazebo_msgs::GetModelState>(
00261                                                                      get_model_state_service_name,
00262                                                                      boost::bind(&GazeboRosApiPlugin::getModelState,this,_1,_2),
00263                                                                      ros::VoidPtr(), &gazebo_queue_);
00264   get_model_state_service_ = nh_->advertiseService(get_model_state_aso);
00265 
00266   // Advertise more services on the custom queue
00267   std::string get_world_properties_service_name("get_world_properties");
00268   ros::AdvertiseServiceOptions get_world_properties_aso =
00269     ros::AdvertiseServiceOptions::create<gazebo_msgs::GetWorldProperties>(
00270                                                                           get_world_properties_service_name,
00271                                                                           boost::bind(&GazeboRosApiPlugin::getWorldProperties,this,_1,_2),
00272                                                                           ros::VoidPtr(), &gazebo_queue_);
00273   get_world_properties_service_ = nh_->advertiseService(get_world_properties_aso);
00274 
00275   // Advertise more services on the custom queue
00276   std::string get_joint_properties_service_name("get_joint_properties");
00277   ros::AdvertiseServiceOptions get_joint_properties_aso =
00278     ros::AdvertiseServiceOptions::create<gazebo_msgs::GetJointProperties>(
00279                                                                           get_joint_properties_service_name,
00280                                                                           boost::bind(&GazeboRosApiPlugin::getJointProperties,this,_1,_2),
00281                                                                           ros::VoidPtr(), &gazebo_queue_);
00282   get_joint_properties_service_ = nh_->advertiseService(get_joint_properties_aso);
00283 
00284   // Advertise more services on the custom queue
00285   std::string get_link_properties_service_name("get_link_properties");
00286   ros::AdvertiseServiceOptions get_link_properties_aso =
00287     ros::AdvertiseServiceOptions::create<gazebo_msgs::GetLinkProperties>(
00288                                                                          get_link_properties_service_name,
00289                                                                          boost::bind(&GazeboRosApiPlugin::getLinkProperties,this,_1,_2),
00290                                                                          ros::VoidPtr(), &gazebo_queue_);
00291   get_link_properties_service_ = nh_->advertiseService(get_link_properties_aso);
00292 
00293   // Advertise more services on the custom queue
00294   std::string get_link_state_service_name("get_link_state");
00295   ros::AdvertiseServiceOptions get_link_state_aso =
00296     ros::AdvertiseServiceOptions::create<gazebo_msgs::GetLinkState>(
00297                                                                     get_link_state_service_name,
00298                                                                     boost::bind(&GazeboRosApiPlugin::getLinkState,this,_1,_2),
00299                                                                     ros::VoidPtr(), &gazebo_queue_);
00300   get_link_state_service_ = nh_->advertiseService(get_link_state_aso);
00301 
00302   // Advertise more services on the custom queue
00303   std::string get_physics_properties_service_name("get_physics_properties");
00304   ros::AdvertiseServiceOptions get_physics_properties_aso =
00305     ros::AdvertiseServiceOptions::create<gazebo_msgs::GetPhysicsProperties>(
00306                                                                             get_physics_properties_service_name,
00307                                                                             boost::bind(&GazeboRosApiPlugin::getPhysicsProperties,this,_1,_2),
00308                                                                             ros::VoidPtr(), &gazebo_queue_);
00309   get_physics_properties_service_ = nh_->advertiseService(get_physics_properties_aso);
00310 
00311   // publish complete link states in world frame
00312   ros::AdvertiseOptions pub_link_states_ao =
00313     ros::AdvertiseOptions::create<gazebo_msgs::LinkStates>(
00314                                                            "link_states",10,
00315                                                            boost::bind(&GazeboRosApiPlugin::onLinkStatesConnect,this),
00316                                                            boost::bind(&GazeboRosApiPlugin::onLinkStatesDisconnect,this),
00317                                                            ros::VoidPtr(), &gazebo_queue_);
00318   pub_link_states_ = nh_->advertise(pub_link_states_ao);
00319 
00320   // publish complete model states in world frame
00321   ros::AdvertiseOptions pub_model_states_ao =
00322     ros::AdvertiseOptions::create<gazebo_msgs::ModelStates>(
00323                                                             "model_states",10,
00324                                                             boost::bind(&GazeboRosApiPlugin::onModelStatesConnect,this),
00325                                                             boost::bind(&GazeboRosApiPlugin::onModelStatesDisconnect,this),
00326                                                             ros::VoidPtr(), &gazebo_queue_);
00327   pub_model_states_ = nh_->advertise(pub_model_states_ao);
00328 
00329   // Advertise more services on the custom queue
00330   std::string set_link_properties_service_name("set_link_properties");
00331   ros::AdvertiseServiceOptions set_link_properties_aso =
00332     ros::AdvertiseServiceOptions::create<gazebo_msgs::SetLinkProperties>(
00333                                                                          set_link_properties_service_name,
00334                                                                          boost::bind(&GazeboRosApiPlugin::setLinkProperties,this,_1,_2),
00335                                                                          ros::VoidPtr(), &gazebo_queue_);
00336   set_link_properties_service_ = nh_->advertiseService(set_link_properties_aso);
00337 
00338   // Advertise more services on the custom queue
00339   std::string set_physics_properties_service_name("set_physics_properties");
00340   ros::AdvertiseServiceOptions set_physics_properties_aso =
00341     ros::AdvertiseServiceOptions::create<gazebo_msgs::SetPhysicsProperties>(
00342                                                                             set_physics_properties_service_name,
00343                                                                             boost::bind(&GazeboRosApiPlugin::setPhysicsProperties,this,_1,_2),
00344                                                                             ros::VoidPtr(), &gazebo_queue_);
00345   set_physics_properties_service_ = nh_->advertiseService(set_physics_properties_aso);
00346 
00347   // Advertise more services on the custom queue
00348   std::string set_model_state_service_name("set_model_state");
00349   ros::AdvertiseServiceOptions set_model_state_aso =
00350     ros::AdvertiseServiceOptions::create<gazebo_msgs::SetModelState>(
00351                                                                      set_model_state_service_name,
00352                                                                      boost::bind(&GazeboRosApiPlugin::setModelState,this,_1,_2),
00353                                                                      ros::VoidPtr(), &gazebo_queue_);
00354   set_model_state_service_ = nh_->advertiseService(set_model_state_aso);
00355 
00356   // Advertise more services on the custom queue
00357   std::string set_model_configuration_service_name("set_model_configuration");
00358   ros::AdvertiseServiceOptions set_model_configuration_aso =
00359     ros::AdvertiseServiceOptions::create<gazebo_msgs::SetModelConfiguration>(
00360                                                                              set_model_configuration_service_name,
00361                                                                              boost::bind(&GazeboRosApiPlugin::setModelConfiguration,this,_1,_2),
00362                                                                              ros::VoidPtr(), &gazebo_queue_);
00363   set_model_configuration_service_ = nh_->advertiseService(set_model_configuration_aso);
00364 
00365   // Advertise more services on the custom queue
00366   std::string set_joint_properties_service_name("set_joint_properties");
00367   ros::AdvertiseServiceOptions set_joint_properties_aso =
00368     ros::AdvertiseServiceOptions::create<gazebo_msgs::SetJointProperties>(
00369                                                                           set_joint_properties_service_name,
00370                                                                           boost::bind(&GazeboRosApiPlugin::setJointProperties,this,_1,_2),
00371                                                                           ros::VoidPtr(), &gazebo_queue_);
00372   set_joint_properties_service_ = nh_->advertiseService(set_joint_properties_aso);
00373 
00374   // Advertise more services on the custom queue
00375   std::string set_link_state_service_name("set_link_state");
00376   ros::AdvertiseServiceOptions set_link_state_aso =
00377     ros::AdvertiseServiceOptions::create<gazebo_msgs::SetLinkState>(
00378                                                                     set_link_state_service_name,
00379                                                                     boost::bind(&GazeboRosApiPlugin::setLinkState,this,_1,_2),
00380                                                                     ros::VoidPtr(), &gazebo_queue_);
00381   set_link_state_service_ = nh_->advertiseService(set_link_state_aso);
00382 
00383   // Advertise topic on custom queue
00384   // topic callback version for set_link_state
00385   ros::SubscribeOptions link_state_so =
00386     ros::SubscribeOptions::create<gazebo_msgs::LinkState>(
00387                                                           "set_link_state",10,
00388                                                           boost::bind( &GazeboRosApiPlugin::updateLinkState,this,_1),
00389                                                           ros::VoidPtr(), &gazebo_queue_);
00390   set_link_state_topic_ = nh_->subscribe(link_state_so);
00391 
00392   // topic callback version for set_model_state
00393   ros::SubscribeOptions model_state_so =
00394     ros::SubscribeOptions::create<gazebo_msgs::ModelState>(
00395                                                            "set_model_state",10,
00396                                                            boost::bind( &GazeboRosApiPlugin::updateModelState,this,_1),
00397                                                            ros::VoidPtr(), &gazebo_queue_);
00398   set_model_state_topic_ = nh_->subscribe(model_state_so);
00399 
00400   // Advertise more services on the custom queue
00401   std::string pause_physics_service_name("pause_physics");
00402   ros::AdvertiseServiceOptions pause_physics_aso =
00403     ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00404                                                           pause_physics_service_name,
00405                                                           boost::bind(&GazeboRosApiPlugin::pausePhysics,this,_1,_2),
00406                                                           ros::VoidPtr(), &gazebo_queue_);
00407   pause_physics_service_ = nh_->advertiseService(pause_physics_aso);
00408 
00409   // Advertise more services on the custom queue
00410   std::string unpause_physics_service_name("unpause_physics");
00411   ros::AdvertiseServiceOptions unpause_physics_aso =
00412     ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00413                                                           unpause_physics_service_name,
00414                                                           boost::bind(&GazeboRosApiPlugin::unpausePhysics,this,_1,_2),
00415                                                           ros::VoidPtr(), &gazebo_queue_);
00416   unpause_physics_service_ = nh_->advertiseService(unpause_physics_aso);
00417 
00418   // Advertise more services on the custom queue
00419   std::string apply_body_wrench_service_name("apply_body_wrench");
00420   ros::AdvertiseServiceOptions apply_body_wrench_aso =
00421     ros::AdvertiseServiceOptions::create<gazebo_msgs::ApplyBodyWrench>(
00422                                                                        apply_body_wrench_service_name,
00423                                                                        boost::bind(&GazeboRosApiPlugin::applyBodyWrench,this,_1,_2),
00424                                                                        ros::VoidPtr(), &gazebo_queue_);
00425   apply_body_wrench_service_ = nh_->advertiseService(apply_body_wrench_aso);
00426 
00427   // Advertise more services on the custom queue
00428   std::string apply_joint_effort_service_name("apply_joint_effort");
00429   ros::AdvertiseServiceOptions apply_joint_effort_aso =
00430     ros::AdvertiseServiceOptions::create<gazebo_msgs::ApplyJointEffort>(
00431                                                                         apply_joint_effort_service_name,
00432                                                                         boost::bind(&GazeboRosApiPlugin::applyJointEffort,this,_1,_2),
00433                                                                         ros::VoidPtr(), &gazebo_queue_);
00434   apply_joint_effort_service_ = nh_->advertiseService(apply_joint_effort_aso);
00435 
00436   // Advertise more services on the custom queue
00437   std::string clear_joint_forces_service_name("clear_joint_forces");
00438   ros::AdvertiseServiceOptions clear_joint_forces_aso =
00439     ros::AdvertiseServiceOptions::create<gazebo_msgs::JointRequest>(
00440                                                                     clear_joint_forces_service_name,
00441                                                                     boost::bind(&GazeboRosApiPlugin::clearJointForces,this,_1,_2),
00442                                                                     ros::VoidPtr(), &gazebo_queue_);
00443   clear_joint_forces_service_ = nh_->advertiseService(clear_joint_forces_aso);
00444 
00445   // Advertise more services on the custom queue
00446   std::string clear_body_wrenches_service_name("clear_body_wrenches");
00447   ros::AdvertiseServiceOptions clear_body_wrenches_aso =
00448     ros::AdvertiseServiceOptions::create<gazebo_msgs::BodyRequest>(
00449                                                                    clear_body_wrenches_service_name,
00450                                                                    boost::bind(&GazeboRosApiPlugin::clearBodyWrenches,this,_1,_2),
00451                                                                    ros::VoidPtr(), &gazebo_queue_);
00452   clear_body_wrenches_service_ = nh_->advertiseService(clear_body_wrenches_aso);
00453 
00454   // Advertise more services on the custom queue
00455   std::string reset_simulation_service_name("reset_simulation");
00456   ros::AdvertiseServiceOptions reset_simulation_aso =
00457     ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00458                                                           reset_simulation_service_name,
00459                                                           boost::bind(&GazeboRosApiPlugin::resetSimulation,this,_1,_2),
00460                                                           ros::VoidPtr(), &gazebo_queue_);
00461   reset_simulation_service_ = nh_->advertiseService(reset_simulation_aso);
00462 
00463   // Advertise more services on the custom queue
00464   std::string reset_world_service_name("reset_world");
00465   ros::AdvertiseServiceOptions reset_world_aso =
00466     ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00467                                                           reset_world_service_name,
00468                                                           boost::bind(&GazeboRosApiPlugin::resetWorld,this,_1,_2),
00469                                                           ros::VoidPtr(), &gazebo_queue_);
00470   reset_world_service_ = nh_->advertiseService(reset_world_aso);
00471 
00472 
00473   // set param for use_sim_time if not set by user already
00474   nh_->setParam("/use_sim_time", true);
00475 
00476   // todo: contemplate setting environment variable ROBOT=sim here???
00477 
00478 }
00479 
00480 void GazeboRosApiPlugin::onLinkStatesConnect()
00481 {
00482   pub_link_states_connection_count_++;
00483   if (pub_link_states_connection_count_ == 1) // connect on first subscriber
00484     pub_link_states_event_   = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishLinkStates,this));
00485 }
00486 
00487 void GazeboRosApiPlugin::onModelStatesConnect()
00488 {
00489   pub_model_states_connection_count_++;
00490   if (pub_model_states_connection_count_ == 1) // connect on first subscriber
00491     pub_model_states_event_   = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishModelStates,this));
00492 }
00493 
00494 void GazeboRosApiPlugin::onLinkStatesDisconnect()
00495 {
00496   pub_link_states_connection_count_--;
00497   if (pub_link_states_connection_count_ <= 0) // disconnect with no subscribers
00498   {
00499     gazebo::event::Events::DisconnectWorldUpdateBegin(pub_link_states_event_);
00500     if (pub_link_states_connection_count_ < 0) // should not be possible
00501       ROS_ERROR("one too mandy disconnect from pub_link_states_ in gazebo_ros.cpp? something weird");
00502   }
00503 }
00504 
00505 void GazeboRosApiPlugin::onModelStatesDisconnect()
00506 {
00507   pub_model_states_connection_count_--;
00508   if (pub_model_states_connection_count_ <= 0) // disconnect with no subscribers
00509   {
00510     gazebo::event::Events::DisconnectWorldUpdateBegin(pub_model_states_event_);
00511     if (pub_model_states_connection_count_ < 0) // should not be possible
00512       ROS_ERROR("one too mandy disconnect from pub_model_states_ in gazebo_ros.cpp? something weird");
00513   }
00514 }
00515 
00516 bool GazeboRosApiPlugin::spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,
00517                                         gazebo_msgs::SpawnModel::Response &res)
00518 {
00519   // get name space for the corresponding model plugins
00520   robot_namespace_ = req.robot_namespace;
00521 
00522   // incoming robot name
00523   std::string model_name = req.model_name;
00524 
00525   // incoming robot model string
00526   std::string model_xml = req.model_xml;
00527 
00528   if (!isURDF(model_xml))
00529   {
00530     ROS_ERROR("SpawnModel: Failure - model format is not URDF.");
00531     res.success = false;
00532     res.status_message = "SpawnModel: Failure - model format is not URDF.";
00533     return false;
00534   }
00535 
00539   {
00540     std::string open_bracket("<?");
00541     std::string close_bracket("?>");
00542     size_t pos1 = model_xml.find(open_bracket,0);
00543     size_t pos2 = model_xml.find(close_bracket,0);
00544     if (pos1 != std::string::npos && pos2 != std::string::npos)
00545       model_xml.replace(pos1,pos2-pos1+2,std::string(""));
00546   }
00547 
00548   // Now, replace package://xxx with the full path to the package
00549   {
00550     std::string package_prefix("package://");
00551     size_t pos1 = model_xml.find(package_prefix,0);
00552     while (pos1 != std::string::npos)
00553     {
00554       size_t pos2 = model_xml.find("/", pos1+10);
00555       //ROS_DEBUG(" pos %d %d",(int)pos1, (int)pos2);
00556       if (pos2 == std::string::npos || pos1 >= pos2)
00557       {
00558         ROS_ERROR("malformed package name?");
00559         break;
00560       }
00561 
00562       std::string package_name = model_xml.substr(pos1+10,pos2-pos1-10);
00563       //ROS_DEBUG("package name [%s]", package_name.c_str());
00564       std::string package_path = ros::package::getPath(package_name);
00565       if (package_path.empty())
00566       {
00567         ROS_FATAL("Package[%s] does not have a path",package_name.c_str());
00568         res.success = false;
00569         res.status_message = std::string("urdf reference package name does not exist: ")+package_name;
00570         return false;
00571       }
00572       ROS_DEBUG_ONCE("Package name [%s] has path [%s]", package_name.c_str(), package_path.c_str());
00573 
00574       model_xml.replace(pos1,(pos2-pos1),package_path);
00575       pos1 = model_xml.find(package_prefix,0);
00576     }
00577   }
00578   // ROS_DEBUG("Model XML\n\n%s\n\n ",model_xml.c_str());
00579 
00580   req.model_xml = model_xml;
00581 
00582   // Model is now considered convert to SDF
00583   return spawnSDFModel(req,res);
00584 }
00585 
00586 // DEPRECATED IN HYDRO
00587 bool GazeboRosApiPlugin::spawnGazeboModel(gazebo_msgs::SpawnModel::Request &req,
00588                                           gazebo_msgs::SpawnModel::Response &res)
00589 {
00590   ROS_WARN_STREAM_NAMED("api_plugin","/gazebo/spawn_gazebo_model is deprecated, use /gazebo/spawn_sdf_model instead");
00591   spawnSDFModel(req, res);
00592 }
00593 
00594 bool GazeboRosApiPlugin::spawnSDFModel(gazebo_msgs::SpawnModel::Request &req,
00595                                        gazebo_msgs::SpawnModel::Response &res)
00596 {
00597   // incoming robot name
00598   std::string model_name = req.model_name;
00599 
00600   // get name space for the corresponding model plugins
00601   robot_namespace_ = req.robot_namespace;
00602 
00603   // get initial pose of model
00604   gazebo::math::Vector3 initial_xyz(req.initial_pose.position.x,req.initial_pose.position.y,req.initial_pose.position.z);
00605   // get initial roll pitch yaw (fixed frame transform)
00606   gazebo::math::Quaternion initial_q(req.initial_pose.orientation.w,req.initial_pose.orientation.x,req.initial_pose.orientation.y,req.initial_pose.orientation.z);
00607 
00608   // refernce frame for initial pose definition, modify initial pose if defined
00609   gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.reference_frame));
00610   if (frame)
00611   {
00612     // convert to relative pose
00613     gazebo::math::Pose frame_pose = frame->GetWorldPose();
00614     initial_xyz = frame_pose.rot.RotateVector(initial_xyz);
00615     initial_xyz += frame_pose.pos;
00616     initial_q *= frame_pose.rot;
00617   }
00618 
00620   else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map")
00621   {
00622     ROS_DEBUG("SpawnModel: reference_frame is empty/world/map, using inertial frame");
00623   }
00624   else
00625   {
00626     res.success = false;
00627     res.status_message = "SpawnModel: reference reference_frame not found, did you forget to scope the link by model name?";
00628     return true;
00629   }
00630 
00631   // incoming robot model string
00632   std::string model_xml = req.model_xml;
00633 
00634   // store resulting Gazebo Model XML to be sent to spawn queue
00635   // get incoming string containg either an URDF or a Gazebo Model XML
00636   // grab from parameter server if necessary convert to SDF if necessary
00637   stripXmlDeclaration(model_xml);
00638 
00639   // put string in TiXmlDocument for manipulation
00640   TiXmlDocument gazebo_model_xml;
00641   gazebo_model_xml.Parse(model_xml.c_str());
00642 
00643   // optional model manipulations: update initial pose && replace model name
00644   if (isSDF(model_xml))
00645   {
00646     updateSDFAttributes(gazebo_model_xml, model_name, initial_xyz, initial_q);
00647 
00648     // Walk recursively through the entire SDF, locate plugin tags and
00649     // add robotNamespace as a child with the correct namespace
00650     if (!this->robot_namespace_.empty())
00651     {
00652       // Get root element for SDF
00653       TiXmlNode* model_tixml = gazebo_model_xml.FirstChild("sdf");
00654       model_tixml = (!model_tixml) ?
00655           gazebo_model_xml.FirstChild("gazebo") : model_tixml;
00656       if (model_tixml)
00657       {
00658         walkChildAddRobotNamespace(model_tixml);
00659       }
00660       else
00661       {
00662         ROS_WARN("Unable to add robot namespace to xml");
00663       }
00664     }
00665   }
00666   else if (isURDF(model_xml))
00667   {
00668     updateURDFModelPose(gazebo_model_xml, initial_xyz, initial_q);
00669     updateURDFName(gazebo_model_xml, model_name);
00670 
00671     // Walk recursively through the entire URDF, locate plugin tags and
00672     // add robotNamespace as a child with the correct namespace
00673     if (!this->robot_namespace_.empty())
00674     {
00675       // Get root element for URDF
00676       TiXmlNode* model_tixml = gazebo_model_xml.FirstChild("robot");
00677       if (model_tixml)
00678       {
00679         walkChildAddRobotNamespace(model_tixml);
00680       }
00681       else
00682       {
00683         ROS_WARN("Unable to add robot namespace to xml");
00684       }
00685     }
00686   }
00687   else
00688   {
00689     ROS_ERROR("GazeboRosApiPlugin SpawnModel Failure: input xml format not recognized");
00690     res.success = false;
00691     res.status_message = std::string("GazeboRosApiPlugin SpawnModel Failure: input model_xml not SDF or URDF, or cannot be converted to Gazebo compatible format.");
00692     return true;
00693   }
00694 
00695   // do spawning check if spawn worked, return response
00696   return spawnAndConform(gazebo_model_xml, model_name, res);
00697 }
00698 
00699 bool GazeboRosApiPlugin::deleteModel(gazebo_msgs::DeleteModel::Request &req,
00700                                      gazebo_msgs::DeleteModel::Response &res)
00701 {
00702   // clear forces, etc for the body in question
00703   gazebo::physics::ModelPtr model = world_->GetModel(req.model_name);
00704   if (!model)
00705   {
00706     ROS_ERROR("DeleteModel: model [%s] does not exist",req.model_name.c_str());
00707     res.success = false;
00708     res.status_message = "DeleteModel: model does not exist";
00709     return true;
00710   }
00711 
00712   // delete wrench jobs on bodies
00713   for (unsigned int i = 0 ; i < model->GetChildCount(); i ++)
00714   {
00715     gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(i));
00716     if (body)
00717     {
00718       // look for it in jobs, delete body wrench jobs
00719       clearBodyWrenches(body->GetScopedName());
00720     }
00721   }
00722 
00723   // delete force jobs on joints
00724   gazebo::physics::Joint_V joints = model->GetJoints();
00725   for (unsigned int i=0;i< joints.size(); i++)
00726   {
00727     // look for it in jobs, delete joint force jobs
00728     clearJointForces(joints[i]->GetName());
00729   }
00730 
00731   // send delete model request
00732   gazebo::msgs::Request *msg = gazebo::msgs::CreateRequest("entity_delete",req.model_name);
00733   request_pub_->Publish(*msg,true);
00734 
00735   ros::Duration model_spawn_timeout(60.0);
00736   ros::Time timeout = ros::Time::now() + model_spawn_timeout;
00737   // wait and verify that model is deleted
00738   while (true)
00739   {
00740     if (ros::Time::now() > timeout)
00741     {
00742       res.success = false;
00743       res.status_message = std::string("DeleteModel: Model pushed to delete queue, but delete service timed out waiting for model to disappear from simulation");
00744       return true;
00745     }
00746     {
00747       //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex());
00748       if (!world_->GetModel(req.model_name)) break;
00749     }
00750     ROS_DEBUG("Waiting for model deletion (%s)",req.model_name.c_str());
00751     usleep(1000);
00752   }
00753 
00754   // set result
00755   res.success = true;
00756   res.status_message = std::string("DeleteModel: successfully deleted model");
00757   return true;
00758 }
00759 
00760 bool GazeboRosApiPlugin::getModelState(gazebo_msgs::GetModelState::Request &req,
00761                                        gazebo_msgs::GetModelState::Response &res)
00762 {
00763   gazebo::physics::ModelPtr model = world_->GetModel(req.model_name);
00764   gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.relative_entity_name));
00765   if (!model)
00766   {
00767     ROS_ERROR("GetModelState: model [%s] does not exist",req.model_name.c_str());
00768     res.success = false;
00769     res.status_message = "GetModelState: model does not exist";
00770     return true;
00771   }
00772   else
00773   {
00774     // get model pose
00775     gazebo::math::Pose       model_pose = model->GetWorldPose();
00776     gazebo::math::Vector3    model_pos = model_pose.pos;
00777     gazebo::math::Quaternion model_rot = model_pose.rot;
00778 
00779     // get model twist
00780     gazebo::math::Vector3 model_linear_vel  = model->GetWorldLinearVel();
00781     gazebo::math::Vector3 model_angular_vel = model->GetWorldAngularVel();
00782 
00783 
00784     if (frame)
00785     {
00786       // convert to relative pose
00787       gazebo::math::Pose frame_pose = frame->GetWorldPose();
00788       model_pos = model_pos - frame_pose.pos;
00789       model_pos = frame_pose.rot.RotateVectorReverse(model_pos);
00790       model_rot *= frame_pose.rot.GetInverse();
00791 
00792       // convert to relative rates
00793       gazebo::math::Vector3 frame_vpos = frame->GetWorldLinearVel(); // get velocity in gazebo frame
00794       gazebo::math::Vector3 frame_veul = frame->GetWorldAngularVel(); // get velocity in gazebo frame
00795       model_linear_vel = frame_pose.rot.RotateVector(model_linear_vel - frame_vpos);
00796       model_angular_vel = frame_pose.rot.RotateVector(model_angular_vel - frame_veul);
00797     }
00799     else if (req.relative_entity_name == "" || req.relative_entity_name == "world" || req.relative_entity_name == "map" || req.relative_entity_name == "/map")
00800     {
00801       ROS_DEBUG("GetModelState: relative_entity_name is empty/world/map, using inertial frame");
00802     }
00803     else
00804     {
00805       res.success = false;
00806       res.status_message = "GetModelState: reference relative_entity_name not found, did you forget to scope the body by model name?";
00807       return true;
00808     }
00809 
00810     // fill in response
00811     res.pose.position.x = model_pos.x;
00812     res.pose.position.y = model_pos.y;
00813     res.pose.position.z = model_pos.z;
00814     res.pose.orientation.w = model_rot.w;
00815     res.pose.orientation.x = model_rot.x;
00816     res.pose.orientation.y = model_rot.y;
00817     res.pose.orientation.z = model_rot.z;
00818 
00819     res.twist.linear.x = model_linear_vel.x;
00820     res.twist.linear.y = model_linear_vel.y;
00821     res.twist.linear.z = model_linear_vel.z;
00822     res.twist.angular.x = model_angular_vel.x;
00823     res.twist.angular.y = model_angular_vel.y;
00824     res.twist.angular.z = model_angular_vel.z;
00825 
00826     res.success = true;
00827     res.status_message = "GetModelState: got properties";
00828     return true;
00829   }
00830   return true;
00831 }
00832 
00833 bool GazeboRosApiPlugin::getModelProperties(gazebo_msgs::GetModelProperties::Request &req,
00834                                             gazebo_msgs::GetModelProperties::Response &res)
00835 {
00836   gazebo::physics::ModelPtr model = world_->GetModel(req.model_name);
00837   if (!model)
00838   {
00839     ROS_ERROR("GetModelProperties: model [%s] does not exist",req.model_name.c_str());
00840     res.success = false;
00841     res.status_message = "GetModelProperties: model does not exist";
00842     return true;
00843   }
00844   else
00845   {
00846     // get model parent name
00847     gazebo::physics::ModelPtr parent_model = boost::dynamic_pointer_cast<gazebo::physics::Model>(model->GetParent());
00848     if (parent_model) res.parent_model_name = parent_model->GetName();
00849 
00850     // get list of child bodies, geoms
00851     res.body_names.clear();
00852     res.geom_names.clear();
00853     for (unsigned int i = 0 ; i < model->GetChildCount(); i ++)
00854     {
00855       gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(i));
00856       if (body)
00857       {
00858         res.body_names.push_back(body->GetName());
00859         // get list of geoms
00860         for (unsigned int j = 0; j < body->GetChildCount() ; j++)
00861         {
00862           gazebo::physics::CollisionPtr geom = boost::dynamic_pointer_cast<gazebo::physics::Collision>(body->GetChild(j));
00863           if (geom)
00864             res.geom_names.push_back(geom->GetName());
00865         }
00866       }
00867     }
00868 
00869     // get list of joints
00870     res.joint_names.clear();
00871 
00872     gazebo::physics::Joint_V joints = model->GetJoints();
00873     for (unsigned int i=0;i< joints.size(); i++)
00874       res.joint_names.push_back( joints[i]->GetName() );
00875 
00876     // get children model names
00877     res.child_model_names.clear();
00878     for (unsigned int j = 0; j < model->GetChildCount(); j++)
00879     {
00880       gazebo::physics::ModelPtr child_model = boost::dynamic_pointer_cast<gazebo::physics::Model>(model->GetChild(j));
00881       if (child_model)
00882         res.child_model_names.push_back(child_model->GetName() );
00883     }
00884 
00885     // is model static
00886     res.is_static = model->IsStatic();
00887 
00888     res.success = true;
00889     res.status_message = "GetModelProperties: got properties";
00890     return true;
00891   }
00892   return true;
00893 }
00894 
00895 bool GazeboRosApiPlugin::getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req,
00896                                             gazebo_msgs::GetWorldProperties::Response &res)
00897 {
00898   res.sim_time = world_->GetSimTime().Double();
00899   res.model_names.clear();
00900   for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
00901     res.model_names.push_back(world_->GetModel(i)->GetName());
00902   gzerr << "disablign rendering has not been implemented, rendering is always enabled\n";
00903   res.rendering_enabled = true; //world->GetRenderEngineEnabled();
00904   res.success = true;
00905   res.status_message = "GetWorldProperties: got properties";
00906   return true;
00907 }
00908 
00909 bool GazeboRosApiPlugin::getJointProperties(gazebo_msgs::GetJointProperties::Request &req,
00910                                             gazebo_msgs::GetJointProperties::Response &res)
00911 {
00912   gazebo::physics::JointPtr joint;
00913   for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
00914   {
00915     joint = world_->GetModel(i)->GetJoint(req.joint_name);
00916     if (joint) break;
00917   }
00918 
00919   if (!joint)
00920   {
00921     res.success = false;
00922     res.status_message = "GetJointProperties: joint not found";
00923     return true;
00924   }
00925   else
00926   {
00928     res.type = res.REVOLUTE;
00929 
00930     res.damping.clear(); // to be added to gazebo
00931     //res.damping.push_back(joint->GetDamping(0));
00932 
00933     res.position.clear(); // use GetAngle(i)
00934     res.position.push_back(joint->GetAngle(0).Radian());
00935 
00936     res.rate.clear(); // use GetVelocity(i)
00937     res.rate.push_back(joint->GetVelocity(0));
00938 
00939     res.success = true;
00940     res.status_message = "GetJointProperties: got properties";
00941     return true;
00942   }
00943 }
00944 
00945 bool GazeboRosApiPlugin::getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req,
00946                                            gazebo_msgs::GetLinkProperties::Response &res)
00947 {
00948   gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name));
00949   if (!body)
00950   {
00951     res.success = false;
00952     res.status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?";
00953     return true;
00954   }
00955   else
00956   {
00958     res.gravity_mode = body->GetGravityMode();
00959 
00960     res.mass = body->GetInertial()->GetMass();
00961 
00962     gazebo::physics::InertialPtr inertia = body->GetInertial();
00963     res.ixx = inertia->GetIXX();
00964     res.iyy = inertia->GetIYY();
00965     res.izz = inertia->GetIZZ();
00966     res.ixy = inertia->GetIXY();
00967     res.ixz = inertia->GetIXZ();
00968     res.iyz = inertia->GetIYZ();
00969 
00970     gazebo::math::Vector3 com = body->GetInertial()->GetCoG();
00971     res.com.position.x = com.x;
00972     res.com.position.y = com.y;
00973     res.com.position.z = com.z;
00974     res.com.orientation.x = 0; // @todo: gazebo do not support rotated inertia yet
00975     res.com.orientation.y = 0;
00976     res.com.orientation.z = 0;
00977     res.com.orientation.w = 1;
00978 
00979     res.success = true;
00980     res.status_message = "GetLinkProperties: got properties";
00981     return true;
00982   }
00983 }
00984 
00985 bool GazeboRosApiPlugin::getLinkState(gazebo_msgs::GetLinkState::Request &req,
00986                                       gazebo_msgs::GetLinkState::Response &res)
00987 {
00988   gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name));
00989   gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.reference_frame));
00990 
00991   if (!body)
00992   {
00993     res.success = false;
00994     res.status_message = "GetLinkState: link not found, did you forget to scope the link by model name?";
00995     return true;
00996   }
00997 
00998   // get body pose
00999   gazebo::math::Pose body_pose = body->GetWorldPose();
01000   // Get inertial rates
01001   gazebo::math::Vector3 body_vpos = body->GetWorldLinearVel(); // get velocity in gazebo frame
01002   gazebo::math::Vector3 body_veul = body->GetWorldAngularVel(); // get velocity in gazebo frame
01003 
01004   if (frame)
01005   {
01006     // convert to relative pose
01007     gazebo::math::Pose frame_pose = frame->GetWorldPose();
01008     body_pose.pos = body_pose.pos - frame_pose.pos;
01009     body_pose.pos = frame_pose.rot.RotateVectorReverse(body_pose.pos);
01010     body_pose.rot *= frame_pose.rot.GetInverse();
01011 
01012     // convert to relative rates
01013     gazebo::math::Vector3 frame_vpos = frame->GetWorldLinearVel(); // get velocity in gazebo frame
01014     gazebo::math::Vector3 frame_veul = frame->GetWorldAngularVel(); // get velocity in gazebo frame
01015     body_vpos = frame_pose.rot.RotateVector(body_vpos - frame_vpos);
01016     body_veul = frame_pose.rot.RotateVector(body_veul - frame_veul);
01017   }
01019   else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map")
01020   {
01021     ROS_DEBUG("GetLinkState: reference_frame is empty/world/map, using inertial frame");
01022   }
01023   else
01024   {
01025     res.success = false;
01026     res.status_message = "GetLinkState: reference reference_frame not found, did you forget to scope the link by model name?";
01027     return true;
01028   }
01029 
01030   res.link_state.link_name = req.link_name;
01031   res.link_state.pose.position.x = body_pose.pos.x;
01032   res.link_state.pose.position.y = body_pose.pos.y;
01033   res.link_state.pose.position.z = body_pose.pos.z;
01034   res.link_state.pose.orientation.x = body_pose.rot.x;
01035   res.link_state.pose.orientation.y = body_pose.rot.y;
01036   res.link_state.pose.orientation.z = body_pose.rot.z;
01037   res.link_state.pose.orientation.w = body_pose.rot.w;
01038   res.link_state.twist.linear.x = body_vpos.x;
01039   res.link_state.twist.linear.y = body_vpos.y;
01040   res.link_state.twist.linear.z = body_vpos.z;
01041   res.link_state.twist.angular.x = body_veul.x;
01042   res.link_state.twist.angular.y = body_veul.y;
01043   res.link_state.twist.angular.z = body_veul.x;
01044   res.link_state.reference_frame = req.reference_frame;
01045 
01046   res.success = true;
01047   res.status_message = "GetLinkState: got state";
01048   return true;
01049 }
01050 
01051 bool GazeboRosApiPlugin::setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req,
01052                                            gazebo_msgs::SetLinkProperties::Response &res)
01053 {
01054   gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_name));
01055   if (!body)
01056   {
01057     res.success = false;
01058     res.status_message = "SetLinkProperties: link not found, did you forget to scope the link by model name?";
01059     return true;
01060   }
01061   else
01062   {
01063     gazebo::physics::InertialPtr mass = body->GetInertial();
01064     // @todo: FIXME: add inertia matrix rotation to Gazebo
01065     // mass.SetInertiaRotation(gazebo::math::Quaternionion(req.com.orientation.w,res.com.orientation.x,req.com.orientation.y req.com.orientation.z));
01066     mass->SetCoG(gazebo::math::Vector3(req.com.position.x,req.com.position.y,req.com.position.z));
01067     mass->SetInertiaMatrix(req.ixx,req.iyy,req.izz,req.ixy,req.ixz,req.iyz);
01068     mass->SetMass(req.mass);
01069     body->SetGravityMode(req.gravity_mode);
01070     // @todo: mass change unverified
01071     res.success = true;
01072     res.status_message = "SetLinkProperties: properties set";
01073     return true;
01074   }
01075 }
01076 
01077 bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req,
01078                                               gazebo_msgs::SetPhysicsProperties::Response &res)
01079 {
01080   // pause simulation if requested
01081   bool is_paused = world_->IsPaused();
01082   world_->SetPaused(true);
01083 
01084   // supported updates
01085   gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine());
01086   pe->SetMaxStepSize(req.time_step);
01087   pe->SetRealTimeUpdateRate(req.max_update_rate);
01088   pe->SetGravity(gazebo::math::Vector3(req.gravity.x,req.gravity.y,req.gravity.z));
01089 
01090   if (world_->GetPhysicsEngine()->GetType() == "ode")
01091   {
01092     // stuff only works in ODE right now
01093     pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies);
01094 #if GAZEBO_MAJOR_VERSION >= 3
01095     pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters);
01096     pe->SetParam("iters", req.ode_config.sor_pgs_iters);
01097     pe->SetParam("sor", req.ode_config.sor_pgs_w);
01098     pe->SetParam("cfm", req.ode_config.cfm);
01099     pe->SetParam("erp", req.ode_config.erp);
01100     pe->SetParam("contact_surface_layer",
01101         req.ode_config.contact_surface_layer);
01102     pe->SetParam("contact_max_correcting_vel",
01103         req.ode_config.contact_max_correcting_vel);
01104     pe->SetParam("max_contacts", req.ode_config.max_contacts);
01105 #else
01106     pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters);
01107     pe->SetSORPGSIters(req.ode_config.sor_pgs_iters);
01108     pe->SetSORPGSW(req.ode_config.sor_pgs_w);
01109     pe->SetWorldCFM(req.ode_config.cfm);
01110     pe->SetWorldERP(req.ode_config.erp);
01111     pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer);
01112     pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel);
01113     pe->SetMaxContacts(req.ode_config.max_contacts);
01114 #endif
01115 
01116     world_->SetPaused(is_paused);
01117 
01118     res.success = true;
01119     res.status_message = "physics engine updated";
01120   }
01121   else
01122   {
01124     ROS_ERROR("ROS set_physics_properties service call does not yet support physics engine [%s].", world_->GetPhysicsEngine()->GetType().c_str());
01125     res.success = false;
01126     res.status_message = "Physics engine [" + world_->GetPhysicsEngine()->GetType() + "]: set_physics_properties not supported.";
01127   }
01128   return res.success;
01129 }
01130 
01131 bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req,
01132                                               gazebo_msgs::GetPhysicsProperties::Response &res)
01133 {
01134   // supported updates
01135   res.time_step = world_->GetPhysicsEngine()->GetMaxStepSize();
01136   res.pause = world_->IsPaused();
01137   res.max_update_rate = world_->GetPhysicsEngine()->GetRealTimeUpdateRate();
01138   gazebo::math::Vector3 gravity = world_->GetPhysicsEngine()->GetGravity();
01139   res.gravity.x = gravity.x;
01140   res.gravity.y = gravity.y;
01141   res.gravity.z = gravity.z;
01142 
01143   // stuff only works in ODE right now
01144   if (world_->GetPhysicsEngine()->GetType() == "ode")
01145   {
01146     res.ode_config.auto_disable_bodies =
01147       world_->GetPhysicsEngine()->GetAutoDisableFlag();
01148 #if GAZEBO_MAJOR_VERSION >= 3
01149     res.ode_config.sor_pgs_precon_iters = boost::any_cast<int>(
01150       world_->GetPhysicsEngine()->GetParam("precon_iters"));
01151     res.ode_config.sor_pgs_iters = boost::any_cast<int>(
01152         world_->GetPhysicsEngine()->GetParam("iters"));
01153     res.ode_config.sor_pgs_w = boost::any_cast<double>(
01154         world_->GetPhysicsEngine()->GetParam("sor"));
01155     res.ode_config.contact_surface_layer = boost::any_cast<double>(
01156       world_->GetPhysicsEngine()->GetParam("contact_surface_layer"));
01157     res.ode_config.contact_max_correcting_vel = boost::any_cast<double>(
01158       world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel"));
01159     res.ode_config.cfm = boost::any_cast<double>(
01160         world_->GetPhysicsEngine()->GetParam("cfm"));
01161     res.ode_config.erp = boost::any_cast<double>(
01162         world_->GetPhysicsEngine()->GetParam("erp"));
01163     res.ode_config.max_contacts = boost::any_cast<int>(
01164       world_->GetPhysicsEngine()->GetParam("max_contacts"));
01165 #else
01166     res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters();
01167     res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters();
01168     res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW();
01169     res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer();
01170     res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel();
01171     res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM();
01172     res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP();
01173     res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts();
01174 #endif
01175 
01176     res.success = true;
01177     res.status_message = "GetPhysicsProperties: got properties";
01178   }
01179   else
01180   {
01182     ROS_ERROR("ROS get_physics_properties service call does not yet support physics engine [%s].", world_->GetPhysicsEngine()->GetType().c_str());
01183     res.success = false;
01184     res.status_message = "Physics engine [" + world_->GetPhysicsEngine()->GetType() + "]: get_physics_properties not supported.";
01185   }
01186   return res.success;
01187 }
01188 
01189 bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Request &req,
01190                                             gazebo_msgs::SetJointProperties::Response &res)
01191 {
01193   gazebo::physics::JointPtr joint;
01194   for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
01195   {
01196     joint = world_->GetModel(i)->GetJoint(req.joint_name);
01197     if (joint) break;
01198   }
01199 
01200   if (!joint)
01201   {
01202     res.success = false;
01203     res.status_message = "SetJointProperties: joint not found";
01204     return true;
01205   }
01206   else
01207   {
01208     for(unsigned int i=0;i< req.ode_joint_config.damping.size();i++)
01209       joint->SetDamping(i,req.ode_joint_config.damping[i]);
01210 #if GAZEBO_MAJOR_VERSION >= 4
01211     for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++)
01212       joint->SetParam("hi_stop",i,req.ode_joint_config.hiStop[i]);
01213     for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++)
01214       joint->SetParam("lo_stop",i,req.ode_joint_config.loStop[i]);
01215     for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++)
01216       joint->SetParam("erp",i,req.ode_joint_config.erp[i]);
01217     for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++)
01218       joint->SetParam("cfm",i,req.ode_joint_config.cfm[i]);
01219     for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++)
01220       joint->SetParam("stop_erp",i,req.ode_joint_config.stop_erp[i]);
01221     for(unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++)
01222       joint->SetParam("stop_cfm",i,req.ode_joint_config.stop_cfm[i]);
01223     for(unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++)
01224       joint->SetParam("fudge_factor",i,req.ode_joint_config.fudge_factor[i]);
01225     for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++)
01226       joint->SetParam("fmax",i,req.ode_joint_config.fmax[i]);
01227     for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++)
01228       joint->SetParam("vel",i,req.ode_joint_config.vel[i]);
01229 #else
01230     for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++)
01231       joint->SetAttribute("hi_stop",i,req.ode_joint_config.hiStop[i]);
01232     for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++)
01233       joint->SetAttribute("lo_stop",i,req.ode_joint_config.loStop[i]);
01234     for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++)
01235       joint->SetAttribute("erp",i,req.ode_joint_config.erp[i]);
01236     for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++)
01237       joint->SetAttribute("cfm",i,req.ode_joint_config.cfm[i]);
01238     for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++)
01239       joint->SetAttribute("stop_erp",i,req.ode_joint_config.stop_erp[i]);
01240     for(unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++)
01241       joint->SetAttribute("stop_cfm",i,req.ode_joint_config.stop_cfm[i]);
01242     for(unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++)
01243       joint->SetAttribute("fudge_factor",i,req.ode_joint_config.fudge_factor[i]);
01244     for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++)
01245       joint->SetAttribute("fmax",i,req.ode_joint_config.fmax[i]);
01246     for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++)
01247       joint->SetAttribute("vel",i,req.ode_joint_config.vel[i]);
01248 #endif
01249 
01250     res.success = true;
01251     res.status_message = "SetJointProperties: properties set";
01252     return true;
01253   }
01254 }
01255 
01256 bool GazeboRosApiPlugin::setModelState(gazebo_msgs::SetModelState::Request &req,
01257                                        gazebo_msgs::SetModelState::Response &res)
01258 {
01259   gazebo::math::Vector3 target_pos(req.model_state.pose.position.x,req.model_state.pose.position.y,req.model_state.pose.position.z);
01260   gazebo::math::Quaternion 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);
01261   target_rot.Normalize(); // eliminates invalid rotation (0, 0, 0, 0)
01262   gazebo::math::Pose target_pose(target_pos,target_rot);
01263   gazebo::math::Vector3 target_pos_dot(req.model_state.twist.linear.x,req.model_state.twist.linear.y,req.model_state.twist.linear.z);
01264   gazebo::math::Vector3 target_rot_dot(req.model_state.twist.angular.x,req.model_state.twist.angular.y,req.model_state.twist.angular.z);
01265 
01266   gazebo::physics::ModelPtr model = world_->GetModel(req.model_state.model_name);
01267   if (!model)
01268   {
01269     ROS_ERROR("Updating ModelState: model [%s] does not exist",req.model_state.model_name.c_str());
01270     res.success = false;
01271     res.status_message = "SetModelState: model does not exist";
01272     return true;
01273   }
01274   else
01275   {
01276     gazebo::physics::LinkPtr relative_entity = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.model_state.reference_frame));
01277     if (relative_entity)
01278     {
01279       gazebo::math::Pose  frame_pose = relative_entity->GetWorldPose(); // - myBody->GetCoMPose();
01280       gazebo::math::Vector3 frame_pos = frame_pose.pos;
01281       gazebo::math::Quaternion frame_rot = frame_pose.rot;
01282 
01283       //std::cout << " debug : " << relative_entity->GetName() << " : " << frame_pose << " : " << target_pose << std::endl;
01284       //target_pose = frame_pose + target_pose; // seems buggy, use my own
01285       target_pose.pos = model->GetWorldPose().pos + frame_rot.RotateVector(target_pos);
01286       target_pose.rot = frame_rot * target_pose.rot;
01287      
01288       // Velocities should be commanded in the requested reference
01289       // frame, so we need to translate them to the world frame
01290       target_pos_dot = frame_rot.RotateVector(target_pos_dot);
01291       target_rot_dot = frame_rot.RotateVector(target_rot_dot);
01292     }
01294     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" )
01295     {
01296       ROS_DEBUG("Updating ModelState: reference frame is empty/world/map, usig inertial frame");
01297     }
01298     else
01299     {
01300       ROS_ERROR("Updating ModelState: for model[%s], specified reference frame entity [%s] does not exist",
01301                 req.model_state.model_name.c_str(),req.model_state.reference_frame.c_str());
01302       res.success = false;
01303       res.status_message = "SetModelState: specified reference frame entity does not exist";
01304       return true;
01305     }
01306 
01307     //ROS_ERROR("target state: %f %f %f",target_pose.pos.x,target_pose.pos.y,target_pose.pos.z);
01308     bool is_paused = world_->IsPaused();
01309     world_->SetPaused(true);
01310     model->SetWorldPose(target_pose);
01311     world_->SetPaused(is_paused);
01312     //gazebo::math::Pose p3d = model->GetWorldPose();
01313     //ROS_ERROR("model updated state: %f %f %f",p3d.pos.x,p3d.pos.y,p3d.pos.z);
01314 
01315     // set model velocity
01316     model->SetLinearVel(target_pos_dot);
01317     model->SetAngularVel(target_rot_dot);
01318 
01319     res.success = true;
01320     res.status_message = "SetModelState: set model state done";
01321     return true;
01322   }
01323 }
01324 
01325 void GazeboRosApiPlugin::updateModelState(const gazebo_msgs::ModelState::ConstPtr& model_state)
01326 {
01327   gazebo_msgs::SetModelState::Response res;
01328   gazebo_msgs::SetModelState::Request req;
01329   req.model_state = *model_state;
01330   /*bool success =*/ setModelState(req,res);
01331 }
01332 
01333 bool GazeboRosApiPlugin::applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,
01334                                           gazebo_msgs::ApplyJointEffort::Response &res)
01335 {
01336   gazebo::physics::JointPtr joint;
01337   for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
01338   {
01339     joint = world_->GetModel(i)->GetJoint(req.joint_name);
01340     if (joint)
01341     {
01342       GazeboRosApiPlugin::ForceJointJob* fjj = new GazeboRosApiPlugin::ForceJointJob;
01343       fjj->joint = joint;
01344       fjj->force = req.effort;
01345       fjj->start_time = req.start_time;
01346       if (fjj->start_time < ros::Time(world_->GetSimTime().Double()))
01347         fjj->start_time = ros::Time(world_->GetSimTime().Double());
01348       fjj->duration = req.duration;
01349       lock_.lock();
01350       force_joint_jobs_.push_back(fjj);
01351       lock_.unlock();
01352 
01353       res.success = true;
01354       res.status_message = "ApplyJointEffort: effort set";
01355       return true;
01356     }
01357   }
01358 
01359   res.success = false;
01360   res.status_message = "ApplyJointEffort: joint not found";
01361   return true;
01362 }
01363 
01364 bool GazeboRosApiPlugin::resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
01365 {
01366   world_->Reset();
01367   return true;
01368 }
01369 
01370 bool GazeboRosApiPlugin::resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
01371 {
01372   world_->ResetEntities(gazebo::physics::Base::MODEL);
01373   return true;
01374 }
01375 
01376 bool GazeboRosApiPlugin::pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
01377 {
01378   world_->SetPaused(true);
01379   return true;
01380 }
01381 
01382 bool GazeboRosApiPlugin::unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
01383 {
01384   world_->SetPaused(false);
01385   return true;
01386 }
01387 
01388 bool GazeboRosApiPlugin::clearJointForces(gazebo_msgs::JointRequest::Request &req,
01389                                           gazebo_msgs::JointRequest::Response &res)
01390 {
01391   return clearJointForces(req.joint_name);
01392 }
01393 bool GazeboRosApiPlugin::clearJointForces(std::string joint_name)
01394 {
01395   bool search = true;
01396   lock_.lock();
01397   while(search)
01398   {
01399     search = false;
01400     for (std::vector<GazeboRosApiPlugin::ForceJointJob*>::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();++iter)
01401     {
01402       if ((*iter)->joint->GetName() == joint_name)
01403       {
01404         // found one, search through again
01405         search = true;
01406         delete (*iter);
01407         force_joint_jobs_.erase(iter);
01408         break;
01409       }
01410     }
01411   }
01412   lock_.unlock();
01413   return true;
01414 }
01415 
01416 bool GazeboRosApiPlugin::clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req,
01417                                            gazebo_msgs::BodyRequest::Response &res)
01418 {
01419   return clearBodyWrenches(req.body_name);
01420 }
01421 bool GazeboRosApiPlugin::clearBodyWrenches(std::string body_name)
01422 {
01423   bool search = true;
01424   lock_.lock();
01425   while(search)
01426   {
01427     search = false;
01428     for (std::vector<GazeboRosApiPlugin::WrenchBodyJob*>::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();++iter)
01429     {
01430       //ROS_ERROR("search %s %s",(*iter)->body->GetScopedName().c_str(), body_name.c_str());
01431       if ((*iter)->body->GetScopedName() == body_name)
01432       {
01433         // found one, search through again
01434         search = true;
01435         delete (*iter);
01436         wrench_body_jobs_.erase(iter);
01437         break;
01438       }
01439     }
01440   }
01441   lock_.unlock();
01442   return true;
01443 }
01444 
01445 bool GazeboRosApiPlugin::setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req,
01446                                                gazebo_msgs::SetModelConfiguration::Response &res)
01447 {
01448   std::string gazebo_model_name = req.model_name;
01449 
01450   // search for model with name
01451   gazebo::physics::ModelPtr gazebo_model = world_->GetModel(req.model_name);
01452   if (!gazebo_model)
01453   {
01454     ROS_ERROR("SetModelConfiguration: model [%s] does not exist",gazebo_model_name.c_str());
01455     res.success = false;
01456     res.status_message = "SetModelConfiguration: model does not exist";
01457     return true;
01458   }
01459 
01460   if (req.joint_names.size() == req.joint_positions.size())
01461   {
01462     std::map<std::string, double> joint_position_map;
01463     for (unsigned int i = 0; i < req.joint_names.size(); i++)
01464     {
01465       joint_position_map[req.joint_names[i]] = req.joint_positions[i];
01466     }
01467 
01468     // make the service call to pause gazebo
01469     bool is_paused = world_->IsPaused();
01470     if (!is_paused) world_->SetPaused(true);
01471 
01472     gazebo_model->SetJointPositions(joint_position_map);
01473 
01474     // resume paused state before this call
01475     world_->SetPaused(is_paused);
01476 
01477     res.success = true;
01478     res.status_message = "SetModelConfiguration: success";
01479     return true;
01480   }
01481   else
01482   {
01483     res.success = false;
01484     res.status_message = "SetModelConfiguration: joint name and position list have different lengths";
01485     return true;
01486   }
01487 }
01488 
01489 bool GazeboRosApiPlugin::setLinkState(gazebo_msgs::SetLinkState::Request &req,
01490                                       gazebo_msgs::SetLinkState::Response &res)
01491 {
01492   gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_state.link_name));
01493   gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.link_state.reference_frame));
01494   if (!body)
01495   {
01496     ROS_ERROR("Updating LinkState: link [%s] does not exist",req.link_state.link_name.c_str());
01497     res.success = false;
01498     res.status_message = "SetLinkState: link does not exist";
01499     return true;
01500   }
01501 
01503   // get reference frame (body/model(link)) pose and
01504   // transform target pose to absolute world frame
01505   gazebo::math::Vector3 target_pos(req.link_state.pose.position.x,req.link_state.pose.position.y,req.link_state.pose.position.z);
01506   gazebo::math::Quaternion 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);
01507   gazebo::math::Pose target_pose(target_pos,target_rot);
01508   gazebo::math::Vector3 target_linear_vel(req.link_state.twist.linear.x,req.link_state.twist.linear.y,req.link_state.twist.linear.z);
01509   gazebo::math::Vector3 target_angular_vel(req.link_state.twist.angular.x,req.link_state.twist.angular.y,req.link_state.twist.angular.z);
01510 
01511   if (frame)
01512   {
01513     gazebo::math::Pose  frame_pose = frame->GetWorldPose(); // - myBody->GetCoMPose();
01514     gazebo::math::Vector3 frame_pos = frame_pose.pos;
01515     gazebo::math::Quaternion frame_rot = frame_pose.rot;
01516 
01517     //std::cout << " debug : " << frame->GetName() << " : " << frame_pose << " : " << target_pose << std::endl;
01518     //target_pose = frame_pose + target_pose; // seems buggy, use my own
01519     target_pose.pos = frame_pos + frame_rot.RotateVector(target_pos);
01520     target_pose.rot = frame_rot * target_pose.rot;
01521 
01522     gazebo::math::Vector3 frame_linear_vel = frame->GetWorldLinearVel();
01523     gazebo::math::Vector3 frame_angular_vel = frame->GetWorldAngularVel();
01524     target_linear_vel -= frame_linear_vel;
01525     target_angular_vel -= frame_angular_vel;
01526   }
01527   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")
01528   {
01529     ROS_INFO("Updating LinkState: reference_frame is empty/world/map, using inertial frame");
01530   }
01531   else
01532   {
01533     ROS_ERROR("Updating LinkState: reference_frame is not a valid link name");
01534     res.success = false;
01535     res.status_message = "SetLinkState: failed";
01536     return true;
01537   }
01538 
01539   //std::cout << " debug : " << target_pose << std::endl;
01540   //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex());
01541 
01542   bool is_paused = world_->IsPaused();
01543   if (!is_paused) world_->SetPaused(true);
01544   body->SetWorldPose(target_pose);
01545   world_->SetPaused(is_paused);
01546 
01547   // set body velocity to desired twist
01548   body->SetLinearVel(target_linear_vel);
01549   body->SetAngularVel(target_angular_vel);
01550 
01551   res.success = true;
01552   res.status_message = "SetLinkState: success";
01553   return true;
01554 }
01555 
01556 void GazeboRosApiPlugin::updateLinkState(const gazebo_msgs::LinkState::ConstPtr& link_state)
01557 {
01558   gazebo_msgs::SetLinkState::Request req;
01559   gazebo_msgs::SetLinkState::Response res;
01560   req.link_state = *link_state;
01561   /*bool success = */ setLinkState(req,res);
01562 }
01563 
01564 void GazeboRosApiPlugin::transformWrench( gazebo::math::Vector3 &target_force, gazebo::math::Vector3 &target_torque,
01565                                           gazebo::math::Vector3 reference_force, gazebo::math::Vector3 reference_torque,
01566                                           gazebo::math::Pose target_to_reference )
01567 {
01568   // rotate force into target frame
01569   target_force = target_to_reference.rot.RotateVector(reference_force);
01570   // rotate torque into target frame
01571   target_torque = target_to_reference.rot.RotateVector(reference_torque);
01572 
01573   // target force is the refence force rotated by the target->reference transform
01574   target_torque = target_torque + target_to_reference.pos.Cross(target_force);
01575 }
01576 
01577 bool GazeboRosApiPlugin::applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req,
01578                                          gazebo_msgs::ApplyBodyWrench::Response &res)
01579 {
01580   gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.body_name));
01581   gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast<gazebo::physics::Link>(world_->GetEntity(req.reference_frame));
01582   if (!body)
01583   {
01584     ROS_ERROR("ApplyBodyWrench: body [%s] does not exist",req.body_name.c_str());
01585     res.success = false;
01586     res.status_message = "ApplyBodyWrench: body does not exist";
01587     return true;
01588   }
01589 
01590   // target wrench
01591   gazebo::math::Vector3 reference_force(req.wrench.force.x,req.wrench.force.y,req.wrench.force.z);
01592   gazebo::math::Vector3 reference_torque(req.wrench.torque.x,req.wrench.torque.y,req.wrench.torque.z);
01593   gazebo::math::Vector3 reference_point(req.reference_point.x,req.reference_point.y,req.reference_point.z);
01594 
01595   gazebo::math::Vector3 target_force;
01596   gazebo::math::Vector3 target_torque;
01597 
01600   reference_torque = reference_torque + reference_point.Cross(reference_force);
01601 
01603   if (frame)
01604   {
01605     // get reference frame (body/model(body)) pose and
01606     // transform target pose to absolute world frame
01607     // @todo: need to modify wrench (target force and torque by frame)
01608     //        transform wrench from reference_point in reference_frame
01609     //        into the reference frame of the body
01610     //        first, translate by reference point to the body frame
01611     gazebo::math::Pose target_to_reference = frame->GetWorldPose() - body->GetWorldPose();
01612     ROS_DEBUG("reference frame for applied wrench: [%f %f %f, %f %f %f]-[%f %f %f, %f %f %f]=[%f %f %f, %f %f %f]",
01613               body->GetWorldPose().pos.x,
01614               body->GetWorldPose().pos.y,
01615               body->GetWorldPose().pos.z,
01616               body->GetWorldPose().rot.GetAsEuler().x,
01617               body->GetWorldPose().rot.GetAsEuler().y,
01618               body->GetWorldPose().rot.GetAsEuler().z,
01619               frame->GetWorldPose().pos.x,
01620               frame->GetWorldPose().pos.y,
01621               frame->GetWorldPose().pos.z,
01622               frame->GetWorldPose().rot.GetAsEuler().x,
01623               frame->GetWorldPose().rot.GetAsEuler().y,
01624               frame->GetWorldPose().rot.GetAsEuler().z,
01625               target_to_reference.pos.x,
01626               target_to_reference.pos.y,
01627               target_to_reference.pos.z,
01628               target_to_reference.rot.GetAsEuler().x,
01629               target_to_reference.rot.GetAsEuler().y,
01630               target_to_reference.rot.GetAsEuler().z
01631               );
01632     transformWrench(target_force, target_torque, reference_force, reference_torque, target_to_reference);
01633     ROS_ERROR("wrench defined as [%s]:[%f %f %f, %f %f %f] --> applied as [%s]:[%f %f %f, %f %f %f]",
01634               frame->GetName().c_str(),
01635               reference_force.x,
01636               reference_force.y,
01637               reference_force.z,
01638               reference_torque.x,
01639               reference_torque.y,
01640               reference_torque.z,
01641               body->GetName().c_str(),
01642               target_force.x,
01643               target_force.y,
01644               target_force.z,
01645               target_torque.x,
01646               target_torque.y,
01647               target_torque.z
01648               );
01649 
01650   }
01651   else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map")
01652   {
01653     ROS_INFO("ApplyBodyWrench: reference_frame is empty/world/map, using inertial frame, transferring from body relative to inertial frame");
01654     // FIXME: transfer to inertial frame
01655     gazebo::math::Pose target_to_reference = body->GetWorldPose();
01656     target_force = reference_force;
01657     target_torque = reference_torque;
01658 
01659   }
01660   else
01661   {
01662     ROS_ERROR("ApplyBodyWrench: reference_frame is not a valid link name");
01663     res.success = false;
01664     res.status_message = "ApplyBodyWrench: reference_frame not found";
01665     return true;
01666   }
01667 
01668   // apply wrench
01669   // schedule a job to do below at appropriate times:
01670   // body->SetForce(force)
01671   // body->SetTorque(torque)
01672   GazeboRosApiPlugin::WrenchBodyJob* wej = new GazeboRosApiPlugin::WrenchBodyJob;
01673   wej->body = body;
01674   wej->force = target_force;
01675   wej->torque = target_torque;
01676   wej->start_time = req.start_time;
01677   if (wej->start_time < ros::Time(world_->GetSimTime().Double()))
01678     wej->start_time = ros::Time(world_->GetSimTime().Double());
01679   wej->duration = req.duration;
01680   lock_.lock();
01681   wrench_body_jobs_.push_back(wej);
01682   lock_.unlock();
01683 
01684   res.success = true;
01685   res.status_message = "";
01686   return true;
01687 }
01688 
01689 bool GazeboRosApiPlugin::isURDF(std::string model_xml)
01690 {
01691   TiXmlDocument doc_in;
01692   doc_in.Parse(model_xml.c_str());
01693   if (doc_in.FirstChild("robot"))
01694     return true;
01695   else
01696     return false;
01697 }
01698 
01699 bool GazeboRosApiPlugin::isSDF(std::string model_xml)
01700 {
01701   // FIXME: very crude check
01702   TiXmlDocument doc_in;
01703   doc_in.Parse(model_xml.c_str());
01704   if (doc_in.FirstChild("gazebo") ||
01705       doc_in.FirstChild("sdf")) // sdf
01706     return true;
01707   else
01708     return false;
01709 }
01710 
01711 void GazeboRosApiPlugin::wrenchBodySchedulerSlot()
01712 {
01713   // MDMutex locks in case model is getting deleted, don't have to do this if we delete jobs first
01714   // boost::recursive_mutex::scoped_lock lock(*world->GetMDMutex());
01715   lock_.lock();
01716   for (std::vector<GazeboRosApiPlugin::WrenchBodyJob*>::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();)
01717   {
01718     // check times and apply wrench if necessary
01719     if (ros::Time(world_->GetSimTime().Double()) >= (*iter)->start_time)
01720       if (ros::Time(world_->GetSimTime().Double()) <= (*iter)->start_time+(*iter)->duration ||
01721           (*iter)->duration.toSec() < 0.0)
01722       {
01723         if ((*iter)->body) // if body exists
01724         {
01725           (*iter)->body->SetForce((*iter)->force);
01726           (*iter)->body->SetTorque((*iter)->torque);
01727         }
01728         else
01729           (*iter)->duration.fromSec(0.0); // mark for delete
01730       }
01731 
01732     if (ros::Time(world_->GetSimTime().Double()) > (*iter)->start_time+(*iter)->duration &&
01733         (*iter)->duration.toSec() >= 0.0)
01734     {
01735       // remove from queue once expires
01736       delete (*iter);
01737       iter = wrench_body_jobs_.erase(iter);
01738     }
01739     else
01740       iter++;
01741   }
01742   lock_.unlock();
01743 }
01744 
01745 void GazeboRosApiPlugin::forceJointSchedulerSlot()
01746 {
01747   // MDMutex locks in case model is getting deleted, don't have to do this if we delete jobs first
01748   // boost::recursive_mutex::scoped_lock lock(*world->GetMDMutex());
01749   lock_.lock();
01750   for (std::vector<GazeboRosApiPlugin::ForceJointJob*>::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();)
01751   {
01752     // check times and apply force if necessary
01753     if (ros::Time(world_->GetSimTime().Double()) >= (*iter)->start_time)
01754       if (ros::Time(world_->GetSimTime().Double()) <= (*iter)->start_time+(*iter)->duration ||
01755           (*iter)->duration.toSec() < 0.0)
01756       {
01757         if ((*iter)->joint) // if joint exists
01758           (*iter)->joint->SetForce(0,(*iter)->force);
01759         else
01760           (*iter)->duration.fromSec(0.0); // mark for delete
01761       }
01762 
01763     if (ros::Time(world_->GetSimTime().Double()) > (*iter)->start_time+(*iter)->duration &&
01764         (*iter)->duration.toSec() >= 0.0)
01765     {
01766       // remove from queue once expires
01767       iter = force_joint_jobs_.erase(iter);
01768     }
01769     else
01770       iter++;
01771   }
01772   lock_.unlock();
01773 }
01774 
01775 void GazeboRosApiPlugin::publishSimTime(const boost::shared_ptr<gazebo::msgs::WorldStatistics const> &msg)
01776 {
01777   ROS_ERROR("CLOCK2");
01778   gazebo::common::Time currentTime = gazebo::msgs::Convert( msg->sim_time() );
01779   rosgraph_msgs::Clock ros_time_;
01780   ros_time_.clock.fromSec(currentTime.Double());
01781   //  publish time to ros
01782   pub_clock_.publish(ros_time_);
01783 }
01784 void GazeboRosApiPlugin::publishSimTime()
01785 {
01786   gazebo::common::Time currentTime = world_->GetSimTime();
01787   rosgraph_msgs::Clock ros_time_;
01788   ros_time_.clock.fromSec(currentTime.Double());
01789   //  publish time to ros
01790   pub_clock_.publish(ros_time_);
01791 }
01792 
01793 void GazeboRosApiPlugin::publishLinkStates()
01794 {
01795   gazebo_msgs::LinkStates link_states;
01796 
01797   // fill link_states
01798   for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
01799   {
01800     gazebo::physics::ModelPtr model = world_->GetModel(i);
01801 
01802     for (unsigned int j = 0 ; j < model->GetChildCount(); j ++)
01803     {
01804       gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast<gazebo::physics::Link>(model->GetChild(j));
01805 
01806       if (body)
01807       {
01808         link_states.name.push_back(body->GetScopedName());
01809         geometry_msgs::Pose pose;
01810         gazebo::math::Pose  body_pose = body->GetWorldPose(); // - myBody->GetCoMPose();
01811         gazebo::math::Vector3 pos = body_pose.pos;
01812         gazebo::math::Quaternion rot = body_pose.rot;
01813         pose.position.x = pos.x;
01814         pose.position.y = pos.y;
01815         pose.position.z = pos.z;
01816         pose.orientation.w = rot.w;
01817         pose.orientation.x = rot.x;
01818         pose.orientation.y = rot.y;
01819         pose.orientation.z = rot.z;
01820         link_states.pose.push_back(pose);
01821         gazebo::math::Vector3 linear_vel  = body->GetWorldLinearVel();
01822         gazebo::math::Vector3 angular_vel = body->GetWorldAngularVel();
01823         geometry_msgs::Twist twist;
01824         twist.linear.x = linear_vel.x;
01825         twist.linear.y = linear_vel.y;
01826         twist.linear.z = linear_vel.z;
01827         twist.angular.x = angular_vel.x;
01828         twist.angular.y = angular_vel.y;
01829         twist.angular.z = angular_vel.z;
01830         link_states.twist.push_back(twist);
01831       }
01832     }
01833   }
01834 
01835   pub_link_states_.publish(link_states);
01836 }
01837 
01838 void GazeboRosApiPlugin::publishModelStates()
01839 {
01840   gazebo_msgs::ModelStates model_states;
01841 
01842   // fill model_states
01843   for (unsigned int i = 0; i < world_->GetModelCount(); i ++)
01844   {
01845     gazebo::physics::ModelPtr model = world_->GetModel(i);
01846     model_states.name.push_back(model->GetName());
01847     geometry_msgs::Pose pose;
01848     gazebo::math::Pose  model_pose = model->GetWorldPose(); // - myBody->GetCoMPose();
01849     gazebo::math::Vector3 pos = model_pose.pos;
01850     gazebo::math::Quaternion rot = model_pose.rot;
01851     pose.position.x = pos.x;
01852     pose.position.y = pos.y;
01853     pose.position.z = pos.z;
01854     pose.orientation.w = rot.w;
01855     pose.orientation.x = rot.x;
01856     pose.orientation.y = rot.y;
01857     pose.orientation.z = rot.z;
01858     model_states.pose.push_back(pose);
01859     gazebo::math::Vector3 linear_vel  = model->GetWorldLinearVel();
01860     gazebo::math::Vector3 angular_vel = model->GetWorldAngularVel();
01861     geometry_msgs::Twist twist;
01862     twist.linear.x = linear_vel.x;
01863     twist.linear.y = linear_vel.y;
01864     twist.linear.z = linear_vel.z;
01865     twist.angular.x = angular_vel.x;
01866     twist.angular.y = angular_vel.y;
01867     twist.angular.z = angular_vel.z;
01868     model_states.twist.push_back(twist);
01869   }
01870   pub_model_states_.publish(model_states);
01871 }
01872 
01873 void GazeboRosApiPlugin::physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level)
01874 {
01875   if (!physics_reconfigure_initialized_)
01876   {
01877     gazebo_msgs::GetPhysicsProperties srv;
01878     physics_reconfigure_get_client_.call(srv);
01879 
01880     config.time_step                   = srv.response.time_step;
01881     config.max_update_rate             = srv.response.max_update_rate;
01882     config.gravity_x                   = srv.response.gravity.x;
01883     config.gravity_y                   = srv.response.gravity.y;
01884     config.gravity_z                   = srv.response.gravity.z;
01885     config.auto_disable_bodies         = srv.response.ode_config.auto_disable_bodies;
01886     config.sor_pgs_precon_iters        = srv.response.ode_config.sor_pgs_precon_iters;
01887     config.sor_pgs_iters               = srv.response.ode_config.sor_pgs_iters;
01888     config.sor_pgs_rms_error_tol       = srv.response.ode_config.sor_pgs_rms_error_tol;
01889     config.sor_pgs_w                   = srv.response.ode_config.sor_pgs_w;
01890     config.contact_surface_layer       = srv.response.ode_config.contact_surface_layer;
01891     config.contact_max_correcting_vel  = srv.response.ode_config.contact_max_correcting_vel;
01892     config.cfm                         = srv.response.ode_config.cfm;
01893     config.erp                         = srv.response.ode_config.erp;
01894     config.max_contacts                = srv.response.ode_config.max_contacts;
01895     physics_reconfigure_initialized_ = true;
01896   }
01897   else
01898   {
01899     bool changed = false;
01900     gazebo_msgs::GetPhysicsProperties srv;
01901     physics_reconfigure_get_client_.call(srv);
01902 
01903     // check for changes
01904     if (config.time_step                      != srv.response.time_step)                                 changed = true;
01905     if (config.max_update_rate                != srv.response.max_update_rate)                           changed = true;
01906     if (config.gravity_x                      != srv.response.gravity.x)                                 changed = true;
01907     if (config.gravity_y                      != srv.response.gravity.y)                                 changed = true;
01908     if (config.gravity_z                      != srv.response.gravity.z)                                 changed = true;
01909     if (config.auto_disable_bodies            != srv.response.ode_config.auto_disable_bodies)            changed = true;
01910     if ((uint32_t)config.sor_pgs_precon_iters != srv.response.ode_config.sor_pgs_precon_iters)           changed = true;
01911     if ((uint32_t)config.sor_pgs_iters        != srv.response.ode_config.sor_pgs_iters)                  changed = true;
01912     if (config.sor_pgs_rms_error_tol          != srv.response.ode_config.sor_pgs_rms_error_tol)          changed = true;
01913     if (config.sor_pgs_w                      != srv.response.ode_config.sor_pgs_w)                      changed = true;
01914     if (config.contact_surface_layer          != srv.response.ode_config.contact_surface_layer)          changed = true;
01915     if (config.contact_max_correcting_vel     != srv.response.ode_config.contact_max_correcting_vel)     changed = true;
01916     if (config.cfm                            != srv.response.ode_config.cfm)                            changed = true;
01917     if (config.erp                            != srv.response.ode_config.erp)                            changed = true;
01918     if ((uint32_t)config.max_contacts         != srv.response.ode_config.max_contacts)                   changed = true;
01919 
01920     if (changed)
01921     {
01922       // pause simulation if requested
01923       gazebo_msgs::SetPhysicsProperties srv;
01924       srv.request.time_step                             = config.time_step                   ;
01925       srv.request.max_update_rate                       = config.max_update_rate             ;
01926       srv.request.gravity.x                             = config.gravity_x                   ;
01927       srv.request.gravity.y                             = config.gravity_y                   ;
01928       srv.request.gravity.z                             = config.gravity_z                   ;
01929       srv.request.ode_config.auto_disable_bodies        = config.auto_disable_bodies         ;
01930       srv.request.ode_config.sor_pgs_precon_iters       = config.sor_pgs_precon_iters        ;
01931       srv.request.ode_config.sor_pgs_iters              = config.sor_pgs_iters               ;
01932       srv.request.ode_config.sor_pgs_rms_error_tol      = config.sor_pgs_rms_error_tol       ;
01933       srv.request.ode_config.sor_pgs_w                  = config.sor_pgs_w                   ;
01934       srv.request.ode_config.contact_surface_layer      = config.contact_surface_layer       ;
01935       srv.request.ode_config.contact_max_correcting_vel = config.contact_max_correcting_vel  ;
01936       srv.request.ode_config.cfm                        = config.cfm                         ;
01937       srv.request.ode_config.erp                        = config.erp                         ;
01938       srv.request.ode_config.max_contacts               = config.max_contacts                ;
01939       physics_reconfigure_set_client_.call(srv);
01940       ROS_INFO("physics dynamics reconfigure update complete");
01941     }
01942     ROS_INFO("physics dynamics reconfigure complete");
01943   }
01944 }
01945 
01946 void GazeboRosApiPlugin::physicsReconfigureThread()
01947 {
01948   physics_reconfigure_set_client_ = nh_->serviceClient<gazebo_msgs::SetPhysicsProperties>("/gazebo/set_physics_properties");
01949   physics_reconfigure_get_client_ = nh_->serviceClient<gazebo_msgs::GetPhysicsProperties>("/gazebo/get_physics_properties");
01950 
01951   // Wait until the rest of this plugin is loaded and the services are being offered
01952   physics_reconfigure_set_client_.waitForExistence();
01953   physics_reconfigure_get_client_.waitForExistence();
01954 
01955   physics_reconfigure_srv_.reset(new dynamic_reconfigure::Server<gazebo_ros::PhysicsConfig>());
01956 
01957   physics_reconfigure_callback_ = boost::bind(&GazeboRosApiPlugin::physicsReconfigureCallback, this, _1, _2);
01958   physics_reconfigure_srv_->setCallback(physics_reconfigure_callback_);
01959 
01960   ROS_INFO("Physics dynamic reconfigure ready.");
01961 }
01962 
01963 void GazeboRosApiPlugin::stripXmlDeclaration(std::string &model_xml)
01964 {
01965   // incoming robot model string is a string containing a Gazebo Model XML
01969   std::string open_bracket("<?");
01970   std::string close_bracket("?>");
01971   size_t pos1 = model_xml.find(open_bracket,0);
01972   size_t pos2 = model_xml.find(close_bracket,0);
01973   if (pos1 != std::string::npos && pos2 != std::string::npos)
01974     model_xml.replace(pos1,pos2-pos1+2,std::string(""));
01975 }
01976 
01977 void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, std::string model_name,
01978                                              gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q)
01979 {
01980   // This function can handle both regular SDF files and <include> SDFs that are used with the
01981   // Gazebo Model Database
01982 
01983   TiXmlElement* pose_element; // This is used by both reguar and database SDFs
01984 
01985   // Check SDF for requires SDF element
01986   TiXmlElement* gazebo_tixml = gazebo_model_xml.FirstChildElement("sdf");
01987   if (!gazebo_tixml)
01988   {
01989     ROS_WARN("Could not find <sdf> element in sdf, so name and initial position cannot be applied");
01990     return;
01991   }
01992 
01993   // Check SDF for optional model element. May not have one
01994   TiXmlElement* model_tixml = gazebo_tixml->FirstChildElement("model");
01995   if (model_tixml)
01996   {
01997     // Update model name
01998     if (model_tixml->Attribute("name") != NULL)
01999     {
02000       // removing old model name
02001       model_tixml->RemoveAttribute("name");
02002     }
02003     // replace with user specified name
02004     model_tixml->SetAttribute("name",model_name);
02005   }
02006   else
02007   {
02008     // Check SDF for world element
02009     TiXmlElement* world_tixml = gazebo_tixml->FirstChildElement("world");
02010     if (!world_tixml)
02011     {
02012       ROS_WARN("Could not find <model> or <world> element in sdf, so name and initial position cannot be applied");
02013       return;
02014     }
02015     // If not <model> element, check SDF for required include element
02016     model_tixml = world_tixml->FirstChildElement("include");
02017     if (!model_tixml)
02018     {
02019       ROS_WARN("Could not find <include> element in sdf, so name and initial position cannot be applied");
02020       return;
02021     }
02022 
02023     // Check for name element
02024     TiXmlElement* name_tixml = model_tixml->FirstChildElement("name");
02025     if (!name_tixml)
02026     {
02027       // Create the name element
02028       name_tixml = new TiXmlElement("name");
02029       model_tixml->LinkEndChild(name_tixml);
02030     }
02031 
02032     // Set the text within the name element
02033     TiXmlText* text = new TiXmlText(model_name);
02034     name_tixml->LinkEndChild( text );
02035   }
02036 
02037 
02038   // Check for the pose element
02039   pose_element = model_tixml->FirstChildElement("pose");
02040   gazebo::math::Pose model_pose;
02041 
02042   // Create the pose element if it doesn't exist
02043   // Remove it if it exists, since we are inserting a new one
02044   if (pose_element)
02045   {
02046     // save pose_element in math::Pose and remove child
02047     model_pose = this->parsePose(pose_element->GetText());
02048     model_tixml->RemoveChild(pose_element);
02049   }
02050 
02051   // Set and link the pose element after adding initial pose
02052   {
02053     // add pose_element Pose to initial pose
02054     gazebo::math::Pose new_model_pose = model_pose + gazebo::math::Pose(initial_xyz, initial_q);
02055 
02056     // Create the string of 6 numbers
02057     std::ostringstream pose_stream;
02058     gazebo::math::Vector3 model_rpy = new_model_pose.rot.GetAsEuler(); // convert to Euler angles for Gazebo XML
02059     pose_stream << new_model_pose.pos.x << " " << new_model_pose.pos.y << " " << new_model_pose.pos.z << " "
02060                 << model_rpy.x << " " << model_rpy.y << " " << model_rpy.z;
02061 
02062     // Add value to pose element
02063     TiXmlText* text = new TiXmlText(pose_stream.str());
02064     TiXmlElement* new_pose_element = new TiXmlElement("pose");
02065     new_pose_element->LinkEndChild(text);
02066     model_tixml->LinkEndChild(new_pose_element);
02067   }
02068 }
02069 
02070 gazebo::math::Pose GazeboRosApiPlugin::parsePose(const std::string &str)
02071 {
02072   std::vector<std::string> pieces;
02073   std::vector<double> vals;
02074 
02075   boost::split(pieces, str, boost::is_any_of(" "));
02076   for (unsigned int i = 0; i < pieces.size(); ++i)
02077   {
02078     if (pieces[i] != "")
02079     {
02080       try
02081       {
02082         vals.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
02083       }
02084       catch(boost::bad_lexical_cast &e)
02085       {
02086         gzerr << "xml key [" << str
02087           << "][" << i << "] value [" << pieces[i]
02088           << "] is not a valid double from a 3-tuple\n";
02089         return gazebo::math::Pose();
02090       }
02091     }
02092   }
02093 
02094   if (vals.size() == 6)
02095     return gazebo::math::Pose(vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]);
02096   else
02097   {
02098     ROS_ERROR("Beware: failed to parse string [%s] as gazebo::math::Pose, returning zeros.", str.c_str());
02099     return gazebo::math::Pose();
02100   }
02101 }
02102 
02103 gazebo::math::Vector3 GazeboRosApiPlugin::parseVector3(const std::string &str)
02104 {
02105   std::vector<std::string> pieces;
02106   std::vector<double> vals;
02107 
02108   boost::split(pieces, str, boost::is_any_of(" "));
02109   for (unsigned int i = 0; i < pieces.size(); ++i)
02110   {
02111     if (pieces[i] != "")
02112     {
02113       try
02114       {
02115         vals.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
02116       }
02117       catch(boost::bad_lexical_cast &e)
02118       {
02119         gzerr << "xml key [" << str
02120           << "][" << i << "] value [" << pieces[i]
02121           << "] is not a valid double from a 3-tuple\n";
02122         return gazebo::math::Vector3();
02123       }
02124     }
02125   }
02126 
02127   if (vals.size() == 3)
02128     return gazebo::math::Vector3(vals[0], vals[1], vals[2]);
02129   else
02130   {
02131     ROS_ERROR("Beware: failed to parse string [%s] as gazebo::math::Vector3, returning zeros.", str.c_str());
02132     return gazebo::math::Vector3();
02133   }
02134 }
02135 
02136 void GazeboRosApiPlugin::updateURDFModelPose(TiXmlDocument &gazebo_model_xml, gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q)
02137 {
02138   TiXmlElement* model_tixml = (gazebo_model_xml.FirstChildElement("robot"));
02139   if (model_tixml)
02140   {
02141     // replace initial pose of robot
02142     // find first instance of xyz and rpy, replace with initial pose
02143     TiXmlElement* origin_key = model_tixml->FirstChildElement("origin");
02144 
02145     if (!origin_key)
02146     {
02147       origin_key = new TiXmlElement("origin");
02148       model_tixml->LinkEndChild(origin_key);
02149     }
02150 
02151     gazebo::math::Vector3 xyz;
02152     gazebo::math::Vector3 rpy;
02153     if (origin_key->Attribute("xyz"))
02154     {
02155       xyz = this->parseVector3(origin_key->Attribute("xyz"));
02156       origin_key->RemoveAttribute("xyz");
02157     }
02158     if (origin_key->Attribute("rpy"))
02159     {
02160       rpy = this->parseVector3(origin_key->Attribute("rpy"));
02161       origin_key->RemoveAttribute("rpy");
02162     }
02163 
02164     // add xyz, rpy to initial pose
02165     gazebo::math::Pose model_pose = gazebo::math::Pose(xyz, rpy) + gazebo::math::Pose(initial_xyz, initial_q);
02166 
02167     std::ostringstream xyz_stream;
02168     xyz_stream << model_pose.pos.x << " " << model_pose.pos.y << " " << model_pose.pos.z;
02169 
02170     std::ostringstream rpy_stream;
02171     gazebo::math::Vector3 model_rpy = model_pose.rot.GetAsEuler(); // convert to Euler angles for Gazebo XML
02172     rpy_stream << model_rpy.x << " " << model_rpy.y << " " << model_rpy.z;
02173 
02174     origin_key->SetAttribute("xyz",xyz_stream.str());
02175     origin_key->SetAttribute("rpy",rpy_stream.str());
02176   }
02177   else
02178     ROS_WARN("could not find <model> element in sdf, so name and initial position is not applied");
02179 }
02180 
02181 void GazeboRosApiPlugin::updateURDFName(TiXmlDocument &gazebo_model_xml, std::string model_name)
02182 {
02183   TiXmlElement* model_tixml = gazebo_model_xml.FirstChildElement("robot");
02184   // replace model name if one is specified by the user
02185   if (model_tixml)
02186   {
02187     if (model_tixml->Attribute("name") != NULL)
02188     {
02189       // removing old model name
02190       model_tixml->RemoveAttribute("name");
02191     }
02192     // replace with user specified name
02193     model_tixml->SetAttribute("name",model_name);
02194   }
02195   else
02196     ROS_WARN("could not find <robot> element in URDF, name not replaced");
02197 }
02198 
02199 void GazeboRosApiPlugin::walkChildAddRobotNamespace(TiXmlNode* robot_xml)
02200 {
02201   TiXmlNode* child = 0;
02202   child = robot_xml->IterateChildren(child);
02203   while (child != NULL)
02204   {
02205     if (child->ValueStr().find(std::string("plugin")) == 0)
02206     {
02207       if (child->FirstChildElement("robotNamespace") == NULL)
02208       {
02209         TiXmlElement* child_elem = child->ToElement()->FirstChildElement("robotNamespace");
02210         while (child_elem)
02211         {
02212           child->ToElement()->RemoveChild(child_elem);
02213           child_elem = child->ToElement()->FirstChildElement("robotNamespace");
02214         }
02215         TiXmlElement* key = new TiXmlElement("robotNamespace");
02216         TiXmlText* val = new TiXmlText(robot_namespace_);
02217         key->LinkEndChild(val);
02218         child->ToElement()->LinkEndChild(key);
02219       }
02220     }
02221     walkChildAddRobotNamespace(child);
02222     child = robot_xml->IterateChildren(child);
02223   }
02224 }
02225 
02226 bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::string model_name,
02227                                          gazebo_msgs::SpawnModel::Response &res)
02228 {
02229   // push to factory iface
02230   std::ostringstream stream;
02231   stream << gazebo_model_xml;
02232   std::string gazebo_model_xml_string = stream.str();
02233   ROS_DEBUG("Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str());
02234 
02235   // publish to factory topic
02236   gazebo::msgs::Factory msg;
02237   gazebo::msgs::Init(msg, "spawn_model");
02238   msg.set_sdf( gazebo_model_xml_string );
02239 
02240   //ROS_ERROR("attempting to spawn model name [%s] [%s]", model_name.c_str(),gazebo_model_xml_string.c_str());
02241 
02242   // FIXME: should use entity_info or add lock to World::receiveMutex
02243   // looking for Model to see if it exists already
02244   gazebo::msgs::Request *entity_info_msg = gazebo::msgs::CreateRequest("entity_info", model_name);
02245   request_pub_->Publish(*entity_info_msg,true);
02246   // todo: should wait for response response_sub_, check to see that if _msg->response == "nonexistant"
02247 
02248   gazebo::physics::ModelPtr model = world_->GetModel(model_name);
02249   if (model)
02250   {
02251     ROS_ERROR("SpawnModel: Failure - model name %s already exist.",model_name.c_str());
02252     res.success = false;
02253     res.status_message = "SpawnModel: Failure - model already exists.";
02254     return true;
02255   }
02256 
02257   // Publish the factory message
02258   factory_pub_->Publish(msg);
02261 
02263   ros::Duration model_spawn_timeout(10.0);
02264   ros::Time timeout = ros::Time::now() + model_spawn_timeout;
02265 
02266   while (ros::ok())
02267   {
02268     if (ros::Time::now() > timeout)
02269     {
02270       res.success = false;
02271       res.status_message = std::string("SpawnModel: Model pushed to spawn queue, but spawn service")
02272         + std::string(" timed out waiting for model to appear in simulation under the name ")
02273         + model_name;
02274       return true;
02275     }
02276 
02277     {
02278       //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex());
02279       if (world_->GetModel(model_name))
02280         break;
02281     }
02282 
02283     ROS_DEBUG_STREAM_ONCE_NAMED("api_plugin","Waiting for " << timeout - ros::Time::now()
02284       << " for model " << model_name << " to spawn");
02285 
02286     usleep(2000);
02287   }
02288 
02289   // set result
02290   res.success = true;
02291   res.status_message = std::string("SpawnModel: Successfully spawned model");
02292   return true;
02293 }
02294 
02295 // Register this plugin with the simulator
02296 GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosApiPlugin)
02297 }
02298 


gazebo_ros
Author(s): John Hsu, Nate Koenig, Dave Coleman
autogenerated on Fri Aug 28 2015 10:47:48