Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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();
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
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
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
00123 const int NUM_JOINTS = 6;
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 }
00156 }