rosparam_shortcuts.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, University of Colorado, Boulder
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 Univ of CO, Boulder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman <dave@dav.ee>
36  Desc: Helpers for loading parameters from the parameter server
37 */
38 
39 // C++
40 #include <string>
41 #include <vector>
42 #include <map>
43 
44 // this package
46 
47 namespace rosparam_shortcuts
48 {
49 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
50 {
51  // Load a param
52  if (!nh.hasParam(param_name))
53  {
54  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
55  return false;
56  }
57  nh.getParam(param_name, value);
58  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
59  << value);
60 
61  return true;
62 }
63 
64 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &params_namespace,
65  std::map<std::string, bool> &parameters)
66 {
67  // Load a param
68  if (!nh.hasParam(params_namespace))
69  {
70  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameters in namespace '" << nh.getNamespace() << "/"
71  << params_namespace << "'.");
72  return false;
73  }
74  nh.getParam(params_namespace, parameters);
75 
76  // Debug
77  for (std::map<std::string, bool>::const_iterator param_it = parameters.begin(); param_it != parameters.end();
78  param_it++)
79  {
80  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << params_namespace << "/"
81  << param_it->first << "' with value " << param_it->second);
82  }
83 
84  return true;
85 }
86 
87 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, double &value)
88 {
89  // Load a param
90  if (!nh.hasParam(param_name))
91  {
92  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
93  return false;
94  }
95  nh.getParam(param_name, value);
96  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
97  << value);
98 
99  return true;
100 }
101 
102 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
103  std::vector<double> &values)
104 {
105  // Load a param
106  if (!nh.hasParam(param_name))
107  {
108  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
109  return false;
110  }
111  nh.getParam(param_name, values);
112 
113  if (values.empty())
114  ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
115  ".");
116 
117  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name
118  << "' with values [" << getDebugArrayString(values) << "]");
119 
120  return true;
121 }
122 
123 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, int &value)
124 {
125  // Load a param
126  if (!nh.hasParam(param_name))
127  {
128  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
129  return false;
130  }
131  nh.getParam(param_name, value);
132  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
133  << value);
134 
135  return true;
136 }
137 
138 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, std::size_t &value)
139 {
140  // Load a param
141  if (!nh.hasParam(param_name))
142  {
143  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
144  return false;
145  }
146  int nonsigned_value;
147  nh.getParam(param_name, nonsigned_value);
148  value = nonsigned_value;
149  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
150  << value);
151 
152  return true;
153 }
154 
155 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, std::string &value)
156 {
157  // Load a param
158  if (!nh.hasParam(param_name))
159  {
160  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
161  return false;
162  }
163  nh.getParam(param_name, value);
164  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
165  << value);
166 
167  return true;
168 }
169 
170 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
171  std::vector<std::string> &values)
172 {
173  // Load a param
174  if (!nh.hasParam(param_name))
175  {
176  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
177  return false;
178  }
179  nh.getParam(param_name, values);
180 
181  if (values.empty())
182  ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
183  ".");
184 
185  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
186  << getDebugArrayString(values));
187 
188  return true;
189 }
190 
191 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, ros::Duration &value)
192 {
193  double temp_value;
194  // Load a param
195  if (!nh.hasParam(param_name))
196  {
197  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
198  return false;
199  }
200  nh.getParam(param_name, temp_value);
201  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
202  << value);
203 
204  // Convert to ros::Duration
205  value = ros::Duration(temp_value);
206 
207  return true;
208 }
209 
210 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
211  Eigen::Affine3d &value)
212 {
213  std::vector<double> values;
214 
215  // Load a param
216  if (!nh.hasParam(param_name))
217  {
218  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
219  return false;
220  }
221  nh.getParam(param_name, values);
222 
223  if (values.empty())
224  ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
225  ".");
226 
227  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name
228  << "' with values [" << getDebugArrayString(values) << "]");
229 
230  // Convert to Eigen::Affine3d
231  convertDoublesToEigen(parent_name, values, value);
232 
233  return true;
234 }
235 
236 std::string getDebugArrayString(std::vector<double> values)
237 {
238  std::stringstream debug_values;
239  for (std::size_t i = 0; i < values.size(); ++i)
240  {
241  debug_values << values[i] << ",";
242  }
243  return debug_values.str();
244 }
245 
246 std::string getDebugArrayString(std::vector<std::string> values)
247 {
248  std::stringstream debug_values;
249  for (std::size_t i = 0; i < values.size(); ++i)
250  {
251  debug_values << values[i] << ",";
252  }
253  return debug_values.str();
254 }
255 
256 bool convertDoublesToEigen(const std::string &parent_name, std::vector<double> values, Eigen::Affine3d &transform)
257 {
258  if (values.size() == 6)
259  {
260  // This version is correct RPY
261  Eigen::AngleAxisd roll_angle(values[3], Eigen::Vector3d::UnitX());
262  Eigen::AngleAxisd pitch_angle(values[4], Eigen::Vector3d::UnitY());
263  Eigen::AngleAxisd yaw_angle(values[5], Eigen::Vector3d::UnitZ());
264  Eigen::Quaternion<double> quaternion = roll_angle * pitch_angle * yaw_angle;
265 
266  transform = Eigen::Translation3d(values[0], values[1], values[2]) * quaternion;
267 
268  return true;
269  }
270  else if (values.size() == 7)
271  {
272  // Quaternion
273  transform = Eigen::Translation3d(values[0], values[1], values[2]) *
274  Eigen::Quaterniond(values[3], values[4], values[5], values[6]);
275  return true;
276  }
277  else
278  {
279  ROS_ERROR_STREAM_NAMED(parent_name, "Invalid number of doubles provided for transform, size=" << values.size());
280  return false;
281  }
282 }
283 
284 void shutdownIfError(const std::string &parent_name, std::size_t error_count)
285 {
286  if (!error_count)
287  return;
288 
289  ROS_ERROR_STREAM_NAMED(parent_name, "Missing " << error_count << " ros parameters that are required. Shutting down "
290  "to prevent undefined behaviors");
291  ros::shutdown();
292  exit(0);
293 }
294 
295 } // namespace rosparam_shortcuts
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
bool convertDoublesToEigen(const std::string &parent_name, std::vector< double > values, Eigen::Affine3d &transform)
Convert from 6 doubles of [x,y,z] [r,p,y] or 7 doubles of [x, y, z, qw, qx, qy, qz] to a transform...
void shutdownIfError(const std::string &parent_name, std::size_t error_count)
Check that there were no errors, and if there were, shutdown.
ROSCPP_DECL void shutdown()
std::string getDebugArrayString(std::vector< double > values)
Output a string of values from an array for debugging.
#define ROS_WARN_STREAM_NAMED(name, args)


rosparam_shortcuts
Author(s): Dave Coleman
autogenerated on Wed Jun 5 2019 21:59:11