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
00030 #ifndef FILTERS_FILTER_BASE_H_
00031 #define FILTERS_FILTER_BASE_H_
00032
00033 #include <typeinfo>
00034 #include "ros/assert.h"
00035 #include "ros/console.h"
00036 #include "ros/ros.h"
00037
00038 #include "boost/scoped_ptr.hpp"
00039 #include <boost/algorithm/string.hpp>
00040
00041 namespace filters
00042 {
00043
00044 typedef std::map<std::string, XmlRpc::XmlRpcValue> string_map_t;
00045
00049 template<typename T>
00050 class FilterBase
00051 {
00052 public:
00055 FilterBase():configured_(false){};
00056
00059 virtual ~FilterBase(){};
00060
00065 bool configure(const std::string& param_name, ros::NodeHandle node_handle = ros::NodeHandle())
00066 {
00067 XmlRpc::XmlRpcValue config;
00068 if (!node_handle.getParam(param_name, config))
00069 {
00070 ROS_ERROR("Could not find parameter %s on the server, are you sure that it was pushed up correctly?", param_name.c_str());
00071 return false;
00072 }
00073 return configure(config);
00074
00075 }
00076
00080 bool configure(XmlRpc::XmlRpcValue& config)
00081 {
00082 if (configured_)
00083 {
00084 ROS_WARN("Filter %s of type %s already being reconfigured", filter_name_.c_str(), filter_type_.c_str());
00085 };
00086 configured_ = false;
00087 bool retval = true;
00088
00089 retval = retval && loadConfiguration(config);
00090 retval = retval && configure();
00091 configured_ = retval;
00092 return retval;
00093 }
00094
00100 virtual bool update(const T& data_in, T& data_out)=0;
00101
00103 std::string getType() {return filter_type_;};
00104
00106 inline const std::string& getName(){return filter_name_;};
00107
00108
00109 protected:
00110
00114 virtual bool configure()=0;
00115
00116
00121 bool getParam(const std::string& name, std::string& value)
00122 {
00123 string_map_t::iterator it = params_.find(name);
00124 if (it == params_.end())
00125 {
00126 return false;
00127 }
00128
00129 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeString)
00130 {
00131 return false;
00132 }
00133
00134 value = std::string(it->second);
00135 return true;
00136 }
00137
00142 bool getParam(const std::string& name, bool& value)
00143 {
00144 string_map_t::iterator it = params_.find(name);
00145 if (it == params_.end())
00146 {
00147 return false;
00148 }
00149
00150 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeBoolean)
00151 {
00152 return false;
00153 }
00154
00155 value = (bool)(it->second);
00156 return true;
00157 }
00158
00163 bool getParam(const std::string&name, double& value)
00164 {
00165 string_map_t::iterator it = params_.find(name);
00166 if (it == params_.end())
00167 {
00168 return false;
00169 }
00170
00171 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeDouble && it->second.getType() != XmlRpc::XmlRpcValue::TypeInt)
00172 {
00173 return false;
00174 }
00175
00176 value = it->second.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(it->second) : (double)(it->second);
00177 return true;
00178 }
00179
00184 bool getParam(const std::string&name, int& value)
00185 {
00186 string_map_t::iterator it = params_.find(name);
00187 if (it == params_.end())
00188 {
00189 return false;
00190 }
00191
00192 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeInt)
00193 {
00194 return false;
00195 }
00196
00197 value = it->second;
00198 return true;
00199 }
00200
00205 bool getParam(const std::string&name, unsigned int& value)
00206 {
00207 int signed_value;
00208 if (!getParam(name, signed_value))
00209 return false;
00210 if (signed_value < 0)
00211 return false;
00212 value = signed_value;
00213 return true;
00214 };
00215
00220 bool getParam(const std::string&name, std::vector<double>& value)
00221 {
00222 string_map_t::iterator it = params_.find(name);
00223 if (it == params_.end())
00224 {
00225 return false;
00226 }
00227
00228 value.clear();
00229
00230 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeArray)
00231 {
00232 return false;
00233 }
00234
00235 XmlRpc::XmlRpcValue double_array = it->second;
00236
00237 for (int i = 0; i < double_array.size(); ++i){
00238 if(double_array[i].getType() != XmlRpc::XmlRpcValue::TypeDouble && double_array[i].getType() != XmlRpc::XmlRpcValue::TypeInt)
00239 {
00240 return false;
00241 }
00242
00243 double double_value = double_array[i].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(double_array[i]) : (double)(double_array[i]);
00244 value.push_back(double_value);
00245 }
00246
00247 return true;
00248 }
00249
00254 bool getParam(const std::string&name, std::vector<std::string>& value)
00255 {
00256 string_map_t::iterator it = params_.find(name);
00257 if (it == params_.end())
00258 {
00259 return false;
00260 }
00261
00262 value.clear();
00263
00264 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeArray)
00265 {
00266 return false;
00267 }
00268
00269 XmlRpc::XmlRpcValue string_array = it->second;
00270
00271 for (unsigned int i = 0; i < string_array.size(); ++i){
00272 if(string_array[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00273 {
00274 return false;
00275 }
00276
00277 value.push_back(string_array[i]);
00278 }
00279
00280 return true;
00281 }
00282
00287 bool getParam(const std::string& name, XmlRpc::XmlRpcValue& value)
00288 {
00289 string_map_t::iterator it = params_.find(name);
00290 if (it == params_.end())
00291 {
00292 return false;
00293 }
00294
00295 value = it->second;
00296 return true;
00297 }
00298
00300 std::string filter_name_;
00302 std::string filter_type_;
00304 bool configured_;
00305
00307 string_map_t params_;
00308
00309 private:
00313 bool setNameAndType(XmlRpc::XmlRpcValue& config)
00314 {
00315 if(!config.hasMember("name"))
00316 {
00317 ROS_ERROR("Filter didn't have name defined, other strings are not allowed");
00318 return false;
00319 }
00320
00321 std::string name = config["name"];
00322
00323 if(!config.hasMember("type"))
00324 {
00325 ROS_ERROR("Filter %s didn't have type defined, other strings are not allowed", name.c_str());
00326 return false;
00327 }
00328
00329 std::string type = config["type"];
00330
00331 filter_name_ = name;
00332 filter_type_ = type;
00333 ROS_DEBUG("Configuring Filter of Type: %s with name %s", type.c_str(), name.c_str());
00334 return true;
00335 }
00336
00337 protected:
00338 bool loadConfiguration(XmlRpc::XmlRpcValue& config)
00339 {
00340 if(config.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00341 {
00342 ROS_ERROR("A filter configuration must be a map with fields name, type, and params");
00343 return false;
00344 }
00345
00346 if (!setNameAndType(config))
00347 {
00348 return false;
00349 }
00350
00351
00352 if(config.hasMember("params"))
00353 {
00354
00355 XmlRpc::XmlRpcValue params = config["params"];
00356
00357 if(params.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00358 {
00359 ROS_ERROR("params must be a map");
00360 return false;
00361 }
00362 else{
00363
00364 for(XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
00365 {
00366 ROS_DEBUG("Loading param %s\n", it->first.c_str());
00367 params_[it->first] = it->second;
00368 }
00369 }
00370 }
00371
00372 return true;
00373 }
00374 };
00375
00376
00377 template <typename T>
00378 class MultiChannelFilterBase : public FilterBase<T>
00379 {
00380 public:
00381 MultiChannelFilterBase():number_of_channels_(0){};
00382
00388 bool configure(unsigned int number_of_channels, const std::string& param_name, ros::NodeHandle node_handle = ros::NodeHandle())
00389 {
00390 XmlRpc::XmlRpcValue config;
00391 if (!node_handle.getParam(param_name, config))
00392 {
00393 ROS_ERROR("Could not find parameter %s on the server, are you sure that it was pushed up correctly?", param_name.c_str());
00394 return false;
00395 }
00396 return configure(number_of_channels, config);
00397
00398 }
00399
00400
00405 bool configure(unsigned int number_of_channels, XmlRpc::XmlRpcValue& config)
00406 {
00407 ROS_DEBUG("FilterBase being configured with XmlRpc xml: %s type: %d", config.toXml().c_str(), config.getType());
00408 if (configured_)
00409 {
00410 ROS_WARN("Filter %s of type %s already being reconfigured", filter_name_.c_str(), filter_type_.c_str());
00411 };
00412 configured_ = false;
00413 number_of_channels_ = number_of_channels;
00414 ROS_DEBUG("MultiChannelFilterBase configured with %d channels", number_of_channels_);
00415 bool retval = true;
00416
00417 retval = retval && FilterBase<T>::loadConfiguration(config);
00418 retval = retval && configure();
00419 configured_ = retval;
00420 return retval;
00421 };
00422
00423
00425 bool configure(XmlRpc::XmlRpcValue& config)
00426 {
00427 ROS_ERROR("MultiChannelFilterBase configure should be called with a number of channels argument, assuming 1");
00428 return configure(1, config);
00429 }
00430
00431 virtual bool configure()=0;
00432
00433
00439 virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out)=0;
00440
00441 virtual bool update(const T& data_in, T& data_out)
00442 {
00443 ROS_ERROR("THIS IS A MULTI FILTER DON'T CALL SINGLE FORM OF UPDATE");
00444 return false;
00445 };
00446
00447
00448 protected:
00449 using FilterBase<T>::configured_;
00450 using FilterBase<T>::filter_type_;
00451 using FilterBase<T>::filter_name_;
00452
00454 unsigned int number_of_channels_;
00455
00456
00457 };
00458
00459 }
00460 #endif //#ifndef FILTERS_FILTER_BASE_H_