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_