35 #ifndef MESSAGE_FILTERS_CACHE_H_ 36 #define MESSAGE_FILTERS_CACHE_H_ 39 #include "boost/thread.hpp" 40 #include "boost/shared_ptr.hpp" 73 Cache(F& f,
unsigned int cache_size = 1)
84 Cache(
unsigned int cache_size = 1)
119 void add(
const MConstPtr& msg)
128 void add(
const EventType& evt)
142 typename std::deque<EventType >::reverse_iterator rev_it =
cache_.rbegin();
147 while(rev_it !=
cache_.rend() && mt::TimeStamp<M>::value(*(*rev_it).getMessage()) > evt_stamp)
151 cache_.insert(rev_it.base(), evt);
173 unsigned int start_index = 0 ;
174 while(start_index <
cache_.size() &&
175 mt::TimeStamp<M>::value(*
cache_[start_index].getMessage()) < start)
181 unsigned int end_index = start_index ;
182 while(end_index <
cache_.size() &&
183 mt::TimeStamp<M>::value(*
cache_[end_index].getMessage()) <= end)
188 std::vector<MConstPtr> interval_elems ;
189 interval_elems.reserve(end_index - start_index) ;
190 for (
unsigned int i=start_index; i<end_index; i++)
192 interval_elems.push_back(
cache_[i].getMessage()) ;
195 return interval_elems ;
211 unsigned int start_index =
cache_.size()-1;
212 while(start_index > 0 &&
213 mt::TimeStamp<M>::value(*
cache_[start_index].getMessage()) > start)
217 unsigned int end_index = start_index;
218 while(end_index <
cache_.size()-1 &&
219 mt::TimeStamp<M>::value(*
cache_[end_index].getMessage()) < end)
224 std::vector<MConstPtr> interval_elems;
225 interval_elems.reserve(end_index - start_index + 1) ;
226 for (
unsigned int i=start_index; i<=end_index; i++)
228 interval_elems.push_back(
cache_[i].getMessage()) ;
231 return interval_elems;
248 int elem_index = -1 ;
250 mt::TimeStamp<M>::value(*
cache_[i].getMessage()) < time)
257 out =
cache_[elem_index].getMessage() ;
276 int elem_index = -1 ;
278 mt::TimeStamp<M>::value(*
cache_[i].getMessage()) > time)
285 out =
cache_[elem_index].getMessage() ;
304 latest_time = mt::TimeStamp<M>::value(*
cache_.back().getMessage());
321 oldest_time = mt::TimeStamp<M>::value(*
cache_.front().getMessage());
boost::shared_ptr< M const > MConstPtr
Cache(unsigned int cache_size=1)
void signalMessage(const MConstPtr &msg)
Call all registered callbacks, passing them the specified message.
void disconnect()
disconnects this connection
void setCacheSize(unsigned int cache_size)
unsigned int cache_size_
Maximum number of elements allowed in the cache.
ros::MessageEvent< M const > EventType
ros::Time getLatestTime() const
Returns the timestamp associated with the newest packet cache.
std::vector< MConstPtr > getSurroundingInterval(const ros::Time &start, const ros::Time &end) const
Retrieve the smallest interval of messages that surrounds an interval from start to end...
std::vector< MConstPtr > getInterval(const ros::Time &start, const ros::Time &end) const
Receive a vector of messages that occur between a start and end time (inclusive). ...
void callback(const EventType &evt)
Connection incoming_connection_
ros::Time getOldestTime() const
Returns the timestamp associated with the oldest packet cache.
MConstPtr getElemBeforeTime(const ros::Time &time) const
Grab the newest element that occurs right before the specified time.
Stores a time history of messages.
Convenience base-class for simple filters which output a single message.
boost::function< void(const EventType &)> EventCallback
std::deque< EventType > cache_
Cache for the messages.
Encapsulates a connection from one filter to another (or to a user-specified callback) ...
void add(const EventType &evt)
Add a message to the cache, and pop off any elements that are too old. This method is registered with...
Cache(F &f, unsigned int cache_size=1)
boost::shared_ptr< M > getMessage() const
void add(const MConstPtr &msg)
Add a message to the cache, and pop off any elements that are too old. This method is registered with...
MConstPtr getElemAfterTime(const ros::Time &time) const
Grab the oldest element that occurs right after the specified time.
boost::mutex cache_lock_
Lock for cache_.