00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 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 = boost::make_shared<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