rosparam_shortcuts.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, University of Colorado, Boulder
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
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman <dave@dav.ee>
00036    Desc:   Helpers for loading parameters from the parameter server
00037 */
00038 
00039 // C++
00040 #include <string>
00041 #include <vector>
00042 #include <map>
00043 
00044 // this package
00045 #include <rosparam_shortcuts/rosparam_shortcuts.h>
00046 
00047 namespace rosparam_shortcuts
00048 {
00049 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
00050 {
00051   // Load a param
00052   if (!nh.hasParam(param_name))
00053   {
00054     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00055     return false;
00056   }
00057   nh.getParam(param_name, value);
00058   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
00059                                                            << value);
00060 
00061   return true;
00062 }
00063 
00064 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &params_namespace,
00065          std::map<std::string, bool> &parameters)
00066 {
00067   // Load a param
00068   if (!nh.hasParam(params_namespace))
00069   {
00070     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameters in namespace '" << nh.getNamespace() << "/"
00071                                                                             << params_namespace << "'.");
00072     return false;
00073   }
00074   nh.getParam(params_namespace, parameters);
00075 
00076   // Debug
00077   for (std::map<std::string, bool>::const_iterator param_it = parameters.begin(); param_it != parameters.end();
00078        param_it++)
00079   {
00080     ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << params_namespace << "/"
00081                                                              << param_it->first << "' with value " << param_it->second);
00082   }
00083 
00084   return true;
00085 }
00086 
00087 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, double &value)
00088 {
00089   // Load a param
00090   if (!nh.hasParam(param_name))
00091   {
00092     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00093     return false;
00094   }
00095   nh.getParam(param_name, value);
00096   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
00097                                                            << value);
00098 
00099   return true;
00100 }
00101 
00102 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
00103          std::vector<double> &values)
00104 {
00105   // Load a param
00106   if (!nh.hasParam(param_name))
00107   {
00108     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00109     return false;
00110   }
00111   nh.getParam(param_name, values);
00112 
00113   if (values.empty())
00114     ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
00115                                                                                                                    ".");
00116 
00117   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name
00118                                                            << "' with values [" << getDebugArrayString(values) << "]");
00119 
00120   return true;
00121 }
00122 
00123 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, int &value)
00124 {
00125   // Load a param
00126   if (!nh.hasParam(param_name))
00127   {
00128     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00129     return false;
00130   }
00131   nh.getParam(param_name, value);
00132   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
00133                                                            << value);
00134 
00135   return true;
00136 }
00137 
00138 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, std::size_t &value)
00139 {
00140   // Load a param
00141   if (!nh.hasParam(param_name))
00142   {
00143     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00144     return false;
00145   }
00146   int nonsigned_value;
00147   nh.getParam(param_name, nonsigned_value);
00148   value = nonsigned_value;
00149   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
00150                                                            << value);
00151 
00152   return true;
00153 }
00154 
00155 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, std::string &value)
00156 {
00157   // Load a param
00158   if (!nh.hasParam(param_name))
00159   {
00160     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00161     return false;
00162   }
00163   nh.getParam(param_name, value);
00164   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
00165                                                            << value);
00166 
00167   return true;
00168 }
00169 
00170 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
00171          std::vector<std::string> &values)
00172 {
00173   // Load a param
00174   if (!nh.hasParam(param_name))
00175   {
00176     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00177     return false;
00178   }
00179   nh.getParam(param_name, values);
00180 
00181   if (values.empty())
00182     ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
00183                                                                                                                    ".");
00184 
00185   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
00186                                                            << getDebugArrayString(values));
00187 
00188   return true;
00189 }
00190 
00191 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, ros::Duration &value)
00192 {
00193   double temp_value;
00194   // Load a param
00195   if (!nh.hasParam(param_name))
00196   {
00197     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00198     return false;
00199   }
00200   nh.getParam(param_name, temp_value);
00201   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value "
00202                                                            << value);
00203 
00204   // Convert to ros::Duration
00205   value = ros::Duration(temp_value);
00206 
00207   return true;
00208 }
00209 
00210 bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
00211          Eigen::Affine3d &value)
00212 {
00213   std::vector<double> values;
00214 
00215   // Load a param
00216   if (!nh.hasParam(param_name))
00217   {
00218     ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'.");
00219     return false;
00220   }
00221   nh.getParam(param_name, values);
00222 
00223   if (values.empty())
00224     ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'"
00225                                                                                                                    ".");
00226 
00227   ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name
00228                                                            << "' with values [" << getDebugArrayString(values) << "]");
00229 
00230   // Convert to Eigen::Affine3d
00231   convertDoublesToEigen(parent_name, values, value);
00232 
00233   return true;
00234 }
00235 
00236 std::string getDebugArrayString(std::vector<double> values)
00237 {
00238   std::stringstream debug_values;
00239   for (std::size_t i = 0; i < values.size(); ++i)
00240   {
00241     debug_values << values[i] << ",";
00242   }
00243   return debug_values.str();
00244 }
00245 
00246 std::string getDebugArrayString(std::vector<std::string> values)
00247 {
00248   std::stringstream debug_values;
00249   for (std::size_t i = 0; i < values.size(); ++i)
00250   {
00251     debug_values << values[i] << ",";
00252   }
00253   return debug_values.str();
00254 }
00255 
00256 bool convertDoublesToEigen(const std::string &parent_name, std::vector<double> values, Eigen::Affine3d &transform)
00257 {
00258   if (values.size() != 6)
00259   {
00260     ROS_ERROR_STREAM_NAMED(parent_name, "Invalid number of doubles provided for transform, size=" << values.size());
00261     return false;
00262   }
00263 
00264   // This version is correct RPY
00265   Eigen::AngleAxisd roll_angle(values[3], Eigen::Vector3d::UnitX());
00266   Eigen::AngleAxisd pitch_angle(values[4], Eigen::Vector3d::UnitY());
00267   Eigen::AngleAxisd yaw_angle(values[5], Eigen::Vector3d::UnitZ());
00268   Eigen::Quaternion<double> quaternion = roll_angle * pitch_angle * yaw_angle;
00269 
00270   transform = Eigen::Translation3d(values[0], values[1], values[2]) * quaternion;
00271 
00272   return true;
00273 }
00274 
00275 void shutdownIfError(const std::string &parent_name, std::size_t error_count)
00276 {
00277   if (!error_count)
00278     return;
00279 
00280   ROS_ERROR_STREAM_NAMED(parent_name, "Missing " << error_count << " ros parameters that are required. Shutting down "
00281                                                                    "to prevent undefined behaviors");
00282   ros::shutdown();
00283   exit(0);
00284 }
00285 
00286 }  // namespace rosparam_shortcuts


rosparam_shortcuts
Author(s): Dave Coleman
autogenerated on Sat Jun 8 2019 19:52:43