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
00031
00032
00033
00034
00035 #ifndef MESSAGE_FILTERS_CHAIN_H
00036 #define MESSAGE_FILTERS_CHAIN_H
00037
00038 #include "simple_filter.h"
00039 #include "pass_through.h"
00040
00041 #include <vector>
00042
00043 namespace message_filters
00044 {
00045
00050 class ChainBase
00051 {
00052 public:
00053 virtual ~ChainBase() {}
00054
00062 template<typename F>
00063 boost::shared_ptr<F> getFilter(size_t index) const
00064 {
00065 boost::shared_ptr<void> filter = getFilterForIndex(index);
00066 if (filter)
00067 {
00068 return boost::static_pointer_cast<F>(filter);
00069 }
00070
00071 return boost::shared_ptr<F>();
00072 }
00073
00074 protected:
00075 virtual boost::shared_ptr<void> getFilterForIndex(size_t index) const = 0;
00076 };
00077 typedef boost::shared_ptr<ChainBase> ChainBasePtr;
00078
00110 template<typename M>
00111 class Chain : public ChainBase, public SimpleFilter<M>
00112 {
00113 public:
00114 typedef boost::shared_ptr<M const> MConstPtr;
00115 typedef ros::MessageEvent<M const> EventType;
00116
00120 Chain()
00121 {
00122 }
00123
00127 template<typename F>
00128 Chain(F& f)
00129 {
00130 connectInput(f);
00131 }
00132
00133 struct NullDeleter
00134 {
00135 void operator()(void const*) const
00136 {
00137 }
00138 };
00139
00143 template<class F>
00144 size_t addFilter(F* filter)
00145 {
00146 boost::shared_ptr<F> ptr(filter, NullDeleter());
00147 return addFilter(ptr);
00148 }
00149
00153 template<class F>
00154 size_t addFilter(const boost::shared_ptr<F>& filter)
00155 {
00156 FilterInfo info;
00157 info.add_func = boost::bind((void(F::*)(const EventType&))&F::add, filter.get(), _1);
00158 info.filter = filter;
00159 info.passthrough.reset(new PassThrough<M>);
00160
00161 last_filter_connection_.disconnect();
00162 info.passthrough->connectInput(*filter);
00163 last_filter_connection_ = info.passthrough->registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&Chain::lastFilterCB, this, _1)));
00164 if (!filters_.empty())
00165 {
00166 filter->connectInput(*filters_.back().passthrough);
00167 }
00168
00169 uint32_t count = filters_.size();
00170 filters_.push_back(info);
00171 return count;
00172 }
00173
00181 template<typename F>
00182 boost::shared_ptr<F> getFilter(size_t index) const
00183 {
00184 if (index >= filters_.size())
00185 {
00186 return boost::shared_ptr<F>();
00187 }
00188
00189 return boost::static_pointer_cast<F>(filters_[index].filter);
00190 }
00191
00195 template<class F>
00196 void connectInput(F& f)
00197 {
00198 incoming_connection_.disconnect();
00199 incoming_connection_ = f.registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&Chain::incomingCB, this, _1)));
00200 }
00201
00205 void add(const MConstPtr& msg)
00206 {
00207 add(EventType(msg));
00208 }
00209
00210 void add(const EventType& evt)
00211 {
00212 if (!filters_.empty())
00213 {
00214 filters_[0].add_func(evt);
00215 }
00216 }
00217
00218 protected:
00219 virtual boost::shared_ptr<void> getFilterForIndex(size_t index) const
00220 {
00221 if (index >= filters_.size())
00222 {
00223 return boost::shared_ptr<void>();
00224 }
00225
00226 return filters_[index].filter;
00227 }
00228
00229 private:
00230 void incomingCB(const EventType& evt)
00231 {
00232 add(evt);
00233 }
00234
00235 void lastFilterCB(const EventType& evt)
00236 {
00237 this->signalMessage(evt);
00238 }
00239
00240 struct FilterInfo
00241 {
00242 boost::function<void(const EventType&)> add_func;
00243 boost::shared_ptr<void> filter;
00244 boost::shared_ptr<PassThrough<M> > passthrough;
00245 };
00246 typedef std::vector<FilterInfo> V_FilterInfo;
00247
00248 V_FilterInfo filters_;
00249
00250 Connection incoming_connection_;
00251 Connection last_filter_connection_;
00252 };
00253 }
00254
00255 #endif // MESSAGE_FILTERS_CHAIN_H