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
45 #include <rosparam_shortcuts/rosparam_shortcuts.h>
46 
47 // Eigen/TF conversion
48 #include <eigen_conversions/eigen_msg.h>
49 
50 namespace rosparam_shortcuts
51 {
52 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
53 {
54  // Load a param
55  if (!nh.hasParam(param_name))
56  {
57  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
58  return false;
59  }
60  nh.getParam(param_name, value);
61  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
62  << value);
63 
64  return true;
65 }
66 
67 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &params_namespace,
68  std::map<std::string, bool> &parameters)
69 {
70  // Load a param
71  if (!nh.hasParam(params_namespace))
72  {
73  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameters in namespace '" << nh.getNamespace() << "/"
74  << params_namespace << "'.");
75  return false;
76  }
77  nh.getParam(params_namespace, parameters);
78 
79  // Debug
80  for (std::map<std::string, bool>::const_iterator param_it = parameters.begin(); param_it != parameters.end();
81  param_it++)
82  {
83  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << params_namespace << "/"
84  << param_it->first << "' with value " << param_it->second);
85  }
86 
87  return true;
88 }
89 
90 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, double &value)
91 {
92  // Load a param
93  if (!nh.hasParam(param_name))
94  {
95  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
96  return false;
97  }
98  nh.getParam(param_name, value);
99  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
100  << value);
101 
102  return true;
103 }
104 
105 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
106  std::vector<double> &values)
107 {
108  // Load a param
109  if (!nh.hasParam(param_name))
110  {
111  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
112  return false;
113  }
114  nh.getParam(param_name, values);
115 
116  if (values.empty())
117  ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
118  ".");
119 
120  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name
121  << "' with values [" << getDebugArrayString(values) << "]");
122 
123  return true;
124 }
125 
126 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, int &value)
127 {
128  // Load a param
129  if (!nh.hasParam(param_name))
130  {
131  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
132  return false;
133  }
134  nh.getParam(param_name, value);
135  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
136  << value);
137 
138  return true;
139 }
140 
141 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, std::size_t &value)
142 {
143  // Load a param
144  if (!nh.hasParam(param_name))
145  {
146  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
147  return false;
148  }
149  int nonsigned_value;
150  nh.getParam(param_name, nonsigned_value);
151  value = nonsigned_value;
152  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
153  << value);
154 
155  return true;
156 }
157 
158 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, std::string &value)
159 {
160  // Load a param
161  if (!nh.hasParam(param_name))
162  {
163  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
164  return false;
165  }
166  nh.getParam(param_name, value);
167  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
168  << value);
169 
170  return true;
171 }
172 
173 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
174  std::vector<std::string> &values)
175 {
176  // Load a param
177  if (!nh.hasParam(param_name))
178  {
179  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
180  return false;
181  }
182  nh.getParam(param_name, values);
183 
184  if (values.empty())
185  ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
186  ".");
187 
188  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
189  << getDebugArrayString(values));
190 
191  return true;
192 }
193 
194 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, ros::Duration &value)
195 {
196  double temp_value;
197  // Load a param
198  if (!nh.hasParam(param_name))
199  {
200  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
201  return false;
202  }
203  nh.getParam(param_name, temp_value);
204  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
205  << value);
206 
207  // Convert to ros::Duration
208  value = ros::Duration(temp_value);
209 
210  return true;
211 }
212 
213 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
214  Eigen::Isometry3d &value)
215 {
216  std::vector<double> values;
217 
218  // Load a param
219  if (!nh.hasParam(param_name))
220  {
221  ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
222  return false;
223  }
224  nh.getParam(param_name, values);
225 
226  if (values.empty())
227  ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
228  ".");
229 
230  ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name
231  << "' with values [" << getDebugArrayString(values) << "]");
232 
233  // Convert to Eigen::Isometry3d
234  convertDoublesToEigen(parent_name, values, value);
235 
236  return true;
237 }
238 
239 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
240  geometry_msgs::Pose &value)
241 {
242  Eigen::Isometry3d eigen_pose;
243  if (!get(parent_name, nh, param_name, eigen_pose))
244  return false;
245 
246  tf::poseEigenToMsg(eigen_pose, value);
247  return true;
248 }
249 
250 std::string getDebugArrayString(std::vector<double> values)
251 {
252  std::stringstream debug_values;
253  for (std::size_t i = 0; i < values.size(); ++i)
254  {
255  debug_values << values[i] << ",";
256  }
257  return debug_values.str();
258 }
259 
260 std::string getDebugArrayString(std::vector<std::string> values)
261 {
262  std::stringstream debug_values;
263  for (std::size_t i = 0; i < values.size(); ++i)
264  {
265  debug_values << values[i] << ",";
266  }
267  return debug_values.str();
268 }
269 
270 bool convertDoublesToEigen(const std::string &parent_name, std::vector<double> values, Eigen::Isometry3d &transform)
271 {
272  if (values.size() == 6)
273  {
274  // This version is correct RPY
275  Eigen::AngleAxisd roll_angle(values[3], Eigen::Vector3d::UnitX());
276  Eigen::AngleAxisd pitch_angle(values[4], Eigen::Vector3d::UnitY());
277  Eigen::AngleAxisd yaw_angle(values[5], Eigen::Vector3d::UnitZ());
278  Eigen::Quaternion<double> quaternion = roll_angle * pitch_angle * yaw_angle;
279 
280  transform = Eigen::Translation3d(values[0], values[1], values[2]) * quaternion;
281 
282  return true;
283  }
284  else if (values.size() == 7)
285  {
286  // Quaternion
287  transform = Eigen::Translation3d(values[0], values[1], values[2]) *
288  Eigen::Quaterniond(values[3], values[4], values[5], values[6]);
289  return true;
290  }
291  else
292  {
293  ROS_ERROR_STREAM_NAMED(parent_name, "Invalid number of doubles provided for transform, size=" << values.size());
294  return false;
295  }
296 }
297 
298 void shutdownIfError(const std::string &parent_name, std::size_t error_count)
299 {
300  if (!error_count)
301  return;
302 
303  ROS_ERROR_STREAM_NAMED(parent_name, "Missing " << error_count << " ros parameters that are required. Shutting down "
304  "to prevent undefined behaviors");
305  ros::shutdown();
306  exit(0);
307 }
308 
309 } // namespace rosparam_shortcuts
rosparam_shortcuts::convertDoublesToEigen
bool convertDoublesToEigen(const std::string &parent_name, std::vector< double > values, Eigen::Isometry3d &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.
Definition: rosparam_shortcuts.cpp:302
rosparam_shortcuts
Definition: rosparam_shortcuts.h:59
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
rosparam_shortcuts::get
bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
Get a paremeter from the ROS param server. Note that does not provide for default values.
Definition: rosparam_shortcuts.cpp:84
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ros::shutdown
ROSCPP_DECL void shutdown()
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
eigen_msg.h
rosparam_shortcuts::getDebugArrayString
std::string getDebugArrayString(std::vector< double > values)
Output a string of values from an array for debugging.
Definition: rosparam_shortcuts.cpp:282
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
tf::poseEigenToMsg
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
rosparam_shortcuts.h
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
ros::Duration
rosparam_shortcuts::shutdownIfError
void shutdownIfError(const std::string &parent_name, std::size_t error_count)
Check that there were no errors, and if there were, shutdown.
Definition: rosparam_shortcuts.cpp:330
ros::NodeHandle

rosparam_shortcuts
Author(s): Dave Coleman
autogenerated on Wed Nov 27 2024 03:47:31