gazebo_ros_utils.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Markus Bader <markus.bader@tuwien.ac.at>
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *********************************************************************/
33 
34 #ifndef GAZEBO_ROS_UTILS_H
35 #define GAZEBO_ROS_UTILS_H
36 #include <map>
37 #include <boost/algorithm/string.hpp>
38 
39 #include <gazebo/common/common.hh>
40 #include <gazebo/physics/physics.hh>
41 #include <gazebo/sensors/Sensor.hh>
42 #include <gazebo/gazebo_config.h>
43 #include <ros/ros.h>
44 
45 #ifndef GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
46 #define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using std::dynamic_pointer_cast
47 #endif
48 
49 namespace gazebo
50 {
51 
57 inline std::string GetModelName ( const sensors::SensorPtr &parent )
58 {
59  std::string modelName;
60  std::vector<std::string> values;
61  std::string scopedName = parent->ScopedName();
62  boost::replace_all ( scopedName, "::", "," );
63  boost::split ( values, scopedName, boost::is_any_of ( "," ) );
64  if ( values.size() < 2 ) {
65  modelName = "";
66  } else {
67  modelName = values[1];
68  }
69  return modelName;
70 }
71 
79 inline std::string GetRobotNamespace ( const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo = NULL )
80 {
81  std::string name_space;
82  std::stringstream ss;
83  if ( sdf->HasElement ( "robotNamespace" ) ) {
84  name_space = sdf->Get<std::string> ( "robotNamespace" );
85  if ( name_space.empty() ) {
86  ss << "The 'robotNamespace' param was empty";
87  name_space = GetModelName ( parent );
88  } else {
89  ss << "Using the 'robotNamespace' param: '" << name_space << "'";
90  }
91  } else {
92  ss << "The 'robotNamespace' param did not exit";
93  }
94  if ( pInfo != NULL ) {
95  ROS_INFO_NAMED("utils", "%s Plugin: %s" , pInfo, ss.str().c_str() );
96  }
97  return name_space;
98 }
99 
106 {
107 private:
108  sdf::ElementPtr sdf_;
109  std::string plugin_;
110  std::string namespace_;
112  std::string tf_prefix_;
113  std::string info_text;
114 
117  void readCommonParameter ();
118 public:
125  GazeboRos ( physics::ModelPtr &_parent, sdf::ElementPtr _sdf, const std::string &_plugin )
126  : sdf_ ( _sdf ), plugin_ ( _plugin ) {
127  namespace_ = _parent->GetName ();
128  if ( !sdf_->HasElement ( "robotNamespace" ) ) {
129  ROS_INFO_NAMED("utils", "%s missing <robotNamespace>, defaults is %s", plugin_.c_str(), namespace_.c_str() );
130  } else {
131  namespace_ = sdf_->GetElement ( "robotNamespace" )->Get<std::string>();
132  if ( namespace_.empty() ) {
133  namespace_ = _parent->GetName();
134  }
135  }
136  if ( !namespace_.empty() )
137  this->namespace_ += "/";
138  rosnode_ = boost::shared_ptr<ros::NodeHandle> ( new ros::NodeHandle ( namespace_ ) );
139  info_text = plugin_ + "(ns = " + namespace_ + ")";
141  }
148  GazeboRos ( sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_plugin )
149  : sdf_ ( _sdf ), plugin_ ( _plugin ) {
150 
151  std::stringstream ss;
152  if ( sdf_->HasElement ( "robotNamespace" ) ) {
153  namespace_ = sdf_->Get<std::string> ( "robotNamespace" );
154  if ( namespace_.empty() ) {
155  ss << "the 'robotNamespace' param was empty";
156  namespace_ = GetModelName ( _parent );
157  } else {
158  ss << "Using the 'robotNamespace' param: '" << namespace_ << "'";
159  }
160  } else {
161  ss << "the 'robotNamespace' param did not exit";
162  }
163  rosnode_ = boost::shared_ptr<ros::NodeHandle> ( new ros::NodeHandle ( namespace_ ) );
164  info_text = plugin_ + "(ns = " + namespace_ + ")";
165  ROS_INFO_NAMED("utils", "%s: %s" , info_text.c_str(), ss.str().c_str() );
167  }
168 
173  const char* info() const;
189  std::string resolveTF ( const std::string &_name );
190 
198  void getParameterBoolean ( bool &_value, const char *_tag_name, const bool &_default );
205  void getParameterBoolean ( bool &_value, const char *_tag_name );
213  physics::JointPtr getJoint ( physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name );
218  void isInitialized();
219 
220 
228  template <class T>
229  void getParameter ( T &_value, const char *_tag_name, const T &_default ) {
230  _value = _default;
231  if ( !sdf_->HasElement ( _tag_name ) ) {
232  ROS_WARN_NAMED("utils", "%s: missing <%s> default is %s", info(), _tag_name, boost::lexical_cast<std::string> ( _default ).c_str() );
233  } else {
234  this->getParameter<T> ( _value, _tag_name );
235  }
236  }
244  template <class T>
245  void getParameter ( T &_value, const char *_tag_name ) {
246  if ( sdf_->HasElement ( _tag_name ) ) {
247  _value = sdf_->GetElement ( _tag_name )->Get<T>();
248  }
249  ROS_DEBUG_NAMED("utils", "%s: <%s> = %s", info(), _tag_name, boost::lexical_cast<std::string> ( _value ).c_str() );
250 
251  }
252 
260  template <class T>
261  void getParameter ( T &_value, const char *_tag_name, const std::map<std::string, T> &_options, const T &_default ) {
262  _value = _default;
263  if ( !sdf_->HasElement ( _tag_name ) ) {
264  ROS_WARN_NAMED("utils", "%s: missing <%s> default is %s", info(), _tag_name, boost::lexical_cast<std::string> ( _default ).c_str() );
265  } else {
266  this->getParameter<T> ( _value, _tag_name, _options );
267  }
268  }
276  template <class T>
277  void getParameter ( T &_value, const char *_tag_name, const std::map<std::string, T> &_options ) {
278  typename std::map<std::string, T >::const_iterator it;
279  if ( sdf_->HasElement ( _tag_name ) ) {
280  std::string value = sdf_->GetElement ( _tag_name )->Get<std::string>();
281  it = _options.find ( value );
282  if ( it == _options.end() ) {
283  ROS_WARN_NAMED("utils", "%s: <%s> no matching key to %s", info(), _tag_name, value.c_str() );
284  } else {
285  _value = it->second;
286  }
287  }
288  ROS_DEBUG_NAMED("utils", "%s: <%s> = %s := %s", info(), _tag_name, ( it == _options.end() ?"default":it->first.c_str() ), boost::lexical_cast<std::string> ( _value ).c_str() );
289  }
290 };
291 
293 }
294 #endif
#define ROS_INFO_NAMED(name,...)
GazeboRos(physics::ModelPtr &_parent, sdf::ElementPtr _sdf, const std::string &_plugin)
std::string GetModelName(const sensors::SensorPtr &parent)
#define ROS_WARN_NAMED(name,...)
boost::shared_ptr< ros::NodeHandle > & node()
std::vector< double > values
boost::shared_ptr< ros::NodeHandle > rosnode_
name of the launched node
sdf::ElementPtr sdf_
physics::JointPtr getJoint(physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name)
std::string resolveTF(const std::string &_name)
void getParameter(T &_value, const char *_tag_name, const T &_default)
#define ROS_DEBUG_NAMED(name,...)
void getParameter(T &_value, const char *_tag_name, const std::map< std::string, T > &_options)
std::string tf_prefix_
rosnode
GazeboRos(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_plugin)
void getParameter(T &_value, const char *_tag_name)
std::string info_text
prefix for the ros tf plublisher if not set it uses the namespace_
void getParameterBoolean(bool &_value, const char *_tag_name, const bool &_default)
std::string plugin_
sdf to read
std::string namespace_
name of the plugin class
const char * info() const
void getParameter(T &_value, const char *_tag_name, const std::map< std::string, T > &_options, const T &_default)
std::string GetRobotNamespace(const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo=NULL)
Reads the name space tag of a sensor plugin.
boost::shared_ptr< GazeboRos > GazeboRosPtr
void readCommonParameter()
info text for log messages to identify the node


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39