gazebo_ros_api_plugin.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2012-2014 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: 25 Apr 2010
00021  */
00022 
00023 #ifndef __GAZEBO_ROS_API_PLUGIN_HH__
00024 #define __GAZEBO_ROS_API_PLUGIN_HH__
00025 
00026 #include <stdio.h>
00027 #include <stdlib.h>
00028 #include <signal.h>
00029 #include <errno.h>
00030 #include <iostream>
00031 
00032 #include <tinyxml.h>
00033 
00034 #include <gazebo/physics/physics.hh>
00035 #include <gazebo/common/common.hh>
00036 #include <gazebo/transport/transport.hh>
00037 
00038 // ROS
00039 #include <ros/ros.h>
00040 #include <ros/callback_queue.h>
00041 #include <ros/subscribe_options.h>
00042 #include <ros/package.h>
00043 #include <rosgraph_msgs/Clock.h>
00044 
00045 // Services
00046 #include "std_srvs/Empty.h"
00047 
00048 #include "gazebo_msgs/JointRequest.h"
00049 #include "gazebo_msgs/BodyRequest.h"
00050 
00051 #include "gazebo_msgs/SpawnModel.h"
00052 #include "gazebo_msgs/DeleteModel.h"
00053 
00054 #include "gazebo_msgs/ApplyBodyWrench.h"
00055 
00056 #include "gazebo_msgs/SetPhysicsProperties.h"
00057 #include "gazebo_msgs/GetPhysicsProperties.h"
00058 
00059 #include "gazebo_msgs/SetJointProperties.h"
00060 
00061 #include "gazebo_msgs/GetWorldProperties.h"
00062 
00063 #include "gazebo_msgs/GetModelProperties.h"
00064 #include "gazebo_msgs/GetModelState.h"
00065 #include "gazebo_msgs/SetModelState.h"
00066 
00067 #include "gazebo_msgs/GetJointProperties.h"
00068 #include "gazebo_msgs/ApplyJointEffort.h"
00069 
00070 #include "gazebo_msgs/GetLinkProperties.h"
00071 #include "gazebo_msgs/SetLinkProperties.h"
00072 #include "gazebo_msgs/SetLinkState.h"
00073 #include "gazebo_msgs/GetLinkState.h"
00074 
00075 // Topics
00076 #include "gazebo_msgs/ModelState.h"
00077 #include "gazebo_msgs/LinkState.h"
00078 #include "gazebo_msgs/ModelStates.h"
00079 #include "gazebo_msgs/LinkStates.h"
00080 
00081 #include "geometry_msgs/Vector3.h"
00082 #include "geometry_msgs/Wrench.h"
00083 #include "geometry_msgs/Pose.h"
00084 #include "geometry_msgs/Twist.h"
00085 
00086 // For model pose transform to set custom joint angles
00087 #include <ros/ros.h>
00088 #include <gazebo_msgs/SetModelConfiguration.h>
00089 #include <boost/shared_ptr.hpp>
00090 
00091 // For physics dynamics reconfigure
00092 #include <dynamic_reconfigure/server.h>
00093 #include <gazebo_ros/PhysicsConfig.h>
00094 #include "gazebo_msgs/SetPhysicsProperties.h"
00095 #include "gazebo_msgs/GetPhysicsProperties.h"
00096 
00097 #include <boost/algorithm/string.hpp>
00098 
00099 namespace gazebo
00100 {
00101 
00103 class GazeboRosApiPlugin : public SystemPlugin
00104 {
00105 public:
00107   GazeboRosApiPlugin();
00108 
00110   ~GazeboRosApiPlugin();
00111   
00113   void shutdownSignal();
00114 
00121   void Load(int argc, char** argv);
00122 
00124   void gazeboQueueThread();
00125 
00127   void advertiseServices();
00128 
00130   void onLinkStatesConnect();
00131 
00133   void onModelStatesConnect();
00134 
00136   void onLinkStatesDisconnect();
00137 
00139   void onModelStatesDisconnect();
00140 
00142   bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,gazebo_msgs::SpawnModel::Response &res);
00143 
00145   ROS_DEPRECATED bool spawnGazeboModel(gazebo_msgs::SpawnModel::Request &req,gazebo_msgs::SpawnModel::Response &res);
00146 
00148   bool spawnSDFModel(gazebo_msgs::SpawnModel::Request &req,gazebo_msgs::SpawnModel::Response &res);
00149 
00151   bool deleteModel(gazebo_msgs::DeleteModel::Request &req,gazebo_msgs::DeleteModel::Response &res);
00152 
00154   bool getModelState(gazebo_msgs::GetModelState::Request &req,gazebo_msgs::GetModelState::Response &res);
00155 
00157   bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req,gazebo_msgs::GetModelProperties::Response &res);
00158 
00160   bool getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req,gazebo_msgs::GetWorldProperties::Response &res);
00161 
00163   bool getJointProperties(gazebo_msgs::GetJointProperties::Request &req,gazebo_msgs::GetJointProperties::Response &res);
00164 
00166   bool getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req,gazebo_msgs::GetLinkProperties::Response &res);
00167 
00169   bool getLinkState(gazebo_msgs::GetLinkState::Request &req,gazebo_msgs::GetLinkState::Response &res);
00170 
00172   bool setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req,gazebo_msgs::SetLinkProperties::Response &res);
00173 
00175   bool setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req,gazebo_msgs::SetPhysicsProperties::Response &res);
00176 
00178   bool getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req,gazebo_msgs::GetPhysicsProperties::Response &res);
00179 
00181   bool setJointProperties(gazebo_msgs::SetJointProperties::Request &req,gazebo_msgs::SetJointProperties::Response &res);
00182 
00184   bool setModelState(gazebo_msgs::SetModelState::Request &req,gazebo_msgs::SetModelState::Response &res);
00185 
00187   void updateModelState(const gazebo_msgs::ModelState::ConstPtr& model_state);
00188 
00190   bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,gazebo_msgs::ApplyJointEffort::Response &res);
00191 
00193   bool resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
00194 
00196   bool resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
00197 
00199   bool pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
00200 
00202   bool unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
00203 
00205   bool clearJointForces(gazebo_msgs::JointRequest::Request &req,gazebo_msgs::JointRequest::Response &res);
00206   bool clearJointForces(std::string joint_name);
00207 
00209   bool clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req,gazebo_msgs::BodyRequest::Response &res);
00210   bool clearBodyWrenches(std::string body_name);
00211 
00213   bool setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req,gazebo_msgs::SetModelConfiguration::Response &res);
00214 
00216   bool setLinkState(gazebo_msgs::SetLinkState::Request &req,gazebo_msgs::SetLinkState::Response &res);
00217 
00219   void updateLinkState(const gazebo_msgs::LinkState::ConstPtr& link_state);
00220 
00222   bool applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req,gazebo_msgs::ApplyBodyWrench::Response &res);
00223 
00224 private:
00225 
00227   void wrenchBodySchedulerSlot();
00228 
00230   void forceJointSchedulerSlot();
00231 
00233   void publishSimTime(const boost::shared_ptr<gazebo::msgs::WorldStatistics const> &msg);
00234   void publishSimTime();
00235 
00237   void publishLinkStates();
00238 
00240   void publishModelStates();
00241 
00243   void stripXmlDeclaration(std::string &model_xml);
00244 
00246   void updateSDFAttributes(TiXmlDocument &gazebo_model_xml, std::string model_name, 
00247                            gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q);
00248 
00250   void updateURDFModelPose(TiXmlDocument &gazebo_model_xml, 
00251                            gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q);
00252 
00254   void updateURDFName(TiXmlDocument &gazebo_model_xml, std::string model_name);
00255 
00257   void walkChildAddRobotNamespace(TiXmlNode* robot_xml);
00258 
00260   bool spawnAndConform(TiXmlDocument &gazebo_model_xml, std::string model_name, 
00261                        gazebo_msgs::SpawnModel::Response &res);
00262 
00265   void transformWrench(gazebo::math::Vector3 &target_force, gazebo::math::Vector3 &target_torque,
00266                        gazebo::math::Vector3 reference_force, gazebo::math::Vector3 reference_torque,
00267                        gazebo::math::Pose target_to_reference );
00268 
00270   void physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level);
00271 
00273   void physicsReconfigureThread();
00274 
00276   void onResponse(ConstResponsePtr &response);
00277 
00279   bool isURDF(std::string model_xml);
00280 
00282   bool isSDF(std::string model_xml);
00283 
00285   void loadGazeboRosApiPlugin(std::string world_name);
00286 
00288   gazebo::math::Pose parsePose(const std::string &str);
00289 
00291   gazebo::math::Vector3 parseVector3(const std::string &str);
00292 
00293   // track if the desconstructor event needs to occur
00294   bool plugin_loaded_;
00295 
00296   // detect if sigint event occurs
00297   bool stop_; 
00298   gazebo::event::ConnectionPtr sigint_event_;
00299 
00300   std::string robot_namespace_;
00301 
00302   gazebo::transport::NodePtr gazebonode_;
00303   gazebo::transport::SubscriberPtr stat_sub_;
00304   gazebo::transport::PublisherPtr factory_pub_;
00305   gazebo::transport::PublisherPtr request_pub_;
00306   gazebo::transport::SubscriberPtr response_sub_;
00307 
00308   boost::shared_ptr<ros::NodeHandle> nh_;
00309   ros::CallbackQueue gazebo_queue_;
00310   boost::shared_ptr<boost::thread> gazebo_callback_queue_thread_;
00311 
00312   gazebo::physics::WorldPtr world_;
00313   gazebo::event::ConnectionPtr wrench_update_event_;
00314   gazebo::event::ConnectionPtr force_update_event_;
00315   gazebo::event::ConnectionPtr time_update_event_;
00316   gazebo::event::ConnectionPtr pub_link_states_event_;
00317   gazebo::event::ConnectionPtr pub_model_states_event_;
00318   gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_;
00319 
00320   ros::ServiceServer spawn_gazebo_model_service_; // DEPRECATED IN HYDRO
00321   ros::ServiceServer spawn_sdf_model_service_;
00322   ros::ServiceServer spawn_urdf_model_service_;
00323   ros::ServiceServer delete_model_service_;
00324   ros::ServiceServer get_model_state_service_;
00325   ros::ServiceServer get_model_properties_service_;
00326   ros::ServiceServer get_world_properties_service_;
00327   ros::ServiceServer get_joint_properties_service_;
00328   ros::ServiceServer get_link_properties_service_;
00329   ros::ServiceServer get_link_state_service_;
00330   ros::ServiceServer set_link_properties_service_;
00331   ros::ServiceServer set_physics_properties_service_;
00332   ros::ServiceServer get_physics_properties_service_;
00333   ros::ServiceServer apply_body_wrench_service_;
00334   ros::ServiceServer set_joint_properties_service_;
00335   ros::ServiceServer set_model_state_service_;
00336   ros::ServiceServer apply_joint_effort_service_;
00337   ros::ServiceServer set_model_configuration_service_;
00338   ros::ServiceServer set_link_state_service_;
00339   ros::ServiceServer reset_simulation_service_;
00340   ros::ServiceServer reset_world_service_;
00341   ros::ServiceServer pause_physics_service_;
00342   ros::ServiceServer unpause_physics_service_;
00343   ros::ServiceServer clear_joint_forces_service_;
00344   ros::ServiceServer clear_body_wrenches_service_;
00345   ros::Subscriber    set_link_state_topic_;
00346   ros::Subscriber    set_model_state_topic_;
00347   ros::Publisher     pub_link_states_;
00348   ros::Publisher     pub_model_states_;
00349   int                pub_link_states_connection_count_;
00350   int                pub_model_states_connection_count_;
00351 
00352   // ROS comm
00353   boost::shared_ptr<ros::AsyncSpinner> async_ros_spin_;
00354 
00355   // physics dynamic reconfigure
00356   boost::shared_ptr<boost::thread> physics_reconfigure_thread_;
00357   bool physics_reconfigure_initialized_;
00358   ros::ServiceClient physics_reconfigure_set_client_;
00359   ros::ServiceClient physics_reconfigure_get_client_;
00360   boost::shared_ptr< dynamic_reconfigure::Server<gazebo_ros::PhysicsConfig> > physics_reconfigure_srv_;
00361   dynamic_reconfigure::Server<gazebo_ros::PhysicsConfig>::CallbackType physics_reconfigure_callback_;
00362 
00363   ros::Publisher     pub_clock_;
00364 
00366   boost::mutex lock_;
00367 
00368   bool world_created_;
00369 
00370   class WrenchBodyJob
00371   {
00372   public:
00373     gazebo::physics::LinkPtr body;
00374     gazebo::math::Vector3 force;
00375     gazebo::math::Vector3 torque;
00376     ros::Time start_time;
00377     ros::Duration duration;
00378   };
00379 
00380   class ForceJointJob
00381   {
00382   public:
00383     gazebo::physics::JointPtr joint;
00384     double force; // should this be a array?
00385     ros::Time start_time;
00386     ros::Duration duration;
00387   };
00388 
00389   std::vector<GazeboRosApiPlugin::WrenchBodyJob*> wrench_body_jobs_;
00390   std::vector<GazeboRosApiPlugin::ForceJointJob*> force_joint_jobs_;
00391 
00392 };
00393 }
00394 #endif


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