filter_chain.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef FILTERS_FILTER_CHAIN_H_
31 #define FILTERS_FILTER_CHAIN_H_
32 
33 #include "ros/ros.h"
34 #include "filters/filter_base.h"
35 #include <pluginlib/class_loader.h>
36 #include <sstream>
37 #include <vector>
38 #include "boost/shared_ptr.hpp"
39 
40 namespace filters
41 {
42 
46 template <typename T>
48 {
49 private:
51 public:
53  FilterChain(std::string data_type): loader_("filters", std::string("filters::FilterBase<") + data_type + std::string(">")), configured_(false)
54  {
55  std::string lib_string = "";
56  std::vector<std::string> libs = loader_.getDeclaredClasses();
57  for (unsigned int i = 0 ; i < libs.size(); i ++)
58  {
59  lib_string = lib_string + std::string(", ") + libs[i];
60  }
61  ROS_DEBUG("In FilterChain ClassLoader found the following libs: %s", lib_string.c_str());
62  };
63 
65  {
66  clear();
67 
68  };
69 
74  bool configure(std::string param_name, ros::NodeHandle node = ros::NodeHandle())
75  {
76  XmlRpc::XmlRpcValue config;
77  if(node.getParam(param_name + "/filter_chain", config))
78  {
79  std::string resolved_name = node.resolveName(param_name).c_str();
80  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());
81  }
82  else if(!node.getParam(param_name, config))
83  {
84  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());
85  configured_ = true;
86  return true;
87  }
88  return this->configure(config, node.getNamespace());
89  }
90 
92  bool update(const T& data_in, T& data_out)
93  {
94  unsigned int list_size = reference_pointers_.size();
95  bool result;
96  if (list_size == 0)
97  {
98  data_out = data_in;
99  result = true;
100  }
101  else if (list_size == 1)
102  result = reference_pointers_[0]->update(data_in, data_out);
103  else if (list_size == 2)
104  {
105  result = reference_pointers_[0]->update(data_in, buffer0_);
106  if (result == false) {return false; };//don't keep processing on failure
107  result = result && reference_pointers_[1]->update(buffer0_, data_out);
108  }
109  else
110  {
111  result = reference_pointers_[0]->update(data_in, buffer0_); //first copy in
112  for (unsigned int i = 1; i < reference_pointers_.size() - 1; i++) // all but first and last (never called if size=2)
113  {
114  if (i %2 == 1)
115  result = result && reference_pointers_[i]->update(buffer0_, buffer1_);
116  else
117  result = result && reference_pointers_[i]->update(buffer1_, buffer0_);
118 
119  if (result == false) {return false; }; //don't keep processing on failure
120  }
121  if (list_size % 2 == 1) // odd number last deposit was in buffer1
122  result = result && reference_pointers_.back()->update(buffer1_, data_out);
123  else
124  result = result && reference_pointers_.back()->update(buffer0_, data_out);
125  }
126  return result;
127 
128  };
130  bool clear()
131  {
132  configured_ = false;
133  reference_pointers_.clear();
134  return true;
135  };
136 
137 
138 
139 
142  bool configure(XmlRpc::XmlRpcValue& config, const std::string& filter_ns)
143  {
144  /*************************** Parse the XmlRpcValue ***********************************/
145  //Verify proper naming and structure
146  if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
147  {
148  ROS_ERROR("%s: The filter chain specification must be a list. but is of of XmlRpcType %d", filter_ns.c_str(), config.getType());
149  ROS_ERROR("The xml passed in is formatted as follows:\n %s", config.toXml().c_str());
150 
151  return false;
152  }
153 
154  //Iterate over all filter in filters (may be just one)
155  for (int i = 0; i < config.size(); ++i)
156  {
157  if(config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
158  {
159  ROS_ERROR("%s: Filters must be specified as maps, but they are XmlRpcType:%d", filter_ns.c_str(), config[i].getType());
160  return false;
161  }
162  else if (!config[i].hasMember("type"))
163  {
164  ROS_ERROR("%s: Could not add a filter because no type was given", filter_ns.c_str());
165  return false;
166  }
167  else if (!config[i].hasMember("name"))
168  {
169  ROS_ERROR("%s: Could not add a filter because no name was given", filter_ns.c_str());
170  return false;
171  }
172  else
173  {
174  //Check for name collisions within the list itself.
175  for (int j = i + 1; j < config.size(); ++j)
176  {
177  if(config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct)
178  {
179  ROS_ERROR("%s: Filters must be specified as maps, but they are XmlRpcType:%d", filter_ns.c_str(), config[j].getType());
180  return false;
181  }
182 
183  if(!config[j].hasMember("name")
184  ||config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
185  || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString)
186  {
187  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());
188  return false;
189  }
190 
191  std::string namei = config[i]["name"];
192  std::string namej = config[j]["name"];
193  if (namei == namej)
194  {
195  ROS_ERROR("%s: A self_filter with the name %s already exists", filter_ns.c_str(), namei.c_str());
196  return false;
197  }
198  }
199 
200 
201  if (std::string(config[i]["type"]).find("/") == std::string::npos)
202  {
203  ROS_ERROR("Bad filter type %s. Filter type must be of form <package_name>/<filter_name>", std::string(config[i]["type"]).c_str());
204  return false;
205  }
206  //Make sure the filter chain has a valid type
207  std::vector<std::string> libs = loader_.getDeclaredClasses();
208  bool found = false;
209  for (std::vector<std::string>::iterator it = libs.begin(); it != libs.end(); ++it)
210  {
211  if (*it == std::string(config[i]["type"]))
212  {
213  found = true;
214  break;
215  }
216  }
217  if (!found)
218  {
219  ROS_ERROR("Couldn't find filter of type %s", std::string(config[i]["type"]).c_str());
220  return false;
221  }
222 
223  }
224  }
225 
226 
227  bool result = true;
228 
229 
230  for (int i = 0; i < config.size(); ++i)
231  {
232  boost::shared_ptr<filters::FilterBase<T> > p(loader_.createInstance(config[i]["type"]));
233  if (p.get() == NULL)
234  return false;
235  result = result && p.get()->configure(config[i]);
236  reference_pointers_.push_back(p);
237  std::string type = config[i]["type"];
238  std::string name = config[i]["name"];
239  ROS_DEBUG("%s: Configured %s:%s filter at %p\n", filter_ns.c_str(), type.c_str(),
240  name.c_str(), p.get());
241  }
242 
243  if (result == true)
244  {
245  configured_ = true;
246  }
247  return result;
248  };
249 
250 private:
251 
252  std::vector<boost::shared_ptr<filters::FilterBase<T> > > reference_pointers_;
253 
256  bool configured_;
257 
258 };
259 
263 template <typename T>
265 {
266 private:
268 public:
270  MultiChannelFilterChain(std::string data_type): loader_("filters", std::string("filters::MultiChannelFilterBase<") + data_type + std::string(">")), configured_(false)
271  {
272  std::string lib_string = "";
273  std::vector<std::string> libs = loader_.getDeclaredClasses();
274  for (unsigned int i = 0 ; i < libs.size(); i ++)
275  {
276  lib_string = lib_string + std::string(", ") + libs[i];
277  }
278  ROS_DEBUG("In MultiChannelFilterChain ClassLoader found the following libs: %s", lib_string.c_str());
279  };
280 
285  bool configure(unsigned int size, std::string param_name, ros::NodeHandle node = ros::NodeHandle())
286  {
287  XmlRpc::XmlRpcValue config;
288  if(node.getParam(param_name + "/filter_chain", config))
289  {
290  std::string resolved_name = node.resolveName(param_name).c_str();
291  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());
292  }
293  else if(!node.getParam(param_name, config))
294  {
295  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());
296  /********************** Do the allocation *********************/
297  buffer0_.resize(size);
298  buffer1_.resize(size);
299  configured_ = true;
300  return false;
301  }
302  return this->configure(size, config);
303  }
304 
306  bool update(const std::vector<T>& data_in, std::vector<T>& data_out)
307  {
308  unsigned int list_size = reference_pointers_.size();
309  bool result;
310  if (list_size == 0)
311  {
312  data_out = data_in;
313  result = true;
314  }
315  else if (list_size == 1)
316  result = reference_pointers_[0]->update(data_in, data_out);
317  else if (list_size == 2)
318  {
319  result = reference_pointers_[0]->update(data_in, buffer0_);
320  if (result == false) {return false; };//don't keep processing on failure
321  result = result && reference_pointers_[1]->update(buffer0_, data_out);
322  }
323  else
324  {
325  result = reference_pointers_[0]->update(data_in, buffer0_); //first copy in
326  for (unsigned int i = 1; i < reference_pointers_.size() - 1; i++) // all but first and last (never if size = 2)
327  {
328  if (i %2 == 1)
329  result = result && reference_pointers_[i]->update(buffer0_, buffer1_);
330  else
331  result = result && reference_pointers_[i]->update(buffer1_, buffer0_);
332 
333  if (result == false) {return false; }; //don't keep processing on failure
334  }
335  if (list_size % 2 == 1) // odd number last deposit was in buffer1
336  result = result && reference_pointers_.back()->update(buffer1_, data_out);
337  else
338  result = result && reference_pointers_.back()->update(buffer0_, data_out);
339  }
340  return result;
341 
342  };
343 
344 
346  {
347  clear();
348 
349  };
350 
352  bool clear()
353  {
354  configured_ = false;
355  reference_pointers_.clear();
356  buffer0_.clear();
357  buffer1_.clear();
358  return true;
359  };
360 
361 
362 
366  bool configure(unsigned int size, XmlRpc::XmlRpcValue& config)
367  {
368  /*************************** Parse the XmlRpcValue ***********************************/
369  //Verify proper naming and structure
370  if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
371  {
372  ROS_ERROR("The filter chain specification must be a list. but is of of XmlRpcType %d", config.getType());
373  ROS_ERROR("The xml passed in is formatted as follows:\n %s", config.toXml().c_str());
374 
375  return false;
376  }
377 
378  //Iterate over all filter in filters (may be just one)
379  for (int i = 0; i < config.size(); ++i)
380  {
381  if(config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
382  {
383  ROS_ERROR("Filters must be specified as maps, but they are XmlRpcType:%d", config[i].getType());
384  return false;
385  }
386  else if (!config[i].hasMember("type"))
387  {
388  ROS_ERROR("Could not add a filter because no type was given");
389  return false;
390  }
391  else if (!config[i].hasMember("name"))
392  {
393  ROS_ERROR("Could not add a filter because no name was given");
394  return false;
395  }
396  else
397  {
398  //Check for name collisions within the list itself.
399  for (int j = i + 1; j < config.size(); ++j)
400  {
401  if(config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct)
402  {
403  ROS_ERROR("Filters must be specified as maps");
404  return false;
405  }
406 
407  if(!config[j].hasMember("name")
408  ||config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
409  || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString)
410  {
411  ROS_ERROR("Filters names must be strings");
412  return false;
413  }
414 
415  std::string namei = config[i]["name"];
416  std::string namej = config[j]["name"];
417  if (namei == namej)
418  {
419  std::string name = config[i]["name"];
420  ROS_ERROR("A self_filter with the name %s already exists", name.c_str());
421  return false;
422  }
423  }
424  //CHeck for backwards compatible declarations
425  if (std::string(config[i]["type"]).find("/") == std::string::npos)
426  {
427  ROS_WARN("Deprecation Warning: No '/' detected in FilterType, Please update to 1.2 plugin syntax. ");
428  std::vector<std::string> libs = loader_.getDeclaredClasses();
429  for (std::vector<std::string>::iterator it = libs.begin(); it != libs.end(); ++it)
430  {
431  size_t position = it->find(std::string(config[i]["type"]));
432  if (position != std::string::npos)
433  {
434  ROS_WARN("Replaced %s with %s", std::string(config[i]["type"]).c_str(), it->c_str());
435  config[i]["type"] = *it;
436  }
437  }
438  }
439  //Make sure the filter chain has a valid type
440  std::vector<std::string> libs = loader_.getDeclaredClasses();
441  bool found = false;
442  for (std::vector<std::string>::iterator it = libs.begin(); it != libs.end(); ++it)
443  {
444  if (*it == std::string(config[i]["type"]))
445  {
446  found = true;
447  break;
448  }
449  }
450  if (!found)
451  {
452  ROS_ERROR("Couldn't find filter of type %s", std::string(config[i]["type"]).c_str());
453  return false;
454  }
455 
456 
457  }
458  }
459 
460  /********************** Do the allocation *********************/
461  buffer0_.resize(size);
462  buffer1_.resize(size);
463 
464  bool result = true;
465 
466 
467  for (int i = 0; i < config.size(); ++i)
468  {
469  boost::shared_ptr<filters::MultiChannelFilterBase<T> > p(loader_.createInstance(config[i]["type"]));
470  if (p.get() == NULL)
471  return false;
472  result = result && p.get()->configure(size, config[i]);
473  reference_pointers_.push_back(p);
474  std::string type = config[i]["type"];
475  std::string name = config[i]["name"];
476  ROS_DEBUG("Configured %s:%s filter at %p\n", type.c_str(),
477  name.c_str(), p.get());
478  }
479 
480  if (result == true)
481  {
482  configured_ = true;
483  }
484  return result;
485  };
486 
487 private:
488 
489  std::vector<boost::shared_ptr<filters::MultiChannelFilterBase<T> > > reference_pointers_;
490 
491  std::vector<T> buffer0_;
492  std::vector<T> buffer1_;
493  bool configured_;
494 
495 };
496 
497 };
498 
499 #endif //#ifndef FILTERS_FILTER_CHAIN_H_
T buffer0_
! A temporary intermediate buffer
Definition: filter_chain.h:254
bool clear()
Clear all filters from this chain.
Definition: filter_chain.h:352
A class which will construct and sequentially call Filters according to xml This is the primary way i...
Definition: filter_chain.h:264
bool configured_
! whether the system is configured
Definition: filter_chain.h:493
pluginlib::ClassLoader< filters::MultiChannelFilterBase< T > > loader_
Definition: filter_chain.h:267
int size() const
FilterChain(std::string data_type)
Create the filter chain object.
Definition: filter_chain.h:53
std::vector< T > buffer0_
! A temporary intermediate buffer
Definition: filter_chain.h:491
T buffer1_
! A temporary intermediate buffer
Definition: filter_chain.h:255
bool update(const std::vector< T > &data_in, std::vector< T > &data_out)
process data through each of the filters added sequentially
Definition: filter_chain.h:306
Type const & getType() const
#define ROS_WARN(...)
std::string toXml() const
std::vector< std::string > getDeclaredClasses()
MultiChannelFilterChain(std::string data_type)
Create the filter chain object.
Definition: filter_chain.h:270
A class which will construct and sequentially call Filters according to xml This is the primary way i...
Definition: filter_chain.h:47
bool clear()
Clear all filters from this chain.
Definition: filter_chain.h:130
std::vector< boost::shared_ptr< filters::FilterBase< T > > > reference_pointers_
! A vector of pointers to currently constructed filters
Definition: filter_chain.h:248
bool configure(unsigned int size, XmlRpc::XmlRpcValue &config)
Configure the filter chain This will call configure on all filters which have been added as well as a...
Definition: filter_chain.h:366
std::vector< boost::shared_ptr< filters::MultiChannelFilterBase< T > > > reference_pointers_
! A vector of pointers to currently constructed filters
Definition: filter_chain.h:485
bool update(const T &data_in, T &data_out)
process data through each of the filters added sequentially
Definition: filter_chain.h:92
bool configure(std::string param_name, ros::NodeHandle node=ros::NodeHandle())
Configure the filter chain from a configuration stored on the parameter server.
Definition: filter_chain.h:74
bool configure(XmlRpc::XmlRpcValue &config, const std::string &filter_ns)
Configure the filter chain This will call configure on all filters which have been added...
Definition: filter_chain.h:142
std::vector< T > buffer1_
! A temporary intermediate buffer
Definition: filter_chain.h:492
bool configured_
! whether the system is configured
Definition: filter_chain.h:256
bool configure(unsigned int size, std::string param_name, ros::NodeHandle node=ros::NodeHandle())
Configure the filter chain from a configuration stored on the parameter server.
Definition: filter_chain.h:285
#define ROS_ERROR(...)
pluginlib::ClassLoader< filters::FilterBase< T > > loader_
Definition: filter_chain.h:50
#define ROS_DEBUG(...)


filters
Author(s):
autogenerated on Sat Jun 8 2019 04:37:52