34 #ifndef GAZEBO_ROS_UTILS_H 35 #define GAZEBO_ROS_UTILS_H 37 #include <boost/algorithm/string.hpp> 39 #include <gazebo/common/common.hh> 40 #include <gazebo/physics/physics.hh> 41 #include <gazebo/sensors/Sensor.hh> 42 #include <gazebo/gazebo_config.h> 45 #ifndef GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST 46 #define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using std::dynamic_pointer_cast 57 inline std::string
GetModelName (
const sensors::SensorPtr &parent )
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 ) {
67 modelName = values[1];
79 inline std::string
GetRobotNamespace (
const sensors::SensorPtr &parent,
const sdf::ElementPtr &sdf,
const char *pInfo = NULL )
81 std::string name_space;
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";
89 ss <<
"Using the 'robotNamespace' param: '" << name_space <<
"'";
92 ss <<
"The 'robotNamespace' param did not exit";
94 if ( pInfo != NULL ) {
95 ROS_INFO_NAMED(
"utils",
"%s Plugin: %s" , pInfo, ss.str().c_str() );
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() );
131 namespace_ = sdf_->GetElement (
"robotNamespace" )->Get<std::string>();
132 if ( namespace_.empty() ) {
133 namespace_ = _parent->GetName();
136 if ( !namespace_.empty() )
137 this->namespace_ +=
"/";
139 info_text = plugin_ +
"(ns = " + namespace_ +
")";
148 GazeboRos ( sensors::SensorPtr _parent, sdf::ElementPtr _sdf,
const std::string &_plugin )
149 : sdf_ ( _sdf ), plugin_ ( _plugin ) {
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";
158 ss <<
"Using the 'robotNamespace' param: '" << namespace_ <<
"'";
161 ss <<
"the 'robotNamespace' param did not exit";
164 info_text = plugin_ +
"(ns = " + namespace_ +
")";
165 ROS_INFO_NAMED(
"utils",
"%s: %s" , info_text.c_str(), ss.str().c_str() );
173 const char*
info()
const;
189 std::string
resolveTF (
const std::string &_name );
213 physics::JointPtr
getJoint ( physics::ModelPtr &_parent,
const char *_tag_name,
const std::string &_joint_default_name );
229 void getParameter ( T &_value,
const char *_tag_name,
const T &_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() );
234 this->getParameter<T> ( _value, _tag_name );
246 if ( sdf_->HasElement ( _tag_name ) ) {
247 _value = sdf_->GetElement ( _tag_name )->Get<T>();
249 ROS_DEBUG_NAMED(
"utils",
"%s: <%s> = %s",
info(), _tag_name, boost::lexical_cast<std::string> ( _value ).c_str() );
261 void getParameter ( T &_value,
const char *_tag_name,
const std::map<std::string, T> &_options,
const T &_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() );
266 this->getParameter<T> ( _value, _tag_name, _options );
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() );
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() );
#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
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