$search
00001 /********************************************************************* 00002 * 00003 * Copyright (c) 2009, Willow Garage, Inc. 00004 * All rights reserved. 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions 00008 * are met: 00009 * 00010 * * Redistributions of source code must retain the above copyright 00011 * notice, this list of conditions and the following disclaimer. 00012 * * Redistributions in binary form must reproduce the above 00013 * copyright notice, this list of conditions and the following 00014 * disclaimer in the documentation and/or other materials provided 00015 * with the distribution. 00016 * * Neither the name of the Willow Garage nor the names of its 00017 * contributors may be used to endorse or promote products derived 00018 * from this software without specific prior written permission. 00019 * 00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 * POSSIBILITY OF SUCH DAMAGE. 00032 *********************************************************************/ 00033 #ifndef _CONFIGURATION_LOADER_H_ 00034 #define _CONFIGURATION_LOADER_H_ 00035 00036 #include <ros/ros.h> 00037 00038 #include <cmath> 00039 00040 #include <geometry_msgs/Vector3.h> 00041 00042 #include "object_manipulator/tools/exceptions.h" 00043 00044 namespace object_manipulator { 00045 00046 // aleeper: I sub-classed these parameter loading classes so we have a common class to work from. 00047 // Hopefully it keeps things cleaner. 00048 00049 class ConfigurationLoader 00050 { 00051 protected: 00053 ros::NodeHandle root_nh_; 00054 00056 inline std::string getStringParam(std::string name) 00057 { 00058 std::string value; 00059 if (!root_nh_.getParamCached(name, value)) throw MissingParamException(name); 00060 //ROS_INFO_STREAM("Hand description param " << name << " resolved to " << value); 00061 return value; 00062 } 00063 00065 inline std::vector<std::string> getVectorParam(std::string name) 00066 { 00067 XmlRpc::XmlRpcValue list; 00068 if (!root_nh_.getParamCached(name, list)) throw MissingParamException(name); 00069 if (list.getType() != XmlRpc::XmlRpcValue::TypeArray) throw BadParamException(name); 00070 //ROS_INFO_STREAM("Hand description vector param " << name << " resolved to:"); 00071 std::vector<std::string> values; 00072 for (int32_t i=0; i<list.size(); i++) 00073 { 00074 if (list[i].getType() != XmlRpc::XmlRpcValue::TypeString) throw BadParamException(name); 00075 values.push_back( static_cast<std::string>(list[i]) ); 00076 //ROS_INFO_STREAM(" " << values.back()); 00077 } 00078 return values; 00079 } 00080 00082 inline std::vector<double> getVectorDoubleParam(std::string name) 00083 { 00084 XmlRpc::XmlRpcValue list; 00085 if (!root_nh_.getParamCached(name, list)) throw MissingParamException(name); 00086 if (list.getType() != XmlRpc::XmlRpcValue::TypeArray) throw BadParamException(name); 00087 std::vector<double> values; 00088 for (int32_t i=0; i<list.size(); i++) 00089 { 00090 if (list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) throw BadParamException(name); 00091 values.push_back( static_cast<double>(list[i]) ); 00092 } 00093 return values; 00094 } 00095 00097 inline double getDoubleParam(std::string name) 00098 { 00099 double value; 00100 if (!root_nh_.getParamCached(name, value)) throw MissingParamException(name); 00101 return value; 00102 } 00103 00105 inline bool getBoolParam(std::string name) 00106 { 00107 bool value; 00108 if (!root_nh_.getParamCached(name, value)) throw MissingParamException(name); 00109 return value; 00110 } 00111 00112 public: 00114 ConfigurationLoader() : root_nh_("~") {} 00115 00116 }; 00117 00119 inline ConfigurationLoader& configurationLoader() 00120 { 00121 static ConfigurationLoader singleton; 00122 return singleton; 00123 } 00124 00125 } //namespace object_manipulator 00126 00127 #endif