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, double& 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::TypeDouble && it->second.getType() != XmlRpc::XmlRpcValue::TypeInt)
00151 {
00152 return false;
00153 }
00154
00155 value = it->second.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(it->second) : (double)(it->second);
00156 return true;
00157 }
00158
00163 bool getParam(const std::string&name, int& 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::TypeInt)
00172 {
00173 return false;
00174 }
00175
00176 value = it->second;
00177 return true;
00178 }
00179
00184 bool getParam(const std::string&name, unsigned int& value)
00185 {
00186 int signed_value;
00187 if (!getParam(name, signed_value))
00188 return false;
00189 if (signed_value < 0)
00190 return false;
00191 value = signed_value;
00192 return true;
00193 };
00194
00199 bool getParam(const std::string&name, std::vector<double>& value)
00200 {
00201 string_map_t::iterator it = params_.find(name);
00202 if (it == params_.end())
00203 {
00204 return false;
00205 }
00206
00207 value.clear();
00208
00209 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeArray)
00210 {
00211 return false;
00212 }
00213
00214 XmlRpc::XmlRpcValue double_array = it->second;
00215
00216 for (int i = 0; i < double_array.size(); ++i){
00217 if(double_array[i].getType() != XmlRpc::XmlRpcValue::TypeDouble && double_array[i].getType() != XmlRpc::XmlRpcValue::TypeInt)
00218 {
00219 return false;
00220 }
00221
00222 double double_value = double_array[i].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(double_array[i]) : (double)(double_array[i]);
00223 value.push_back(double_value);
00224 }
00225
00226 return true;
00227 }
00228
00233 bool getParam(const std::string&name, std::vector<std::string>& value)
00234 {
00235 string_map_t::iterator it = params_.find(name);
00236 if (it == params_.end())
00237 {
00238 return false;
00239 }
00240
00241 value.clear();
00242
00243 if(it->second.getType() != XmlRpc::XmlRpcValue::TypeArray)
00244 {
00245 return false;
00246 }
00247
00248 XmlRpc::XmlRpcValue string_array = it->second;
00249
00250 for (unsigned int i = 0; i < string_array.size(); ++i){
00251 if(string_array[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00252 {
00253 return false;
00254 }
00255
00256 value.push_back(string_array[i]);
00257 }
00258
00259 return true;
00260 }
00261
00266 bool getParam(const std::string& name, XmlRpc::XmlRpcValue& value)
00267 {
00268 string_map_t::iterator it = params_.find(name);
00269 if (it == params_.end())
00270 {
00271 return false;
00272 }
00273
00274 value = it->second;
00275 return true;
00276 }
00277
00279 std::string filter_name_;
00281 std::string filter_type_;
00283 bool configured_;
00284
00286 string_map_t params_;
00287
00288 private:
00292 bool setNameAndType(XmlRpc::XmlRpcValue& config)
00293 {
00294 if(!config.hasMember("name"))
00295 {
00296 ROS_ERROR("Filter didn't have name defined, other strings are not allowed");
00297 return false;
00298 }
00299
00300 std::string name = config["name"];
00301
00302 if(!config.hasMember("type"))
00303 {
00304 ROS_ERROR("Filter %s didn't have type defined, other strings are not allowed", name.c_str());
00305 return false;
00306 }
00307
00308 std::string type = config["type"];
00309
00310 filter_name_ = name;
00311 filter_type_ = type;
00312 ROS_DEBUG("Configuring Filter of Type: %s with name %s", type.c_str(), name.c_str());
00313 return true;
00314 }
00315
00316 protected:
00317 bool loadConfiguration(XmlRpc::XmlRpcValue& config)
00318 {
00319 if(config.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00320 {
00321 ROS_ERROR("A filter configuration must be a map with fields name, type, and params");
00322 return false;
00323 }
00324
00325 if (!setNameAndType(config))
00326 {
00327 return false;
00328 }
00329
00330
00331 if(config.hasMember("params"))
00332 {
00333
00334 XmlRpc::XmlRpcValue params = config["params"];
00335
00336 if(params.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00337 {
00338 ROS_ERROR("params must be a map");
00339 return false;
00340 }
00341 else{
00342
00343 for(XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
00344 {
00345 ROS_DEBUG("Loading param %s\n", it->first.c_str());
00346 params_[it->first] = it->second;
00347 }
00348 }
00349 }
00350
00351 return true;
00352 }
00353 };
00354
00355
00356 template <typename T>
00357 class MultiChannelFilterBase : public FilterBase<T>
00358 {
00359 public:
00360 MultiChannelFilterBase():number_of_channels_(0){};
00361
00367 bool configure(unsigned int number_of_channels, const std::string& param_name, ros::NodeHandle node_handle = ros::NodeHandle())
00368 {
00369 XmlRpc::XmlRpcValue config;
00370 if (!node_handle.getParam(param_name, config))
00371 {
00372 ROS_ERROR("Could not find parameter %s on the server, are you sure that it was pushed up correctly?", param_name.c_str());
00373 return false;
00374 }
00375 return configure(number_of_channels, config);
00376
00377 }
00378
00379
00384 bool configure(unsigned int number_of_channels, XmlRpc::XmlRpcValue& config)
00385 {
00386 ROS_DEBUG("FilterBase being configured with XmlRpc xml: %s type: %d", config.toXml().c_str(), config.getType());
00387 if (configured_)
00388 {
00389 ROS_WARN("Filter %s of type %s already being reconfigured", filter_name_.c_str(), filter_type_.c_str());
00390 };
00391 configured_ = false;
00392 number_of_channels_ = number_of_channels;
00393 ROS_DEBUG("MultiChannelFilterBase configured with %d channels", number_of_channels_);
00394 bool retval = true;
00395
00396 retval = retval && FilterBase<T>::loadConfiguration(config);
00397 retval = retval && configure();
00398 configured_ = retval;
00399 return retval;
00400 };
00401
00402
00404 bool configure(XmlRpc::XmlRpcValue& config)
00405 {
00406 ROS_ERROR("MultiChannelFilterBase configure should be called with a number of channels argument, assuming 1");
00407 return configure(1, config);
00408 }
00409
00410 virtual bool configure()=0;
00411
00412
00418 virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out)=0;
00419
00420 virtual bool update(const T& data_in, T& data_out)
00421 {
00422 ROS_ERROR("THIS IS A MULTI FILTER DON'T CALL SINGLE FORM OF UPDATE");
00423 return false;
00424 };
00425
00426
00427 protected:
00428 using FilterBase<T>::configured_;
00429 using FilterBase<T>::filter_type_;
00430 using FilterBase<T>::filter_name_;
00431
00433 unsigned int number_of_channels_;
00434
00435
00436 };
00437
00438 }
00439 #endif //#ifndef FILTERS_FILTER_BASE_H_