p36850791
/
kinetic-devel
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

to ros_comm

This commit is contained in:
Tully Foote 2010-10-26 21:03:28 +00:00
parent 29e29a75e4
commit 6daa0e3834
36 changed files with 0 additions and 6998 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
core/message_filters/CMakeLists.txt
View File

@ -1,16 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()
include_directories(include/message_filters)
rosbuild_add_boost_directories()
rosbuild_add_library(message_filters src/connection.cpp)
rosbuild_link_boost(message_filters thread signals)
#rosbuild_add_executable(filter_example src/filter_example.cpp)
#target_link_libraries(filter_example message_filters)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
add_subdirectory(test EXCLUDE_FROM_ALL)

1
core/message_filters/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

204
core/message_filters/conf.py
View File

@ -1,204 +0,0 @@
# -*- coding: utf-8 -*-
#
# message_filters documentation build configuration file, created by
# sphinx-quickstart on Mon Jun 1 14:21:53 2009.
#
# This file is execfile()d with the current directory set to its containing dir.
#
# Note that not all possible configuration values are present in this
# autogenerated file.
#
# All configuration values have a default; values that are commented out
# serve to show the default.
import roslib
roslib.load_manifest('message_filters')
import sys, os
# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
#sys.path.append(os.path.abspath('.'))
# -- General configuration -----------------------------------------------------
# Add any Sphinx extension module names here, as strings. They can be extensions
# coming with Sphinx (named 'sphinx.ext.*') or your custom ones.
extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath']
# Add any paths that contain templates here, relative to this directory.
templates_path = ['_templates']
# The suffix of source filenames.
source_suffix = '.rst'
# The encoding of source files.
#source_encoding = 'utf-8'
# The master toctree document.
master_doc = 'index'
# General information about the project.
project = u'message_filters'
copyright = u'2009, Willow Garage, Inc.'
# The version info for the project you're documenting, acts as replacement for
# |version| and |release|, also used in various other places throughout the
# built documents.
#
# The short X.Y version.
version = '0.1'
# The full version, including alpha/beta/rc tags.
release = '0.1.0'
# The language for content autogenerated by Sphinx. Refer to documentation
# for a list of supported languages.
#language = None
# There are two options for replacing |today|: either, you set today to some
# non-false value, then it is used:
#today = ''
# Else, today_fmt is used as the format for a strftime call.
#today_fmt = '%B %d, %Y'
# List of documents that shouldn't be included in the build.
#unused_docs = []
# List of directories, relative to source directory, that shouldn't be searched
# for source files.
exclude_trees = ['_build']
# The reST default role (used for this markup: `text`) to use for all documents.
#default_role = None
# If true, '()' will be appended to :func: etc. cross-reference text.
#add_function_parentheses = True
# If true, the current module name will be prepended to all description
# unit titles (such as .. function::).
#add_module_names = True
# If true, sectionauthor and moduleauthor directives will be shown in the
# output. They are ignored by default.
#show_authors = False
# The name of the Pygments (syntax highlighting) style to use.
pygments_style = 'sphinx'
# A list of ignored prefixes for module index sorting.
#modindex_common_prefix = []
# -- Options for HTML output ---------------------------------------------------
# The theme to use for HTML and HTML Help pages. Major themes that come with
# Sphinx are currently 'default' and 'sphinxdoc'.
html_theme = 'default'
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
# documentation.
#html_theme_options = {}
# Add any paths that contain custom themes here, relative to this directory.
#html_theme_path = []
# The name for this set of Sphinx documents. If None, it defaults to
# "<project> v<release> documentation".
#html_title = None
# A shorter title for the navigation bar. Default is the same as html_title.
#html_short_title = None
# The name of an image file (relative to this directory) to place at the top
# of the sidebar.
#html_logo = None
# The name of an image file (within the static path) to use as favicon of the
# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32
# pixels large.
#html_favicon = None
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ['_static']
# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
# using the given strftime format.
#html_last_updated_fmt = '%b %d, %Y'
# If true, SmartyPants will be used to convert quotes and dashes to
# typographically correct entities.
#html_use_smartypants = True
# Custom sidebar templates, maps document names to template names.
#html_sidebars = {}
# Additional templates that should be rendered to pages, maps page names to
# template names.
#html_additional_pages = {}
# If false, no module index is generated.
#html_use_modindex = True
# If false, no index is generated.
#html_use_index = True
# If true, the index is split into individual pages for each letter.
#html_split_index = False
# If true, links to the reST sources are added to the pages.
#html_show_sourcelink = True
# If true, an OpenSearch description file will be output, and all pages will
# contain a <link> tag referring to it. The value of this option must be the
# base URL from which the finished HTML is served.
#html_use_opensearch = ''
# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml").
#html_file_suffix = ''
# Output file base name for HTML help builder.
htmlhelp_basename = 'tfdoc'
# -- Options for LaTeX output --------------------------------------------------
# The paper size ('letter' or 'a4').
#latex_paper_size = 'letter'
# The font size ('10pt', '11pt' or '12pt').
#latex_font_size = '10pt'
# Grouping the document tree into LaTeX files. List of tuples
# (source start file, target name, title, author, documentclass [howto/manual]).
latex_documents = [
('index', 'message_filters.tex', u'stereo\\_utils Documentation',
u'James Bowman', 'manual'),
]
# The name of an image file (relative to this directory) to place at the top of
# the title page.
#latex_logo = None
# For "manual" documents, if this is true, then toplevel headings are parts,
# not chapters.
#latex_use_parts = False
# Additional stuff for the LaTeX preamble.
#latex_preamble = ''
# Documents to append as an appendix to all manuals.
#latex_appendices = []
# If false, no module index is generated.
#latex_use_modindex = True
# Example configuration for intersphinx: refer to the Python standard library.
intersphinx_mapping = {
'http://docs.python.org/': None,
'http://opencv.willowgarage.com/documentation/': None,
'http://opencv.willowgarage.com/documentation/python/': None,
'http://docs.scipy.org/doc/numpy' : None
}

325
core/message_filters/include/message_filters/cache.h
View File

@ -1,325 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_CACHE_H_
#define MESSAGE_FILTERS_CACHE_H_
#include <deque>
#include "boost/thread.hpp"
#include "boost/shared_ptr.hpp"
#include <boost/signals.hpp>
#include "ros/time.h"
#include "connection.h"
#include "simple_filter.h"
namespace message_filters
{
/**
* \brief Stores a time history of messages
*
* Given a stream of messages, the most recent N messages are cached in a ring buffer,
* from which time intervals of the cache can then be retrieved by the client.
*
* Cache immediately passes messages through to its output connections.
*
* \section connections CONNECTIONS
*
* Cache's input and output connections are both of the same signature as roscpp subscription callbacks, ie.
\verbatim
void callback(const boost::shared_ptr<M const>&);
\endverbatim
*/
template<class M>
class Cache : public SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef ros::MessageEvent<M const> EventType;
template<class F>
Cache(F& f, unsigned int cache_size = 1)
{
setCacheSize(cache_size) ;
connectInput(f) ;
}
/**
* Initializes a Message Cache without specifying a parent filter. This implies that in
* order to populate the cache, the user then has to call add themselves, or connectInput() is
* called later
*/
Cache(unsigned int cache_size = 1)
{
setCacheSize(cache_size);
}
template<class F>
void connectInput(F& f)
{
incoming_connection_ = f.registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&Cache::callback, this, _1)));
}
~Cache()
{
incoming_connection_.disconnect();
}
/**
* Set the size of the cache.
* \param cache_size The new size the cache should be. Must be > 0
*/
void setCacheSize(unsigned int cache_size)
{
if (cache_size == 0)
{
//ROS_ERROR("Cannot set max_size to 0") ;
return ;
}
cache_size_ = cache_size ;
}
/**
* \brief Add a message to the cache, and pop off any elements that are too old.
* This method is registered with a data provider when connectTo is called.
*/
void add(const MConstPtr& msg)
{
add(EventType(msg));
}
/**
* \brief Add a message to the cache, and pop off any elements that are too old.
* This method is registered with a data provider when connectTo is called.
*/
void add(const EventType& evt)
{
namespace mt = ros::message_traits;
//printf(" Cache Size: %u\n", cache_.size()) ;
{
boost::mutex::scoped_lock lock(cache_lock_);
while (cache_.size() >= cache_size_) // Keep popping off old data until we have space for a new msg
cache_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
// No longer naively pushing msgs to back. Want to make sure they're sorted correctly
//cache_.push_back(msg) ; // Add the newest message to the back of the deque
typename std::deque<EventType >::reverse_iterator rev_it = cache_.rbegin();
// Keep walking backwards along deque until we hit the beginning,
// or until we find a timestamp that's smaller than (or equal to) msg's timestamp
ros::Time evt_stamp = mt::TimeStamp<M>::value(*evt.getMessage());
while(rev_it != cache_.rend() && mt::TimeStamp<M>::value(*(*rev_it).getMessage()) > evt_stamp)
rev_it++;
// Add msg to the cache
cache_.insert(rev_it.base(), evt);
}
signalMessage(evt);
}
/**
* \brief Receive a vector of messages that occur between a start and end time (inclusive).
*
* This call is non-blocking, and only aggregates messages it has already received.
* It will not wait for messages have not yet been received, but occur in the interval.
* \param start The start of the requested interval
* \param end The end of the requested interval
*/
std::vector<MConstPtr> getInterval(const ros::Time& start, const ros::Time& end) const
{
namespace mt = ros::message_traits;
boost::mutex::scoped_lock lock(cache_lock_);
// Find the starting index. (Find the first index after [or at] the start of the interval)
unsigned int start_index = 0 ;
while(start_index < cache_.size() &&
mt::TimeStamp<M>::value(*cache_[start_index].getMessage()) < start)
{
start_index++ ;
}
// Find the ending index. (Find the first index after the end of interval)
unsigned int end_index = start_index ;
while(end_index < cache_.size() &&
mt::TimeStamp<M>::value(*cache_[end_index].getMessage()) <= end)
{
end_index++ ;
}
std::vector<MConstPtr> interval_elems ;
interval_elems.reserve(end_index - start_index) ;
for (unsigned int i=start_index; i<end_index; i++)
{
interval_elems.push_back(cache_[i].getMessage()) ;
}
return interval_elems ;
}
/**
* \brief Retrieve the smallest interval of messages that surrounds an interval from start to end.
*
* If the messages in the cache do not surround (start,end), then this will return the interval
* that gets closest to surrounding (start,end)
*/
std::vector<MConstPtr> getSurroundingInterval(const ros::Time& start, const ros::Time& end) const
{
namespace mt = ros::message_traits;
boost::mutex::scoped_lock lock(cache_lock_);
// Find the starting index. (Find the first index after [or at] the start of the interval)
unsigned int start_index = cache_.size()-1;
while(start_index > 0 &&
mt::TimeStamp<M>::value(*cache_[start_index].getMessage()) > start)
{
start_index--;
}
unsigned int end_index = start_index;
while(end_index < cache_.size()-1 &&
mt::TimeStamp<M>::value(*cache_[end_index].getMessage()) < end)
{
end_index++;
}
std::vector<MConstPtr> interval_elems;
interval_elems.reserve(end_index - start_index + 1) ;
for (unsigned int i=start_index; i<=end_index; i++)
{
interval_elems.push_back(cache_[i].getMessage()) ;
}
return interval_elems;
}
/**
* \brief Grab the newest element that occurs right before the specified time.
* \param time Time that must occur right after the returned elem
* \returns shared_ptr to the newest elem that occurs before 'time'. NULL if doesn't exist
*/
MConstPtr getElemBeforeTime(const ros::Time& time) const
{
namespace mt = ros::message_traits;
boost::mutex::scoped_lock lock(cache_lock_);
MConstPtr out ;
unsigned int i=0 ;
int elem_index = -1 ;
while (i<cache_.size() &&
mt::TimeStamp<M>::value(*cache_[i].getMessage()) < time)
{
elem_index = i ;
i++ ;
}
if (elem_index >= 0)
out = cache_[elem_index].getMessage() ;
return out ;
}
/**
* \brief Grab the oldest element that occurs right after the specified time.
* \param time Time that must occur right before the returned elem
* \returns shared_ptr to the oldest elem that occurs after 'time'. NULL if doesn't exist
*/
MConstPtr getElemAfterTime(const ros::Time& time) const
{
namespace mt = ros::message_traits;
boost::mutex::scoped_lock lock(cache_lock_);
MConstPtr out ;
int i=cache_.size()-1 ;
int elem_index = -1 ;
while (i>=0 &&
mt::TimeStamp<M>::value(*cache_[i].getMessage()) > time)
{
elem_index = i ;
i-- ;
}
if (elem_index >= 0)
out = cache_[elem_index].getMessage() ;
else
out.reset() ;
return out ;
}
/**
* \brief Returns the timestamp associated with the newest packet cache
*/
ros::Time getLatestTime() const
{
namespace mt = ros::message_traits;
boost::mutex::scoped_lock lock(cache_lock_);
ros::Time latest_time;
if (cache_.size() > 0)
latest_time = mt::TimeStamp<M>::value(*cache_.back().getMessage());
return latest_time ;
}
private:
void callback(const EventType& evt)
{
add(evt);
}
mutable boost::mutex cache_lock_ ; //!< Lock for cache_
std::deque<EventType> cache_ ; //!< Cache for the messages
unsigned int cache_size_ ; //!< Maximum number of elements allowed in the cache.
Connection incoming_connection_;
};
}
#endif /* MESSAGE_FILTERS_CACHE_H_ */

255
core/message_filters/include/message_filters/chain.h
View File

@ -1,255 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_CHAIN_H
#define MESSAGE_FILTERS_CHAIN_H
#include "simple_filter.h"
#include "pass_through.h"
#include <vector>
namespace message_filters
{
/**
* \brief Base class for Chain, allows you to store multiple chains in the same container. Provides filter retrieval
* by index.
*/
class ChainBase
{
public:
virtual ~ChainBase() {}
/**
* \brief Retrieve a filter from this chain by index. Returns an empty shared_ptr if the index is greater than
* the size of the chain. \b NOT type-safe
*
* \param F [template] The type of the filter
* \param index The index of the filter (returned by addFilter())
*/
template<typename F>
boost::shared_ptr<F> getFilter(size_t index) const
{
boost::shared_ptr<void> filter = getFilterForIndex(index);
if (filter)
{
return boost::static_pointer_cast<F>(filter);
}
return boost::shared_ptr<F>();
}
protected:
virtual boost::shared_ptr<void> getFilterForIndex(size_t index) const = 0;
};
typedef boost::shared_ptr<ChainBase> ChainBasePtr;
/**
* \brief Chains a dynamic number of simple filters together. Allows retrieval of filters by index after they are added.
*
* The Chain filter provides a container for simple filters. It allows you to store an N-long set of filters inside a single
* structure, making it much easier to manage them.
*
* Adding filters to the chain is done by adding shared_ptrs of them to the filter. They are automatically connected to each other
* and the output of the last filter in the chain is forwarded to the callback you've registered with Chain::registerCallback
*
* Example:
\verbatim
void myCallback(const MsgConstPtr& msg)
{
}
Chain<Msg> c;
c.addFilter(boost::shared_ptr<PassThrough<Msg> >(new PassThrough<Msg>));
c.addFilter(boost::shared_ptr<PassThrough<Msg> >(new PassThrough<Msg>));
c.registerCallback(myCallback);
\endverbatim
*
* It is also possible to pass bare pointers in, which will not be automatically deleted when Chain is destructed:
\verbatim
Chain<Msg> c;
PassThrough<Msg> p;
c.addFilter(&p);
c.registerCallback(myCallback);
\endverbatim
*
*/
template<typename M>
class Chain : public ChainBase, public SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef ros::MessageEvent<M const> EventType;
/**
* \brief Default constructor
*/
Chain()
{
}
/**
* \brief Constructor with filter. Calls connectInput(f)
*/
template<typename F>
Chain(F& f)
{
connectInput(f);
}
struct NullDeleter
{
void operator()(void const*) const
{
}
};
/**
* \brief Add a filter to this chain, by bare pointer. Returns the index of that filter in the chain.
*/
template<class F>
size_t addFilter(F* filter)
{
boost::shared_ptr<F> ptr(filter, NullDeleter());
return addFilter(ptr);
}
/**
* \brief Add a filter to this chain, by shared_ptr. Returns the index of that filter in the chain
*/
template<class F>
size_t addFilter(const boost::shared_ptr<F>& filter)
{
FilterInfo info;
info.add_func = boost::bind((void(F::*)(const EventType&))&F::add, filter.get(), _1);
info.filter = filter;
info.passthrough.reset(new PassThrough<M>);
last_filter_connection_.disconnect();
info.passthrough->connectInput(*filter);
last_filter_connection_ = info.passthrough->registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&Chain::lastFilterCB, this, _1)));
if (!filters_.empty())
{
filter->connectInput(*filters_.back().passthrough);
}
uint32_t count = filters_.size();
filters_.push_back(info);
return count;
}
/**
* \brief Retrieve a filter from this chain by index. Returns an empty shared_ptr if the index is greater than
* the size of the chain. \b NOT type-safe
*
* \param F [template] The type of the filter
* \param index The index of the filter (returned by addFilter())
*/
template<typename F>
boost::shared_ptr<F> getFilter(size_t index) const
{
if (index >= filters_.size())
{
return boost::shared_ptr<F>();
}
return boost::static_pointer_cast<F>(filters_[index].filter);
}
/**
* \brief Connect this filter's input to another filter's output.
*/
template<class F>
void connectInput(F& f)
{
incoming_connection_.disconnect();
incoming_connection_ = f.registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&Chain::incomingCB, this, _1)));
}
/**
* \brief Add a message to the start of this chain
*/
void add(const MConstPtr& msg)
{
add(EventType(msg));
}
void add(const EventType& evt)
{
if (!filters_.empty())
{
filters_[0].add_func(evt);
}
}
protected:
virtual boost::shared_ptr<void> getFilterForIndex(size_t index) const
{
if (index >= filters_.size())
{
return boost::shared_ptr<void>();
}
return filters_[index].filter;
}
private:
void incomingCB(const EventType& evt)
{
add(evt);
}
void lastFilterCB(const EventType& evt)
{
signalMessage(evt);
}
struct FilterInfo
{
boost::function<void(const EventType&)> add_func;
boost::shared_ptr<void> filter;
boost::shared_ptr<PassThrough<M> > passthrough;
};
typedef std::vector<FilterInfo> V_FilterInfo;
V_FilterInfo filters_;
Connection incoming_connection_;
Connection last_filter_connection_;
};
}
#endif // MESSAGE_FILTERS_CHAIN_H

71
core/message_filters/include/message_filters/connection.h
View File

@ -1,71 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_CONNECTION_H
#define MESSAGE_FILTERS_CONNECTION_H
#include <boost/function.hpp>
#include <boost/signals/connection.hpp>
namespace message_filters
{
/**
* \brief Encapsulates a connection from one filter to another (or to a user-specified callback)
*/
class Connection
{
public:
typedef boost::function<void(void)> VoidDisconnectFunction;
typedef boost::function<void(const Connection&)> WithConnectionDisconnectFunction;
Connection() {}
Connection(const VoidDisconnectFunction& func);
Connection(const WithConnectionDisconnectFunction& func, boost::signals::connection conn);
/**
* \brief disconnects this connection
*/
void disconnect();
boost::signals::connection getBoostConnection() const { return connection_; }
private:
VoidDisconnectFunction void_disconnect_;
WithConnectionDisconnectFunction connection_disconnect_;
boost::signals::connection connection_;
};
}
#endif // MESSAGE_FILTERS_CONNECTION_H

83
core/message_filters/include/message_filters/null_types.h
View File

@ -1,83 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_NULL_TYPES_H
#define MESSAGE_FILTERS_NULL_TYPES_H
#include "connection.h"
#include <boost/shared_ptr.hpp>
#include <ros/time.h>
namespace message_filters
{
struct NullType
{
};
typedef boost::shared_ptr<NullType const> NullTypeConstPtr;
template<class M>
struct NullFilter
{
template<typename C>
Connection registerCallback(const C& callback)
{
return Connection();
}
template<typename P>
Connection registerCallback(const boost::function<void(P)>& callback)
{
return Connection();
}
};
}
namespace ros
{
namespace message_traits
{
template<>
struct TimeStamp<message_filters::NullType>
{
static ros::Time value(const message_filters::NullType&)
{
return ros::Time();
}
};
}
}
#endif // MESSAGE_FILTERS_NULL_TYPES_H

93
core/message_filters/include/message_filters/pass_through.h
View File

@ -1,93 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_PASSTHROUGH_H
#define MESSAGE_FILTERS_PASSTHROUGH_H
#include "simple_filter.h"
#include <vector>
namespace message_filters
{
/**
* \brief Simple passthrough filter. What comes in goes out immediately.
*/
template<typename M>
class PassThrough : public SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef ros::MessageEvent<M const> EventType;
PassThrough()
{
}
template<typename F>
PassThrough(F& f)
{
connectInput(f);
}
template<class F>
void connectInput(F& f)
{
incoming_connection_.disconnect();
incoming_connection_ = f.registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&PassThrough::cb, this, _1)));
}
void add(const MConstPtr& msg)
{
add(EventType(msg));
}
void add(const EventType& evt)
{
signalMessage(evt);
}
private:
void cb(const EventType& evt)
{
add(evt);
}
Connection incoming_connection_;
};
} // namespace message_filters
#endif // MESSAGE_FILTERS_PASSTHROUGH_H

132
core/message_filters/include/message_filters/signal1.h
View File

@ -1,132 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_SIGNAL1_H
#define MESSAGE_FILTERS_SIGNAL1_H
#include <boost/noncopyable.hpp>
#include "connection.h"
#include <ros/message_event.h>
#include <ros/parameter_adapter.h>
#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>
namespace message_filters
{
template<class M>
class CallbackHelper1
{
public:
virtual ~CallbackHelper1() {}
virtual void call(const ros::MessageEvent<M const>& event, bool nonconst_need_copy) = 0;
typedef boost::shared_ptr<CallbackHelper1<M> > Ptr;
};
template<typename P, typename M>
class CallbackHelper1T : public CallbackHelper1<M>
{
public:
typedef ros::ParameterAdapter<P> Adapter;
typedef boost::function<void(typename Adapter::Parameter)> Callback;
typedef typename Adapter::Event Event;
CallbackHelper1T(const Callback& cb)
: callback_(cb)
{
}
virtual void call(const ros::MessageEvent<M const>& event, bool nonconst_force_copy)
{
Event my_event(event, nonconst_force_copy || event.nonConstWillCopy());
callback_(Adapter::getParameter(my_event));
}
private:
Callback callback_;
};
template<class M>
class Signal1
{
typedef boost::shared_ptr<CallbackHelper1<M> > CallbackHelper1Ptr;
typedef std::vector<CallbackHelper1Ptr> V_CallbackHelper1;
public:
template<typename P>
CallbackHelper1Ptr addCallback(const boost::function<void(P)>& callback)
{
CallbackHelper1T<P, M>* helper = new CallbackHelper1T<P, M>(callback);
boost::mutex::scoped_lock lock(mutex_);
callbacks_.push_back(CallbackHelper1Ptr(helper));
return callbacks_.back();
}
void removeCallback(const CallbackHelper1Ptr& helper)
{
boost::mutex::scoped_lock lock(mutex_);
typename V_CallbackHelper1::iterator it = std::find(callbacks_.begin(), callbacks_.end(), helper);
if (it != callbacks_.end())
{
callbacks_.erase(it);
}
}
void call(const ros::MessageEvent<M const>& event)
{
boost::mutex::scoped_lock lock(mutex_);
bool nonconst_force_copy = callbacks_.size() > 1;
typename V_CallbackHelper1::iterator it = callbacks_.begin();
typename V_CallbackHelper1::iterator end = callbacks_.end();
for (; it != end; ++it)
{
const CallbackHelper1Ptr& helper = *it;
helper->call(event, nonconst_force_copy);
}
}
private:
boost::mutex mutex_;
V_CallbackHelper1 callbacks_;
};
} // message_filters
#endif // MESSAGE_FILTERS_SIGNAL1_H

317
core/message_filters/include/message_filters/signal9.h
View File

@ -1,317 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_SIGNAL9_H
#define MESSAGE_FILTERS_SIGNAL9_H
#include <boost/noncopyable.hpp>
#include "connection.h"
#include "null_types.h"
#include <ros/message_event.h>
#include <ros/parameter_adapter.h>
#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>
namespace message_filters
{
using ros::ParameterAdapter;
template<typename M0, typename M1, typename M2, typename M3, typename M4, typename M5, typename M6, typename M7, typename M8>
class CallbackHelper9
{
public:
typedef ros::MessageEvent<M0 const> M0Event;
typedef ros::MessageEvent<M1 const> M1Event;
typedef ros::MessageEvent<M2 const> M2Event;
typedef ros::MessageEvent<M3 const> M3Event;
typedef ros::MessageEvent<M4 const> M4Event;
typedef ros::MessageEvent<M5 const> M5Event;
typedef ros::MessageEvent<M6 const> M6Event;
typedef ros::MessageEvent<M7 const> M7Event;
typedef ros::MessageEvent<M8 const> M8Event;
virtual ~CallbackHelper9() {}
virtual void call(bool nonconst_force_copy, const M0Event& e0, const M1Event& e1, const M2Event& e2, const M3Event& e3,
const M4Event& e4, const M5Event& e5, const M6Event& e6, const M7Event& e7, const M8Event& e8) = 0;
typedef boost::shared_ptr<CallbackHelper9> Ptr;
};
template<typename P0, typename P1, typename P2, typename P3, typename P4, typename P5, typename P6, typename P7, typename P8>
class CallbackHelper9T :
public CallbackHelper9<typename ParameterAdapter<P0>::Message,
typename ParameterAdapter<P1>::Message,
typename ParameterAdapter<P2>::Message,
typename ParameterAdapter<P3>::Message,
typename ParameterAdapter<P4>::Message,
typename ParameterAdapter<P5>::Message,
typename ParameterAdapter<P6>::Message,
typename ParameterAdapter<P7>::Message,
typename ParameterAdapter<P8>::Message>
{
private:
typedef ParameterAdapter<P0> A0;
typedef ParameterAdapter<P1> A1;
typedef ParameterAdapter<P2> A2;
typedef ParameterAdapter<P3> A3;
typedef ParameterAdapter<P4> A4;
typedef ParameterAdapter<P5> A5;
typedef ParameterAdapter<P6> A6;
typedef ParameterAdapter<P7> A7;
typedef ParameterAdapter<P8> A8;
typedef typename A0::Event M0Event;
typedef typename A1::Event M1Event;
typedef typename A2::Event M2Event;
typedef typename A3::Event M3Event;
typedef typename A4::Event M4Event;
typedef typename A5::Event M5Event;
typedef typename A6::Event M6Event;
typedef typename A7::Event M7Event;
typedef typename A8::Event M8Event;
public:
typedef boost::function<void(typename A0::Parameter, typename A1::Parameter, typename A2::Parameter,
typename A3::Parameter, typename A4::Parameter, typename A5::Parameter,
typename A6::Parameter, typename A7::Parameter, typename A8::Parameter)> Callback;
CallbackHelper9T(const Callback& cb)
: callback_(cb)
{
}
virtual void call(bool nonconst_force_copy, const M0Event& e0, const M1Event& e1, const M2Event& e2, const M3Event& e3,
const M4Event& e4, const M5Event& e5, const M6Event& e6, const M7Event& e7, const M8Event& e8)
{
M0Event my_e0(e0, nonconst_force_copy || e0.nonConstWillCopy());
M1Event my_e1(e1, nonconst_force_copy || e0.nonConstWillCopy());
M2Event my_e2(e2, nonconst_force_copy || e0.nonConstWillCopy());
M3Event my_e3(e3, nonconst_force_copy || e0.nonConstWillCopy());
M4Event my_e4(e4, nonconst_force_copy || e0.nonConstWillCopy());
M5Event my_e5(e5, nonconst_force_copy || e0.nonConstWillCopy());
M6Event my_e6(e6, nonconst_force_copy || e0.nonConstWillCopy());
M7Event my_e7(e7, nonconst_force_copy || e0.nonConstWillCopy());
M8Event my_e8(e8, nonconst_force_copy || e0.nonConstWillCopy());
callback_(A0::getParameter(e0),
A1::getParameter(e1),
A2::getParameter(e2),
A3::getParameter(e3),
A4::getParameter(e4),
A5::getParameter(e5),
A6::getParameter(e6),
A7::getParameter(e7),
A8::getParameter(e8));
}
private:
Callback callback_;
};
template<typename M0, typename M1, typename M2, typename M3, typename M4, typename M5, typename M6, typename M7, typename M8>
class Signal9
{
typedef boost::shared_ptr<CallbackHelper9<M0, M1, M2, M3, M4, M5, M6, M7, M8> > CallbackHelper9Ptr;
typedef std::vector<CallbackHelper9Ptr> V_CallbackHelper9;
public:
typedef ros::MessageEvent<M0 const> M0Event;
typedef ros::MessageEvent<M1 const> M1Event;
typedef ros::MessageEvent<M2 const> M2Event;
typedef ros::MessageEvent<M3 const> M3Event;
typedef ros::MessageEvent<M4 const> M4Event;
typedef ros::MessageEvent<M5 const> M5Event;
typedef ros::MessageEvent<M6 const> M6Event;
typedef ros::MessageEvent<M7 const> M7Event;
typedef ros::MessageEvent<M8 const> M8Event;
typedef boost::shared_ptr<M0 const> M0ConstPtr;
typedef boost::shared_ptr<M1 const> M1ConstPtr;
typedef boost::shared_ptr<M2 const> M2ConstPtr;
typedef boost::shared_ptr<M3 const> M3ConstPtr;
typedef boost::shared_ptr<M4 const> M4ConstPtr;
typedef boost::shared_ptr<M5 const> M5ConstPtr;
typedef boost::shared_ptr<M6 const> M6ConstPtr;
typedef boost::shared_ptr<M7 const> M7ConstPtr;
typedef boost::shared_ptr<M8 const> M8ConstPtr;
typedef const boost::shared_ptr<NullType const>& NullP;
template<typename P0, typename P1, typename P2, typename P3, typename P4, typename P5, typename P6, typename P7, typename P8>
Connection addCallback(const boost::function<void(P0, P1, P2, P3, P4, P5, P6, P7, P8)>& callback)
{
CallbackHelper9T<P0, P1, P2, P3, P4, P5, P6, P7, P8>* helper = new CallbackHelper9T<P0, P1, P2, P3, P4, P5, P6, P7, P8>(callback);
boost::mutex::scoped_lock lock(mutex_);
callbacks_.push_back(CallbackHelper9Ptr(helper));
return Connection(boost::bind(&Signal9::removeCallback, this, callbacks_.back()));
}
template<typename P0, typename P1>
Connection addCallback(void(*callback)(P0, P1))
{
return addCallback(boost::function<void(P0, P1, NullP, NullP, NullP, NullP, NullP, NullP, NullP)>(boost::bind(callback, _1, _2)));
}
template<typename P0, typename P1, typename P2>
Connection addCallback(void(*callback)(P0, P1, P2))
{
return addCallback(boost::function<void(P0, P1, P2, NullP, NullP, NullP, NullP, NullP, NullP)>(boost::bind(callback, _1, _2, _3)));
}
template<typename P0, typename P1, typename P2, typename P3>
Connection addCallback(void(*callback)(P0, P1, P2, P3))
{
return addCallback(boost::function<void(P0, P1, P2, P3, NullP, NullP, NullP, NullP, NullP)>(boost::bind(callback, _1, _2, _3, _4)));
}
template<typename P0, typename P1, typename P2, typename P3, typename P4>
Connection addCallback(void(*callback)(P0, P1, P2, P3, P4))
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, NullP, NullP, NullP, NullP)>(boost::bind(callback, _1, _2, _3, _4, _5)));
}
template<typename P0, typename P1, typename P2, typename P3, typename P4, typename P5>
Connection addCallback(void(*callback)(P0, P1, P2, P3, P4, P5))
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, P5, NullP, NullP, NullP)>(boost::bind(callback, _1, _2, _3, _4, _5, _6)));
}
template<typename P0, typename P1, typename P2, typename P3, typename P4, typename P5, typename P6>
Connection addCallback(void(*callback)(P0, P1, P2, P3, P4, P5, P6))
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, P5, P6, NullP, NullP)>(boost::bind(callback, _1, _2, _3, _4, _5, _6, _7)));
}
template<typename P0, typename P1, typename P2, typename P3, typename P4, typename P5, typename P6, typename P7>
Connection addCallback(void(*callback)(P0, P1, P2, P3, P4, P5, P6, P7))
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, P5, P6, P7, NullP)>(boost::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8)));
}
template<typename P0, typename P1, typename P2, typename P3, typename P4, typename P5, typename P6, typename P7, typename P8>
Connection addCallback(void(*callback)(P0, P1, P2, P3, P4, P5, P6, P7, P8))
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, P5, P6, P7, P8)>(boost::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8, _9)));
}
template<typename T, typename P0, typename P1>
Connection addCallback(void(T::*callback)(P0, P1), T* t)
{
return addCallback(boost::function<void(P0, P1, NullP, NullP, NullP, NullP, NullP, NullP, NullP)>(boost::bind(callback, t, _1, _2)));
}
template<typename T, typename P0, typename P1, typename P2>
Connection addCallback(void(T::*callback)(P0, P1, P2), T* t)
{
return addCallback(boost::function<void(P0, P1, P2, NullP, NullP, NullP, NullP, NullP, NullP)>(boost::bind(callback, t, _1, _2, _3)));
}
template<typename T, typename P0, typename P1, typename P2, typename P3>
Connection addCallback(void(T::*callback)(P0, P1, P2, P3), T* t)
{
return addCallback(boost::function<void(P0, P1, P2, P3, NullP, NullP, NullP, NullP, NullP)>(boost::bind(callback, t, _1, _2, _3, _4)));
}
template<typename T, typename P0, typename P1, typename P2, typename P3, typename P4>
Connection addCallback(void(T::*callback)(P0, P1, P2, P3, P4), T* t)
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, NullP, NullP, NullP, NullP)>(boost::bind(callback, t, _1, _2, _3, _4, _5)));
}
template<typename T, typename P0, typename P1, typename P2, typename P3, typename P4, typename P5>
Connection addCallback(void(T::*callback)(P0, P1, P2, P3, P4, P5), T* t)
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, P5, NullP, NullP, NullP)>(boost::bind(callback, t, _1, _2, _3, _4, _5, _6)));
}
template<typename T, typename P0, typename P1, typename P2, typename P3, typename P4, typename P5, typename P6>
Connection addCallback(void(T::*callback)(P0, P1, P2, P3, P4, P5, P6), T* t)
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, P5, P6, NullP, NullP)>(boost::bind(callback, t, _1, _2, _3, _4, _5, _6, _7)));
}
template<typename T, typename P0, typename P1, typename P2, typename P3, typename P4, typename P5, typename P6, typename P7>
Connection addCallback(void(T::*callback)(P0, P1, P2, P3, P4, P5, P6, P7), T* t)
{
return addCallback(boost::function<void(P0, P1, P2, P3, P4, P5, P6, P7, NullP)>(boost::bind(callback, t, _1, _2, _3, _4, _5, _6, _7, _8)));
}
template<typename C>
Connection addCallback( C& callback)
{
return addCallback<const M0ConstPtr&,
const M1ConstPtr&,
const M2ConstPtr&,
const M3ConstPtr&,
const M4ConstPtr&,
const M5ConstPtr&,
const M6ConstPtr&,
const M7ConstPtr&,
const M8ConstPtr&>(boost::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8, _9));
}
void removeCallback(const CallbackHelper9Ptr& helper)
{
boost::mutex::scoped_lock lock(mutex_);
typename V_CallbackHelper9::iterator it = std::find(callbacks_.begin(), callbacks_.end(), helper);
if (it != callbacks_.end())
{
callbacks_.erase(it);
}
}
void call(const M0Event& e0, const M1Event& e1, const M2Event& e2, const M3Event& e3, const M4Event& e4,
const M5Event& e5, const M6Event& e6, const M7Event& e7, const M8Event& e8)
{
boost::mutex::scoped_lock lock(mutex_);
bool nonconst_force_copy = callbacks_.size() > 1;
typename V_CallbackHelper9::iterator it = callbacks_.begin();
typename V_CallbackHelper9::iterator end = callbacks_.end();
for (; it != end; ++it)
{
const CallbackHelper9Ptr& helper = *it;
helper->call(nonconst_force_copy, e0, e1, e2, e3, e4, e5, e6, e7, e8);
}
}
private:
boost::mutex mutex_;
V_CallbackHelper9 callbacks_;
};
} // message_filters
#endif // MESSAGE_FILTERS_SIGNAL9_H

149
core/message_filters/include/message_filters/simple_filter.h
View File

@ -1,149 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_SIMPLE_FILTER_H
#define MESSAGE_FILTERS_SIMPLE_FILTER_H
#include <boost/noncopyable.hpp>
#include "connection.h"
#include "signal1.h"
#include <ros/message_event.h>
#include <ros/subscription_callback_helper.h>
#include <boost/bind.hpp>
#include <string>
namespace message_filters
{
/**
* \brief Convenience base-class for simple filters which output a single message
*
* SimpleFilter provides some of the tricky callback registering functionality, so that
* simple filters do not have to duplicate it. It also provides getName()/setName() for debugging
* purposes.
*/
template<class M>
class SimpleFilter : public boost::noncopyable
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef boost::function<void(const MConstPtr&)> Callback;
typedef ros::MessageEvent<M const> EventType;
typedef boost::function<void(const EventType&)> EventCallback;
/**
* \brief Register a callback to be called when this filter has passed
* \param callback The callback to call
*/
template<typename C>
Connection registerCallback(const C& callback)
{
typename CallbackHelper1<M>::Ptr helper = signal_.addCallback(Callback(callback));
return Connection(boost::bind(&Signal::removeCallback, &signal_, helper));
}
/**
* \brief Register a callback to be called when this filter has passed
* \param callback The callback to call
*/
template<typename P>
Connection registerCallback(const boost::function<void(P)>& callback)
{
return Connection(boost::bind(&Signal::removeCallback, &signal_, signal_.addCallback(callback)));
}
/**
* \brief Register a callback to be called when this filter has passed
* \param callback The callback to call
*/
template<typename P>
Connection registerCallback(void(*callback)(P))
{
return Connection(boost::bind(&Signal::removeCallback, &signal_, signal_.addCallback(callback)));
}
/**
* \brief Register a callback to be called when this filter has passed
* \param callback The callback to call
*/
template<typename T, typename P>
Connection registerCallback(void(T::*callback)(P), T* t)
{
typename CallbackHelper1<M>::Ptr helper = signal_.template addCallback<P>(boost::bind(callback, t, _1));
return Connection(boost::bind(&Signal::removeCallback, &signal_, helper));
}
/**
* \brief Set the name of this filter. For debugging use.
*/
void setName(const std::string& name) { name_ = name; }
/**
* \brief Get the name of this filter. For debugging use.
*/
const std::string& getName() { return name_; }
protected:
/**
* \brief Call all registered callbacks, passing them the specified message
*/
void signalMessage(const MConstPtr& msg)
{
ros::MessageEvent<M const> event(msg);
signal_.call(event);
}
/**
* \brief Call all registered callbacks, passing them the specified message
*/
void signalMessage(const ros::MessageEvent<M const>& event)
{
signal_.call(event);
}
private:
typedef Signal1<M> Signal;
Signal signal_;
std::string name_;
};
}
#endif

215
core/message_filters/include/message_filters/subscriber.h
View File

@ -1,215 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_SUBSCRIBER_H
#define MESSAGE_FILTERS_SUBSCRIBER_H
#include <ros/ros.h>
#include <boost/signals.hpp>
#include <boost/thread/mutex.hpp>
#include "connection.h"
#include "simple_filter.h"
namespace message_filters
{
class SubscriberBase
{
public:
virtual ~SubscriberBase() {}
/**
* \brief Subscribe to a topic.
*
* If this Subscriber is already subscribed to a topic, this function will first unsubscribe.
*
* \param nh The ros::NodeHandle to use to subscribe.
* \param topic The topic to subscribe to.
* \param queue_size The subscription queue size
* \param transport_hints The transport hints to pass along
* \param callback_queue The callback queue to pass along
*/
virtual void subscribe(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, const ros::TransportHints& transport_hints = ros::TransportHints(), ros::CallbackQueueInterface* callback_queue = 0) = 0;
/**
* \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic.
*/
virtual void subscribe() = 0;
/**
* \brief Force immediate unsubscription of this subscriber from its topic
*/
virtual void unsubscribe() = 0;
};
typedef boost::shared_ptr<SubscriberBase> SubscriberBasePtr;
/**
* \brief ROS subscription filter.
*
* This class acts as a highest-level filter, simply passing messages from a ROS subscription through to the
* filters which have connected to it.
*
* When this object is destroyed it will unsubscribe from the ROS subscription.
*
* The Subscriber object is templated on the type of message being subscribed to.
*
* \section connections CONNECTIONS
*
* Subscriber has no input connection.
*
* The output connection for the Subscriber object is the same signature as for roscpp subscription callbacks, ie.
\verbatim
void callback(const boost::shared_ptr<M const>&);
\endverbatim
*/
template<class M>
class Subscriber : public SubscriberBase, public SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef ros::MessageEvent<M const> EventType;
/**
* \brief Constructor
*
* See the ros::NodeHandle::subscribe() variants for more information on the parameters
*
* \param nh The ros::NodeHandle to use to subscribe.
* \param topic The topic to subscribe to.
* \param queue_size The subscription queue size
* \param transport_hints The transport hints to pass along
* \param callback_queue The callback queue to pass along
*/
Subscriber(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, const ros::TransportHints& transport_hints = ros::TransportHints(), ros::CallbackQueueInterface* callback_queue = 0)
{
subscribe(nh, topic, queue_size, transport_hints, callback_queue);
}
/**
* \brief Empty constructor, use subscribe() to subscribe to a topic
*/
Subscriber()
{
}
~Subscriber()
{
unsubscribe();
}
/**
* \brief Subscribe to a topic.
*
* If this Subscriber is already subscribed to a topic, this function will first unsubscribe.
*
* \param nh The ros::NodeHandle to use to subscribe.
* \param topic The topic to subscribe to.
* \param queue_size The subscription queue size
* \param transport_hints The transport hints to pass along
* \param callback_queue The callback queue to pass along
*/
void subscribe(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, const ros::TransportHints& transport_hints = ros::TransportHints(), ros::CallbackQueueInterface* callback_queue = 0)
{
unsubscribe();
if (!topic.empty())
{
ops_.template initByFullCallbackType<const EventType&>(topic, queue_size, boost::bind(&Subscriber<M>::cb, this, _1));
ops_.callback_queue = callback_queue;
ops_.transport_hints = transport_hints;
sub_ = nh.subscribe(ops_);
nh_ = nh;
}
}
/**
* \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic.
*/
void subscribe()
{
unsubscribe();
if (!ops_.topic.empty())
{
sub_ = nh_.subscribe(ops_);
}
}
/**
* \brief Force immediate unsubscription of this subscriber from its topic
*/
void unsubscribe()
{
sub_.shutdown();
}
std::string getTopic() const
{
return ops_.topic;
}
/**
* \brief Returns the internal ros::Subscriber object
*/
const ros::Subscriber& getSubscriber() const { return sub_; }
/**
* \brief Does nothing. Provided so that Subscriber may be used in a message_filters::Chain
*/
template<typename F>
void connectInput(F& f)
{
}
/**
* \brief Does nothing. Provided so that Subscriber may be used in a message_filters::Chain
*/
void add(const EventType& e)
{
}
private:
void cb(const EventType& e)
{
signalMessage(e);
}
ros::Subscriber sub_;
ros::SubscribeOptions ops_;
ros::NodeHandle nh_;
};
}
#endif

833
core/message_filters/include/message_filters/sync_policies/approximate_time.h
View File

@ -1,833 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_SYNC_APPROXIMATE_TIME_H
#define MESSAGE_FILTERS_SYNC_APPROXIMATE_TIME_H
#include "message_filters/synchronizer.h"
#include "message_filters/connection.h"
#include "message_filters/null_types.h"
#include "message_filters/signal9.h"
#include <boost/tuple/tuple.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/signals.hpp>
#include <boost/bind.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/noncopyable.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/vector.hpp>
#include <roslib/Header.h>
#include <ros/message_traits.h>
#include <ros/message_event.h>
#include <deque>
#include <vector>
#include <string>
namespace message_filters
{
namespace sync_policies
{
namespace mpl = boost::mpl;
template<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType,
typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType>
struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
{
typedef Synchronizer<ApproximateTime> Sync;
typedef PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> Super;
typedef typename Super::Messages Messages;
typedef typename Super::Signal Signal;
typedef typename Super::Events Events;
typedef typename Super::RealTypeCount RealTypeCount;
typedef typename Super::M0Event M0Event;
typedef typename Super::M1Event M1Event;
typedef typename Super::M2Event M2Event;
typedef typename Super::M3Event M3Event;
typedef typename Super::M4Event M4Event;
typedef typename Super::M5Event M5Event;
typedef typename Super::M6Event M6Event;
typedef typename Super::M7Event M7Event;
typedef typename Super::M8Event M8Event;
typedef std::deque<M0Event> M0Deque;
typedef std::deque<M1Event> M1Deque;
typedef std::deque<M2Event> M2Deque;
typedef std::deque<M3Event> M3Deque;
typedef std::deque<M4Event> M4Deque;
typedef std::deque<M5Event> M5Deque;
typedef std::deque<M6Event> M6Deque;
typedef std::deque<M7Event> M7Deque;
typedef std::deque<M8Event> M8Deque;
typedef std::vector<M0Event> M0Vector;
typedef std::vector<M1Event> M1Vector;
typedef std::vector<M2Event> M2Vector;
typedef std::vector<M3Event> M3Vector;
typedef std::vector<M4Event> M4Vector;
typedef std::vector<M5Event> M5Vector;
typedef std::vector<M6Event> M6Vector;
typedef std::vector<M7Event> M7Vector;
typedef std::vector<M8Event> M8Vector;
typedef boost::tuple<M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event> Tuple;
typedef boost::tuple<M0Deque, M1Deque, M2Deque, M3Deque, M4Deque, M5Deque, M6Deque, M7Deque, M8Deque> DequeTuple;
typedef boost::tuple<M0Vector, M1Vector, M2Vector, M3Vector, M4Vector, M5Vector, M6Vector, M7Vector, M8Vector> VectorTuple;
ApproximateTime(uint32_t queue_size)
: parent_(0)
, queue_size_(queue_size)
, num_non_empty_deques_(0)
, pivot_(NO_PIVOT)
, max_interval_duration_(ros::DURATION_MAX)
, age_penalty_(0.1)
, has_dropped_messages_(9, false)
, inter_message_lower_bounds_(9, ros::Duration(0))
, warned_about_incorrect_bound_(9, false)
{
ROS_ASSERT(queue_size_ > 0); // The synchronizer will tend to drop many messages with a queue size of 1. At least 2 is recommended.
}
ApproximateTime(const ApproximateTime& e)
{
*this = e;
}
ApproximateTime& operator=(const ApproximateTime& rhs)
{
parent_ = rhs.parent_;
queue_size_ = rhs.queue_size_;
num_non_empty_deques_ = rhs.num_non_empty_deques_;
pivot_time_ = rhs.pivot_time_;
pivot_ = rhs.pivot_;
max_interval_duration_ = rhs.max_interval_duration_;
age_penalty_ = rhs.age_penalty_;
candidate_start_ = rhs.candidate_start_;
candidate_end_ = rhs.candidate_end_;
deques_ = rhs.deques_;
past_ = rhs.past_;
has_dropped_messages_ = rhs.has_dropped_messages_;
inter_message_lower_bounds_ = rhs.inter_message_lower_bounds_;
warned_about_incorrect_bound_ = rhs.warned_about_incorrect_bound_;
return *this;
}
void initParent(Sync* parent)
{
parent_ = parent;
}
template<int i>
void checkInterMessageBound()
{
namespace mt = ros::message_traits;
if (warned_about_incorrect_bound_[i])
{
return;
}
std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(deques_);
std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(past_);
ROS_ASSERT(!deque.empty());
const typename mpl::at_c<Messages, i>::type &msg = *(deque.back()).getMessage();
ros::Time msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(msg);
ros::Time previous_msg_time;
if (deque.size() == (size_t) 1)
{
if (v.empty())
{
// We have already published (or have never received) the previous message, we cannot check the bound
return;
}
const typename mpl::at_c<Messages, i>::type &previous_msg = *(v.back()).getMessage();
previous_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(previous_msg);
}
else
{
// There are at least 2 elements in the deque. Check that the gap respects the bound if it was provided.
const typename mpl::at_c<Messages, i>::type &previous_msg = *(deque[deque.size()-2]).getMessage();
previous_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(previous_msg);
}
if (msg_time < previous_msg_time)
{
ROS_WARN_STREAM("Messages of type " << i << " arrived out of order (will print only once)");
warned_about_incorrect_bound_[i] = true;
}
else if ((msg_time - previous_msg_time) < inter_message_lower_bounds_[i])
{
ROS_WARN_STREAM("Messages of type " << i << " arrived closer (" << (msg_time - previous_msg_time)
<< ") than the lower bound you provided (" << inter_message_lower_bounds_[i]
<< ") (will print only once)");
warned_about_incorrect_bound_[i] = true;
}
}
template<int i>
void add(const typename mpl::at_c<Events, i>::type& evt)
{
boost::mutex::scoped_lock lock(data_mutex_);
std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(deques_);
deque.push_back(evt);
if (deque.size() == (size_t)1) {
// We have just added the first message, so it was empty before
++num_non_empty_deques_;
if (num_non_empty_deques_ == (uint32_t)RealTypeCount::value)
{
// All deques have messages
process();
}
}
else
{
checkInterMessageBound<i>();
}
// Check whether we have more messages than allowed in the queue.
// Note that during the above call to process(), queue i may contain queue_size_+1 messages.
std::vector<typename mpl::at_c<Events, i>::type>& past = boost::get<i>(past_);
if (deque.size() + past.size() > queue_size_)
{
// Cancel ongoing candidate search, if any:
num_non_empty_deques_ = 0; // We will recompute it from scratch
recover<0>();
recover<1>();
recover<2>();
recover<3>();
recover<4>();
recover<5>();
recover<6>();
recover<7>();
recover<8>();
// Drop the oldest message in the offending topic
ROS_ASSERT(!deque.empty());
deque.pop_front();
has_dropped_messages_[i] = true;
if (pivot_ != NO_PIVOT)
{
// The candidate is no longer valid. Destroy it.
candidate_ = Tuple();
pivot_ = NO_PIVOT;
// There might still be enough messages to create a new candidate:
process();
}
}
}
void setAgePenalty(double age_penalty)
{
// For correctness we only need age_penalty > -1.0, but most likely a negative age_penalty is a mistake.
ROS_ASSERT(age_penalty >= 0);
age_penalty_ = age_penalty;
}
void setInterMessageLowerBound(int i, ros::Duration lower_bound) {
// For correctness we only need age_penalty > -1.0, but most likely a negative age_penalty is a mistake.
ROS_ASSERT(lower_bound >= ros::Duration(0,0));
inter_message_lower_bounds_[i] = lower_bound;
}
void setMaxIntervalDuration(ros::Duration max_interval_duration) {
// For correctness we only need age_penalty > -1.0, but most likely a negative age_penalty is a mistake.
ROS_ASSERT(max_interval_duration >= ros::Duration(0,0));
max_interval_duration_ = max_interval_duration;
}
private:
// Assumes that deque number <index> is non empty
template<int i>
void dequeDeleteFront()
{
std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(deques_);
ROS_ASSERT(!deque.empty());
deque.pop_front();
if (deque.empty())
{
--num_non_empty_deques_;
}
}
// Assumes that deque number <index> is non empty
void dequeDeleteFront(uint32_t index)
{
switch (index)
{
case 0:
dequeDeleteFront<0>();
break;
case 1:
dequeDeleteFront<1>();
break;
case 2:
dequeDeleteFront<2>();
break;
case 3:
dequeDeleteFront<3>();
break;
case 4:
dequeDeleteFront<4>();
break;
case 5:
dequeDeleteFront<5>();
break;
case 6:
dequeDeleteFront<6>();
break;
case 7:
dequeDeleteFront<7>();
break;
case 8:
dequeDeleteFront<8>();
break;
default:
ROS_BREAK();
}
}
// Assumes that deque number <index> is non empty
template<int i>
void dequeMoveFrontToPast()
{
std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(deques_);
std::vector<typename mpl::at_c<Events, i>::type>& vector = boost::get<i>(past_);
ROS_ASSERT(!deque.empty());
vector.push_back(deque.front());
deque.pop_front();
if (deque.empty())
{
--num_non_empty_deques_;
}
}
// Assumes that deque number <index> is non empty
void dequeMoveFrontToPast(uint32_t index)
{
switch (index)
{
case 0:
dequeMoveFrontToPast<0>();
break;
case 1:
dequeMoveFrontToPast<1>();
break;
case 2:
dequeMoveFrontToPast<2>();
break;
case 3:
dequeMoveFrontToPast<3>();
break;
case 4:
dequeMoveFrontToPast<4>();
break;
case 5:
dequeMoveFrontToPast<5>();
break;
case 6:
dequeMoveFrontToPast<6>();
break;
case 7:
dequeMoveFrontToPast<7>();
break;
case 8:
dequeMoveFrontToPast<8>();
break;
default:
ROS_BREAK();
}
}
void makeCandidate()
{
//printf("Creating candidate\n");
// Create candidate tuple
candidate_ = Tuple(); // Discards old one if any
boost::get<0>(candidate_) = boost::get<0>(deques_).front();
boost::get<1>(candidate_) = boost::get<1>(deques_).front();
if (RealTypeCount::value > 2)
{
boost::get<2>(candidate_) = boost::get<2>(deques_).front();
if (RealTypeCount::value > 3)
{
boost::get<3>(candidate_) = boost::get<3>(deques_).front();
if (RealTypeCount::value > 4)
{
boost::get<4>(candidate_) = boost::get<4>(deques_).front();
if (RealTypeCount::value > 5)
{
boost::get<5>(candidate_) = boost::get<5>(deques_).front();
if (RealTypeCount::value > 6)
{
boost::get<6>(candidate_) = boost::get<6>(deques_).front();
if (RealTypeCount::value > 7)
{
boost::get<7>(candidate_) = boost::get<7>(deques_).front();
if (RealTypeCount::value > 8)
{
boost::get<8>(candidate_) = boost::get<8>(deques_).front();
}
}
}
}
}
}
}
// Delete all past messages, since we have found a better candidate
boost::get<0>(past_).clear();
boost::get<1>(past_).clear();
boost::get<2>(past_).clear();
boost::get<3>(past_).clear();
boost::get<4>(past_).clear();
boost::get<5>(past_).clear();
boost::get<6>(past_).clear();
boost::get<7>(past_).clear();
boost::get<8>(past_).clear();
//printf("Candidate created\n");
}
// ASSUMES: num_messages <= past_[i].size()
template<int i>
void recover(size_t num_messages)
{
if (i >= RealTypeCount::value)
{
return;
}
std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(past_);
std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(deques_);
ROS_ASSERT(num_messages <= v.size());
while (num_messages > 0)
{
q.push_front(v.back());
v.pop_back();
num_messages--;
}
if (!q.empty())
{
++num_non_empty_deques_;
}
}
template<int i>
void recover()
{
if (i >= RealTypeCount::value)
{
return;
}
std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(past_);
std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(deques_);
while (!v.empty())
{
q.push_front(v.back());
v.pop_back();
}
if (!q.empty())
{
++num_non_empty_deques_;
}
}
template<int i>
void recoverAndDelete()
{
if (i >= RealTypeCount::value)
{
return;
}
std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(past_);
std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(deques_);
while (!v.empty())
{
q.push_front(v.back());
v.pop_back();
}
ROS_ASSERT(!q.empty());
q.pop_front();
if (!q.empty())
{
++num_non_empty_deques_;
}
}
// Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value
void publishCandidate()
{
//printf("Publishing candidate\n");
// Publish
parent_->signal(boost::get<0>(candidate_), boost::get<1>(candidate_), boost::get<2>(candidate_), boost::get<3>(candidate_),
boost::get<4>(candidate_), boost::get<5>(candidate_), boost::get<6>(candidate_), boost::get<7>(candidate_),
boost::get<8>(candidate_));
// Delete this candidate
candidate_ = Tuple();
pivot_ = NO_PIVOT;
// Recover hidden messages, and delete the ones corresponding to the candidate
num_non_empty_deques_ = 0; // We will recompute it from scratch
recoverAndDelete<0>();
recoverAndDelete<1>();
recoverAndDelete<2>();
recoverAndDelete<3>();
recoverAndDelete<4>();
recoverAndDelete<5>();
recoverAndDelete<6>();
recoverAndDelete<7>();
recoverAndDelete<8>();
}
// Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value
// Returns: the oldest message on the deques
void getCandidateStart(uint32_t &start_index, ros::Time &start_time)
{
return getCandidateBoundary(start_index, start_time, false);
}
// Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value
// Returns: the latest message among the heads of the deques, i.e. the minimum
// time to end an interval started at getCandidateStart_index()
void getCandidateEnd(uint32_t &end_index, ros::Time &end_time)
{
return getCandidateBoundary(end_index, end_time, true);
}
// ASSUMES: all deques are non-empty
// end = true: look for the latest head of deque
// false: look for the earliest head of deque
void getCandidateBoundary(uint32_t &index, ros::Time &time, bool end)
{
namespace mt = ros::message_traits;
M0Event& m0 = boost::get<0>(deques_).front();
M1Event& m1 = boost::get<1>(deques_).front();
M2Event& m2 = boost::get<2>(deques_).front();
M3Event& m3 = boost::get<3>(deques_).front();
M4Event& m4 = boost::get<4>(deques_).front();
M5Event& m5 = boost::get<5>(deques_).front();
M6Event& m6 = boost::get<6>(deques_).front();
M7Event& m7 = boost::get<7>(deques_).front();
M8Event& m8 = boost::get<8>(deques_).front();
time = mt::TimeStamp<M0>::value(*m0.getMessage());
index = 0;
if ((RealTypeCount::value > 1) && ((mt::TimeStamp<M1>::value(*m1.getMessage()) < time) ^ end))
{
time = mt::TimeStamp<M1>::value(*m1.getMessage());
index = 1;
}
if ((RealTypeCount::value > 2) && ((mt::TimeStamp<M2>::value(*m2.getMessage()) < time) ^ end))
{
time = mt::TimeStamp<M2>::value(*m2.getMessage());
index = 2;
}
if ((RealTypeCount::value > 3) && ((mt::TimeStamp<M3>::value(*m3.getMessage()) < time) ^ end))
{
time = mt::TimeStamp<M3>::value(*m3.getMessage());
index = 3;
}
if ((RealTypeCount::value > 4) && ((mt::TimeStamp<M4>::value(*m4.getMessage()) < time) ^ end))
{
time = mt::TimeStamp<M4>::value(*m4.getMessage());
index = 4;
}
if ((RealTypeCount::value > 5) && ((mt::TimeStamp<M5>::value(*m5.getMessage()) < time) ^ end))
{
time = mt::TimeStamp<M5>::value(*m5.getMessage());
index = 5;
}
if ((RealTypeCount::value > 6) && ((mt::TimeStamp<M6>::value(*m6.getMessage()) < time) ^ end))
{
time = mt::TimeStamp<M6>::value(*m6.getMessage());
index = 6;
}
if ((RealTypeCount::value > 7) && ((mt::TimeStamp<M7>::value(*m7.getMessage()) < time) ^ end))
{
time = mt::TimeStamp<M7>::value(*m7.getMessage());
index = 7;
}
if ((RealTypeCount::value > 8) && ((mt::TimeStamp<M8>::value(*m8.getMessage()) < time) ^ end))
{
time = mt::TimeStamp<M8>::value(*m8.getMessage());
index = 8;
}
}
// ASSUMES: we have a pivot and candidate
template<int i>
ros::Time getVirtualTime()
{
namespace mt = ros::message_traits;
if (i >= RealTypeCount::value)
{
return ros::Time(0,0); // Dummy return value
}
ROS_ASSERT(pivot_ != NO_PIVOT);
std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(past_);
std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(deques_);
if (q.empty())
{
ROS_ASSERT(!v.empty()); // Because we have a candidate
ros::Time last_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*(v.back()).getMessage());
ros::Time msg_time_lower_bound = last_msg_time + inter_message_lower_bounds_[i];
if (msg_time_lower_bound > pivot_time_) // Take the max
{
return msg_time_lower_bound;
}
return pivot_time_;
}
ros::Time current_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*(q.front()).getMessage());
return current_msg_time;
}
// ASSUMES: we have a pivot and candidate
void getVirtualCandidateStart(uint32_t &start_index, ros::Time &start_time)
{
return getVirtualCandidateBoundary(start_index, start_time, false);
}
// ASSUMES: we have a pivot and candidate
void getVirtualCandidateEnd(uint32_t &end_index, ros::Time &end_time)
{
return getVirtualCandidateBoundary(end_index, end_time, true);
}
// ASSUMES: we have a pivot and candidate
// end = true: look for the latest head of deque
// false: look for the earliest head of deque
void getVirtualCandidateBoundary(uint32_t &index, ros::Time &time, bool end)
{
namespace mt = ros::message_traits;
std::vector<ros::Time> virtual_times(9);
virtual_times[0] = getVirtualTime<0>();
virtual_times[1] = getVirtualTime<1>();
virtual_times[2] = getVirtualTime<2>();
virtual_times[3] = getVirtualTime<3>();
virtual_times[4] = getVirtualTime<4>();
virtual_times[5] = getVirtualTime<5>();
virtual_times[6] = getVirtualTime<6>();
virtual_times[7] = getVirtualTime<7>();
virtual_times[8] = getVirtualTime<8>();
time = virtual_times[0];
index = 0;
for (int i = 0; i < RealTypeCount::value; i++)
{
if ((virtual_times[i] < time) ^ end)
{
time = virtual_times[i];
index = i;
}
}
}
// assumes data_mutex_ is already locked
void process()
{
// While no deque is empty
while (num_non_empty_deques_ == (uint32_t)RealTypeCount::value)
{
// Find the start and end of the current interval
//printf("Entering while loop in this state [\n");
//show_internal_state();
//printf("]\n");
ros::Time end_time, start_time;
uint32_t end_index, start_index;
getCandidateEnd(end_index, end_time);
getCandidateStart(start_index, start_time);
for (uint32_t i = 0; i < (uint32_t)RealTypeCount::value; i++)
{
if (i != end_index)
{
// No dropped message could have been better to use than the ones we have,
// so it becomes ok to use this topic as pivot in the future
has_dropped_messages_[i] = false;
}
}
if (pivot_ == NO_PIVOT)
{
// We do not have a candidate
// INVARIANT: the past_ vectors are empty
// INVARIANT: (candidate_ has no filled members)
if (end_time - start_time > max_interval_duration_)
{
// This interval is too big to be a valid candidate, move to the next
dequeDeleteFront(start_index);
continue;
}
if (has_dropped_messages_[end_index])
{
// The topic that would become pivot has dropped messages, so it is not a good pivot
dequeDeleteFront(start_index);
continue;
}
// This is a valid candidate, and we don't have any, so take it
makeCandidate();
candidate_start_ = start_time;
candidate_end_ = end_time;
pivot_ = end_index;
pivot_time_ = end_time;
dequeMoveFrontToPast(start_index);
}
else
{
// We already have a candidate
// Is this one better than the current candidate?
// INVARIANT: has_dropped_messages_ is all false
if ((end_time - candidate_end_) * (1 + age_penalty_) >= (start_time - candidate_start_))
{
// This is not a better candidate, move to the next
dequeMoveFrontToPast(start_index);
}
else
{
// This is a better candidate
makeCandidate();
candidate_start_ = start_time;
candidate_end_ = end_time;
dequeMoveFrontToPast(start_index);
// Keep the same pivot (and pivot time)
}
}
// INVARIANT: we have a candidate and pivot
ROS_ASSERT(pivot_ != NO_PIVOT);
//printf("start_index == %d, pivot_ == %d\n", start_index, pivot_);
if (start_index == pivot_) // TODO: replace with start_time == pivot_time_
{
// We have exhausted all possible candidates for this pivot, we now can output the best one
publishCandidate();
}
else if ((end_time - candidate_end_) * (1 + age_penalty_) >= (pivot_time_ - candidate_start_))
{
// We have not exhausted all candidates, but this candidate is already provably optimal
// Indeed, any future candidate must contain the interval [pivot_time_ end_time], which
// is already too big.
// Note: this case is subsumed by the next, but it may save some unnecessary work and
// it makes things (a little) easier to understand
publishCandidate();
}
else if (num_non_empty_deques_ < (uint32_t)RealTypeCount::value)
{
uint32_t num_non_empty_deques_before_virtual_search = num_non_empty_deques_;
// Before giving up, use the rate bounds, if provided, to further try to prove optimality
std::vector<int> num_virtual_moves(9,0);
while (1)
{
ros::Time end_time, start_time;
uint32_t end_index, start_index;
getVirtualCandidateEnd(end_index, end_time);
getVirtualCandidateStart(start_index, start_time);
if ((end_time - candidate_end_) * (1 + age_penalty_) >= (pivot_time_ - candidate_start_))
{
// We have proved optimality
// As above, any future candidate must contain the interval [pivot_time_ end_time], which
// is already too big.
publishCandidate(); // This cleans up the virtual moves as a byproduct
break; // From the while(1) loop only
}
if ((end_time - candidate_end_) * (1 + age_penalty_) < (start_time - candidate_start_))
{
// We cannot prove optimality
// Indeed, we have a virtual (i.e. optimistic) candidate that is better than the current
// candidate
// Cleanup the virtual search:
num_non_empty_deques_ = 0; // We will recompute it from scratch
recover<0>(num_virtual_moves[0]);
recover<1>(num_virtual_moves[1]);
recover<2>(num_virtual_moves[2]);
recover<3>(num_virtual_moves[3]);
recover<4>(num_virtual_moves[4]);
recover<5>(num_virtual_moves[5]);
recover<6>(num_virtual_moves[6]);
recover<7>(num_virtual_moves[7]);
recover<8>(num_virtual_moves[8]);
ROS_ASSERT(num_non_empty_deques_before_virtual_search == num_non_empty_deques_);
break;
}
// Note: we cannot reach this point with start_index == pivot_ since in that case we would
// have start_time == pivot_time, in which case the two tests above are the negation
// of each other, so that one must be true. Therefore the while loop always terminates.
ROS_ASSERT(start_index != pivot_);
ROS_ASSERT(start_time < pivot_time_);
dequeMoveFrontToPast(start_index);
num_virtual_moves[start_index]++;
} // while(1)
}
} // while(num_non_empty_deques_ == (uint32_t)RealTypeCount::value)
}
Sync* parent_;
uint32_t queue_size_;
static const uint32_t NO_PIVOT = 9; // Special value for the pivot indicating that no pivot has been selected
DequeTuple deques_;
uint32_t num_non_empty_deques_;
VectorTuple past_;
Tuple candidate_; // NULL if there is no candidate, in which case there is no pivot.
ros::Time candidate_start_;
ros::Time candidate_end_;
ros::Time pivot_time_;
uint32_t pivot_; // Equal to NO_PIVOT if there is no candidate
boost::mutex data_mutex_; // Protects all of the above
ros::Duration max_interval_duration_; // TODO: initialize with a parameter
double age_penalty_;
std::vector<bool> has_dropped_messages_;
std::vector<ros::Duration> inter_message_lower_bounds_;
std::vector<bool> warned_about_incorrect_bound_;
};
} // namespace sync
} // namespace message_filters
#endif // MESSAGE_FILTERS_SYNC_APPROXIMATE_TIME_H

245
core/message_filters/include/message_filters/sync_policies/exact_time.h
View File

@ -1,245 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_SYNC_EXACT_TIME_H
#define MESSAGE_FILTERS_SYNC_EXACT_TIME_H
#include "message_filters/synchronizer.h"
#include "message_filters/connection.h"
#include "message_filters/null_types.h"
#include "message_filters/signal9.h"
#include <boost/tuple/tuple.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/signals.hpp>
#include <boost/bind.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/noncopyable.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/vector.hpp>
#include <roslib/Header.h>
#include <ros/message_traits.h>
#include <ros/message_event.h>
#include <deque>
#include <vector>
#include <string>
namespace message_filters
{
namespace sync_policies
{
namespace mpl = boost::mpl;
template<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType,
typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType>
struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
{
typedef Synchronizer<ExactTime> Sync;
typedef PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> Super;
typedef typename Super::Messages Messages;
typedef typename Super::Signal Signal;
typedef typename Super::Events Events;
typedef typename Super::RealTypeCount RealTypeCount;
typedef typename Super::M0Event M0Event;
typedef typename Super::M1Event M1Event;
typedef typename Super::M2Event M2Event;
typedef typename Super::M3Event M3Event;
typedef typename Super::M4Event M4Event;
typedef typename Super::M5Event M5Event;
typedef typename Super::M6Event M6Event;
typedef typename Super::M7Event M7Event;
typedef typename Super::M8Event M8Event;
typedef boost::tuple<M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event> Tuple;
ExactTime(uint32_t queue_size)
: parent_(0)
, queue_size_(queue_size)
{
}
ExactTime(const ExactTime& e)
{
*this = e;
}
ExactTime& operator=(const ExactTime& rhs)
{
parent_ = rhs.parent_;
queue_size_ = rhs.queue_size_;
last_signal_time_ = rhs.last_signal_time_;
tuples_ = rhs.tuples_;
return *this;
}
void initParent(Sync* parent)
{
parent_ = parent;
}
template<int i>
void add(const typename mpl::at_c<Events, i>::type& evt)
{
ROS_ASSERT(parent_);
namespace mt = ros::message_traits;
boost::mutex::scoped_lock lock(mutex_);
Tuple& t = tuples_[mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*evt.getMessage())];
boost::get<i>(t) = evt;
checkTuple(t);
}
template<class C>
Connection registerDropCallback(const C& callback)
{
return drop_signal_.template addCallback(callback);
}
template<class C>
Connection registerDropCallback(C& callback)
{
return drop_signal_.template addCallback(callback);
}
template<class C, typename T>
Connection registerDropCallback(const C& callback, T* t)
{
return drop_signal_.template addCallback(callback, t);
}
template<class C, typename T>
Connection registerDropCallback(C& callback, T* t)
{
return drop_signal_.template addCallback(callback, t);
}
private:
// assumes mutex_ is already locked
void checkTuple(Tuple& t)
{
namespace mt = ros::message_traits;
bool full = true;
full = full && (bool)boost::get<0>(t).getMessage();
full = full && (bool)boost::get<1>(t).getMessage();
full = full && (RealTypeCount::value > 2 ? (bool)boost::get<2>(t).getMessage() : true);
full = full && (RealTypeCount::value > 3 ? (bool)boost::get<3>(t).getMessage() : true);
full = full && (RealTypeCount::value > 4 ? (bool)boost::get<4>(t).getMessage() : true);
full = full && (RealTypeCount::value > 5 ? (bool)boost::get<5>(t).getMessage() : true);
full = full && (RealTypeCount::value > 6 ? (bool)boost::get<6>(t).getMessage() : true);
full = full && (RealTypeCount::value > 7 ? (bool)boost::get<7>(t).getMessage() : true);
full = full && (RealTypeCount::value > 8 ? (bool)boost::get<8>(t).getMessage() : true);
if (full)
{
parent_->signal(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t),
boost::get<3>(t), boost::get<4>(t), boost::get<5>(t),
boost::get<6>(t), boost::get<7>(t), boost::get<8>(t));
last_signal_time_ = mt::TimeStamp<M0>::value(*boost::get<0>(t).getMessage());
tuples_.erase(last_signal_time_);
clearOldTuples();
}
if (queue_size_ > 0)
{
while (tuples_.size() > queue_size_)
{
Tuple& t2 = tuples_.begin()->second;
drop_signal_.call(boost::get<0>(t2), boost::get<1>(t2), boost::get<2>(t2),
boost::get<3>(t2), boost::get<4>(t2), boost::get<5>(t2),
boost::get<6>(t2), boost::get<7>(t2), boost::get<8>(t2));
tuples_.erase(tuples_.begin());
}
}
}
// assumes mutex_ is already locked
void clearOldTuples()
{
typename M_TimeToTuple::iterator it = tuples_.begin();
typename M_TimeToTuple::iterator end = tuples_.end();
for (; it != end;)
{
if (it->first <= last_signal_time_)
{
typename M_TimeToTuple::iterator old = it;
++it;
Tuple& t = old->second;
drop_signal_.call(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t),
boost::get<3>(t), boost::get<4>(t), boost::get<5>(t),
boost::get<6>(t), boost::get<7>(t), boost::get<8>(t));
tuples_.erase(old);
}
else
{
// the map is sorted by time, so we can ignore anything after this if this one's time is ok
break;
}
}
}
private:
Sync* parent_;
uint32_t queue_size_;
typedef std::map<ros::Time, Tuple> M_TimeToTuple;
M_TimeToTuple tuples_;
ros::Time last_signal_time_;
Signal drop_signal_;
boost::mutex mutex_;
};
} // namespace sync
} // namespace message_filters
#endif // MESSAGE_FILTERS_SYNC_EXACT_TIME_H

395
core/message_filters/include/message_filters/synchronizer.h
View File

@ -1,395 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_SYNCHRONIZER_H
#define MESSAGE_FILTERS_SYNCHRONIZER_H
#include <boost/tuple/tuple.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/signals.hpp>
#include <boost/bind.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/noncopyable.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/function_types/function_arity.hpp>
#include <boost/function_types/is_nonmember_callable_builtin.hpp>
#include <roslib/Header.h>
#include "connection.h"
#include "null_types.h"
#include "signal9.h"
#include <ros/message_traits.h>
#include <ros/message_event.h>
#include <deque>
#include <vector>
#include <string>
namespace message_filters
{
namespace mpl = boost::mpl;
template<class Policy>
class Synchronizer : public boost::noncopyable, public Policy
{
public:
typedef typename Policy::Messages Messages;
typedef typename Policy::Events Events;
typedef typename Policy::Signal Signal;
typedef typename mpl::at_c<Messages, 0>::type M0;
typedef typename mpl::at_c<Messages, 1>::type M1;
typedef typename mpl::at_c<Messages, 2>::type M2;
typedef typename mpl::at_c<Messages, 3>::type M3;
typedef typename mpl::at_c<Messages, 4>::type M4;
typedef typename mpl::at_c<Messages, 5>::type M5;
typedef typename mpl::at_c<Messages, 6>::type M6;
typedef typename mpl::at_c<Messages, 7>::type M7;
typedef typename mpl::at_c<Messages, 8>::type M8;
typedef typename mpl::at_c<Events, 0>::type M0Event;
typedef typename mpl::at_c<Events, 1>::type M1Event;
typedef typename mpl::at_c<Events, 2>::type M2Event;
typedef typename mpl::at_c<Events, 3>::type M3Event;
typedef typename mpl::at_c<Events, 4>::type M4Event;
typedef typename mpl::at_c<Events, 5>::type M5Event;
typedef typename mpl::at_c<Events, 6>::type M6Event;
typedef typename mpl::at_c<Events, 7>::type M7Event;
typedef typename mpl::at_c<Events, 8>::type M8Event;
static const uint8_t MAX_MESSAGES = 9;
template<class F0, class F1>
Synchronizer(F0& f0, F1& f1)
{
connectInput(f0, f1);
init();
}
template<class F0, class F1, class F2>
Synchronizer(F0& f0, F1& f1, F2& f2)
{
connectInput(f0, f1, f2);
init();
}
template<class F0, class F1, class F2, class F3>
Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3)
{
connectInput(f0, f1, f2, f3);
init();
}
template<class F0, class F1, class F2, class F3, class F4>
Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4)
{
connectInput(f0, f1, f2, f3, f4);
init();
}
template<class F0, class F1, class F2, class F3, class F4, class F5>
Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5)
{
connectInput(f0, f1, f2, f3, f4, f5);
init();
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6>
Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6)
{
connectInput(f0, f1, f2, f3, f4, f5, f6);
init();
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7>
Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7)
{
connectInput(f0, f1, f2, f3, f4, f5, f6, f7);
init();
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8>
Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8)
{
connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8);
init();
}
Synchronizer()
{
init();
}
template<class F0, class F1>
Synchronizer(const Policy& policy, F0& f0, F1& f1)
: Policy(policy)
{
connectInput(f0, f1);
init();
}
template<class F0, class F1, class F2>
Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2)
: Policy(policy)
{
connectInput(f0, f1, f2);
init();
}
template<class F0, class F1, class F2, class F3>
Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3)
: Policy(policy)
{
connectInput(f0, f1, f2, f3);
init();
}
template<class F0, class F1, class F2, class F3, class F4>
Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4)
: Policy(policy)
{
connectInput(f0, f1, f2, f3, f4);
init();
}
template<class F0, class F1, class F2, class F3, class F4, class F5>
Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5)
: Policy(policy)
{
connectInput(f0, f1, f2, f3, f4, f5);
init();
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6>
Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6)
: Policy(policy)
{
connectInput(f0, f1, f2, f3, f4, f5, f6);
init();
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7>
Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7)
: Policy(policy)
{
connectInput(f0, f1, f2, f3, f4, f5, f6, f7);
init();
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8>
Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8)
: Policy(policy)
{
connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8);
init();
}
Synchronizer(const Policy& policy)
: Policy(policy)
{
init();
}
~Synchronizer()
{
disconnectAll();
}
void init()
{
Policy::initParent(this);
}
template<class F0, class F1>
void connectInput(F0& f0, F1& f1)
{
NullFilter<M2> f2;
connectInput(f0, f1, f2);
}
template<class F0, class F1, class F2>
void connectInput(F0& f0, F1& f1, F2& f2)
{
NullFilter<M3> f3;
connectInput(f0, f1, f2, f3);
}
template<class F0, class F1, class F2, class F3>
void connectInput(F0& f0, F1& f1, F2& f2, F3& f3)
{
NullFilter<M4> f4;
connectInput(f0, f1, f2, f3, f4);
}
template<class F0, class F1, class F2, class F3, class F4>
void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4)
{
NullFilter<M5> f5;
connectInput(f0, f1, f2, f3, f4, f5);
}
template<class F0, class F1, class F2, class F3, class F4, class F5>
void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5)
{
NullFilter<M6> f6;
connectInput(f0, f1, f2, f3, f4, f5, f6);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6>
void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6)
{
NullFilter<M7> f7;
connectInput(f0, f1, f2, f3, f4, f5, f6, f7);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7>
void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7)
{
NullFilter<M8> f8;
connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8>
void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8)
{
disconnectAll();
input_connections_[0] = f0.registerCallback(boost::function<void(const M0Event&)>(boost::bind(&Synchronizer::template cb<0>, this, _1)));
input_connections_[1] = f1.registerCallback(boost::function<void(const M1Event&)>(boost::bind(&Synchronizer::template cb<1>, this, _1)));
input_connections_[2] = f2.registerCallback(boost::function<void(const M2Event&)>(boost::bind(&Synchronizer::template cb<2>, this, _1)));
input_connections_[3] = f3.registerCallback(boost::function<void(const M3Event&)>(boost::bind(&Synchronizer::template cb<3>, this, _1)));
input_connections_[4] = f4.registerCallback(boost::function<void(const M4Event&)>(boost::bind(&Synchronizer::template cb<4>, this, _1)));
input_connections_[5] = f5.registerCallback(boost::function<void(const M5Event&)>(boost::bind(&Synchronizer::template cb<5>, this, _1)));
input_connections_[6] = f6.registerCallback(boost::function<void(const M6Event&)>(boost::bind(&Synchronizer::template cb<6>, this, _1)));
input_connections_[7] = f7.registerCallback(boost::function<void(const M7Event&)>(boost::bind(&Synchronizer::template cb<7>, this, _1)));
input_connections_[8] = f8.registerCallback(boost::function<void(const M8Event&)>(boost::bind(&Synchronizer::template cb<8>, this, _1)));
}
template<class C>
Connection registerCallback(C& callback)
{
return signal_.template addCallback(callback);
}
template<class C>
Connection registerCallback(const C& callback)
{
return signal_.template addCallback(callback);
}
template<class C, typename T>
Connection registerCallback(const C& callback, T* t)
{
return signal_.template addCallback(callback, t);
}
template<class C, typename T>
Connection registerCallback(C& callback, T* t)
{
return signal_.template addCallback(callback, t);
}
void setName(const std::string& name) { name_ = name; }
const std::string& getName() { return name_; }
void signal(const M0Event& e0, const M1Event& e1, const M2Event& e2, const M3Event& e3, const M4Event& e4,
const M5Event& e5, const M6Event& e6, const M7Event& e7, const M8Event& e8)
{
signal_.call(e0, e1, e2, e3, e4, e5, e6, e7, e8);
}
Policy* getPolicy() { return static_cast<Policy*>(this); }
using Policy::add;
template<int i>
void add(const boost::shared_ptr<typename mpl::at_c<Messages, i>::type const>& msg)
{
this->template add<i>(typename mpl::at_c<Events, i>::type(msg));
}
private:
void disconnectAll()
{
for (int i = 0; i < MAX_MESSAGES; ++i)
{
input_connections_[i].disconnect();
}
}
template<int i>
void cb(const typename mpl::at_c<Events, i>::type& evt)
{
add<i>(evt);
}
uint32_t queue_size_;
Signal signal_;
Connection input_connections_[MAX_MESSAGES];
std::string name_;
};
template<typename M0, typename M1, typename M2, typename M3, typename M4,
typename M5, typename M6, typename M7, typename M8>
struct PolicyBase
{
typedef mpl::vector<M0, M1, M2, M3, M4, M5, M6, M7, M8> Messages;
typedef Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8> Signal;
typedef mpl::vector<ros::MessageEvent<M0 const>, ros::MessageEvent<M1 const>, ros::MessageEvent<M2 const>, ros::MessageEvent<M3 const>,
ros::MessageEvent<M4 const>, ros::MessageEvent<M5 const>, ros::MessageEvent<M6 const>, ros::MessageEvent<M7 const>,
ros::MessageEvent<M8 const> > Events;
typedef typename mpl::fold<Messages, mpl::int_<0>, mpl::if_<mpl::not_<boost::is_same<mpl::_2, NullType> >, mpl::next<mpl::_1>, mpl::_1> >::type RealTypeCount;
typedef typename mpl::at_c<Events, 0>::type M0Event;
typedef typename mpl::at_c<Events, 1>::type M1Event;
typedef typename mpl::at_c<Events, 2>::type M2Event;
typedef typename mpl::at_c<Events, 3>::type M3Event;
typedef typename mpl::at_c<Events, 4>::type M4Event;
typedef typename mpl::at_c<Events, 5>::type M5Event;
typedef typename mpl::at_c<Events, 6>::type M6Event;
typedef typename mpl::at_c<Events, 7>::type M7Event;
typedef typename mpl::at_c<Events, 8>::type M8Event;
};
} // namespace message_filters
#endif // MESSAGE_FILTERS_SYNCHRONIZER_H

243
core/message_filters/include/message_filters/time_sequencer.h
View File

@ -1,243 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_TIME_SEQUENCER_H
#define MESSAGE_FILTERS_TIME_SEQUENCER_H
#include <ros/ros.h>
#include "connection.h"
#include "simple_filter.h"
namespace message_filters
{
/**
* \class TimeSequencer
*
* \brief Sequences messages based on the timestamp of their header.
*
* The TimeSequencer object is templated on the type of message being sequenced.
*
* \section behavior BEHAVIOR
* At construction, the TimeSequencer takes a ros::Duration
* "delay" which specifies how long to queue up messages to
* provide a time sequencing over them. As messages arrive they are
* sorted according to their time stamps. A callback for a message is
* never invoked until the messages' time stamp is out of date by at
* least delay. However, for all messages which are out of date
* by at least delay, their callback are invoked and guaranteed
* to be in temporal order. If a message arrives from a time \b prior
* to a message which has already had its callback invoked, it is
* thrown away.
*
* \section connections CONNECTIONS
*
* TimeSequencer's input and output connections are both of the same signature as roscpp subscription callbacks, ie.
\verbatim
void callback(const boost::shared_ptr<M const>&);
\endverbatim
*
*/
template<class M>
class TimeSequencer : public SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef ros::MessageEvent<M const> EventType;
/**
* \brief Constructor
* \param f A filter to connect this sequencer's input to
* \param delay The minimum time to hold a message before passing it through.
* \param update_rate The rate at which to check for messages which have passed "delay"
* \param queue_size The number of messages to store
* \param nh (optional) The NodeHandle to use to create the ros::Timer that runs at update_rate
*/
template<class F>
TimeSequencer(F& f, ros::Duration delay, ros::Duration update_rate, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle())
: delay_(delay)
, update_rate_(update_rate)
, queue_size_(queue_size)
, nh_(nh)
{
init();
connectInput(f);
}
/**
* \brief Constructor
*
* This version of the constructor does not take a filter immediately. You can connect to a filter later with the connectInput() function
*
* \param delay The minimum time to hold a message before passing it through.
* \param update_rate The rate at which to check for messages which have passed "delay"
* \param queue_size The number of messages to store
* \param nh (optional) The NodeHandle to use to create the ros::Timer that runs at update_rate
*/
TimeSequencer(ros::Duration delay, ros::Duration update_rate, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle())
: delay_(delay)
, update_rate_(update_rate)
, queue_size_(queue_size)
, nh_(nh)
{
init();
}
/**
* \brief Connect this filter's input to another filter's output.
*/
template<class F>
void connectInput(F& f)
{
incoming_connection_.disconnect();
incoming_connection_ = f.registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&TimeSequencer::cb, this, _1)));
}
~TimeSequencer()
{
update_timer_.stop();
incoming_connection_.disconnect();
}
void add(const EventType& evt)
{
namespace mt = ros::message_traits;
boost::mutex::scoped_lock lock(messages_mutex_);
if (mt::TimeStamp<M>::value(*evt.getMessage()) < last_time_)
{
return;
}
messages_.insert(evt);
if (queue_size_ != 0 && messages_.size() > queue_size_)
{
messages_.erase(*messages_.begin());
}
}
/**
* \brief Manually add a message to the cache.
*/
void add(const MConstPtr& msg)
{
EventType evt(msg);
add(evt);
}
private:
class MessageSort
{
public:
bool operator()(const EventType& lhs, const EventType& rhs) const
{
namespace mt = ros::message_traits;
return mt::TimeStamp<M>::value(*lhs.getMessage()) < mt::TimeStamp<M>::value(*rhs.getMessage());
}
};
typedef std::multiset<EventType, MessageSort> S_Message;
typedef std::vector<EventType> V_Message;
void cb(const EventType& evt)
{
add(evt);
}
void dispatch()
{
namespace mt = ros::message_traits;
V_Message to_call;
{
boost::mutex::scoped_lock lock(messages_mutex_);
while (!messages_.empty())
{
const EventType& e = *messages_.begin();
ros::Time stamp = mt::TimeStamp<M>::value(*e.getMessage());
if (stamp + delay_ <= ros::Time::now())
{
last_time_ = stamp;
to_call.push_back(e);
messages_.erase(messages_.begin());
}
else
{
break;
}
}
}
{
typename V_Message::iterator it = to_call.begin();
typename V_Message::iterator end = to_call.end();
for (; it != end; ++it)
{
signalMessage(*it);
}
}
}
void update(const ros::TimerEvent&)
{
dispatch();
}
void init()
{
update_timer_ = nh_.createTimer(update_rate_, &TimeSequencer::update, this);
}
ros::Duration delay_;
ros::Duration update_rate_;
uint32_t queue_size_;
ros::NodeHandle nh_;
ros::Timer update_timer_;
Connection incoming_connection_;
S_Message messages_;
boost::mutex messages_mutex_;
ros::Time last_time_;
};
}
#endif

228
core/message_filters/include/message_filters/time_synchronizer.h
View File

@ -1,228 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MESSAGE_FILTERS_TIME_SYNCHRONIZER_H
#define MESSAGE_FILTERS_TIME_SYNCHRONIZER_H
#include "synchronizer.h"
#include "sync_policies/exact_time.h"
#include <boost/shared_ptr.hpp>
#include <ros/message_event.h>
namespace message_filters
{
namespace mpl = boost::mpl;
/**
* \brief Synchronizes up to 9 messages by their timestamps.
*
* TimeSynchronizer synchronizes up to 9 incoming channels by the timestamps contained in their messages' headers.
* TimeSynchronizer takes anywhere from 2 to 9 message types as template parameters, and passes them through to a
* callback which takes a shared pointer of each.
*
* The required queue size parameter when constructing the TimeSynchronizer tells it how many sets of messages it should
* store (by timestamp) while waiting for messages to arrive and complete their "set"
*
* \section connections CONNECTIONS
*
* The input connections for the TimeSynchronizer object is the same signature as for roscpp subscription callbacks, ie.
\verbatim
void callback(const boost::shared_ptr<M const>&);
\endverbatim
* The output connection for the TimeSynchronizer object is dependent on the number of messages being synchronized. For
* a 3-message synchronizer for example, it would be:
\verbatim
void callback(const boost::shared_ptr<M0 const>&, const boost::shared_ptr<M1 const>&, const boost::shared_ptr<M2 const>&);
\endverbatim
* \section usage USAGE
* Example usage would be:
\verbatim
TimeSynchronizer<sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image> sync_policies(caminfo_sub, limage_sub, rimage_sub, 3);
sync_policies.registerCallback(callback);
\endverbatim
* The callback is then of the form:
\verbatim
void callback(const sensor_msgs::CameraInfo::ConstPtr&, const sensor_msgs::Image::ConstPtr&, const sensor_msgs::Image::ConstPtr&);
\endverbatim
*
*/
template<class M0, class M1, class M2 = NullType, class M3 = NullType, class M4 = NullType,
class M5 = NullType, class M6 = NullType, class M7 = NullType, class M8 = NullType>
class TimeSynchronizer : public Synchronizer<sync_policies::ExactTime<M0, M1, M2, M3, M4, M5, M6, M7, M8> >
{
public:
typedef sync_policies::ExactTime<M0, M1, M2, M3, M4, M5, M6, M7, M8> Policy;
typedef Synchronizer<Policy> Base;
typedef boost::shared_ptr<M0 const> M0ConstPtr;
typedef boost::shared_ptr<M1 const> M1ConstPtr;
typedef boost::shared_ptr<M2 const> M2ConstPtr;
typedef boost::shared_ptr<M3 const> M3ConstPtr;
typedef boost::shared_ptr<M4 const> M4ConstPtr;
typedef boost::shared_ptr<M5 const> M5ConstPtr;
typedef boost::shared_ptr<M6 const> M6ConstPtr;
typedef boost::shared_ptr<M7 const> M7ConstPtr;
typedef boost::shared_ptr<M8 const> M8ConstPtr;
using Base::add;
using Base::connectInput;
using Base::registerCallback;
using Base::setName;
using Base::getName;
using Policy::registerDropCallback;
typedef typename Base::M0Event M0Event;
typedef typename Base::M1Event M1Event;
typedef typename Base::M2Event M2Event;
typedef typename Base::M3Event M3Event;
typedef typename Base::M4Event M4Event;
typedef typename Base::M5Event M5Event;
typedef typename Base::M6Event M6Event;
typedef typename Base::M7Event M7Event;
typedef typename Base::M8Event M8Event;
template<class F0, class F1>
TimeSynchronizer(F0& f0, F1& f1, uint32_t queue_size)
: Base(Policy(queue_size))
{
connectInput(f0, f1);
}
template<class F0, class F1, class F2>
TimeSynchronizer(F0& f0, F1& f1, F2& f2, uint32_t queue_size)
: Base(Policy(queue_size))
{
connectInput(f0, f1, f2);
}
template<class F0, class F1, class F2, class F3>
TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, uint32_t queue_size)
: Base(Policy(queue_size))
{
connectInput(f0, f1, f2, f3);
}
template<class F0, class F1, class F2, class F3, class F4>
TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, uint32_t queue_size)
: Base(Policy(queue_size))
{
connectInput(f0, f1, f2, f3, f4);
}
template<class F0, class F1, class F2, class F3, class F4, class F5>
TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, uint32_t queue_size)
: Base(Policy(queue_size))
{
connectInput(f0, f1, f2, f3, f4, f5);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6>
TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, uint32_t queue_size)
: Base(Policy(queue_size))
{
connectInput(f0, f1, f2, f3, f4, f5, f6);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7>
TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, uint32_t queue_size)
: Base(Policy(queue_size))
{
connectInput(f0, f1, f2, f3, f4, f5, f6, f7);
}
template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8>
TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8, uint32_t queue_size)
: Base(Policy(queue_size))
{
connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8);
}
TimeSynchronizer(uint32_t queue_size)
: Base(Policy(queue_size))
{
}
////////////////////////////////////////////////////////////////
// For backwards compatibility
////////////////////////////////////////////////////////////////
void add0(const M0ConstPtr& msg)
{
this->template add<0>(M0Event(msg));
}
void add1(const M1ConstPtr& msg)
{
this->template add<1>(M1Event(msg));
}
void add2(const M2ConstPtr& msg)
{
this->template add<2>(M2Event(msg));
}
void add3(const M3ConstPtr& msg)
{
this->template add<3>(M3Event(msg));
}
void add4(const M4ConstPtr& msg)
{
this->template add<4>(M4Event(msg));
}
void add5(const M5ConstPtr& msg)
{
this->template add<5>(M5Event(msg));
}
void add6(const M6ConstPtr& msg)
{
this->template add<6>(M6Event(msg));
}
void add7(const M7ConstPtr& msg)
{
this->template add<7>(M7Event(msg));
}
void add8(const M8ConstPtr& msg)
{
this->template add<8>(M8Event(msg));
}
};
}
#endif // MESSAGE_FILTERS_TIME_SYNCHRONIZER_H

73
core/message_filters/index.rst
View File

@ -1,73 +0,0 @@
Message Filters --- chained message processing
==============================================
:mod:`message_filters` is a collection of message "filters" which take messages in,
either from a ROS subscription or another filter,
and may or may not output the message
at some time in the future, depending on a policy defined for that filter.
message_filters also defines a common interface for these filters, allowing you to chain them together.
The filters currently implemented in this package are:
* :class:`message_filters.Subscriber` - A source filter, which wraps a ROS subscription. Most filter chains will begin with a Subscriber.
* :class:`message_filters.Cache` - Caches messages which pass through it, allowing later lookup by time stamp.
* :class:`message_filters.TimeSynchronizer` - Synchronizes multiple messages by their timestamps, only passing them through when all have arrived.
* :class:`message_filters.TimeSequencer` - Tries to pass messages through ordered by their timestamps, even if some arrive out of order.
Here's a simple example of using a Subscriber with a Cache::
def myCallback(posemsg):
print posemsg
sub = message_filters.Subscriber("pose_topic", robot_msgs.msg.Pose)
cache = message_filters.Cache(sub, 10)
cache.registerCallback(myCallback)
The Subscriber here acts as the source of messages. Each message is passed to the cache, which then passes it through to the
user's callback ``myCallback``.
Using the time synchronizer::
from message_filters import TimeSynchronizer, Subscriber
def gotimage(image, camerainfo):
assert image.header.stamp == camerainfo.header.stamp
print "got an Image and CameraInfo"
tss = TimeSynchronizer(Subscriber("/wide_stereo/left/image_rect_color", sensor_msgs.msg.Image),
Subscriber("/wide_stereo/left/camera_info", sensor_msgs.msg.CameraInfo))
tss.registerCallback(gotimage)
The message filter interface
----------------------------
For an object to be usable as a message filter, it needs to have one method,
``registerCallback``. To collect messages from a message filter, register a callback with::
anyfilter.registerCallback(my_callback)
The signature of ``my_callback`` varies according to the message filter. For many filters it is simply::
def my_callback(msg):
where ``msg`` is the message.
Message filters that accept input from an upstream
message filter (e.g. :class:`message_filters.Cache`) register their own
message handler as a callback.
Output connections are registered through the ``registerCallback()`` function.
.. automodule:: message_filters
:members: Subscriber, Cache, TimeSynchronizer, TimeSequencer
:inherited-members:
Indices and tables
==================
* :ref:`genindex`
* :ref:`search`

59
core/message_filters/mainpage.dox
View File

@ -1,59 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b message_filters is a set of message "filters" which take messages in, usually through a callback from somewhere else,
and may or may not spit them out at some time in the future, depending on the conditions which need to be met for that
filter to do so.
\b message_filters also sets up a common pattern for these filters, allowing you to chain them together, even though they
have no explicit base class.
\section codeapi Code API
The filters currently implemented in this package are:
- message_filters::Subscriber - A source filter, which wraps a ROS subscription. Most filter chains will begin with a Subscriber.
- message_filters::Cache - Caches messages which pass through it, allowing later lookup by time stamp.
- message_filters::TimeSynchronizer - Synchronizes multiple messages by their timestamps, only passing them through when all have arrived.
- message_filters::TimeSequencer - Tries to pass messages through ordered by their timestamps, even if some arrive out of order.
There is also a base-class provided for simple filters: message_filters::SimpleFilter. This provides callback management and disconnection
for any filter that derives from it. A simple filter is defined as one that outputs a single message. message_filters::SimpleFilter provides
the registerCallback() method for any of its derived classes. message_filters::Subscriber, message_filters::Cache and message_filters::TimeSequencer
are all derived from message_filters::SimpleFilter.
Here's a simple example of using a Subscriber with a Cache:
\verbatim
void myCallback(const robot_msgs::Pose::ConstPtr& pose)
{}
ros::NodeHandle nh;
message_filters::Subscriber<robot_msgs::Pose> sub(nh, "pose_topic", 1);
message_filters::Cache<robot_msgs::Pose> cache(sub, 10);
cache.registerCallback(myCallback);
\endverbatim
The Subscriber here acts as the source of messages. Each message is passed to the cache, which then passes it through to the
user's callback (myCallback)
\section connections CONNECTIONS
Every filter can have up to two types of connections, input and output. Source filters (such as message_filters::Subscriber) only
have output connections, whereas most other filters have both input and output connections.
The two connection types do not have to be identical. For example, message_filters::TimeSynchronizer's input connection takes one
parameter, but its output connection has somewhere between 2 and 9 parameters, depending on the number of connections being
synchronized.
Input connections are registered either in the filter's constructor, or by calling connectInput() on the filter. For example:
\verbatim
message_filters::Cache<robot_msgs::Pose> cache(10);
cache.connectInput(sub);
\endverbatim
This connects cache's input to sub's output.
Output connections are registered through the registerCallback() function.
*/

18
core/message_filters/manifest.xml
View File

@ -1,18 +0,0 @@
<package>
<description>A set of message filters which take in messages and may output those messages at a later time, based on the conditions that filter needs met.</description>
<author>Josh Faust (jfaust@willowgarage.com), Vijay Pradeep (vpradeep@willowgarage.com)</author>
<license>BSD</license>
<review status="Doc reviewed" notes=""/>
<depend package="roscpp" />
<depend package="rospy" />
<depend package="rostest" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmessage_filters `rosboost-cfg --lflags thread,signals`" />
<rosdoc config="rosdoc.yaml" />
</export>
<url>http://pr.willowgarage.com/wiki/message_filters</url>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
<platform os="macports" version="macports"/>
</package>

7
core/message_filters/rosdoc.yaml
View File

@ -1,7 +0,0 @@
- builder: sphinx
name: Python API
output_dir: python
- builder: doxygen
name: C++ API
output_dir: c++
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'

64
core/message_filters/src/connection.cpp
View File

@ -1,64 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include "connection.h"
namespace message_filters
{
Connection::Connection(const VoidDisconnectFunction& func)
: void_disconnect_(func)
{
}
Connection::Connection(const WithConnectionDisconnectFunction& func, boost::signals::connection c)
: connection_disconnect_(func)
, connection_(c)
{
}
void Connection::disconnect()
{
if (void_disconnect_)
{
void_disconnect_();
}
else if (connection_disconnect_)
{
connection_disconnect_(*this);
}
}
}

144
core/message_filters/src/message_filters/__init__.py
View File

@ -1,144 +0,0 @@
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
"""
Message Filter Objects
======================
"""
import threading
import rospy
class SimpleFilter:
def __init__(self):
self.callbacks = {}
def registerCallback(self, cb, *args):
"""
Register a callback function `cb` to be called when this filter
has output.
The filter calls the function ``cb`` with a filter-dependent list of arguments,
followed by the call-supplied arguments ``args``.
"""
conn = len(self.callbacks)
self.callbacks[conn] = (cb, args)
return conn
def signalMessage(self, *msg):
for (cb, args) in self.callbacks.values():
cb(*(msg + args))
class Subscriber(SimpleFilter):
"""
ROS subscription filter. Identical arguments as :class:`rospy.Subscriber`.
This class acts as a highest-level filter, simply passing messages
from a ROS subscription through to the filters which have connected
to it.
"""
def __init__(self, *args):
SimpleFilter.__init__(self)
self.topic = args[0]
self.sub = rospy.Subscriber(*args, **{"callback" : self.callback})
def callback(self, msg):
self.signalMessage(msg)
def getTopic(self):
return self.topic
class Cache(SimpleFilter):
"""
Stores a time history of messages.
Given a stream of messages, the most recent ``cache_size`` messages
are cached in a ring buffer, from which time intervals of the cache
can then be retrieved by the client.
"""
def __init__(self, f, cache_size = 1):
SimpleFilter.__init__(self)
self.connectInput(f)
self.cache_size = cache_size
def connectInput(self, f):
self.incoming_connection = f.registerCallback(self.add)
def add(self, msg):
# Add msg to cache... XXX TODO
self.signalMessage(msg)
class TimeSynchronizer(SimpleFilter):
"""
Synchronizes messages by their timestamps.
:class:`TimeSynchronizer` synchronizes incoming message filters by the
timestamps contained in their messages' headers. TimeSynchronizer
listens on multiple input message filters ``fs``, and invokes the callback
when it has a collection of messages with matching timestamps.
The signature of the callback function is::
def callback(msg1, ... msgN):
where N is the number of input message filters, and each message is
the output of the corresponding filter in ``fs``.
The required ``queue size`` parameter specifies how many sets of
messages it should store from each input filter (by timestamp)
while waiting for messages to arrive and complete their "set".
"""
def __init__(self, fs, queue_size):
SimpleFilter.__init__(self)
self.connectInput(fs)
self.queue_size = queue_size
self.lock = threading.Lock()
def connectInput(self, fs):
self.queues = [{} for f in fs]
self.input_connections = [f.registerCallback(self.add, q) for (f, q) in zip(fs, self.queues)]
def add(self, msg, my_queue):
self.lock.acquire()
my_queue[msg.header.stamp] = msg
while len(my_queue) > self.queue_size:
del my_queue[min(my_queue)]
# common is the set of timestamps that occur in all queues
common = reduce(set.intersection, [set(q) for q in self.queues])
for t in sorted(common):
# msgs is list of msgs (one from each queue) with stamp t
msgs = [q[t] for q in self.queues]
self.signalMessage(*msgs)
for q in self.queues:
del q[t]
self.lock.release()

36
core/message_filters/test/CMakeLists.txt
View File

@ -1,36 +0,0 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
# ********** Tests **********
rosbuild_add_gtest(test/msg_cache_unittest msg_cache_unittest.cpp)
target_link_libraries(test/msg_cache_unittest message_filters)
rosbuild_add_gtest(test/time_synchronizer_unittest time_synchronizer_unittest.cpp)
target_link_libraries(test/time_synchronizer_unittest message_filters)
rosbuild_add_gtest(test/test_synchronizer test_synchronizer.cpp)
target_link_libraries(test/test_synchronizer message_filters)
rosbuild_add_gtest(test/test_exact_time_policy test_exact_time_policy.cpp)
target_link_libraries(test/test_exact_time_policy message_filters)
rosbuild_add_gtest(test/test_approximate_time_policy test_approximate_time_policy.cpp)
target_link_libraries(test/test_approximate_time_policy message_filters)
rosbuild_add_gtest(test/test_simple test_simple.cpp)
target_link_libraries(test/test_simple message_filters)
rosbuild_add_gtest(test/test_chain test_chain.cpp)
target_link_libraries(test/test_chain message_filters)
# Needs to be a rostest because it spins up a node, which blocks until it hears from the master (unfortunately)
rosbuild_add_executable(test/time_sequencer_unittest EXCLUDE_FROM_ALL time_sequencer_unittest.cpp)
rosbuild_declare_test(test/time_sequencer_unittest)
rosbuild_add_gtest_build_flags(test/time_sequencer_unittest)
target_link_libraries(test/time_sequencer_unittest message_filters)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/time_sequencer_unittest.xml)
rosbuild_add_executable(test/test_subscriber EXCLUDE_FROM_ALL test_subscriber.cpp)
rosbuild_declare_test(test/test_subscriber)
rosbuild_add_gtest_build_flags(test/test_subscriber)
target_link_libraries(test/test_subscriber message_filters)
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test_subscriber.xml)

84
core/message_filters/test/directed.py
View File

@ -1,84 +0,0 @@
# image_transport::SubscriberFilter wide_left; // "/wide_stereo/left/image_raw"
# image_transport::SubscriberFilter wide_right; // "/wide_stereo/right/image_raw"
# message_filters::Subscriber<CameraInfo> wide_left_info; // "/wide_stereo/left/camera_info"
# message_filters::Subscriber<CameraInfo> wide_right_info; // "/wide_stereo/right/camera_info"
# message_filters::TimeSynchronizer<Image, CameraInfo, Image, CameraInfo> wide;
#
# PersonDataRecorder() :
# wide_left(nh_, "/wide_stereo/left/image_raw", 10),
# wide_right(nh_, "/wide_stereo/right/image_raw", 10),
# wide_left_info(nh_, "/wide_stereo/left/camera_info", 10),
# wide_right_info(nh_, "/wide_stereo/right/camera_info", 10),
# wide(wide_left, wide_left_info, wide_right, wide_right_info, 4),
#
# wide.registerCallback(boost::bind(&PersonDataRecorder::wideCB, this, _1, _2, _3, _4));
import roslib
roslib.load_manifest('message_filters')
import rostest
import rospy
import random
import unittest
from message_filters import SimpleFilter, Subscriber, Cache, TimeSynchronizer
class MockHeader:
pass
class MockMessage:
def __init__(self, stamp, data):
self.header = MockHeader()
self.header.stamp = stamp
self.data = data
class MockFilter(SimpleFilter):
pass
class TestDirected(unittest.TestCase):
def cb_collector_2msg(self, msg1, msg2):
self.collector.append((msg1, msg2))
def test_synchronizer(self):
m0 = MockFilter()
m1 = MockFilter()
ts = TimeSynchronizer([m0, m1], 1)
ts.registerCallback(self.cb_collector_2msg)
if 0:
# Simple case, pairs of messages, make sure that they get combined
for t in range(10):
self.collector = []
msg0 = MockMessage(t, 33)
msg1 = MockMessage(t, 34)
m0.signalMessage(msg0)
self.assertEqual(self.collector, [])
m1.signalMessage(msg1)
self.assertEqual(self.collector, [(msg0, msg1)])
# Scramble sequences of length N. Make sure that TimeSequencer recombines them.
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [MockMessage(t, random.random()) for t in range(N)]
seq1 = [MockMessage(t, random.random()) for t in range(N)]
# random.shuffle(seq0)
ts = TimeSynchronizer([m0, m1], N)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for msg in random.sample(seq1, N):
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
if __name__ == '__main__':
if 0:
rostest.unitrun('message_filters', 'directed', TestDirected)
else:
suite = unittest.TestSuite()
suite.addTest(TestDirected('test_synchronizer'))
unittest.TextTestRunner(verbosity=2).run(suite)

225
core/message_filters/test/msg_cache_unittest.cpp
View File

@ -1,225 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "ros/time.h"
#include <ros/init.h>
#include "message_filters/cache.h"
using namespace std ;
using namespace message_filters ;
struct Header
{
ros::Time stamp ;
} ;
struct Msg
{
Header header ;
int data ;
} ;
typedef boost::shared_ptr<Msg const> MsgConstPtr;
namespace ros
{
namespace message_traits
{
template<>
struct TimeStamp<Msg>
{
static ros::Time value(const Msg& m)
{
return m.header.stamp;
}
};
}
}
void fillCacheEasy(Cache<Msg>& cache, unsigned int start, unsigned int end)
{
for (unsigned int i=start; i < end; i++)
{
Msg* msg = new Msg ;
msg->data = i ;
msg->header.stamp.fromSec(i*10) ;
boost::shared_ptr<Msg const> msg_ptr(msg) ;
cache.add(msg_ptr) ;
}
}
TEST(Cache, easyInterval)
{
Cache<Msg> cache(10) ;
fillCacheEasy(cache, 0, 5) ;
vector<boost::shared_ptr<Msg const> > interval_data = cache.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
ASSERT_EQ(interval_data.size(), (unsigned int) 3) ;
EXPECT_EQ(interval_data[0]->data, 1) ;
EXPECT_EQ(interval_data[1]->data, 2) ;
EXPECT_EQ(interval_data[2]->data, 3) ;
// Look for an interval past the end of the cache
interval_data = cache.getInterval(ros::Time().fromSec(55), ros::Time().fromSec(65)) ;
EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
// Look for an interval that fell off the back of the cache
fillCacheEasy(cache, 5, 20) ;
interval_data = cache.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
}
TEST(Cache, easySurroundingInterval)
{
Cache<Msg> cache(10);
fillCacheEasy(cache, 1, 6);
vector<boost::shared_ptr<Msg const> > interval_data;
interval_data = cache.getSurroundingInterval(ros::Time(15,0), ros::Time(35,0)) ;
ASSERT_EQ(interval_data.size(), (unsigned int) 4);
EXPECT_EQ(interval_data[0]->data, 1);
EXPECT_EQ(interval_data[1]->data, 2);
EXPECT_EQ(interval_data[2]->data, 3);
EXPECT_EQ(interval_data[3]->data, 4);
interval_data = cache.getSurroundingInterval(ros::Time(0,0), ros::Time(35,0)) ;
ASSERT_EQ(interval_data.size(), (unsigned int) 4);
EXPECT_EQ(interval_data[0]->data, 1);
interval_data = cache.getSurroundingInterval(ros::Time(35,0), ros::Time(35,0)) ;
ASSERT_EQ(interval_data.size(), (unsigned int) 2);
EXPECT_EQ(interval_data[0]->data, 3);
EXPECT_EQ(interval_data[1]->data, 4);
interval_data = cache.getSurroundingInterval(ros::Time(55,0), ros::Time(55,0)) ;
ASSERT_EQ(interval_data.size(), (unsigned int) 1);
EXPECT_EQ(interval_data[0]->data, 5);
}
boost::shared_ptr<Msg const> buildMsg(double time, int data)
{
Msg* msg = new Msg ;
msg->data = data ;
msg->header.stamp.fromSec(time) ;
boost::shared_ptr<Msg const> msg_ptr(msg) ;
return msg_ptr ;
}
TEST(Cache, easyUnsorted)
{
Cache<Msg> cache(10) ;
cache.add(buildMsg(10.0, 1)) ;
cache.add(buildMsg(30.0, 3)) ;
cache.add(buildMsg(70.0, 7)) ;
cache.add(buildMsg( 5.0, 0)) ;
cache.add(buildMsg(20.0, 2)) ;
vector<boost::shared_ptr<Msg const> > interval_data = cache.getInterval(ros::Time().fromSec(3), ros::Time().fromSec(15)) ;
ASSERT_EQ(interval_data.size(), (unsigned int) 2) ;
EXPECT_EQ(interval_data[0]->data, 0) ;
EXPECT_EQ(interval_data[1]->data, 1) ;
// Grab all the data
interval_data = cache.getInterval(ros::Time().fromSec(0), ros::Time().fromSec(80)) ;
ASSERT_EQ(interval_data.size(), (unsigned int) 5) ;
EXPECT_EQ(interval_data[0]->data, 0) ;
EXPECT_EQ(interval_data[1]->data, 1) ;
EXPECT_EQ(interval_data[2]->data, 2) ;
EXPECT_EQ(interval_data[3]->data, 3) ;
EXPECT_EQ(interval_data[4]->data, 7) ;
}
TEST(Cache, easyElemBeforeAfter)
{
Cache<Msg> cache(10) ;
boost::shared_ptr<Msg const> elem_ptr ;
fillCacheEasy(cache, 5, 10) ;
elem_ptr = cache.getElemAfterTime( ros::Time().fromSec(85.0)) ;
ASSERT_FALSE(!elem_ptr) ;
EXPECT_EQ(elem_ptr->data, 9) ;
elem_ptr = cache.getElemBeforeTime( ros::Time().fromSec(85.0)) ;
ASSERT_FALSE(!elem_ptr) ;
EXPECT_EQ(elem_ptr->data, 8) ;
elem_ptr = cache.getElemBeforeTime( ros::Time().fromSec(45.0)) ;
EXPECT_TRUE(!elem_ptr) ;
}
struct EventHelper
{
public:
void cb(const ros::MessageEvent<Msg const>& evt)
{
event_ = evt;
}
ros::MessageEvent<Msg const> event_;
};
TEST(Cache, eventInEventOut)
{
Cache<Msg> c0(10);
Cache<Msg> c1(c0, 10);
EventHelper h;
c1.registerCallback(&EventHelper::cb, &h);
ros::MessageEvent<Msg const> evt(MsgConstPtr(new Msg), ros::Time(4));
c0.add(evt);
EXPECT_EQ(h.event_.getReceiptTime(), evt.getReceiptTime());
EXPECT_EQ(h.event_.getMessage(), evt.getMessage());
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "blah");
ros::Time::init();
return RUN_ALL_TESTS();
}

543
core/message_filters/test/test_approximate_time_policy.cpp
View File

@ -1,543 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
#include <vector>
#include <ros/ros.h>
//#include <pair>
using namespace message_filters;
using namespace message_filters::sync_policies;
struct Header
{
ros::Time stamp;
};
struct Msg
{
Header header;
int data;
};
typedef boost::shared_ptr<Msg> MsgPtr;
typedef boost::shared_ptr<Msg const> MsgConstPtr;
namespace ros
{
namespace message_traits
{
template<>
struct TimeStamp<Msg>
{
static ros::Time value(const Msg& m)
{
return m.header.stamp;
}
};
}
}
typedef std::pair<ros::Time, ros::Time> TimePair;
typedef std::pair<ros::Time, unsigned int> TimeAndTopic;
struct TimeQuad
{
TimeQuad(ros::Time p, ros::Time q, ros::Time r, ros::Time s)
{
time[0] = p;
time[1] = q;
time[2] = r;
time[3] = s;
}
ros::Time time[4];
};
//----------------------------------------------------------
// Test Class (for 2 inputs)
//----------------------------------------------------------
class ApproximateTimeSynchronizerTest
{
public:
ApproximateTimeSynchronizerTest(const std::vector<TimeAndTopic> &input,
const std::vector<TimePair> &output,
uint32_t queue_size) :
input_(input), output_(output), output_position_(0), sync_(queue_size)
{
sync_.registerCallback(boost::bind(&ApproximateTimeSynchronizerTest::callback, this, _1, _2));
}
void callback(const MsgConstPtr& p, const MsgConstPtr& q)
{
//printf("Call_back called\n");
//printf("Call back: <%f, %f>\n", p->header.stamp.toSec(), q->header.stamp.toSec());
ASSERT_TRUE(p);
ASSERT_TRUE(q);
ASSERT_LT(output_position_, output_.size());
EXPECT_EQ(output_[output_position_].first, p->header.stamp);
EXPECT_EQ(output_[output_position_].second, q->header.stamp);
++output_position_;
}
void run()
{
for (unsigned int i = 0; i < input_.size(); i++)
{
if (input_[i].second == 0)
{
MsgPtr p(new Msg);
p->header.stamp = input_[i].first;
sync_.add<0>(p);
}
else
{
MsgPtr q(new Msg);
q->header.stamp = input_[i].first;
sync_.add<1>(q);
}
}
//printf("Done running test\n");
EXPECT_EQ(output_.size(), output_position_);
}
private:
const std::vector<TimeAndTopic> &input_;
const std::vector<TimePair> &output_;
unsigned int output_position_;
typedef Synchronizer<ApproximateTime<Msg, Msg> > Sync2;
public:
Sync2 sync_;
};
//----------------------------------------------------------
// Test Class (for 4 inputs)
//----------------------------------------------------------
class ApproximateTimeSynchronizerTestQuad
{
public:
ApproximateTimeSynchronizerTestQuad(const std::vector<TimeAndTopic> &input,
const std::vector<TimeQuad> &output,
uint32_t queue_size) :
input_(input), output_(output), output_position_(0), sync_(queue_size)
{
sync_.registerCallback(boost::bind(&ApproximateTimeSynchronizerTestQuad::callback, this, _1, _2, _3, _4));
}
void callback(const MsgConstPtr& p, const MsgConstPtr& q, const MsgConstPtr& r, const MsgConstPtr& s)
{
//printf("Call_back called\n");
//printf("Call back: <%f, %f>\n", p->header.stamp.toSec(), q->header.stamp.toSec());
ASSERT_TRUE(p);
ASSERT_TRUE(q);
ASSERT_TRUE(r);
ASSERT_TRUE(s);
ASSERT_LT(output_position_, output_.size());
EXPECT_EQ(output_[output_position_].time[0], p->header.stamp);
EXPECT_EQ(output_[output_position_].time[1], q->header.stamp);
EXPECT_EQ(output_[output_position_].time[2], r->header.stamp);
EXPECT_EQ(output_[output_position_].time[3], s->header.stamp);
++output_position_;
}
void run()
{
for (unsigned int i = 0; i < input_.size(); i++)
{
MsgPtr p(new Msg);
p->header.stamp = input_[i].first;
switch (input_[i].second)
{
case 0:
sync_.add<0>(p);
break;
case 1:
sync_.add<1>(p);
break;
case 2:
sync_.add<2>(p);
break;
case 3:
sync_.add<3>(p);
break;
}
}
//printf("Done running test\n");
EXPECT_EQ(output_.size(), output_position_);
}
private:
const std::vector<TimeAndTopic> &input_;
const std::vector<TimeQuad> &output_;
unsigned int output_position_;
typedef Synchronizer<ApproximateTime<Msg, Msg, Msg, Msg> > Sync4;
public:
Sync4 sync_;
};
//----------------------------------------------------------
// Test Suite
//----------------------------------------------------------
TEST(ApproxTimeSync, ExactMatch) {
// Input A: a..b..c
// Input B: A..B..C
// Output: a..b..c
// A..B..C
std::vector<TimeAndTopic> input;
std::vector<TimePair> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t,1)); // A
input.push_back(TimeAndTopic(t+s*3,0)); // b
input.push_back(TimeAndTopic(t+s*3,1)); // B
input.push_back(TimeAndTopic(t+s*6,0)); // c
input.push_back(TimeAndTopic(t+s*6,1)); // C
output.push_back(TimePair(t, t));
output.push_back(TimePair(t+s*3, t+s*3));
output.push_back(TimePair(t+s*6, t+s*6));
ApproximateTimeSynchronizerTest sync_test(input, output, 10);
sync_test.run();
}
TEST(ApproxTimeSync, PerfectMatch) {
// Input A: a..b..c.
// Input B: .A..B..C
// Output: ...a..b.
// ...A..B.
std::vector<TimeAndTopic> input;
std::vector<TimePair> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s,1)); // A
input.push_back(TimeAndTopic(t+s*3,0)); // b
input.push_back(TimeAndTopic(t+s*4,1)); // B
input.push_back(TimeAndTopic(t+s*6,0)); // c
input.push_back(TimeAndTopic(t+s*7,1)); // C
output.push_back(TimePair(t, t+s));
output.push_back(TimePair(t+s*3, t+s*4));
ApproximateTimeSynchronizerTest sync_test(input, output, 10);
sync_test.run();
}
TEST(ApproxTimeSync, ImperfectMatch) {
// Input A: a.xb..c.
// Input B: .A...B.C
// Output: ..a...c.
// ..A...B.
std::vector<TimeAndTopic> input;
std::vector<TimePair> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s,1)); // A
input.push_back(TimeAndTopic(t+s*2,0)); // x
input.push_back(TimeAndTopic(t+s*3,0)); // b
input.push_back(TimeAndTopic(t+s*5,1)); // B
input.push_back(TimeAndTopic(t+s*6,0)); // c
input.push_back(TimeAndTopic(t+s*7,1)); // C
output.push_back(TimePair(t, t+s));
output.push_back(TimePair(t+s*6, t+s*5));
ApproximateTimeSynchronizerTest sync_test(input, output, 10);
sync_test.run();
}
TEST(ApproxTimeSync, Acceleration) {
// Time: 0123456789012345678
// Input A: a...........b....c.
// Input B: .......A.......B..C
// Output: ............b.....c
// ............A.....C
std::vector<TimeAndTopic> input;
std::vector<TimePair> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s*7,1)); // A
input.push_back(TimeAndTopic(t+s*12,0)); // b
input.push_back(TimeAndTopic(t+s*15,1)); // B
input.push_back(TimeAndTopic(t+s*17,0)); // c
input.push_back(TimeAndTopic(t+s*18,1)); // C
output.push_back(TimePair(t+s*12, t+s*7));
output.push_back(TimePair(t+s*17, t+s*18));
ApproximateTimeSynchronizerTest sync_test(input, output, 10);
sync_test.run();
}
TEST(ApproxTimeSync, DroppedMessages) {
// Queue size 1 (too small)
// Time: 012345678901234
// Input A: a...b...c.d..e.
// Input B: .A.B...C...D..E
// Output: .......b.....d.
// .......B.....D.
std::vector<TimeAndTopic> input;
std::vector<TimePair> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s,1)); // A
input.push_back(TimeAndTopic(t+s*3,1)); // B
input.push_back(TimeAndTopic(t+s*4,0)); // b
input.push_back(TimeAndTopic(t+s*7,1)); // C
input.push_back(TimeAndTopic(t+s*8,0)); // c
input.push_back(TimeAndTopic(t+s*10,0)); // d
input.push_back(TimeAndTopic(t+s*11,1)); // D
input.push_back(TimeAndTopic(t+s*13,0)); // e
input.push_back(TimeAndTopic(t+s*14,1)); // E
output.push_back(TimePair(t+s*4, t+s*3));
output.push_back(TimePair(t+s*10, t+s*11));
ApproximateTimeSynchronizerTest sync_test(input, output, 1);
sync_test.run();
// Queue size 2 (just enough)
// Time: 012345678901234
// Input A: a...b...c.d..e.
// Input B: .A.B...C...D..E
// Output: ....a..b...c.d.
// ....A..B...C.D.
std::vector<TimePair> output2;
output2.push_back(TimePair(t, t+s));
output2.push_back(TimePair(t+s*4, t+s*3));
output2.push_back(TimePair(t+s*8, t+s*7));
output2.push_back(TimePair(t+s*10, t+s*11));
ApproximateTimeSynchronizerTest sync_test2(input, output2, 2);
sync_test2.run();
}
TEST(ApproxTimeSync, LongQueue) {
// Queue size 5
// Time: 012345678901234
// Input A: abcdefghiklmnp.
// Input B: ...j......o....
// Output: ..........l....
// ..........o....
std::vector<TimeAndTopic> input;
std::vector<TimePair> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s,0)); // b
input.push_back(TimeAndTopic(t+s*2,0)); // c
input.push_back(TimeAndTopic(t+s*3,0)); // d
input.push_back(TimeAndTopic(t+s*4,0)); // e
input.push_back(TimeAndTopic(t+s*5,0)); // f
input.push_back(TimeAndTopic(t+s*6,0)); // g
input.push_back(TimeAndTopic(t+s*7,0)); // h
input.push_back(TimeAndTopic(t+s*8,0)); // i
input.push_back(TimeAndTopic(t+s*3,1)); // j
input.push_back(TimeAndTopic(t+s*9,0)); // k
input.push_back(TimeAndTopic(t+s*10,0)); // l
input.push_back(TimeAndTopic(t+s*11,0)); // m
input.push_back(TimeAndTopic(t+s*12,0)); // n
input.push_back(TimeAndTopic(t+s*10,1)); // o
input.push_back(TimeAndTopic(t+s*13,0)); // l
output.push_back(TimePair(t+s*10, t+s*10));
ApproximateTimeSynchronizerTest sync_test(input, output, 5);
sync_test.run();
}
TEST(ApproxTimeSync, DoublePublish) {
// Input A: a..b
// Input B: .A.B
// Output: ...b
// ...B
// +
// a
// A
std::vector<TimeAndTopic> input;
std::vector<TimePair> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s,1)); // A
input.push_back(TimeAndTopic(t+s*3,1)); // B
input.push_back(TimeAndTopic(t+s*3,0)); // b
output.push_back(TimePair(t, t+s));
output.push_back(TimePair(t+s*3, t+s*3));
ApproximateTimeSynchronizerTest sync_test(input, output, 10);
sync_test.run();
}
TEST(ApproxTimeSync, FourTopics) {
// Time: 012345678901234
// Input A: a....e..i.m..n.
// Input B: .b....g..j....o
// Input C: ..c...h...k....
// Input D: ...d.f.....l...
// Output: ......a....e..m
// ......b....g..j
// ......c....h..k
// ......d....f..l
std::vector<TimeAndTopic> input;
std::vector<TimeQuad> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s,1)); // b
input.push_back(TimeAndTopic(t+s*2,2)); // c
input.push_back(TimeAndTopic(t+s*3,3)); // d
input.push_back(TimeAndTopic(t+s*5,0)); // e
input.push_back(TimeAndTopic(t+s*5,3)); // f
input.push_back(TimeAndTopic(t+s*6,1)); // g
input.push_back(TimeAndTopic(t+s*6,2)); // h
input.push_back(TimeAndTopic(t+s*8,0)); // i
input.push_back(TimeAndTopic(t+s*9,1)); // j
input.push_back(TimeAndTopic(t+s*10,2)); // k
input.push_back(TimeAndTopic(t+s*11,3)); // l
input.push_back(TimeAndTopic(t+s*10,0)); // m
input.push_back(TimeAndTopic(t+s*13,0)); // n
input.push_back(TimeAndTopic(t+s*14,1)); // o
output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
output.push_back(TimeQuad(t+s*5, t+s*6, t+s*6, t+s*5));
output.push_back(TimeQuad(t+s*10, t+s*9, t+s*10, t+s*11));
ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10);
sync_test.run();
}
TEST(ApproxTimeSync, EarlyPublish) {
// Time: 012345678901234
// Input A: a......e
// Input B: .b......
// Input C: ..c.....
// Input D: ...d....
// Output: .......a
// .......b
// .......c
// .......d
std::vector<TimeAndTopic> input;
std::vector<TimeQuad> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s,1)); // b
input.push_back(TimeAndTopic(t+s*2,2)); // c
input.push_back(TimeAndTopic(t+s*3,3)); // d
input.push_back(TimeAndTopic(t+s*7,0)); // e
output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10);
sync_test.run();
}
TEST(ApproxTimeSync, RateBound) {
// Rate bound A: 1.5
// Input A: a..b..c.
// Input B: .A..B..C
// Output: .a..b...
// .A..B...
std::vector<TimeAndTopic> input;
std::vector<TimePair> output;
ros::Time t(0, 0);
ros::Duration s(1, 0);
input.push_back(TimeAndTopic(t,0)); // a
input.push_back(TimeAndTopic(t+s,1)); // A
input.push_back(TimeAndTopic(t+s*3,0)); // b
input.push_back(TimeAndTopic(t+s*4,1)); // B
input.push_back(TimeAndTopic(t+s*6,0)); // c
input.push_back(TimeAndTopic(t+s*7,1)); // C
output.push_back(TimePair(t, t+s));
output.push_back(TimePair(t+s*3, t+s*4));
ApproximateTimeSynchronizerTest sync_test(input, output, 10);
sync_test.sync_.setInterMessageLowerBound(0, s*1.5);
sync_test.run();
// Rate bound A: 2
// Input A: a..b..c.
// Input B: .A..B..C
// Output: .a..b..c
// .A..B..C
output.push_back(TimePair(t+s*6, t+s*7));
ApproximateTimeSynchronizerTest sync_test2(input, output, 10);
sync_test2.sync_.setInterMessageLowerBound(0, s*2);
sync_test2.run();
}
int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "blah");
ros::Time::init();
return RUN_ALL_TESTS();
}

188
core/message_filters/test/test_chain.cpp
View File

@ -1,188 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "ros/time.h"
#include <ros/init.h>
#include "message_filters/chain.h"
using namespace message_filters;
struct Msg
{
};
typedef boost::shared_ptr<Msg> MsgPtr;
typedef boost::shared_ptr<Msg const> MsgConstPtr;
class Helper
{
public:
Helper()
: count_(0)
{}
void cb()
{
++count_;
}
int32_t count_;
};
typedef boost::shared_ptr<PassThrough<Msg> > PassThroughPtr;
TEST(Chain, simple)
{
Helper h;
Chain<Msg> c;
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.registerCallback(boost::bind(&Helper::cb, &h));
c.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 1);
c.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 2);
}
TEST(Chain, multipleFilters)
{
Helper h;
Chain<Msg> c;
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.registerCallback(boost::bind(&Helper::cb, &h));
c.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 1);
c.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 2);
}
TEST(Chain, addingFilters)
{
Helper h;
Chain<Msg> c;
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.registerCallback(boost::bind(&Helper::cb, &h));
c.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 1);
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 2);
}
TEST(Chain, inputFilter)
{
Helper h;
Chain<Msg> c;
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
c.registerCallback(boost::bind(&Helper::cb, &h));
PassThrough<Msg> p;
c.connectInput(p);
p.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 1);
p.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 2);
}
TEST(Chain, nonSharedPtrFilter)
{
Helper h;
Chain<Msg> c;
PassThrough<Msg> p;
c.addFilter(&p);
c.registerCallback(boost::bind(&Helper::cb, &h));
c.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 1);
c.add(MsgPtr(new Msg));
EXPECT_EQ(h.count_, 2);
}
TEST(Chain, retrieveFilter)
{
Chain<Msg> c;
ASSERT_FALSE(c.getFilter<PassThrough<Msg> >(0));
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
ASSERT_TRUE(c.getFilter<PassThrough<Msg> >(0));
ASSERT_FALSE(c.getFilter<PassThrough<Msg> >(1));
}
TEST(Chain, retrieveFilterThroughBaseClass)
{
Chain<Msg> c;
ChainBase* cb = &c;
ASSERT_FALSE(cb->getFilter<PassThrough<Msg> >(0));
c.addFilter(PassThroughPtr(new PassThrough<Msg>));
ASSERT_TRUE(cb->getFilter<PassThrough<Msg> >(0));
ASSERT_FALSE(cb->getFilter<PassThrough<Msg> >(1));
}
struct PTDerived : public PassThrough<Msg>
{
};
TEST(Chain, retrieveBaseClass)
{
Chain<Msg> c;
c.addFilter(PassThroughPtr(new PTDerived));
ASSERT_TRUE(c.getFilter<PassThrough<Msg> >(0));
ASSERT_TRUE(c.getFilter<PTDerived>(0));
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "blah");
ros::Time::init();
return RUN_ALL_TESTS();
}

206
core/message_filters/test/test_exact_time_policy.cpp
View File

@ -1,206 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "ros/ros.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/exact_time.h"
using namespace message_filters;
using namespace message_filters::sync_policies;
struct Header
{
ros::Time stamp;
};
struct Msg
{
Header header;
int data;
};
typedef boost::shared_ptr<Msg> MsgPtr;
typedef boost::shared_ptr<Msg const> MsgConstPtr;
namespace ros
{
namespace message_traits
{
template<>
struct TimeStamp<Msg>
{
static ros::Time value(const Msg& m)
{
return m.header.stamp;
}
};
}
}
class Helper
{
public:
Helper()
: count_(0)
, drop_count_(0)
{}
void cb()
{
++count_;
}
void dropcb()
{
++drop_count_;
}
int32_t count_;
int32_t drop_count_;
};
typedef ExactTime<Msg, Msg> Policy2;
typedef ExactTime<Msg, Msg, Msg> Policy3;
typedef Synchronizer<Policy2> Sync2;
typedef Synchronizer<Policy3> Sync3;
//////////////////////////////////////////////////////////////////////////////////////////////////
// From here on we assume that testing the 3-message version is sufficient, so as not to duplicate
// tests for everywhere from 2-9
//////////////////////////////////////////////////////////////////////////////////////////////////
TEST(ExactTime, multipleTimes)
{
Sync3 sync(2);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time();
sync.add<0>(m);
ASSERT_EQ(h.count_, 0);
m.reset(new Msg);
m->header.stamp = ros::Time(0.1);
sync.add<1>(m);
ASSERT_EQ(h.count_, 0);
sync.add<0>(m);
ASSERT_EQ(h.count_, 0);
sync.add<2>(m);
ASSERT_EQ(h.count_, 1);
}
TEST(ExactTime, queueSize)
{
Sync3 sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time();
sync.add<0>(m);
ASSERT_EQ(h.count_, 0);
sync.add<1>(m);
ASSERT_EQ(h.count_, 0);
m.reset(new Msg);
m->header.stamp = ros::Time(0.1);
sync.add<1>(m);
ASSERT_EQ(h.count_, 0);
m.reset(new Msg);
m->header.stamp = ros::Time(0);
sync.add<1>(m);
ASSERT_EQ(h.count_, 0);
sync.add<2>(m);
ASSERT_EQ(h.count_, 0);
}
TEST(ExactTime, dropCallback)
{
Sync2 sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
sync.getPolicy()->registerDropCallback(boost::bind(&Helper::dropcb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time();
sync.add<0>(m);
ASSERT_EQ(h.drop_count_, 0);
m->header.stamp = ros::Time(0.1);
sync.add<0>(m);
ASSERT_EQ(h.drop_count_, 1);
}
struct EventHelper
{
void callback(const ros::MessageEvent<Msg const>& e1, const ros::MessageEvent<Msg const>& e2)
{
e1_ = e1;
e2_ = e2;
}
ros::MessageEvent<Msg const> e1_;
ros::MessageEvent<Msg const> e2_;
};
TEST(ExactTime, eventInEventOut)
{
Sync2 sync(2);
EventHelper h;
sync.registerCallback(&EventHelper::callback, &h);
ros::MessageEvent<Msg const> evt(MsgPtr(new Msg), ros::Time(4));
sync.add<0>(evt);
sync.add<1>(evt);
ASSERT_TRUE(h.e1_.getMessage());
ASSERT_TRUE(h.e2_.getMessage());
ASSERT_EQ(h.e1_.getReceiptTime(), evt.getReceiptTime());
ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime());
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "blah");
ros::Time::init();
ros::Time::setNow(ros::Time());
return RUN_ALL_TESTS();
}

158
core/message_filters/test/test_simple.cpp
View File

@ -1,158 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "ros/time.h"
#include <ros/init.h>
#include "message_filters/simple_filter.h"
using namespace message_filters;
struct Msg
{
};
typedef boost::shared_ptr<Msg> MsgPtr;
typedef boost::shared_ptr<Msg const> MsgConstPtr;
struct Filter : public SimpleFilter<Msg>
{
typedef ros::MessageEvent<Msg const> EventType;
void add(const EventType& evt)
{
signalMessage(evt);
}
};
class Helper
{
public:
Helper()
{
counts_.assign(0);
}
void cb0(const MsgConstPtr& msg)
{
++counts_[0];
}
void cb1(const Msg& msg)
{
++counts_[1];
}
void cb2(MsgConstPtr msg)
{
++counts_[2];
}
void cb3(const ros::MessageEvent<Msg const>& evt)
{
++counts_[3];
}
void cb4(Msg msg)
{
++counts_[4];
}
void cb5(const MsgPtr& msg)
{
++counts_[5];
}
void cb6(MsgPtr msg)
{
++counts_[6];
}
void cb7(const ros::MessageEvent<Msg>& evt)
{
++counts_[7];
}
boost::array<int32_t, 30> counts_;
};
TEST(SimpleFilter, callbackTypes)
{
Helper h;
Filter f;
f.registerCallback(boost::bind(&Helper::cb0, &h, _1));
f.registerCallback<const Msg&>(boost::bind(&Helper::cb1, &h, _1));
f.registerCallback<MsgConstPtr>(boost::bind(&Helper::cb2, &h, _1));
f.registerCallback<const ros::MessageEvent<Msg const>&>(boost::bind(&Helper::cb3, &h, _1));
f.registerCallback<Msg>(boost::bind(&Helper::cb4, &h, _1));
f.registerCallback<const MsgPtr&>(boost::bind(&Helper::cb5, &h, _1));
f.registerCallback<MsgPtr>(boost::bind(&Helper::cb6, &h, _1));
f.registerCallback<const ros::MessageEvent<Msg>&>(boost::bind(&Helper::cb7, &h, _1));
f.add(Filter::EventType(MsgPtr(new Msg)));
EXPECT_EQ(h.counts_[0], 1);
EXPECT_EQ(h.counts_[1], 1);
EXPECT_EQ(h.counts_[2], 1);
EXPECT_EQ(h.counts_[3], 1);
EXPECT_EQ(h.counts_[4], 1);
EXPECT_EQ(h.counts_[5], 1);
EXPECT_EQ(h.counts_[6], 1);
EXPECT_EQ(h.counts_[7], 1);
}
struct OldFilter
{
Connection registerCallback(const boost::function<void(const MsgConstPtr&)>& func)
{
return Connection();
}
};
TEST(SimpleFilter, oldRegisterWithNewFilter)
{
OldFilter f;
Helper h;
f.registerCallback(boost::bind(&Helper::cb3, &h, _1));
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "blah");
ros::Time::init();
return RUN_ALL_TESTS();
}

209
core/message_filters/test/test_subscriber.cpp
View File

@ -1,209 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "ros/time.h"
#include "roscpp/Logger.h"
#include "message_filters/subscriber.h"
#include "message_filters/chain.h"
using namespace message_filters;
typedef roscpp::Logger Msg;
typedef roscpp::LoggerPtr MsgPtr;
typedef roscpp::LoggerConstPtr MsgConstPtr;
class Helper
{
public:
Helper()
: count_(0)
{}
void cb(const MsgConstPtr&)
{
++count_;
}
int32_t count_;
};
TEST(Subscriber, simple)
{
ros::NodeHandle nh;
Helper h;
Subscriber<Msg> sub(nh, "test_topic", 0);
sub.registerCallback(boost::bind(&Helper::cb, &h, _1));
ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
ros::Time start = ros::Time::now();
while (h.count_ == 0 && (ros::Time::now() - start) < ros::Duration(1.0))
{
pub.publish(Msg());
ros::Duration(0.01).sleep();
ros::spinOnce();
}
ASSERT_GT(h.count_, 0);
}
TEST(Subscriber, subUnsubSub)
{
ros::NodeHandle nh;
Helper h;
Subscriber<Msg> sub(nh, "test_topic", 0);
sub.registerCallback(boost::bind(&Helper::cb, &h, _1));
ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
sub.unsubscribe();
sub.subscribe();
ros::Time start = ros::Time::now();
while (h.count_ == 0 && (ros::Time::now() - start) < ros::Duration(1.0))
{
pub.publish(Msg());
ros::Duration(0.01).sleep();
ros::spinOnce();
}
ASSERT_GT(h.count_, 0);
}
TEST(Subscriber, subInChain)
{
ros::NodeHandle nh;
Helper h;
Chain<Msg> c;
c.addFilter(boost::shared_ptr<Subscriber<Msg> >(new Subscriber<Msg>(nh, "test_topic", 0)));
c.registerCallback(boost::bind(&Helper::cb, &h, _1));
ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
ros::Time start = ros::Time::now();
while (h.count_ == 0 && (ros::Time::now() - start) < ros::Duration(1.0))
{
pub.publish(Msg());
ros::Duration(0.01).sleep();
ros::spinOnce();
}
ASSERT_GT(h.count_, 0);
}
struct ConstHelper
{
void cb(const MsgConstPtr& msg)
{
msg_ = msg;
}
MsgConstPtr msg_;
};
struct NonConstHelper
{
void cb(const MsgPtr& msg)
{
msg_ = msg;
}
MsgPtr msg_;
};
TEST(Subscriber, singleNonConstCallback)
{
ros::NodeHandle nh;
NonConstHelper h;
Subscriber<Msg> sub(nh, "test_topic", 0);
sub.registerCallback(&NonConstHelper::cb, &h);
ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
MsgPtr msg(new Msg);
pub.publish(msg);
ros::spinOnce();
ASSERT_TRUE(h.msg_);
ASSERT_EQ(msg.get(), h.msg_.get());
}
TEST(Subscriber, multipleNonConstCallbacksFilterSubscriber)
{
ros::NodeHandle nh;
NonConstHelper h, h2;
Subscriber<Msg> sub(nh, "test_topic", 0);
sub.registerCallback(&NonConstHelper::cb, &h);
sub.registerCallback(&NonConstHelper::cb, &h2);
ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
MsgPtr msg(new Msg);
pub.publish(msg);
ros::spinOnce();
ASSERT_TRUE(h.msg_);
ASSERT_TRUE(h2.msg_);
EXPECT_NE(msg.get(), h.msg_.get());
EXPECT_NE(msg.get(), h2.msg_.get());
EXPECT_NE(h.msg_.get(), h2.msg_.get());
}
TEST(Subscriber, multipleCallbacksSomeFilterSomeDirect)
{
ros::NodeHandle nh;
NonConstHelper h, h2;
Subscriber<Msg> sub(nh, "test_topic", 0);
sub.registerCallback(&NonConstHelper::cb, &h);
ros::Subscriber sub2 = nh.subscribe("test_topic", 0, &NonConstHelper::cb, &h2);
ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
MsgPtr msg(new Msg);
pub.publish(msg);
ros::spinOnce();
ASSERT_TRUE(h.msg_);
ASSERT_TRUE(h2.msg_);
EXPECT_NE(msg.get(), h.msg_.get());
EXPECT_NE(msg.get(), h2.msg_.get());
EXPECT_NE(h.msg_.get(), h2.msg_.get());
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_subscriber");
ros::NodeHandle nh;
return RUN_ALL_TESTS();
}

4
core/message_filters/test/test_subscriber.xml
View File

@ -1,4 +0,0 @@
<launch>
<test test-name="test_subscriber" pkg="message_filters" type="test_subscriber"/>
</launch>

462
core/message_filters/test/test_synchronizer.cpp
View File

@ -1,462 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "ros/time.h"
#include <ros/init.h>
#include "message_filters/synchronizer.h"
using namespace message_filters;
struct Header
{
ros::Time stamp;
};
struct Msg
{
Header header;
int data;
};
typedef boost::shared_ptr<Msg> MsgPtr;
typedef boost::shared_ptr<Msg const> MsgConstPtr;
template<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType,
typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType>
struct NullPolicy : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
{
typedef Synchronizer<NullPolicy> Sync;
typedef PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> Super;
typedef typename Super::Messages Messages;
typedef typename Super::Signal Signal;
typedef typename Super::Events Events;
typedef typename Super::RealTypeCount RealTypeCount;
NullPolicy()
{
for (int i = 0; i < RealTypeCount::value; ++i)
{
added_[i] = 0;
}
}
void initParent(Sync* parent)
{
}
template<int i>
void add(const typename mpl::at_c<Events, i>::type& evt)
{
++added_.at(i);
}
boost::array<int32_t, RealTypeCount::value> added_;
};
typedef NullPolicy<Msg, Msg> Policy2;
typedef NullPolicy<Msg, Msg, Msg> Policy3;
typedef NullPolicy<Msg, Msg, Msg, Msg> Policy4;
typedef NullPolicy<Msg, Msg, Msg, Msg, Msg> Policy5;
typedef NullPolicy<Msg, Msg, Msg, Msg, Msg, Msg> Policy6;
typedef NullPolicy<Msg, Msg, Msg, Msg, Msg, Msg, Msg> Policy7;
typedef NullPolicy<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> Policy8;
typedef NullPolicy<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> Policy9;
TEST(Synchronizer, compile2)
{
NullFilter<Msg> f0, f1;
Synchronizer<Policy2> sync(f0, f1);
}
TEST(Synchronizer, compile3)
{
NullFilter<Msg> f0, f1, f2;
Synchronizer<Policy3> sync(f0, f1, f2);
}
TEST(Synchronizer, compile4)
{
NullFilter<Msg> f0, f1, f2, f3;
Synchronizer<Policy4> sync(f0, f1, f2, f3);
}
TEST(Synchronizer, compile5)
{
NullFilter<Msg> f0, f1, f2, f3, f4;
Synchronizer<Policy5> sync(f0, f1, f2, f3, f4);
}
TEST(Synchronizer, compile6)
{
NullFilter<Msg> f0, f1, f2, f3, f4, f5;
Synchronizer<Policy6> sync(f0, f1, f2, f3, f4, f5);
}
TEST(Synchronizer, compile7)
{
NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6;
Synchronizer<Policy7> sync(f0, f1, f2, f3, f4, f5, f6);
}
TEST(Synchronizer, compile8)
{
NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7;
Synchronizer<Policy8> sync(f0, f1, f2, f3, f4, f5, f6, f7);
}
TEST(Synchronizer, compile9)
{
NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7, f8;
Synchronizer<Policy9> sync(f0, f1, f2, f3, f4, f5, f6, f7, f8);
}
void function2(const MsgConstPtr&, const MsgConstPtr&) {}
void function3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const ros::MessageEvent<Msg const>&, const ros::MessageEvent<Msg>&, const MsgConstPtr&) {}
TEST(Synchronizer, compileFunction2)
{
Synchronizer<Policy2> sync;
sync.registerCallback(function2);
}
TEST(Synchronizer, compileFunction3)
{
Synchronizer<Policy3> sync;
sync.registerCallback(function3);
}
TEST(Synchronizer, compileFunction4)
{
Synchronizer<Policy4> sync;
sync.registerCallback(function4);
}
TEST(Synchronizer, compileFunction5)
{
Synchronizer<Policy5> sync;
sync.registerCallback(function5);
}
TEST(Synchronizer, compileFunction6)
{
Synchronizer<Policy6> sync;
sync.registerCallback(function6);
}
TEST(Synchronizer, compileFunction7)
{
Synchronizer<Policy7> sync;
sync.registerCallback(function7);
}
TEST(Synchronizer, compileFunction8)
{
Synchronizer<Policy8> sync;
sync.registerCallback(function8);
}
TEST(Synchronizer, compileFunction9)
{
Synchronizer<Policy9> sync;
sync.registerCallback(function9);
}
struct MethodHelper
{
void method2(const MsgConstPtr&, const MsgConstPtr&) {}
void method3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const ros::MessageEvent<Msg const>&, const ros::MessageEvent<Msg>&) {}
// Can only do 8 here because the object instance counts as a parameter and bind only supports 9
};
TEST(Synchronizer, compileMethod2)
{
MethodHelper h;
Synchronizer<Policy2> sync;
sync.registerCallback(&MethodHelper::method2, &h);
}
TEST(Synchronizer, compileMethod3)
{
MethodHelper h;
Synchronizer<Policy3> sync;
sync.registerCallback(&MethodHelper::method3, &h);
}
TEST(Synchronizer, compileMethod4)
{
MethodHelper h;
Synchronizer<Policy4> sync;
sync.registerCallback(&MethodHelper::method4, &h);
}
TEST(Synchronizer, compileMethod5)
{
MethodHelper h;
Synchronizer<Policy5> sync;
sync.registerCallback(&MethodHelper::method5, &h);
}
TEST(Synchronizer, compileMethod6)
{
MethodHelper h;
Synchronizer<Policy6> sync;
sync.registerCallback(&MethodHelper::method6, &h);
}
TEST(Synchronizer, compileMethod7)
{
MethodHelper h;
Synchronizer<Policy7> sync;
sync.registerCallback(&MethodHelper::method7, &h);
}
TEST(Synchronizer, compileMethod8)
{
MethodHelper h;
Synchronizer<Policy8> sync;
sync.registerCallback(&MethodHelper::method8, &h);
}
TEST(Synchronizer, add2)
{
Synchronizer<Policy2> sync;
MsgPtr m(new Msg);
ASSERT_EQ(sync.added_[0], 0);
sync.add<0>(m);
ASSERT_EQ(sync.added_[0], 1);
ASSERT_EQ(sync.added_[1], 0);
sync.add<1>(m);
ASSERT_EQ(sync.added_[1], 1);
}
TEST(Synchronizer, add3)
{
Synchronizer<Policy3> sync;
MsgPtr m(new Msg);
ASSERT_EQ(sync.added_[0], 0);
sync.add<0>(m);
ASSERT_EQ(sync.added_[0], 1);
ASSERT_EQ(sync.added_[1], 0);
sync.add<1>(m);
ASSERT_EQ(sync.added_[1], 1);
ASSERT_EQ(sync.added_[2], 0);
sync.add<2>(m);
ASSERT_EQ(sync.added_[2], 1);
}
TEST(Synchronizer, add4)
{
Synchronizer<Policy4> sync;
MsgPtr m(new Msg);
ASSERT_EQ(sync.added_[0], 0);
sync.add<0>(m);
ASSERT_EQ(sync.added_[0], 1);
ASSERT_EQ(sync.added_[1], 0);
sync.add<1>(m);
ASSERT_EQ(sync.added_[1], 1);
ASSERT_EQ(sync.added_[2], 0);
sync.add<2>(m);
ASSERT_EQ(sync.added_[2], 1);
ASSERT_EQ(sync.added_[3], 0);
sync.add<3>(m);
ASSERT_EQ(sync.added_[3], 1);
}
TEST(Synchronizer, add5)
{
Synchronizer<Policy5> sync;
MsgPtr m(new Msg);
ASSERT_EQ(sync.added_[0], 0);
sync.add<0>(m);
ASSERT_EQ(sync.added_[0], 1);
ASSERT_EQ(sync.added_[1], 0);
sync.add<1>(m);
ASSERT_EQ(sync.added_[1], 1);
ASSERT_EQ(sync.added_[2], 0);
sync.add<2>(m);
ASSERT_EQ(sync.added_[2], 1);
ASSERT_EQ(sync.added_[3], 0);
sync.add<3>(m);
ASSERT_EQ(sync.added_[3], 1);
ASSERT_EQ(sync.added_[4], 0);
sync.add<4>(m);
ASSERT_EQ(sync.added_[4], 1);
}
TEST(Synchronizer, add6)
{
Synchronizer<Policy6> sync;
MsgPtr m(new Msg);
ASSERT_EQ(sync.added_[0], 0);
sync.add<0>(m);
ASSERT_EQ(sync.added_[0], 1);
ASSERT_EQ(sync.added_[1], 0);
sync.add<1>(m);
ASSERT_EQ(sync.added_[1], 1);
ASSERT_EQ(sync.added_[2], 0);
sync.add<2>(m);
ASSERT_EQ(sync.added_[2], 1);
ASSERT_EQ(sync.added_[3], 0);
sync.add<3>(m);
ASSERT_EQ(sync.added_[3], 1);
ASSERT_EQ(sync.added_[4], 0);
sync.add<4>(m);
ASSERT_EQ(sync.added_[4], 1);
ASSERT_EQ(sync.added_[5], 0);
sync.add<5>(m);
ASSERT_EQ(sync.added_[5], 1);
}
TEST(Synchronizer, add7)
{
Synchronizer<Policy7> sync;
MsgPtr m(new Msg);
ASSERT_EQ(sync.added_[0], 0);
sync.add<0>(m);
ASSERT_EQ(sync.added_[0], 1);
ASSERT_EQ(sync.added_[1], 0);
sync.add<1>(m);
ASSERT_EQ(sync.added_[1], 1);
ASSERT_EQ(sync.added_[2], 0);
sync.add<2>(m);
ASSERT_EQ(sync.added_[2], 1);
ASSERT_EQ(sync.added_[3], 0);
sync.add<3>(m);
ASSERT_EQ(sync.added_[3], 1);
ASSERT_EQ(sync.added_[4], 0);
sync.add<4>(m);
ASSERT_EQ(sync.added_[4], 1);
ASSERT_EQ(sync.added_[5], 0);
sync.add<5>(m);
ASSERT_EQ(sync.added_[5], 1);
ASSERT_EQ(sync.added_[6], 0);
sync.add<6>(m);
ASSERT_EQ(sync.added_[6], 1);
}
TEST(Synchronizer, add8)
{
Synchronizer<Policy8> sync;
MsgPtr m(new Msg);
ASSERT_EQ(sync.added_[0], 0);
sync.add<0>(m);
ASSERT_EQ(sync.added_[0], 1);
ASSERT_EQ(sync.added_[1], 0);
sync.add<1>(m);
ASSERT_EQ(sync.added_[1], 1);
ASSERT_EQ(sync.added_[2], 0);
sync.add<2>(m);
ASSERT_EQ(sync.added_[2], 1);
ASSERT_EQ(sync.added_[3], 0);
sync.add<3>(m);
ASSERT_EQ(sync.added_[3], 1);
ASSERT_EQ(sync.added_[4], 0);
sync.add<4>(m);
ASSERT_EQ(sync.added_[4], 1);
ASSERT_EQ(sync.added_[5], 0);
sync.add<5>(m);
ASSERT_EQ(sync.added_[5], 1);
ASSERT_EQ(sync.added_[6], 0);
sync.add<6>(m);
ASSERT_EQ(sync.added_[6], 1);
ASSERT_EQ(sync.added_[7], 0);
sync.add<7>(m);
ASSERT_EQ(sync.added_[7], 1);
}
TEST(Synchronizer, add9)
{
Synchronizer<Policy9> sync;
MsgPtr m(new Msg);
ASSERT_EQ(sync.added_[0], 0);
sync.add<0>(m);
ASSERT_EQ(sync.added_[0], 1);
ASSERT_EQ(sync.added_[1], 0);
sync.add<1>(m);
ASSERT_EQ(sync.added_[1], 1);
ASSERT_EQ(sync.added_[2], 0);
sync.add<2>(m);
ASSERT_EQ(sync.added_[2], 1);
ASSERT_EQ(sync.added_[3], 0);
sync.add<3>(m);
ASSERT_EQ(sync.added_[3], 1);
ASSERT_EQ(sync.added_[4], 0);
sync.add<4>(m);
ASSERT_EQ(sync.added_[4], 1);
ASSERT_EQ(sync.added_[5], 0);
sync.add<5>(m);
ASSERT_EQ(sync.added_[5], 1);
ASSERT_EQ(sync.added_[6], 0);
sync.add<6>(m);
ASSERT_EQ(sync.added_[6], 1);
ASSERT_EQ(sync.added_[7], 0);
sync.add<7>(m);
ASSERT_EQ(sync.added_[7], 1);
ASSERT_EQ(sync.added_[8], 0);
sync.add<8>(m);
ASSERT_EQ(sync.added_[8], 1);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "blah");
ros::Time::init();
ros::Time::setNow(ros::Time());
return RUN_ALL_TESTS();
}

156
core/message_filters/test/time_sequencer_unittest.cpp
View File

@ -1,156 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "ros/time.h"
#include "message_filters/time_sequencer.h"
using namespace message_filters;
struct Header
{
ros::Time stamp;
};
struct Msg
{
Header header;
int data;
};
typedef boost::shared_ptr<Msg> MsgPtr;
typedef boost::shared_ptr<Msg const> MsgConstPtr;
namespace ros
{
namespace message_traits
{
template<>
struct TimeStamp<Msg>
{
static ros::Time value(const Msg& m)
{
return m.header.stamp;
}
};
}
}
class Helper
{
public:
Helper()
: count_(0)
{}
void cb(const MsgConstPtr&)
{
++count_;
}
int32_t count_;
};
TEST(TimeSequencer, simple)
{
TimeSequencer<Msg> seq(ros::Duration(1.0), ros::Duration(0.01), 10);
Helper h;
seq.registerCallback(boost::bind(&Helper::cb, &h, _1));
MsgPtr msg(new Msg);
msg->header.stamp = ros::Time::now();
seq.add(msg);
ros::WallDuration(0.1).sleep();
ros::spinOnce();
ASSERT_EQ(h.count_, 0);
ros::Time::setNow(ros::Time::now() + ros::Duration(2.0));
ros::WallDuration(0.1).sleep();
ros::spinOnce();
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSequencer, compilation)
{
TimeSequencer<Msg> seq(ros::Duration(1.0), ros::Duration(0.01), 10);
TimeSequencer<Msg> seq2(ros::Duration(1.0), ros::Duration(0.01), 10);
seq2.connectInput(seq);
}
struct EventHelper
{
public:
void cb(const ros::MessageEvent<Msg const>& evt)
{
event_ = evt;
}
ros::MessageEvent<Msg const> event_;
};
TEST(TimeSequencer, eventInEventOut)
{
TimeSequencer<Msg> seq(ros::Duration(1.0), ros::Duration(0.01), 10);
TimeSequencer<Msg> seq2(seq, ros::Duration(1.0), ros::Duration(0.01), 10);
EventHelper h;
seq2.registerCallback(&EventHelper::cb, &h);
ros::MessageEvent<Msg const> evt(MsgConstPtr(new Msg), ros::Time::now());
seq.add(evt);
ros::Time::setNow(ros::Time::now() + ros::Duration(2));
while (!h.event_.getMessage())
{
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
EXPECT_EQ(h.event_.getReceiptTime(), evt.getReceiptTime());
EXPECT_EQ(h.event_.getMessage(), evt.getMessage());
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "time_sequencer_test");
ros::NodeHandle nh;
ros::Time::setNow(ros::Time());
return RUN_ALL_TESTS();
}

4
core/message_filters/test/time_sequencer_unittest.xml
View File

@ -1,4 +0,0 @@
<launch>
<test test-name="time_sequencer" pkg="message_filters" type="time_sequencer_unittest"/>
</launch>

553
core/message_filters/test/time_synchronizer_unittest.cpp
View File

@ -1,553 +0,0 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <gtest/gtest.h>
#include "ros/time.h"
#include "message_filters/time_synchronizer.h"
#include "message_filters/pass_through.h"
#include <ros/init.h>
using namespace message_filters;
struct Header
{
ros::Time stamp;
};
struct Msg
{
Header header;
int data;
};
typedef boost::shared_ptr<Msg> MsgPtr;
typedef boost::shared_ptr<Msg const> MsgConstPtr;
namespace ros
{
namespace message_traits
{
template<>
struct TimeStamp<Msg>
{
static ros::Time value(const Msg& m)
{
return m.header.stamp;
}
};
}
}
class Helper
{
public:
Helper()
: count_(0)
, drop_count_(0)
{}
void cb()
{
++count_;
}
void dropcb()
{
++drop_count_;
}
int32_t count_;
int32_t drop_count_;
};
TEST(TimeSynchronizer, compile2)
{
NullFilter<Msg> f0, f1;
TimeSynchronizer<Msg, Msg> sync(f0, f1, 1);
}
TEST(TimeSynchronizer, compile3)
{
NullFilter<Msg> f0, f1, f2;
TimeSynchronizer<Msg, Msg, Msg> sync(f0, f1, f2, 1);
}
TEST(TimeSynchronizer, compile4)
{
NullFilter<Msg> f0, f1, f2, f3;
TimeSynchronizer<Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, 1);
}
TEST(TimeSynchronizer, compile5)
{
NullFilter<Msg> f0, f1, f2, f3, f4;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, 1);
}
TEST(TimeSynchronizer, compile6)
{
NullFilter<Msg> f0, f1, f2, f3, f4, f5;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, 1);
}
TEST(TimeSynchronizer, compile7)
{
NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, 1);
}
TEST(TimeSynchronizer, compile8)
{
NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, f7, 1);
}
TEST(TimeSynchronizer, compile9)
{
NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7, f8;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, f7, f8, 1);
}
void function2(const MsgConstPtr&, const MsgConstPtr&) {}
void function3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const ros::MessageEvent<Msg const>&, const ros::MessageEvent<Msg>&, const MsgConstPtr&) {}
TEST(TimeSynchronizer, compileFunction2)
{
TimeSynchronizer<Msg, Msg> sync(1);
sync.registerCallback(function2);
}
TEST(TimeSynchronizer, compileFunction3)
{
TimeSynchronizer<Msg, Msg, Msg> sync(1);
sync.registerCallback(function3);
}
TEST(TimeSynchronizer, compileFunction4)
{
TimeSynchronizer<Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(function4);
}
TEST(TimeSynchronizer, compileFunction5)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(function5);
}
TEST(TimeSynchronizer, compileFunction6)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(function6);
}
TEST(TimeSynchronizer, compileFunction7)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(function7);
}
TEST(TimeSynchronizer, compileFunction8)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(function8);
}
TEST(TimeSynchronizer, compileFunction9)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(function9);
}
struct MethodHelper
{
void method2(const MsgConstPtr&, const MsgConstPtr&) {}
void method3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const ros::MessageEvent<Msg const>&, const ros::MessageEvent<Msg>&) {}
// Can only do 8 here because the object instance counts as a parameter and bind only supports 9
};
TEST(TimeSynchronizer, compileMethod2)
{
MethodHelper h;
TimeSynchronizer<Msg, Msg> sync(1);
sync.registerCallback(&MethodHelper::method2, &h);
}
TEST(TimeSynchronizer, compileMethod3)
{
MethodHelper h;
TimeSynchronizer<Msg, Msg, Msg> sync(1);
sync.registerCallback(&MethodHelper::method3, &h);
}
TEST(TimeSynchronizer, compileMethod4)
{
MethodHelper h;
TimeSynchronizer<Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(&MethodHelper::method4, &h);
}
TEST(TimeSynchronizer, compileMethod5)
{
MethodHelper h;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(&MethodHelper::method5, &h);
}
TEST(TimeSynchronizer, compileMethod6)
{
MethodHelper h;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(&MethodHelper::method6, &h);
}
TEST(TimeSynchronizer, compileMethod7)
{
MethodHelper h;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(&MethodHelper::method7, &h);
}
TEST(TimeSynchronizer, compileMethod8)
{
MethodHelper h;
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
sync.registerCallback(&MethodHelper::method8, &h);
}
TEST(TimeSynchronizer, immediate2)
{
TimeSynchronizer<Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSynchronizer, immediate3)
{
TimeSynchronizer<Msg, Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSynchronizer, immediate4)
{
TimeSynchronizer<Msg, Msg, Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 0);
sync.add3(m);
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSynchronizer, immediate5)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 0);
sync.add3(m);
ASSERT_EQ(h.count_, 0);
sync.add4(m);
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSynchronizer, immediate6)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 0);
sync.add3(m);
ASSERT_EQ(h.count_, 0);
sync.add4(m);
ASSERT_EQ(h.count_, 0);
sync.add5(m);
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSynchronizer, immediate7)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 0);
sync.add3(m);
ASSERT_EQ(h.count_, 0);
sync.add4(m);
ASSERT_EQ(h.count_, 0);
sync.add5(m);
ASSERT_EQ(h.count_, 0);
sync.add6(m);
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSynchronizer, immediate8)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 0);
sync.add3(m);
ASSERT_EQ(h.count_, 0);
sync.add4(m);
ASSERT_EQ(h.count_, 0);
sync.add5(m);
ASSERT_EQ(h.count_, 0);
sync.add6(m);
ASSERT_EQ(h.count_, 0);
sync.add7(m);
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSynchronizer, immediate9)
{
TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 0);
sync.add3(m);
ASSERT_EQ(h.count_, 0);
sync.add4(m);
ASSERT_EQ(h.count_, 0);
sync.add5(m);
ASSERT_EQ(h.count_, 0);
sync.add6(m);
ASSERT_EQ(h.count_, 0);
sync.add7(m);
ASSERT_EQ(h.count_, 0);
sync.add8(m);
ASSERT_EQ(h.count_, 1);
}
//////////////////////////////////////////////////////////////////////////////////////////////////
// From here on we assume that testing the 3-message version is sufficient, so as not to duplicate
// tests for everywhere from 2-9
//////////////////////////////////////////////////////////////////////////////////////////////////
TEST(TimeSynchronizer, multipleTimes)
{
TimeSynchronizer<Msg, Msg, Msg> sync(2);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
m.reset(new Msg);
m->header.stamp = ros::Time(0.1);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 1);
}
TEST(TimeSynchronizer, queueSize)
{
TimeSynchronizer<Msg, Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time();
sync.add0(m);
ASSERT_EQ(h.count_, 0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
m.reset(new Msg);
m->header.stamp = ros::Time(0.1);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
m.reset(new Msg);
m->header.stamp = ros::Time(0);
sync.add1(m);
ASSERT_EQ(h.count_, 0);
sync.add2(m);
ASSERT_EQ(h.count_, 0);
}
TEST(TimeSynchronizer, dropCallback)
{
TimeSynchronizer<Msg, Msg> sync(1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
sync.registerDropCallback(boost::bind(&Helper::dropcb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time();
sync.add0(m);
ASSERT_EQ(h.drop_count_, 0);
m->header.stamp = ros::Time(0.1);
sync.add0(m);
ASSERT_EQ(h.drop_count_, 1);
}
struct EventHelper
{
void callback(const ros::MessageEvent<Msg const>& e1, const ros::MessageEvent<Msg const>& e2)
{
e1_ = e1;
e2_ = e2;
}
ros::MessageEvent<Msg const> e1_;
ros::MessageEvent<Msg const> e2_;
};
TEST(TimeSynchronizer, eventInEventOut)
{
TimeSynchronizer<Msg, Msg> sync(2);
EventHelper h;
sync.registerCallback(&EventHelper::callback, &h);
ros::MessageEvent<Msg const> evt(MsgPtr(new Msg), ros::Time(4));
sync.add<0>(evt);
sync.add<1>(evt);
ASSERT_TRUE(h.e1_.getMessage());
ASSERT_TRUE(h.e2_.getMessage());
ASSERT_EQ(h.e1_.getReceiptTime(), evt.getReceiptTime());
ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime());
}
TEST(TimeSynchronizer, connectConstructor)
{
PassThrough<Msg> pt1, pt2;
TimeSynchronizer<Msg, Msg> sync(pt1, pt2, 1);
Helper h;
sync.registerCallback(boost::bind(&Helper::cb, &h));
MsgPtr m(new Msg);
m->header.stamp = ros::Time::now();
pt1.add(m);
ASSERT_EQ(h.count_, 0);
pt2.add(m);
ASSERT_EQ(h.count_, 1);
}
//TEST(TimeSynchronizer, connectToSimple)
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "blah");
ros::Time::init();
ros::Time::setNow(ros::Time());
return RUN_ALL_TESTS();
}