$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Johannes Meyer, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #ifndef HECTOR_POSE_ESTIMATION_PARAMETERS_H 00030 #define HECTOR_POSE_ESTIMATION_PARAMETERS_H 00031 00032 #include <list> 00033 #include <string> 00034 #include <boost/shared_ptr.hpp> 00035 #include <boost/function.hpp> 00036 #include <boost/type_traits.hpp> 00037 00038 namespace ros { 00039 class NodeHandle; 00040 } 00041 00042 namespace hector_pose_estimation { 00043 00044 class Parameter; 00045 template <typename T> class TypedParameter; 00046 typedef boost::shared_ptr<Parameter> ParameterPtr; 00047 typedef boost::shared_ptr<const Parameter> ParameterConstPtr; 00048 typedef boost::function<void(ParameterPtr)> ParameterRegisterFunc; 00049 00050 class Parameter { 00051 public: 00052 std::string key; 00053 Parameter(const std::string& key) : key(key) {} 00054 virtual ParameterPtr clone() = 0; 00055 virtual const char *type() const = 0; 00056 00057 template <typename T> bool hasType() const { 00058 return dynamic_cast<const TypedParameter<T> *>(this) != 0; 00059 } 00060 00061 template <typename T> T& as() const { 00062 const TypedParameter<T>& p = dynamic_cast<const TypedParameter<T> &>(*this); 00063 return p.value; 00064 } 00065 }; 00066 00067 template <typename T> 00068 class TypedParameter : public Parameter { 00069 public: 00070 typedef typename boost::remove_reference<typename boost::remove_const<T>::type>::type param_type; 00071 00072 param_type& value; 00073 TypedParameter(const std::string& key, param_type &value) : Parameter(key), value(value) {} 00074 TypedParameter(const Parameter& other) : Parameter(other), value(dynamic_cast<const TypedParameter<T> &>(other).value) {} 00075 00076 ParameterPtr clone() { return ParameterPtr(new TypedParameter<T>(*this)); } 00077 const char *type() const { return typeid(param_type).name(); } 00078 }; 00079 00080 class ParameterList : public std::list<ParameterPtr> { 00081 public: 00082 using std::list<ParameterPtr>::iterator; 00083 using std::list<ParameterPtr>::const_iterator; 00084 00085 ParameterList() {} 00086 ~ParameterList() {} 00087 00088 template <typename T> 00089 ParameterList& add(const std::string& key, T& value, const T& default_value) { 00090 value = default_value; 00091 return add(key, value); 00092 } 00093 00094 template <typename T> 00095 ParameterList& add(const std::string& key, T& value) { 00096 erase(key); 00097 push_back(ParameterPtr(new TypedParameter<T>(key, value))); 00098 return *this; 00099 } 00100 00101 template <typename T> 00102 ParameterList& add(const std::string& key, T* value) { 00103 erase(key); 00104 push_back(ParameterPtr(new TypedParameter<T>(key, *value))); 00105 return *this; 00106 } 00107 00108 ParameterList& add(ParameterList const& other) { 00109 for(ParameterList::const_iterator it = other.begin(); it != other.end(); ++it) push_back(*it); 00110 return *this; 00111 } 00112 00113 ParameterList& copy(const std::string& prefix, ParameterList const& parameters) { 00114 for(ParameterList::const_iterator it = parameters.begin(); it != parameters.end(); ++it) { 00115 ParameterPtr copy((*it)->clone()); 00116 if (!prefix.empty()) copy->key = prefix + "/" + copy->key; 00117 push_back(copy); 00118 } 00119 return *this; 00120 } 00121 00122 ParameterList& copy(ParameterList const& parameters) { 00123 copy(std::string(), parameters); 00124 return *this; 00125 } 00126 00127 template <typename T> 00128 T& get(const std::string& key) const { 00129 for(const_iterator it = begin(); it != end(); ++it) { 00130 if ((*it)->key == key) { 00131 return (*it)->as<T>(); 00132 } 00133 } 00134 throw std::bad_cast(); 00135 } 00136 00137 using std::list<ParameterPtr>::erase; 00138 iterator erase(const std::string& key) { 00139 iterator it = begin(); 00140 for(; it != end(); ++it) { 00141 if ((*it)->key == key) return erase(it); 00142 } 00143 return it; 00144 } 00145 00146 void registerParamsRos(ros::NodeHandle nh) const; 00147 void registerParams(const ParameterRegisterFunc& func) const; 00148 }; 00149 00150 static inline ParameterList operator+(ParameterList const& list1, ParameterList const& list2) 00151 { 00152 ParameterList result; 00153 return result.add(list1).add(list2); 00154 } 00155 00156 } // namespace hector_pose_estimation 00157 00158 #endif // HECTOR_POSE_ESTIMATION_PARAMETERS_H