$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 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; };//don't keep processing on failure 00107 result = result && reference_pointers_[1]->update(buffer0_, data_out); 00108 } 00109 else 00110 { 00111 result = reference_pointers_[0]->update(data_in, buffer0_); //first copy in 00112 for (unsigned int i = 1; i < reference_pointers_.size() - 1; i++) // all but first and last (never called if size=2) 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; }; //don't keep processing on failure 00120 } 00121 if (list_size % 2 == 1) // odd number last deposit was in buffer1 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 /*************************** Parse the XmlRpcValue ***********************************/ 00145 //Verify proper naming and structure 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 //Iterate over all filter in filters (may be just one) 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 //Check for name collisions within the list itself. 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 //CHeck for backwards compatible declarations 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 //Make sure the filter chain has a valid type 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_.createClassInstance(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 /********************** Do the allocation *********************/ 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; };//don't keep processing on failure 00331 result = result && reference_pointers_[1]->update(buffer0_, data_out); 00332 } 00333 else 00334 { 00335 result = reference_pointers_[0]->update(data_in, buffer0_); //first copy in 00336 for (unsigned int i = 1; i < reference_pointers_.size() - 1; i++) // all but first and last (never if size = 2) 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; }; //don't keep processing on failure 00344 } 00345 if (list_size % 2 == 1) // odd number last deposit was in buffer1 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 /*************************** Parse the XmlRpcValue ***********************************/ 00379 //Verify proper naming and structure 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 //Iterate over all filter in filters (may be just one) 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 //Check for name collisions within the list itself. 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 //CHeck for backwards compatible declarations 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 //Make sure the filter chain has a valid type 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 /********************** Do the allocation *********************/ 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_.createClassInstance(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_