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_CHAIN_H_
00031 #define FILTERS_FILTER_CHAIN_H_
00032
00033 #include "ros/ros.h"
00034 #include "filters/filter_base.h"
00035 #include <pluginlib/class_loader.h>
00036 #include <sstream>
00037 #include <vector>
00038 #include "boost/shared_ptr.hpp"
00039
00040 namespace filters
00041 {
00042
00046 template <typename T>
00047 class FilterChain
00048 {
00049 private:
00050 pluginlib::ClassLoader<filters::FilterBase<T> > loader_;
00051 public:
00053 FilterChain(std::string data_type): loader_("filters", std::string("filters::FilterBase<") + data_type + std::string(">")), configured_(false)
00054 {
00055 std::string lib_string = "";
00056 std::vector<std::string> libs = loader_.getDeclaredClasses();
00057 for (unsigned int i = 0 ; i < libs.size(); i ++)
00058 {
00059 lib_string = lib_string + std::string(", ") + libs[i];
00060 }
00061 ROS_DEBUG("In FilterChain ClassLoader found the following libs: %s", lib_string.c_str());
00062 };
00063
00064 ~FilterChain()
00065 {
00066 clear();
00067
00068 };
00069
00074 bool configure(std::string param_name, ros::NodeHandle node = ros::NodeHandle())
00075 {
00076 XmlRpc::XmlRpcValue config;
00077 if(node.getParam(param_name + "/filter_chain", config))
00078 {
00079 std::string resolved_name = node.resolveName(param_name).c_str();
00080 ROS_WARN("Filter chains no longer check implicit nested 'filter_chain' parameter. This node is configured to look directly at '%s'. Please move your chain description from '%s/filter_chain' to '%s'", resolved_name.c_str(), resolved_name.c_str(), resolved_name.c_str());
00081 }
00082 else if(!node.getParam(param_name, config))
00083 {
00084 ROS_DEBUG("Could not load the filter chain configuration from parameter %s, are you sure it was pushed to the parameter server? Assuming that you meant to leave it empty.", param_name.c_str());
00085 configured_ = true;
00086 return true;
00087 }
00088 return this->configure(config, node.getNamespace());
00089 }
00090
00092 bool update(const T& data_in, T& data_out)
00093 {
00094 unsigned int list_size = reference_pointers_.size();
00095 bool result;
00096 if (list_size == 0)
00097 {
00098 data_out = data_in;
00099 result = true;
00100 }
00101 else if (list_size == 1)
00102 result = reference_pointers_[0]->update(data_in, data_out);
00103 else if (list_size == 2)
00104 {
00105 result = reference_pointers_[0]->update(data_in, buffer0_);
00106 if (result == false) {return false; };
00107 result = result && reference_pointers_[1]->update(buffer0_, data_out);
00108 }
00109 else
00110 {
00111 result = reference_pointers_[0]->update(data_in, buffer0_);
00112 for (unsigned int i = 1; i < reference_pointers_.size() - 1; i++)
00113 {
00114 if (i %2 == 1)
00115 result = result && reference_pointers_[i]->update(buffer0_, buffer1_);
00116 else
00117 result = result && reference_pointers_[i]->update(buffer1_, buffer0_);
00118
00119 if (result == false) {return false; };
00120 }
00121 if (list_size % 2 == 1)
00122 result = result && reference_pointers_.back()->update(buffer1_, data_out);
00123 else
00124 result = result && reference_pointers_.back()->update(buffer0_, data_out);
00125 }
00126 return result;
00127
00128 };
00130 bool clear()
00131 {
00132 configured_ = false;
00133 reference_pointers_.clear();
00134 return true;
00135 };
00136
00137
00138
00139
00142 bool configure(XmlRpc::XmlRpcValue& config, const std::string& filter_ns)
00143 {
00144
00145
00146 if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
00147 {
00148 ROS_ERROR("%s: The filter chain specification must be a list. but is of of XmlRpcType %d", filter_ns.c_str(), config.getType());
00149 ROS_ERROR("The xml passed in is formatted as follows:\n %s", config.toXml().c_str());
00150
00151 return false;
00152 }
00153
00154
00155 for (int i = 0; i < config.size(); ++i)
00156 {
00157 if(config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
00158 {
00159 ROS_ERROR("%s: Filters must be specified as maps, but they are XmlRpcType:%d", filter_ns.c_str(), config[i].getType());
00160 return false;
00161 }
00162 else if (!config[i].hasMember("type"))
00163 {
00164 ROS_ERROR("%s: Could not add a filter because no type was given", filter_ns.c_str());
00165 return false;
00166 }
00167 else if (!config[i].hasMember("name"))
00168 {
00169 ROS_ERROR("%s: Could not add a filter because no name was given", filter_ns.c_str());
00170 return false;
00171 }
00172 else
00173 {
00174
00175 for (int j = i + 1; j < config.size(); ++j)
00176 {
00177 if(config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct)
00178 {
00179 ROS_ERROR("%s: Filters must be specified as maps, but they are XmlRpcType:%d", filter_ns.c_str(), config[j].getType());
00180 return false;
00181 }
00182
00183 if(!config[j].hasMember("name")
00184 ||config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
00185 || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString)
00186 {
00187 ROS_ERROR("%s: Filters names must be strings, but they are XmlRpcTypes:%d and %d", filter_ns.c_str(), config[i].getType(), config[j].getType());
00188 return false;
00189 }
00190
00191 std::string namei = config[i]["name"];
00192 std::string namej = config[j]["name"];
00193 if (namei == namej)
00194 {
00195 ROS_ERROR("%s: A self_filter with the name %s already exists", filter_ns.c_str(), namei.c_str());
00196 return false;
00197 }
00198 }
00199
00200
00201
00202 if (std::string(config[i]["type"]).find("/") == std::string::npos)
00203 {
00204 ROS_WARN("Deprecation Warning: No '/' detected in FilterType, Please update to 1.2 plugin syntax. ");
00205 std::vector<std::string> libs = loader_.getDeclaredClasses();
00206 for (std::vector<std::string>::iterator it = libs.begin(); it != libs.end(); ++it)
00207 {
00208 size_t position = it->find(std::string(config[i]["type"]));
00209 if (position != std::string::npos)
00210 {
00211 ROS_WARN("Replaced %s with %s", std::string(config[i]["type"]).c_str(), it->c_str());
00212 config[i]["type"] = *it;
00213 }
00214 }
00215 }
00216
00217 std::vector<std::string> libs = loader_.getDeclaredClasses();
00218 bool found = false;
00219 for (std::vector<std::string>::iterator it = libs.begin(); it != libs.end(); ++it)
00220 {
00221 if (*it == std::string(config[i]["type"]))
00222 {
00223 found = true;
00224 break;
00225 }
00226 }
00227 if (!found)
00228 {
00229 ROS_ERROR("Couldn't find filter of type %s", std::string(config[i]["type"]).c_str());
00230 return false;
00231 }
00232
00233 }
00234 }
00235
00236
00237 bool result = true;
00238
00239
00240 for (int i = 0; i < config.size(); ++i)
00241 {
00242 boost::shared_ptr<filters::FilterBase<T> > p(loader_.createInstance(config[i]["type"]));
00243 if (p.get() == NULL)
00244 return false;
00245 result = result && p.get()->configure(config[i]);
00246 reference_pointers_.push_back(p);
00247 std::string type = config[i]["type"];
00248 std::string name = config[i]["name"];
00249 ROS_DEBUG("%s: Configured %s:%s filter at %p\n", filter_ns.c_str(), type.c_str(),
00250 name.c_str(), p.get());
00251 }
00252
00253 if (result == true)
00254 {
00255 configured_ = true;
00256 }
00257 return result;
00258 };
00259
00260 private:
00261
00262 std::vector<boost::shared_ptr<filters::FilterBase<T> > > reference_pointers_;
00263
00264 T buffer0_;
00265 T buffer1_;
00266 bool configured_;
00267
00268 };
00269
00273 template <typename T>
00274 class MultiChannelFilterChain
00275 {
00276 private:
00277 pluginlib::ClassLoader<filters::MultiChannelFilterBase<T> > loader_;
00278 public:
00280 MultiChannelFilterChain(std::string data_type): loader_("filters", std::string("filters::MultiChannelFilterBase<") + data_type + std::string(">")), configured_(false)
00281 {
00282 std::string lib_string = "";
00283 std::vector<std::string> libs = loader_.getDeclaredClasses();
00284 for (unsigned int i = 0 ; i < libs.size(); i ++)
00285 {
00286 lib_string = lib_string + std::string(", ") + libs[i];
00287 }
00288 ROS_DEBUG("In MultiChannelFilterChain ClassLoader found the following libs: %s", lib_string.c_str());
00289 };
00290
00295 bool configure(unsigned int size, std::string param_name, ros::NodeHandle node = ros::NodeHandle())
00296 {
00297 XmlRpc::XmlRpcValue config;
00298 if(node.getParam(param_name + "/filter_chain", config))
00299 {
00300 std::string resolved_name = node.resolveName(param_name).c_str();
00301 ROS_WARN("Filter chains no longer check implicit nested 'filter_chain' parameter. This node is configured to look directly at '%s'. Please move your chain description from '%s/filter_chain' to '%s'", resolved_name.c_str(), resolved_name.c_str(), resolved_name.c_str());
00302 }
00303 else if(!node.getParam(param_name, config))
00304 {
00305 ROS_ERROR("Could not load the configuration for %s, are you sure it was pushed to the parameter server? Assuming that you meant to leave it blank.", param_name.c_str());
00306
00307 buffer0_.resize(size);
00308 buffer1_.resize(size);
00309 configured_ = true;
00310 return false;
00311 }
00312 return this->configure(size, config);
00313 }
00314
00316 bool update(const std::vector<T>& data_in, std::vector<T>& data_out)
00317 {
00318 unsigned int list_size = reference_pointers_.size();
00319 bool result;
00320 if (list_size == 0)
00321 {
00322 data_out = data_in;
00323 result = true;
00324 }
00325 else if (list_size == 1)
00326 result = reference_pointers_[0]->update(data_in, data_out);
00327 else if (list_size == 2)
00328 {
00329 result = reference_pointers_[0]->update(data_in, buffer0_);
00330 if (result == false) {return false; };
00331 result = result && reference_pointers_[1]->update(buffer0_, data_out);
00332 }
00333 else
00334 {
00335 result = reference_pointers_[0]->update(data_in, buffer0_);
00336 for (unsigned int i = 1; i < reference_pointers_.size() - 1; i++)
00337 {
00338 if (i %2 == 1)
00339 result = result && reference_pointers_[i]->update(buffer0_, buffer1_);
00340 else
00341 result = result && reference_pointers_[i]->update(buffer1_, buffer0_);
00342
00343 if (result == false) {return false; };
00344 }
00345 if (list_size % 2 == 1)
00346 result = result && reference_pointers_.back()->update(buffer1_, data_out);
00347 else
00348 result = result && reference_pointers_.back()->update(buffer0_, data_out);
00349 }
00350 return result;
00351
00352 };
00353
00354
00355 ~MultiChannelFilterChain()
00356 {
00357 clear();
00358
00359 };
00360
00362 bool clear()
00363 {
00364 configured_ = false;
00365 reference_pointers_.clear();
00366 buffer0_.clear();
00367 buffer1_.clear();
00368 return true;
00369 };
00370
00371
00372
00376 bool configure(unsigned int size, XmlRpc::XmlRpcValue& config)
00377 {
00378
00379
00380 if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
00381 {
00382 ROS_ERROR("The filter chain specification must be a list. but is of of XmlRpcType %d", config.getType());
00383 ROS_ERROR("The xml passed in is formatted as follows:\n %s", config.toXml().c_str());
00384
00385 return false;
00386 }
00387
00388
00389 for (int i = 0; i < config.size(); ++i)
00390 {
00391 if(config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
00392 {
00393 ROS_ERROR("Filters must be specified as maps, but they are XmlRpcType:%d", config[i].getType());
00394 return false;
00395 }
00396 else if (!config[i].hasMember("type"))
00397 {
00398 ROS_ERROR("Could not add a filter because no type was given");
00399 return false;
00400 }
00401 else if (!config[i].hasMember("name"))
00402 {
00403 ROS_ERROR("Could not add a filter because no name was given");
00404 return false;
00405 }
00406 else
00407 {
00408
00409 for (int j = i + 1; j < config.size(); ++j)
00410 {
00411 if(config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct)
00412 {
00413 ROS_ERROR("Filters must be specified as maps");
00414 return false;
00415 }
00416
00417 if(!config[j].hasMember("name")
00418 ||config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
00419 || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString)
00420 {
00421 ROS_ERROR("Filters names must be strings");
00422 return false;
00423 }
00424
00425 std::string namei = config[i]["name"];
00426 std::string namej = config[j]["name"];
00427 if (namei == namej)
00428 {
00429 std::string name = config[i]["name"];
00430 ROS_ERROR("A self_filter with the name %s already exists", name.c_str());
00431 return false;
00432 }
00433 }
00434
00435 if (std::string(config[i]["type"]).find("/") == std::string::npos)
00436 {
00437 ROS_WARN("Deprecation Warning: No '/' detected in FilterType, Please update to 1.2 plugin syntax. ");
00438 std::vector<std::string> libs = loader_.getDeclaredClasses();
00439 for (std::vector<std::string>::iterator it = libs.begin(); it != libs.end(); ++it)
00440 {
00441 size_t position = it->find(std::string(config[i]["type"]));
00442 if (position != std::string::npos)
00443 {
00444 ROS_WARN("Replaced %s with %s", std::string(config[i]["type"]).c_str(), it->c_str());
00445 config[i]["type"] = *it;
00446 }
00447 }
00448 }
00449
00450 std::vector<std::string> libs = loader_.getDeclaredClasses();
00451 bool found = false;
00452 for (std::vector<std::string>::iterator it = libs.begin(); it != libs.end(); ++it)
00453 {
00454 if (*it == std::string(config[i]["type"]))
00455 {
00456 found = true;
00457 break;
00458 }
00459 }
00460 if (!found)
00461 {
00462 ROS_ERROR("Couldn't find filter of type %s", std::string(config[i]["type"]).c_str());
00463 return false;
00464 }
00465
00466
00467 }
00468 }
00469
00470
00471 buffer0_.resize(size);
00472 buffer1_.resize(size);
00473
00474 bool result = true;
00475
00476
00477 for (int i = 0; i < config.size(); ++i)
00478 {
00479 boost::shared_ptr<filters::MultiChannelFilterBase<T> > p(loader_.createInstance(config[i]["type"]));
00480 if (p.get() == NULL)
00481 return false;
00482 result = result && p.get()->configure(size, config[i]);
00483 reference_pointers_.push_back(p);
00484 std::string type = config[i]["type"];
00485 std::string name = config[i]["name"];
00486 ROS_DEBUG("Configured %s:%s filter at %p\n", type.c_str(),
00487 name.c_str(), p.get());
00488 }
00489
00490 if (result == true)
00491 {
00492 configured_ = true;
00493 }
00494 return result;
00495 };
00496
00497 private:
00498
00499 std::vector<boost::shared_ptr<filters::MultiChannelFilterBase<T> > > reference_pointers_;
00500
00501 std::vector<T> buffer0_;
00502 std::vector<T> buffer1_;
00503 bool configured_;
00504
00505 };
00506
00507 };
00508
00509 #endif //#ifndef FILTERS_FILTER_CHAIN_H_