gazebo_ros_utils.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Markus Bader <markus.bader@tuwien.ac.at>
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived from
00019  *     this software without specific prior written permission.
00020  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  *  POSSIBILITY OF SUCH DAMAGE.
00032  *********************************************************************/
00033 
00034 #ifndef GAZEBO_ROS_UTILS_H
00035 #define GAZEBO_ROS_UTILS_H
00036 #include <map>
00037 #include <boost/algorithm/string.hpp>
00038 
00039 #include <gazebo/common/common.hh>
00040 #include <gazebo/physics/physics.hh>
00041 #include <gazebo/sensors/Sensor.hh>
00042 #include <gazebo/gazebo_config.h>
00043 #include <ros/ros.h>
00044 
00045 #ifndef GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
00046 # if GAZEBO_MAJOR_VERSION >= 7
00047 #define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using std::dynamic_pointer_cast
00048 # else
00049 #define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using boost::dynamic_pointer_cast
00050 # endif
00051 #endif
00052 
00053 namespace gazebo
00054 {
00055 
00061 inline std::string GetModelName ( const sensors::SensorPtr &parent )
00062 {
00063     std::string modelName;
00064     std::vector<std::string> values;
00065 # if GAZEBO_MAJOR_VERSION >= 7
00066     std::string scopedName = parent->ScopedName();
00067 # else
00068     std::string scopedName = parent->GetScopedName();
00069 #endif
00070     boost::replace_all ( scopedName, "::", "," );
00071     boost::split ( values, scopedName, boost::is_any_of ( "," ) );
00072     if ( values.size() < 2 ) {
00073         modelName = "";
00074     } else {
00075         modelName = values[1];
00076     }
00077     return modelName;
00078 }
00079 
00087 inline std::string GetRobotNamespace ( const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo = NULL )
00088 {
00089     std::string name_space;
00090     std::stringstream ss;
00091     if ( sdf->HasElement ( "robotNamespace" ) ) {
00092         name_space = sdf->Get<std::string> ( "robotNamespace" );
00093         if ( name_space.empty() ) {
00094             ss << "the 'robotNamespace' param was empty";
00095             name_space = GetModelName ( parent );
00096         } else {
00097             ss << "Using the 'robotNamespace' param: '" <<  name_space << "'";
00098         }
00099     } else {
00100         ss << "the 'robotNamespace' param did not exit";
00101     }
00102     if ( pInfo != NULL ) {
00103         ROS_INFO ( "%s Plugin (robotNamespace = %s), Info: %s" , pInfo, name_space.c_str(), ss.str().c_str() );
00104     }
00105     return name_space;
00106 }
00107 
00113 class GazeboRos
00114 {
00115 private:
00116     sdf::ElementPtr sdf_;       
00117     std::string plugin_;        
00118     std::string namespace_;     
00119     boost::shared_ptr<ros::NodeHandle> rosnode_; 
00120     std::string tf_prefix_;     
00121     std::string info_text;      
00122 
00125     void readCommonParameter ();
00126 public:
00133     GazeboRos ( physics::ModelPtr &_parent, sdf::ElementPtr _sdf, const std::string &_plugin )
00134         : sdf_ ( _sdf ), plugin_ ( _plugin ) {
00135         namespace_ = _parent->GetName ();
00136         if ( !sdf_->HasElement ( "robotNamespace" ) ) {
00137             ROS_INFO ( "%s missing <robotNamespace>, defaults is %s", plugin_.c_str(), namespace_.c_str() );
00138         }  else {
00139             namespace_ = sdf_->GetElement ( "robotNamespace" )->Get<std::string>();
00140             if ( namespace_.empty() ) {
00141                 namespace_ = _parent->GetName();
00142             }
00143         }
00144         if ( !namespace_.empty() )
00145             this->namespace_ += "/";
00146         rosnode_ = boost::shared_ptr<ros::NodeHandle> ( new ros::NodeHandle ( namespace_ ) );
00147         info_text = plugin_ + "(ns = " + namespace_ + ")";
00148         readCommonParameter ();
00149     }
00156     GazeboRos ( sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_plugin )
00157         : sdf_ ( _sdf ), plugin_ ( _plugin ) {
00158 
00159         std::stringstream ss;
00160         if ( sdf_->HasElement ( "robotNamespace" ) ) {
00161             namespace_ = sdf_->Get<std::string> ( "robotNamespace" );
00162             if ( namespace_.empty() ) {
00163                 ss << "the 'robotNamespace' param was empty";
00164                 namespace_ = GetModelName ( _parent );
00165             } else {
00166                 ss << "Using the 'robotNamespace' param: '" <<  namespace_ << "'";
00167             }
00168         } else {
00169             ss << "the 'robotNamespace' param did not exit";
00170         }
00171         info_text = plugin_ + "(ns = " + namespace_ + ")";
00172         ROS_INFO ( "%s: %s" , info_text.c_str(), ss.str().c_str() );
00173         readCommonParameter ();
00174     }
00175 
00180     const char* info() const;
00185     boost::shared_ptr<ros::NodeHandle>& node();;
00190     const boost::shared_ptr<ros::NodeHandle>& node() const;
00196     std::string resolveTF ( const std::string &_name );
00197 
00205     void getParameterBoolean ( bool &_value, const char *_tag_name, const bool &_default );
00212     void getParameterBoolean ( bool &_value, const char *_tag_name );
00220     physics::JointPtr getJoint ( physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name );
00225     void isInitialized();
00226 
00227 
00235     template <class T>
00236     void getParameter ( T &_value, const char *_tag_name, const T &_default ) {
00237         _value = _default;
00238         if ( !sdf_->HasElement ( _tag_name ) ) {
00239             ROS_WARN ( "%s: missing <%s> default is %s", info(), _tag_name, boost::lexical_cast<std::string> ( _default ).c_str() );
00240         } else {
00241             this->getParameter<T> ( _value, _tag_name );
00242         }
00243     }
00251     template <class T>
00252     void getParameter ( T &_value, const char *_tag_name ) {
00253         if ( sdf_->HasElement ( _tag_name ) ) {
00254             _value = sdf_->GetElement ( _tag_name )->Get<T>();
00255         }
00256         ROS_DEBUG ( "%s: <%s> = %s", info(), _tag_name, boost::lexical_cast<std::string> ( _value ).c_str() );
00257 
00258     }
00259 
00267     template <class T>
00268     void getParameter ( T &_value, const char *_tag_name, const std::map<std::string, T> &_options, const T &_default ) {
00269         _value = _default;
00270         if ( !sdf_->HasElement ( _tag_name ) ) {
00271             ROS_WARN ( "%s: missing <%s> default is %s", info(), _tag_name, boost::lexical_cast<std::string> ( _default ).c_str() );
00272         } else {
00273             this->getParameter<T> ( _value, _tag_name, _options );
00274         }
00275     }
00283     template <class T>
00284     void getParameter ( T &_value, const char *_tag_name, const std::map<std::string, T> &_options ) {
00285         typename std::map<std::string, T >::const_iterator it;
00286         if ( sdf_->HasElement ( _tag_name ) ) {
00287             std::string value = sdf_->GetElement ( _tag_name )->Get<std::string>();
00288             it = _options.find ( value );
00289             if ( it == _options.end() ) {
00290                 ROS_WARN ( "%s: <%s> no matching key to %s", info(), _tag_name, value.c_str() );
00291             } else {
00292                 _value = it->second;
00293             }
00294         }
00295         ROS_DEBUG ( "%s: <%s> = %s := %s",  info(), _tag_name, ( it == _options.end() ?"default":it->first.c_str() ), boost::lexical_cast<std::string> ( _value ).c_str() );
00296     }
00297 };
00298 
00299 typedef boost::shared_ptr<GazeboRos> GazeboRosPtr;
00300 }
00301 #endif
00302 
00303 
00304 


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:23