param_utils.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2012, Southwest Research Institute
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 are met:
00009  *
00010  *       * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *       * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *       * Neither the name of the Southwest Research Institute, nor the names
00016  *       of its contributors may be used to endorse or promote products derived
00017  *       from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #include <sstream>
00033 
00034 #include "industrial_utils/param_utils.h"
00035 #include "industrial_utils/utils.h"
00036 #include "ros/ros.h"
00037 #include "urdf/model.h"
00038 
00039 namespace industrial_utils
00040 {
00041 namespace param
00042 {
00043 bool getListParam(const std::string param_name, std::vector<std::string> & list_param)
00044 {
00045   bool rtn = false;
00046   XmlRpc::XmlRpcValue rpc_list;
00047 
00048   list_param.clear(); //clear out return value
00049 
00050   rtn = ros::param::get(param_name, rpc_list);
00051 
00052   if (rtn)
00053   {
00054     rtn = (rpc_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00055 
00056     if (rtn)
00057     {
00058 
00059       for (int i = 0; i < rpc_list.size(); ++i)
00060       {
00061         rtn = (rpc_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00062         if (rtn)
00063         {
00064           ROS_INFO_STREAM("Adding " << rpc_list[i] << " to list parameter");
00065           list_param.push_back(static_cast<std::string>(rpc_list[i]));
00066         }
00067         else
00068         {
00069           ROS_ERROR_STREAM("List item for: " << param_name << " not of string type");
00070         }
00071       }
00072     }
00073     else
00074     {
00075       ROS_ERROR_STREAM("Parameter: " << param_name << " not of list type");
00076     }
00077   }
00078   else
00079   {
00080     ROS_ERROR_STREAM("Failed to get parameter: " << param_name);
00081   }
00082 
00083   return rtn;
00084 
00085 }
00086 
00087 std::string vec2str(const std::vector<std::string> &vec)
00088 {
00089   std::string s, delim = ", ";
00090   std::stringstream ss;
00091   std::copy(vec.begin(), vec.end(), std::ostream_iterator<std::string>(ss, delim.c_str()));
00092   s = ss.str();
00093   return "[" + s.erase(s.length()-2) + "]";
00094 }
00095 
00096 bool getJointNames(const std::string joint_list_param, const std::string urdf_param,
00097                            std::vector<std::string> & joint_names)
00098 {
00099   joint_names.clear();
00100 
00101   // 1) Try to read explicit list of joint names
00102   if (ros::param::has(joint_list_param) && getListParam(joint_list_param, joint_names))
00103   {
00104     ROS_INFO_STREAM("Found user-specified joint names in '" << joint_list_param << "': " << vec2str(joint_names));
00105     return true;
00106   }
00107   else
00108     ROS_WARN_STREAM("Unable to find user-specified joint names in '" << joint_list_param << "'");
00109 
00110   // 2) Try to find joint names from URDF model
00111   urdf::Model model;
00112   if ( ros::param::has(urdf_param)
00113        && model.initParam(urdf_param)
00114        && findChainJointNames(model.getRoot(), true, joint_names) )
00115   {
00116     ROS_INFO_STREAM("Using joint names from URDF: '" << urdf_param << "': " << vec2str(joint_names));
00117     return true;
00118   }
00119   else
00120     ROS_WARN_STREAM("Unable to find URDF joint names in '" << urdf_param << "'");
00121 
00122   // 3) Use default joint-names
00123   const int NUM_JOINTS = 6;  //Most robots have 6 joints
00124   for (int i=0; i<NUM_JOINTS; ++i)
00125   {
00126     std::stringstream tmp;
00127     tmp << "joint_" << i+1;
00128     joint_names.push_back(tmp.str());
00129   }
00130 
00131   ROS_INFO_STREAM("Using standard 6-DOF joint names: " << vec2str(joint_names));
00132   return true;
00133 }
00134 
00135 bool getJointVelocityLimits(const std::string urdf_param_name, std::map<std::string, double> &velocity_limits)
00136 {
00137   urdf::Model model;
00138   std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator iter;
00139 
00140   if (!ros::param::has(urdf_param_name) || !model.initParam(urdf_param_name))
00141     return false;
00142     
00143   velocity_limits.clear();
00144   for (iter=model.joints_.begin(); iter!=model.joints_.end(); ++iter)
00145   {
00146     std::string joint_name(iter->first);
00147     boost::shared_ptr<urdf::JointLimits> limits = iter->second->limits;
00148     if ( limits && (limits->velocity > 0) )
00149       velocity_limits.insert(std::pair<std::string,double>(joint_name,limits->velocity));
00150   }
00151   
00152   return true;
00153 }
00154 
00155 } //industrial_utils::param
00156 } //industrial_utils


industrial_utils
Author(s): Shaun M. Edwards
autogenerated on Fri Aug 28 2015 11:12:05