gazebo_ros_utils.cpp
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 
36 
37 #include <sdf/sdf.hh>
38 #include <tf/transform_listener.h>
39 
40 using namespace gazebo;
41 
42 const char* GazeboRos::info() const {
43  return info_text.c_str();
44 }
46  return rosnode_;
47 }
49  return rosnode_;
50 }
51 
52 std::string GazeboRos ::resolveTF(const std::string &name) {
53  return tf::resolve(tf_prefix_, name);
54 }
56 
57  ROS_INFO_NAMED("utils", "Starting plugin %s", info());
58 
59  std::string debugLevel;
60  getParameter<std::string>(debugLevel, "rosDebugLevel", "na");
61  if(boost::iequals(debugLevel, std::string("Debug"))) {
64  }
65  } else if(boost::iequals(debugLevel, std::string("Info"))) {
68  }
69  } else if(boost::iequals(debugLevel, std::string("Warn"))) {
72  }
73  } else if(boost::iequals(debugLevel, std::string("Error"))) {
76  }
77  } else if(boost::iequals(debugLevel, std::string("Fatal"))) {
80  }
81  }
82  if (sdf_->HasElement("rosDebugLevel")) {
83  ROS_INFO_NAMED("utils", "%s: <rosDebugLevel> = %s", info(), debugLevel.c_str());
84  }
85 
86 
88  if(tf_prefix_.empty())
89  {
90  tf_prefix_ = namespace_;
91  boost::trim_right_if(tf_prefix_,boost::is_any_of("/"));
92  }
93  ROS_INFO_NAMED("utils", "%s: <tf_prefix> = %s", info(), tf_prefix_.c_str());
94 }
95 
96 
97 void GazeboRos ::getParameterBoolean(bool &_value, const char *_tag_name, const bool &_default) {
98  _value = _default;
99  if (!sdf_->HasElement(_tag_name)) {
100  ROS_WARN_NAMED("utils", "%s: missing <%s> default is %s",
101  info(), _tag_name, (_default?"true":"false"));
102  } else {
103  getParameterBoolean(_value, _tag_name);
104  }
105 
106 }
107 void GazeboRos ::getParameterBoolean(bool &_value, const char *_tag_name) {
108 
109  if (sdf_->HasElement(_tag_name)) {
110  std::string value = sdf_->GetElement(_tag_name)->Get<std::string>();
111  if(boost::iequals(value, std::string("true")) || boost::iequals(value, std::string("1")))
112  {
113  _value = true;
114  }
115  else if(boost::iequals(value, std::string("false")) || boost::iequals(value, std::string("0")))
116  {
117  _value = false;
118  }
119  else
120  {
121  ROS_WARN_NAMED("utils", "%s: <%s> must be either true or false but is '%s'",
122  info(), _tag_name, value.c_str());
123  }
124  }
125  ROS_DEBUG_NAMED("utils", "%s: <%s> = %s",
126  info(), _tag_name, (_value?"true":"false"));
127 
128 }
129 
130 physics::JointPtr GazeboRos::getJoint(physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name) {
131  std::string joint_name;
132  getParameter<std::string>(joint_name, _tag_name, _joint_default_name);
133  physics::JointPtr joint = _parent->GetJoint(joint_name);
134  if (!joint)
135  {
136  char error[200];
137  snprintf(error, 200,
138  "%s: couldn't get wheel hinge joint named %s",
139  info() , joint_name.c_str());
140  gzthrow(error);
141  }
142  return joint;
143 }
144 
146  if (!ros::isInitialized())
147  {
148  ROS_FATAL_STREAM(info() << "A ROS node for Gazebo has not been initialized, unable to load plugin. "
149  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
150  return;
151  }
152 }
#define ROS_INFO_NAMED(name,...)
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
std::string getPrefixParam(ros::NodeHandle &nh)
#define ROS_WARN_NAMED(name,...)
boost::shared_ptr< ros::NodeHandle > & node()
boost::shared_ptr< ros::NodeHandle > rosnode_
name of the launched node
ROSCPP_DECL bool isInitialized()
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)
std::string resolve(const std::string &prefix, const std::string &frame_name)
#define ROS_DEBUG_NAMED(name,...)
std::string tf_prefix_
rosnode
#define ROS_FATAL_STREAM(args)
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 namespace_
name of the plugin class
const char * info() const
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
#define ROSCONSOLE_DEFAULT_NAME
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