param_utils.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Southwest Research Institute
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 are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include <sstream>
33 
35 #include "industrial_utils/utils.h"
36 #include "ros/ros.h"
37 #include <urdf/urdfdom_compatibility.h>
38 
39 namespace industrial_utils
40 {
41 namespace param
42 {
43 bool getListParam(const std::string param_name, std::vector<std::string> & list_param)
44 {
45  bool rtn = false;
46  XmlRpc::XmlRpcValue rpc_list;
47 
48  list_param.clear(); //clear out return value
49 
50  rtn = ros::param::get(param_name, rpc_list);
51 
52  if (rtn)
53  {
54  rtn = (rpc_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
55 
56  if (rtn)
57  {
58 
59  for (int i = 0; i < rpc_list.size(); ++i)
60  {
61  rtn = (rpc_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
62  if (rtn)
63  {
64  ROS_INFO_STREAM("Adding " << rpc_list[i] << " to list parameter");
65  list_param.push_back(static_cast<std::string>(rpc_list[i]));
66  }
67  else
68  {
69  ROS_ERROR_STREAM("List item for: " << param_name << " not of string type");
70  }
71  }
72  }
73  else
74  {
75  ROS_ERROR_STREAM("Parameter: " << param_name << " not of list type");
76  }
77  }
78  else
79  {
80  ROS_ERROR_STREAM("Failed to get parameter: " << param_name);
81  }
82 
83  return rtn;
84 
85 }
86 
87 std::string vec2str(const std::vector<std::string> &vec)
88 {
89  std::string s, delim = ", ";
90  std::stringstream ss;
91  std::copy(vec.begin(), vec.end(), std::ostream_iterator<std::string>(ss, delim.c_str()));
92  s = ss.str();
93  return "[" + s.erase(s.length()-2) + "]";
94 }
95 
96 bool getJointNames(const std::string joint_list_param, const std::string urdf_param,
97  std::vector<std::string> & joint_names)
98 {
99  joint_names.clear();
100 
101  // 1) Try to read explicit list of joint names
102  if (ros::param::has(joint_list_param) && getListParam(joint_list_param, joint_names))
103  {
104  ROS_INFO_STREAM("Found user-specified joint names in '" << joint_list_param << "': " << vec2str(joint_names));
105  return true;
106  }
107  else
108  ROS_WARN_STREAM("Unable to find user-specified joint names in '" << joint_list_param << "'");
109 
110  // 2) Try to find joint names from URDF model
111  urdf::Model model;
112  if ( ros::param::has(urdf_param)
113  && model.initParam(urdf_param)
114  && findChainJointNames(model.getRoot(), true, joint_names) )
115  {
116  ROS_INFO_STREAM("Using joint names from URDF: '" << urdf_param << "': " << vec2str(joint_names));
117  return true;
118  }
119  else
120  ROS_WARN_STREAM("Unable to find URDF joint names in '" << urdf_param << "'");
121 
122  // 3) Raise an error
124  "Cannot find user-specified joint names. Tried ROS parameter '" << joint_list_param << "'"
125  << " and the URDF in '" << urdf_param << "'.");
126  return false;
127 }
128 
129 bool getJointVelocityLimits(const std::string urdf_param_name, std::map<std::string, double> &velocity_limits)
130 {
131  urdf::Model model;
132  std::map<std::string, urdf::JointSharedPtr >::iterator iter;
133 
134  if (!ros::param::has(urdf_param_name) || !model.initParam(urdf_param_name))
135  return false;
136 
137  velocity_limits.clear();
138  for (iter=model.joints_.begin(); iter!=model.joints_.end(); ++iter)
139  {
140  std::string joint_name(iter->first);
141  urdf::JointLimitsSharedPtr limits = iter->second->limits;
142  if ( limits && (limits->velocity > 0) )
143  velocity_limits.insert(std::pair<std::string,double>(joint_name,limits->velocity));
144  }
145 
146  return true;
147 }
148 
149 } //industrial_utils::param
150 } //industrial_utils
bool findChainJointNames(const urdf::LinkConstSharedPtr &link, bool ignore_fixed, std::vector< std::string > &joint_names)
Definition: utils.cpp:72
bool param(const std::string &param_name, T &param_val, const T &default_val)
bool getJointNames(const std::string joint_list_param, const std::string urdf_param, std::vector< std::string > &joint_names)
Tries to get a set of joint names using several fallback methods: 1) check parameter for an explicit ...
Definition: param_utils.cpp:96
int size() const
XmlRpcServer s
Type const & getType() const
std::string vec2str(const std::vector< std::string > &vec)
Definition: param_utils.cpp:87
ROSCPP_DECL bool get(const std::string &key, std::string &s)
ROSCPP_DECL bool has(const std::string &key)
URDF_EXPORT bool initParam(const std::string &param)
#define ROS_WARN_STREAM(args)
bool getJointVelocityLimits(const std::string urdf_param_name, std::map< std::string, double > &velocity_limits)
Tries to read joint velocity limits from the specified URDF parameter.
#define ROS_INFO_STREAM(args)
bool getListParam(const std::string param_name, std::vector< std::string > &list_param)
Gets parameter list as vector of strings.
Definition: param_utils.cpp:43
#define ROS_ERROR_STREAM(args)


industrial_utils
Author(s): Shaun Edwards
autogenerated on Sat Sep 21 2019 03:30:08