Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_pose_estimation/parameters.h>
00030 #include <hector_pose_estimation/ros/parameters.h>
00031
00032 #include <boost/algorithm/string.hpp>
00033
00034 #include <hector_pose_estimation/matrix.h>
00035
00036 #include <iostream>
00037
00038 namespace hector_pose_estimation {
00039
00040 namespace {
00041 struct null_deleter {
00042 void operator()(void const *) const {}
00043 };
00044 }
00045
00046 ParameterList& ParameterList::add(ParameterPtr const& parameter) {
00047 erase(parameter->key);
00048 push_back(parameter);
00049 return *this;
00050 }
00051
00052 ParameterList& ParameterList::add(ParameterList const& other) {
00053 for(ParameterList::const_iterator it = other.begin(); it != other.end(); ++it) push_back(*it);
00054 return *this;
00055 }
00056
00057 ParameterList& ParameterList::add(Parameter& alias, const std::string& key) {
00058 if (!key.empty()) alias.key = key;
00059 return add(ParameterPtr(&alias, null_deleter()));
00060 }
00061
00062 ParameterList& ParameterList::copy(const std::string& prefix, ParameterList const& parameters) {
00063 for(ParameterList::const_iterator it = parameters.begin(); it != parameters.end(); ++it) {
00064 ParameterPtr copy((*it)->clone());
00065 if (!copy) continue;
00066 if (!prefix.empty()) copy->key = prefix + copy->key;
00067 push_back(copy);
00068 }
00069 return *this;
00070 }
00071
00072 ParameterList& ParameterList::copy(ParameterList const& parameters) {
00073 copy(std::string(), parameters);
00074 return *this;
00075 }
00076
00077 ParameterPtr const& ParameterList::get(const std::string& key) const {
00078 for(const_iterator it = begin(); it != end(); ++it) {
00079 if ((*it)->key == key) {
00080 return *it;
00081 }
00082 }
00083 throw std::runtime_error("parameter not found");
00084 }
00085
00086 ParameterList::iterator ParameterList::erase(const std::string& key) {
00087 iterator it = begin();
00088 for(; it != end(); ++it) {
00089 if ((*it)->key == key) return erase(it);
00090 }
00091 return it;
00092 }
00093
00094 template <typename T>
00095 struct ParameterRegistryROS::Handler
00096 {
00097 bool operator()(const ParameterPtr& parameter, ros::NodeHandle& nh, bool set_all = false) {
00098 try {
00099 ParameterT<T> p(*parameter);
00100 std::string param_key(boost::algorithm::to_lower_copy(parameter->key));
00101 if (!nh.getParam(param_key, p.value())) {
00102 if (set_all) {
00103 nh.setParam(param_key, p.value());
00104 ROS_DEBUG_STREAM("Registered parameter " << param_key << " with new value " << p.value());
00105 }
00106 } else {
00107 ROS_DEBUG_STREAM("Found parameter " << param_key << " with value " << p.value());
00108 }
00109 return true;
00110 } catch(std::bad_cast&) {
00111 return false;
00112 }
00113 }
00114 };
00115
00116 template <>
00117 struct ParameterRegistryROS::Handler<ColumnVector>
00118 {
00119 bool operator()(const ParameterPtr& parameter, ros::NodeHandle& nh, bool set_all = false) {
00120 try {
00121 ParameterT<ColumnVector> p(*parameter);
00122 std::string param_key(boost::algorithm::to_lower_copy(parameter->key));
00123 XmlRpc::XmlRpcValue vector;
00124 if (!nh.getParam(param_key, vector)) {
00125 if (set_all) {
00127 ROS_DEBUG_STREAM("Not registered vector parameter " << param_key << ". Using defaults.");
00128 }
00129 } else {
00130 if (vector.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00131 ROS_WARN_STREAM("Found parameter " << param_key << ", but it's not an array!");
00132 return false;
00133 }
00134 p.value().resize(vector.size());
00135 for(int i = 0; i < vector.size(); ++i) p.value()[i] = vector[i];
00136 ROS_DEBUG_STREAM("Found parameter " << param_key << " with value " << p.value());
00137 }
00138 return true;
00139 } catch(std::bad_cast&) {
00140 return false;
00141 }
00142 }
00143 };
00144
00145 template <typename T>
00146 std::ostream& operator<<(std::ostream& os, const std::vector<T>& vector) {
00147 os << "[";
00148 for(typename std::vector<T>::const_iterator it = vector.begin(); it != vector.end(); ++it) {
00149 if (it != vector.begin()) os << ", ";
00150 os << *it;
00151 }
00152 os << "]";
00153 return os;
00154 }
00155
00156 template <typename T>
00157 struct ParameterRegistryROS::Handler< std::vector<T> >
00158 {
00159 bool operator()(const ParameterPtr& parameter, ros::NodeHandle& nh, bool set_all = false) {
00160 try {
00161 ParameterT< std::vector<T> > p(*parameter);
00162 std::string param_key(boost::algorithm::to_lower_copy(parameter->key));
00163 XmlRpc::XmlRpcValue vector;
00164 if (!nh.getParam(param_key, vector)) {
00165 if (set_all) {
00167 ROS_DEBUG_STREAM("Not registered vector parameter " << param_key << ". Using defaults.");
00168 }
00169 } else {
00170 if (vector.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00171 ROS_WARN_STREAM("Found parameter " << param_key << ", but it's not an array!");
00172 return false;
00173 }
00174 p.value().resize(vector.size());
00175 for(int i = 0; i < vector.size(); ++i) p.value()[i] = vector[i];
00176 ROS_DEBUG_STREAM("Found parameter " << param_key << " with value " << p.value());
00177 }
00178 return true;
00179 } catch(std::bad_cast&) {
00180 return false;
00181 }
00182 }
00183 };
00184
00185 ParameterRegistryROS::ParameterRegistryROS(ros::NodeHandle nh)
00186 : nh_(nh)
00187 , set_all_(false)
00188 {
00189 nh_.getParam("set_all_parameters", set_all_);
00190 }
00191
00192 void ParameterRegistryROS::operator ()(ParameterPtr parameter) {
00193
00194 if (parameter->hasType<ParameterList>()) {
00195 ParameterList with_prefix;
00196 with_prefix.copy(parameter->key + "/", parameter->as<ParameterList>());
00197 with_prefix.initialize(*this);
00198 return;
00199 }
00200
00201 ROS_DEBUG_STREAM("Registering ROS parameter " << parameter->key);
00202
00203 if (Handler<std::string>()(parameter, nh_, set_all_) ||
00204 Handler<double>()(parameter, nh_, set_all_) ||
00205 Handler<std::vector<double> >()(parameter, nh_, set_all_) ||
00206 Handler<int>()(parameter, nh_, set_all_) ||
00207 Handler<bool>()(parameter, nh_, set_all_) ||
00208 Handler<ColumnVector>()(parameter, nh_, set_all_)
00209 ) {
00210 return;
00211 }
00212
00213 ROS_ERROR("Parameter %s has unknown type %s!", parameter->key.c_str(), parameter->type());
00214 }
00215
00216 void ParameterList::initialize(ParameterRegisterFunc func) const {
00217 for(const_iterator it = begin(); it != end(); ++it) {
00218 const ParameterPtr& parameter = *it;
00219 if (parameter->empty()) continue;
00220 if (parameter->isAlias()) continue;
00221
00222 func(*it);
00223 }
00224 }
00225
00226 }