gazebo_ros_utils.cpp
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 
00035 #include <gazebo_plugins/gazebo_ros_utils.h>
00036 
00037 #include <sdf/sdf.hh>
00038 #include <tf/transform_listener.h>
00039 
00040 using namespace gazebo;
00041 
00042 const char* GazeboRos::info() const {
00043     return info_text.c_str();
00044 }
00045 boost::shared_ptr<ros::NodeHandle>& GazeboRos ::node() {
00046     return rosnode_;
00047 }
00048 const boost::shared_ptr<ros::NodeHandle>& GazeboRos ::node() const {
00049     return rosnode_;
00050 }
00051 
00052 std::string GazeboRos ::resolveTF(const std::string &name) {
00053     return tf::resolve(tf_prefix_, name);
00054 }
00055 void GazeboRos ::readCommonParameter() {
00056 
00057     ROS_INFO("Starting plugin %s!", info());
00058 
00059     std::string debugLevel;
00060     getParameter<std::string>(debugLevel, "rosDebugLevel", "na");
00061     if(boost::iequals(debugLevel, std::string("Debug"))) {
00062         if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) {
00063             ros::console::notifyLoggerLevelsChanged();
00064         }
00065     } else if(boost::iequals(debugLevel, std::string("Info"))) {
00066         if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) {
00067             ros::console::notifyLoggerLevelsChanged();
00068         }
00069     } else if(boost::iequals(debugLevel, std::string("Warn"))) {
00070         if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn) ) {
00071             ros::console::notifyLoggerLevelsChanged();
00072         }
00073     } else if(boost::iequals(debugLevel, std::string("Error"))) {
00074         if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error) ) {
00075             ros::console::notifyLoggerLevelsChanged();
00076         }
00077     } else if(boost::iequals(debugLevel, std::string("Fatal"))) {
00078         if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Fatal) ) {
00079             ros::console::notifyLoggerLevelsChanged();
00080         }
00081     }
00082     if (sdf_->HasElement("rosDebugLevel")) {
00083         ROS_INFO("%s: <rosDebugLevel> = %s", info(), debugLevel.c_str());
00084     }
00085 
00086 
00087     tf_prefix_ = tf::getPrefixParam(*rosnode_);
00088     if(tf_prefix_.empty())
00089     {
00090         tf_prefix_ = namespace_;
00091         boost::trim_right_if(tf_prefix_,boost::is_any_of("/"));
00092     }
00093     ROS_INFO("%s: <tf_prefix> = %s", info(), tf_prefix_.c_str());
00094 }
00095 
00096 
00097 void GazeboRos ::getParameterBoolean(bool &_value, const char *_tag_name, const bool &_default) {
00098     _value = _default;
00099     if (!sdf_->HasElement(_tag_name)) {
00100         ROS_WARN("%s: missing <%s> default is %s",
00101                  info(), _tag_name,  (_default?"ture":"false"));
00102     } else {
00103         getParameterBoolean(_value, _tag_name);
00104     }
00105 
00106 }
00107 void GazeboRos ::getParameterBoolean(bool &_value, const char *_tag_name) {
00108 
00109     if (sdf_->HasElement(_tag_name)) {
00110         std::string value = sdf_->GetElement(_tag_name)->Get<std::string>();
00111         if(boost::iequals(value, std::string("true")))
00112         {
00113             _value = true;
00114         }
00115         else if(boost::iequals(value, std::string("false")))
00116         {
00117             _value = false;
00118         }
00119         else
00120         {
00121             ROS_WARN("%s: <%s> must be either true or false",
00122                      info(), _tag_name);
00123         }
00124     }
00125     ROS_DEBUG("%s: <%s> = %s",
00126               info(), _tag_name,  (_value?"ture":"false"));
00127 
00128 }
00129 
00130 physics::JointPtr GazeboRos::getJoint(physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name) {
00131     std::string joint_name;
00132     getParameter<std::string>(joint_name, _tag_name, _joint_default_name);
00133     physics::JointPtr joint = _parent->GetJoint(joint_name);
00134     if (!joint)
00135     {
00136         char error[200];
00137         snprintf(error, 200,
00138                  "%s: couldn't get wheel hinge joint named %s",
00139                  info() , joint_name.c_str());
00140         gzthrow(error);
00141     }
00142     return joint;
00143 }
00144 
00145 void GazeboRos::isInitialized() {
00146     if (!ros::isInitialized())
00147     {
00148         ROS_FATAL_STREAM(info() << "A ROS node for Gazebo has not been initialized, unable to load plugin. "
00149                          << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00150         return;
00151     }
00152 }
00153 
00154 
00155 


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25