v01

thirdparty/ros/ros_comm/utilities/message_filters/CHANGELOG.rst

@@ -0,0 +1,177 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package message_filters
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.14 (2018-08-23)
+--------------------
+* rename Python message_filters.Cache.getLastestTime to getLatestTime (`#1450 <https://github.com/ros/ros_comm/issues/1450>`_)
+
+1.12.13 (2018-02-21)
+--------------------
+
+1.12.12 (2017-11-16)
+--------------------
+
+1.12.11 (2017-11-07)
+--------------------
+
+1.12.10 (2017-11-06)
+--------------------
+
+1.12.9 (2017-11-06)
+-------------------
+
+1.12.8 (2017-11-06)
+-------------------
+
+1.12.7 (2017-02-17)
+-------------------
+
+1.12.6 (2016-10-26)
+-------------------
+* use boost::bind to bind the callback function (`#906 <https://github.com/ros/ros_comm/pull/906>`_)
+
+1.12.5 (2016-09-30)
+-------------------
+
+1.12.4 (2016-09-19)
+-------------------
+
+1.12.3 (2016-09-17)
+-------------------
+* add fast approximate time synchronization in message_filters (in pure Python) (`#802 <https://github.com/ros/ros_comm/issues/802>`_)
+
+1.12.2 (2016-06-03)
+-------------------
+* allow saving timestamp-less messages to Cache, add getLast method (`#806 <https://github.com/ros/ros_comm/pull/806>`_)
+
+1.12.1 (2016-04-18)
+-------------------
+* use directory specific compiler flags (`#785 <https://github.com/ros/ros_comm/pull/785>`_)
+
+1.12.0 (2016-03-18)
+-------------------
+
+1.11.18 (2016-03-17)
+--------------------
+* fix compiler warnings
+
+1.11.17 (2016-03-11)
+--------------------
+* use boost::make_shared instead of new for constructing boost::shared_ptr (`#740 <https://github.com/ros/ros_comm/issues/740>`_)
+* add __getattr_\_ to handle sub in message_filters as standard one (`#700 <https://github.com/ros/ros_comm/pull/700>`_)
+
+1.11.16 (2015-11-09)
+--------------------
+
+1.11.15 (2015-10-13)
+--------------------
+* add unregister() method to message_filter.Subscriber (`#683 <https://github.com/ros/ros_comm/pull/683>`_)
+
+1.11.14 (2015-09-19)
+--------------------
+
+1.11.13 (2015-04-28)
+--------------------
+
+1.11.12 (2015-04-27)
+--------------------
+
+1.11.11 (2015-04-16)
+--------------------
+* implement message filter cache in Python (`#599 <https://github.com/ros/ros_comm/pull/599>`_)
+
+1.11.10 (2014-12-22)
+--------------------
+
+1.11.9 (2014-08-18)
+-------------------
+
+1.11.8 (2014-08-04)
+-------------------
+
+1.11.7 (2014-07-18)
+-------------------
+
+1.11.6 (2014-07-10)
+-------------------
+
+1.11.5 (2014-06-24)
+-------------------
+
+1.11.4 (2014-06-16)
+-------------------
+* add approximate Python time synchronizer (used to be in camera_calibration) (`#424 <https://github.com/ros/ros_comm/issues/424>`_)
+
+1.11.3 (2014-05-21)
+-------------------
+
+1.11.2 (2014-05-08)
+-------------------
+
+1.11.1 (2014-05-07)
+-------------------
+* update API to use boost::signals2 (`#267 <https://github.com/ros/ros_comm/issues/267>`_)
+
+1.11.0 (2014-03-04)
+-------------------
+* suppress boost::signals deprecation warning (`#362 <https://github.com/ros/ros_comm/issues/362>`_)
+
+1.10.0 (2014-02-11)
+-------------------
+
+1.9.54 (2014-01-27)
+-------------------
+
+1.9.53 (2014-01-14)
+-------------------
+* add kwargs for message_filters.Subscriber
+
+1.9.52 (2014-01-08)
+-------------------
+
+1.9.51 (2014-01-07)
+-------------------
+* update code after refactoring into rosbag_storage and roscpp_core (`#299 <https://github.com/ros/ros_comm/issues/299>`_)
+* fix segmentation fault on OS X 10.9 (clang / libc++)
+
+1.9.50 (2013-10-04)
+-------------------
+
+1.9.49 (2013-09-16)
+-------------------
+
+1.9.48 (2013-08-21)
+-------------------
+
+1.9.47 (2013-07-03)
+-------------------
+* check for CATKIN_ENABLE_TESTING to enable configure without tests
+
+1.9.46 (2013-06-18)
+-------------------
+
+1.9.45 (2013-06-06)
+-------------------
+* fix template syntax for signal\_.template addCallback() to work with Intel compiler
+
+1.9.44 (2013-03-21)
+-------------------
+* fix install destination for dll's under Windows
+
+1.9.43 (2013-03-13)
+-------------------
+* fix exports of message filter symbols for Windows
+
+1.9.42 (2013-03-08)
+-------------------
+
+1.9.41 (2013-01-24)
+-------------------
+
+1.9.40 (2013-01-13)
+-------------------
+
+1.9.39 (2012-12-29)
+-------------------
+* first public release for Groovy

thirdparty/ros/ros_comm/utilities/message_filters/CMakeLists.txt

@@ -0,0 +1,90 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(message_filters)
+
+if(NOT WIN32)
+  set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
+endif()
+
+find_package(catkin REQUIRED COMPONENTS roscpp xmlrpcpp rosconsole)
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES message_filters
+  CATKIN_DEPENDS roscpp xmlrpcpp rosconsole
+)
+catkin_python_setup()
+
+find_package(Boost REQUIRED COMPONENTS signals thread)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+link_directories(${catkin_LIBRARY_DIRS})
+
+add_library(${PROJECT_NAME} src/connection.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h")
+
+if(CATKIN_ENABLE_TESTING)
+  # Ugly workaround for check_test_ran macro issue
+  #add_subdirectory(test)
+  find_package(catkin COMPONENTS rostest rosunit)
+
+  include_directories(${GTEST_INCLUDE_DIRS})
+
+  # ********** Tests **********
+  catkin_add_gtest(${PROJECT_NAME}-msg_cache_unittest test/msg_cache_unittest.cpp)
+  if(TARGET ${PROJECT_NAME}-msg_cache_unittest)
+    target_link_libraries(${PROJECT_NAME}-msg_cache_unittest message_filters ${GTEST_LIBRARIES})
+  endif()
+
+  catkin_add_gtest(${PROJECT_NAME}-time_synchronizer_unittest test/time_synchronizer_unittest.cpp)
+  if(TARGET ${PROJECT_NAME}-time_synchronizer_unittest)
+    target_link_libraries(${PROJECT_NAME}-time_synchronizer_unittest message_filters ${GTEST_LIBRARIES})
+  endif()
+
+  catkin_add_gtest(${PROJECT_NAME}-test_synchronizer test/test_synchronizer.cpp)
+  if(TARGET ${PROJECT_NAME}-test_synchronizer)
+    target_link_libraries(${PROJECT_NAME}-test_synchronizer message_filters ${GTEST_LIBRARIES})
+  endif()
+
+  catkin_add_gtest(${PROJECT_NAME}-test_exact_time_policy test/test_exact_time_policy.cpp)
+  if(TARGET ${PROJECT_NAME}-test_exact_time_policy)
+    target_link_libraries(${PROJECT_NAME}-test_exact_time_policy message_filters ${GTEST_LIBRARIES})
+  endif()
+
+  catkin_add_gtest(${PROJECT_NAME}-test_approximate_time_policy test/test_approximate_time_policy.cpp)
+  if(TARGET ${PROJECT_NAME}-test_approximate_time_policy)
+    target_link_libraries(${PROJECT_NAME}-test_approximate_time_policy message_filters ${GTEST_LIBRARIES})
+  endif()
+
+  catkin_add_gtest(${PROJECT_NAME}-test_simple test/test_simple.cpp)
+  if(TARGET ${PROJECT_NAME}-test_simple)
+    target_link_libraries(${PROJECT_NAME}-test_simple message_filters ${GTEST_LIBRARIES})
+  endif()
+
+  catkin_add_gtest(${PROJECT_NAME}-test_chain test/test_chain.cpp)
+  if(TARGET ${PROJECT_NAME}-test_chain)
+    target_link_libraries(${PROJECT_NAME}-test_chain message_filters ${GTEST_LIBRARIES})
+  endif()
+
+  # Needs to be a rostest because it spins up a node, which blocks until it hears from the master (unfortunately)
+  add_rostest_gtest(${PROJECT_NAME}-time_sequencer_unittest test/time_sequencer_unittest.xml test/time_sequencer_unittest.cpp)
+  if(TARGET ${PROJECT_NAME}-time_sequencer_unittest)
+    target_link_libraries(${PROJECT_NAME}-time_sequencer_unittest message_filters)
+  endif()
+
+  add_rostest_gtest(${PROJECT_NAME}-test_subscriber test/test_subscriber.xml test/test_subscriber.cpp)
+  if(TARGET ${PROJECT_NAME}-test_subscriber)
+    target_link_libraries(${PROJECT_NAME}-test_subscriber message_filters)
+  endif()
+
+  # Unit test of the approximate synchronizer
+  catkin_add_nosetests(test/test_approxsync.py)
+  catkin_add_nosetests(test/test_message_filters_cache.py)
+endif()

thirdparty/ros/ros_comm/utilities/message_filters/conf.py

@@ -0,0 +1,203 @@
+# -*- 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 sys
+import 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 = []
+
+exclude_patterns = ['CHANGELOG.rst']
+
+# 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 = []
+
+# 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://docs.scipy.org/doc/numpy' : None
+    }

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/cache.h

@@ -0,0 +1,343 @@
+/*********************************************************************
+* 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 "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);
+
+    }
+
+    this->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 ;
+  }
+
+  /**
+   * \brief Returns the timestamp associated with the oldest packet cache
+   */
+  ros::Time getOldestTime() const
+  {
+    namespace mt = ros::message_traits;
+
+    boost::mutex::scoped_lock lock(cache_lock_);
+
+    ros::Time oldest_time;
+
+    if (cache_.size() > 0)
+      oldest_time = mt::TimeStamp<M>::value(*cache_.front().getMessage());
+
+    return oldest_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_ */

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/chain.h

@@ -0,0 +1,255 @@
+/*********************************************************************
+* 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::make_shared<PassThrough<Msg> >());
+c.addFilter(boost::make_shared<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 = boost::make_shared<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)
+  {
+    this->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

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/connection.h

@@ -0,0 +1,72 @@
+/*********************************************************************
+* 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/signals2/connection.hpp>
+#include "macros.h"
+
+namespace message_filters
+{
+
+/**
+ * \brief Encapsulates a connection from one filter to another (or to a user-specified callback)
+ */
+class MESSAGE_FILTERS_DECL 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::signals2::connection conn);
+
+  /**
+   * \brief disconnects this connection
+   */
+  void disconnect();
+
+  boost::signals2::connection getBoostConnection() const { return connection_; }
+
+private:
+  VoidDisconnectFunction void_disconnect_;
+  WithConnectionDisconnectFunction connection_disconnect_;
+  boost::signals2::connection connection_;
+};
+
+}
+
+#endif // MESSAGE_FILTERS_CONNECTION_H

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/macros.h

@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2008, Willow Garage, Inc.
+ *
+ * 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 names of Stanford University or 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.
+ */
+
+#ifndef MESSAGE_FILTERS_MACROS_H_
+#define MESSAGE_FILTERS_MACROS_H_
+
+#include <ros/macros.h> // for the DECL's
+
+// Import/export for windows dll's and visibility for gcc shared libraries.
+
+#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
+  #ifdef message_filters_EXPORTS // we are building a shared lib/dll
+    #define MESSAGE_FILTERS_DECL ROS_HELPER_EXPORT
+  #else // we are using shared lib/dll
+    #define MESSAGE_FILTERS_DECL ROS_HELPER_IMPORT
+  #endif
+#else // ros is being built around static libraries
+  #define MESSAGE_FILTERS_DECL
+#endif
+
+#endif /* MESSAGE_FILTERS_MACROS_H_ */

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/null_types.h

@@ -0,0 +1,85 @@
+/*********************************************************************
+* 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>
+
+#include <ros/message_traits.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&)
+  {
+    return Connection();
+  }
+
+  template<typename P>
+  Connection registerCallback(const boost::function<void(P)>&)
+  {
+    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

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/pass_through.h

@@ -0,0 +1,93 @@
+/*********************************************************************
+* 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)
+  {
+    this->signalMessage(evt);
+  }
+
+private:
+  void cb(const EventType& evt)
+  {
+    add(evt);
+  }
+
+  Connection incoming_connection_;
+};
+
+} // namespace message_filters
+
+#endif // MESSAGE_FILTERS_PASSTHROUGH_H
+

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/signal1.h

@@ -0,0 +1,132 @@
+/*********************************************************************
+* 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
+
+

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/signal9.h

@@ -0,0 +1,317 @@
+/*********************************************************************
+* 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
+
+

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/simple_filter.h

@@ -0,0 +1,150 @@
+/*********************************************************************
+* 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))
+  {
+    typename CallbackHelper1<M>::Ptr helper = signal_.template addCallback<P>(boost::bind(callback, _1));
+    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 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
+

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/subscriber.h

@@ -0,0 +1,216 @@
+/*********************************************************************
+* 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/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)
+  {
+    (void)f;
+  }
+
+  /**
+   * \brief Does nothing.  Provided so that Subscriber may be used in a message_filters::Chain
+   */
+  void add(const EventType& e)
+  {
+    (void)e;
+  }
+
+private:
+
+  void cb(const EventType& e)
+  {
+    this->signalMessage(e);
+  }
+
+  ros::Subscriber sub_;
+  ros::SubscribeOptions ops_;
+  ros::NodeHandle nh_;
+};
+
+}
+
+#endif

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/sync_policies/approximate_time.h

@@ -0,0 +1,857 @@
+/*********************************************************************
+* 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/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 <ros/assert.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();
+    time = mt::TimeStamp<M0>::value(*m0.getMessage());
+    index = 0;
+    if (RealTypeCount::value > 1)
+    {
+      M1Event& m1 = boost::get<1>(deques_).front();
+      if ((mt::TimeStamp<M1>::value(*m1.getMessage()) < time) ^ end)
+      {
+        time = mt::TimeStamp<M1>::value(*m1.getMessage());
+        index = 1;
+      }
+    }
+    if (RealTypeCount::value > 2)
+    {
+      M2Event& m2 = boost::get<2>(deques_).front();
+      if ((mt::TimeStamp<M2>::value(*m2.getMessage()) < time) ^ end)
+      {
+        time = mt::TimeStamp<M2>::value(*m2.getMessage());
+        index = 2;
+      }
+    }
+    if (RealTypeCount::value > 3)
+    {
+      M3Event& m3 = boost::get<3>(deques_).front();
+      if ((mt::TimeStamp<M3>::value(*m3.getMessage()) < time) ^ end)
+      {
+        time = mt::TimeStamp<M3>::value(*m3.getMessage());
+        index = 3;
+      }
+    }
+    if (RealTypeCount::value > 4)
+    {
+      M4Event& m4 = boost::get<4>(deques_).front();
+      if ((mt::TimeStamp<M4>::value(*m4.getMessage()) < time) ^ end)
+      {
+        time = mt::TimeStamp<M4>::value(*m4.getMessage());
+        index = 4;
+      }
+    }
+    if (RealTypeCount::value > 5)
+    {
+      M5Event& m5 = boost::get<5>(deques_).front();
+      if ((mt::TimeStamp<M5>::value(*m5.getMessage()) < time) ^ end)
+      {
+        time = mt::TimeStamp<M5>::value(*m5.getMessage());
+        index = 5;
+      }
+    }
+    if (RealTypeCount::value > 6)
+    {
+      M6Event& m6 = boost::get<6>(deques_).front();
+      if ((mt::TimeStamp<M6>::value(*m6.getMessage()) < time) ^ end)
+      {
+        time = mt::TimeStamp<M6>::value(*m6.getMessage());
+        index = 6;
+      }
+    }
+    if (RealTypeCount::value > 7)
+    {
+      M7Event& m7 = boost::get<7>(deques_).front();
+      if ((mt::TimeStamp<M7>::value(*m7.getMessage()) < time) ^ end)
+      {
+        time = mt::TimeStamp<M7>::value(*m7.getMessage());
+        index = 7;
+      }
+    }
+    if (RealTypeCount::value > 8)
+    {
+      M8Event& m8 = boost::get<8>(deques_).front();
+      if ((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]);
+            (void)num_non_empty_deques_before_virtual_search; // unused variable warning stopper
+            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
+

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/sync_policies/exact_time.h

@@ -0,0 +1,244 @@
+/*********************************************************************
+* 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/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 <ros/assert.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
+

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/synchronizer.h

@@ -0,0 +1,393 @@
+/*********************************************************************
+* 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/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 "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_.addCallback(callback);
+  }
+
+  template<class C>
+  Connection registerCallback(const C& callback)
+  {
+    return signal_.addCallback(callback);
+  }
+
+  template<class C, typename T>
+  Connection registerCallback(const C& callback, T* t)
+  {
+    return signal_.addCallback(callback, t);
+  }
+
+  template<class C, typename T>
+  Connection registerCallback(C& callback, T* t)
+  {
+    return signal_.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)
+  {
+    this->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

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/time_sequencer.h

@@ -0,0 +1,243 @@
+/*********************************************************************
+* 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)
+      {
+        this->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

thirdparty/ros/ros_comm/utilities/message_filters/include/message_filters/time_synchronizer.h

@@ -0,0 +1,228 @@
+/*********************************************************************
+* 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

thirdparty/ros/ros_comm/utilities/message_filters/index.rst

@@ -0,0 +1,73 @@
+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`
+

thirdparty/ros/ros_comm/utilities/message_filters/mainpage.dox

@@ -0,0 +1,59 @@
+/**
+\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.
+
+*/

thirdparty/ros/ros_comm/utilities/message_filters/package.xml

@@ -0,0 +1,31 @@
+<package>
+  <name>message_filters</name>
+  <version>1.12.14</version>
+  <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>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>BSD</license>
+
+  <url>http://ros.org/wiki/message_filters</url>
+
+  <author>Josh Faust</author>
+  <author>Vijay Pradeep</author>
+
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>boost</build_depend>
+  <build_depend>rosconsole</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rostest</build_depend>
+  <build_depend>rosunit</build_depend>
+  <build_depend>xmlrpcpp</build_depend>
+
+  <run_depend>rosconsole</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>xmlrpcpp</run_depend>
+
+  <export>
+    <rosdoc config="rosdoc.yaml"/>
+  </export>
+</package>

thirdparty/ros/ros_comm/utilities/message_filters/rosdoc.yaml

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

thirdparty/ros/ros_comm/utilities/message_filters/setup.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['message_filters'],
+    package_dir={'': 'src'},
+    requires=['genmsg', 'genpy', 'roslib', 'rospkg']
+)
+
+setup(**d)

thirdparty/ros/ros_comm/utilities/message_filters/src/connection.cpp

@@ -0,0 +1,64 @@
+/*********************************************************************
+* 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 "message_filters/connection.h"
+
+namespace message_filters
+{
+
+
+Connection::Connection(const VoidDisconnectFunction& func)
+: void_disconnect_(func)
+{
+}
+
+Connection::Connection(const WithConnectionDisconnectFunction& func, boost::signals2::connection c)
+: connection_disconnect_(func)
+, connection_(c)
+{
+}
+
+void Connection::disconnect()
+{
+  if (void_disconnect_)
+  {
+    void_disconnect_();
+  }
+  else if (connection_disconnect_)
+  {
+    connection_disconnect_(*this);
+  }
+}
+
+}

thirdparty/ros/ros_comm/utilities/message_filters/src/message_filters/__init__.py

@@ -0,0 +1,295 @@
+# 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 itertools
+import threading
+import rospy
+
+
+class SimpleFilter(object):
+
+    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, **kwargs):
+        SimpleFilter.__init__(self)
+        self.topic = args[0]
+        kwargs['callback'] = self.callback
+        self.sub = rospy.Subscriber(*args, **kwargs)
+
+    def callback(self, msg):
+        self.signalMessage(msg)
+
+    def getTopic(self):
+        return self.topic
+
+    def __getattr__(self, key):
+        """Serve same API as rospy.Subscriber"""
+        return self.sub.__getattribute__(key)
+
+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. The ``allow_headerless``
+    option specifies whether to allow storing headerless messages with
+    current ROS time instead of timestamp. You should avoid this as
+    much as you can, since the delays are unpredictable.
+    """
+
+    def __init__(self, f, cache_size=1, allow_headerless=False):
+        SimpleFilter.__init__(self)
+        self.connectInput(f)
+        self.cache_size = cache_size
+        # Array to store messages
+        self.cache_msgs = []
+        # Array to store msgs times, auxiliary structure to facilitate
+        # sorted insertion
+        self.cache_times = []
+        # Whether to allow storing headerless messages with current ROS
+        # time instead of timestamp.
+        self.allow_headerless = allow_headerless
+
+    def connectInput(self, f):
+        self.incoming_connection = f.registerCallback(self.add)
+
+    def add(self, msg):
+        if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
+            if not self.allow_headerless:
+                rospy.logwarn("Cannot use message filters with non-stamped messages. "
+                              "Use the 'allow_headerless' constructor option to "
+                              "auto-assign ROS time to headerless messages.")
+                return
+            stamp = rospy.Time.now()
+        else:
+            stamp = msg.header.stamp
+
+        # Insert sorted
+        self.cache_times.append(stamp)
+        self.cache_msgs.append(msg)
+
+        # Implement a ring buffer, discard older if oversized
+        if (len(self.cache_msgs) > self.cache_size):
+            del self.cache_msgs[0]
+            del self.cache_times[0]
+
+        # Signal new input
+        self.signalMessage(msg)
+
+    def getInterval(self, from_stamp, to_stamp):
+        """Query the current cache content between from_stamp to to_stamp."""
+        assert from_stamp <= to_stamp
+
+        return [msg for (msg, time) in zip(self.cache_msgs, self.cache_times)
+                if from_stamp <= time <= to_stamp]
+
+    def getElemAfterTime(self, stamp):
+        """Return the oldest element after or equal the passed time stamp."""
+        newer = [msg for (msg, time) in zip(self.cache_msgs, self.cache_times)
+                 if time >= stamp]
+        if not newer:
+            return None
+        return newer[0]
+
+    def getElemBeforeTime(self, stamp):
+        """Return the newest element before or equal the passed time stamp."""
+        older = [msg for (msg, time) in zip(self.cache_msgs, self.cache_times)
+                 if time <= stamp]
+        if not older:
+            return None
+        return older[-1]
+
+    def getLatestTime(self):
+        """Return the newest recorded timestamp."""
+        if not self.cache_times:
+            return None
+        return self.cache_times[-1]
+
+    def getLastestTime(self):
+        """Return the newest recorded timestamp (equivalent to `getLatestTime()`, but included for backwards compatibility)."""
+        return self.getLatestTime()
+
+    def getOldestTime(self):
+        """Return the oldest recorded timestamp."""
+        if not self.cache_times:
+            return None
+        return self.cache_times[0]
+        
+    def getLast(self):
+        if self.getLastestTime() is None:
+            return None
+        return self.getElemAfterTime(self.getLastestTime())
+
+
+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, i_q)
+            for i_q, (f, q) in enumerate(zip(fs, self.queues))]
+
+    def add(self, msg, my_queue, my_queue_index=None):
+        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()
+
+class ApproximateTimeSynchronizer(TimeSynchronizer):
+
+    """
+    Approximately synchronizes messages by their timestamps.
+
+    :class:`ApproximateTimeSynchronizer` synchronizes incoming message filters by the
+    timestamps contained in their messages' headers. The API is the same as TimeSynchronizer
+    except for an extra `slop` parameter in the constructor that defines the delay (in seconds)
+    with which messages can be synchronized. The ``allow_headerless`` option specifies whether
+    to allow storing headerless messages with current ROS time instead of timestamp. You should
+    avoid this as much as you can, since the delays are unpredictable.
+    """
+
+    def __init__(self, fs, queue_size, slop, allow_headerless=False):
+        TimeSynchronizer.__init__(self, fs, queue_size)
+        self.slop = rospy.Duration.from_sec(slop)
+        self.allow_headerless = allow_headerless
+
+    def add(self, msg, my_queue, my_queue_index=None):
+        if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
+            if not self.allow_headerless:
+                rospy.logwarn("Cannot use message filters with non-stamped messages. "
+                              "Use the 'allow_headerless' constructor option to "
+                              "auto-assign ROS time to headerless messages.")
+                return
+            stamp = rospy.Time.now()
+        else:
+            stamp = msg.header.stamp
+
+        self.lock.acquire()
+        my_queue[stamp] = msg
+        while len(my_queue) > self.queue_size:
+            del my_queue[min(my_queue)]
+        # self.queues = [topic_0 {stamp: msg}, topic_1 {stamp: msg}, ...]
+        if my_queue_index is None:
+            search_queues = self.queues
+        else:
+            search_queues = self.queues[:my_queue_index] + \
+                self.queues[my_queue_index+1:]
+        # sort and leave only reasonable stamps for synchronization
+        stamps = []
+        for queue in search_queues:
+            topic_stamps = []
+            for s in queue:
+                stamp_delta = abs(s - stamp)
+                if stamp_delta > self.slop:
+                    continue  # far over the slop
+                topic_stamps.append((s, stamp_delta))
+            if not topic_stamps:
+                self.lock.release()
+                return
+            topic_stamps = sorted(topic_stamps, key=lambda x: x[1])
+            stamps.append(topic_stamps)
+        for vv in itertools.product(*[zip(*s)[0] for s in stamps]):
+            vv = list(vv)
+            # insert the new message
+            if my_queue_index is not None:
+                vv.insert(my_queue_index, stamp)
+            qt = list(zip(self.queues, vv))
+            if ( ((max(vv) - min(vv)) < self.slop) and
+                (len([1 for q,t in qt if t not in q]) == 0) ):
+                msgs = [q[t] for q,t in qt]
+                self.signalMessage(*msgs)
+                for q,t in qt:
+                    del q[t]
+                break  # fast finish after the synchronization
+        self.lock.release()

thirdparty/ros/ros_comm/utilities/message_filters/test/directed.py

@@ -0,0 +1,82 @@
+#  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 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)

thirdparty/ros/ros_comm/utilities/message_filters/test/msg_cache_unittest.cpp

@@ -0,0 +1,225 @@
+/*********************************************************************
+* 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(boost::make_shared<Msg const>(), 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();
+}
+

thirdparty/ros/ros_comm/utilities/message_filters/test/test_approximate_time_policy.cpp

@@ -0,0 +1,543 @@
+
+/*********************************************************************
+* 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(boost::make_shared<Msg>());
+        p->header.stamp = input_[i].first;
+        sync_.add<0>(p);
+      }
+      else
+      {
+        MsgPtr q(boost::make_shared<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(boost::make_shared<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();
+}

thirdparty/ros/ros_comm/utilities/message_filters/test/test_approxsync.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python
+#
+# 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.
+
+import rostest
+import rospy
+import unittest
+import random
+
+import message_filters
+from message_filters import ApproximateTimeSynchronizer
+
+class MockHeader:
+    pass
+
+class MockMessage:
+    def __init__(self, stamp, data):
+        self.header = MockHeader()
+        self.header.stamp = stamp
+        self.data = data
+
+class MockHeaderlessMessage:
+    def __init__(self, data):
+        self.data = data
+
+class MockFilter(message_filters.SimpleFilter):
+    pass
+
+class TestApproxSync(unittest.TestCase):
+
+    def cb_collector_2msg(self, msg1, msg2):
+        self.collector.append((msg1, msg2))
+
+    def test_approx(self):
+        m0 = MockFilter()
+        m1 = MockFilter()
+        ts = ApproximateTimeSynchronizer([m0, m1], 1, 0.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(rospy.Time(t), random.random()) for t in range(N)]
+            seq1 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
+            # random.shuffle(seq0)
+            ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1)
+            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)))
+
+        # Scramble sequences of length N of headerless and header-having messages.
+        # Make sure that TimeSequencer recombines them.
+        rospy.rostime.set_rostime_initialized(True)
+        random.seed(0)
+        for N in range(1, 10):
+            m0 = MockFilter()
+            m1 = MockFilter()
+            seq0 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
+            seq1 = [MockHeaderlessMessage(random.random()) for t in range(N)]
+            # random.shuffle(seq0)
+            ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1, allow_headerless=True)
+            ts.registerCallback(self.cb_collector_2msg)
+            self.collector = []
+            for msg in random.sample(seq0, N):
+                m0.signalMessage(msg)
+            self.assertEqual(self.collector, [])
+            for i in random.sample(range(N), N):
+                msg = seq1[i]
+                rospy.rostime._set_rostime(rospy.Time(i+0.05))
+                m1.signalMessage(msg)
+            self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
+
+if __name__ == '__main__':
+    if 1:
+        rostest.unitrun('camera_calibration', 'testapproxsync', TestApproxSync)
+    else:
+        suite = unittest.TestSuite()
+        suite.addTest(TestApproxSync('test_approx'))
+        unittest.TextTestRunner(verbosity=2).run(suite)

thirdparty/ros/ros_comm/utilities/message_filters/test/test_chain.cpp

@@ -0,0 +1,188 @@
+/*********************************************************************
+* 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(boost::make_shared<PassThrough<Msg> >());
+  c.registerCallback(boost::bind(&Helper::cb, &h));
+
+  c.add(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 1);
+  c.add(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 2);
+}
+
+TEST(Chain, multipleFilters)
+{
+  Helper h;
+  Chain<Msg> c;
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+  c.registerCallback(boost::bind(&Helper::cb, &h));
+
+  c.add(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 1);
+  c.add(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 2);
+}
+
+TEST(Chain, addingFilters)
+{
+  Helper h;
+  Chain<Msg> c;
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+  c.registerCallback(boost::bind(&Helper::cb, &h));
+
+  c.add(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 1);
+
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+
+  c.add(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 2);
+}
+
+TEST(Chain, inputFilter)
+{
+  Helper h;
+  Chain<Msg> c;
+  c.addFilter(boost::make_shared<PassThrough<Msg> >());
+  c.registerCallback(boost::bind(&Helper::cb, &h));
+
+  PassThrough<Msg> p;
+  c.connectInput(p);
+  p.add(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 1);
+
+  p.add(boost::make_shared<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(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 1);
+  c.add(boost::make_shared<Msg>());
+  EXPECT_EQ(h.count_, 2);
+}
+
+TEST(Chain, retrieveFilter)
+{
+  Chain<Msg> c;
+
+  ASSERT_FALSE(c.getFilter<PassThrough<Msg> >(0));
+
+  c.addFilter(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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();
+}
+
+

thirdparty/ros/ros_comm/utilities/message_filters/test/test_exact_time_policy.cpp

@@ -0,0 +1,206 @@
+/*********************************************************************
+* 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(boost::make_shared<Msg>());
+  m->header.stamp = ros::Time();
+
+  sync.add<0>(m);
+  ASSERT_EQ(h.count_, 0);
+
+  m = boost::make_shared<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(boost::make_shared<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 = boost::make_shared<Msg>();
+  m->header.stamp = ros::Time(0.1);
+  sync.add<1>(m);
+  ASSERT_EQ(h.count_, 0);
+
+  m = boost::make_shared<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(boost::make_shared<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(boost::make_shared<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();
+}
+
+

thirdparty/ros/ros_comm/utilities/message_filters/test/test_message_filters_cache.py

@@ -0,0 +1,164 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright 2015 Martin Llofriu, Open Source Robotics Foundation, 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.
+
+import rospy
+import unittest
+
+from std_msgs.msg import String
+
+from message_filters import Cache, Subscriber
+
+PKG = 'message_filters'
+
+
+class AnonymMsg:
+    class AnonymHeader:
+        stamp = None
+
+        def __init__(self):
+            self.stamp = rospy.Time()
+
+    header = None
+
+    def __init__(self):
+        self.header = AnonymMsg.AnonymHeader()
+
+
+class TestCache(unittest.TestCase):
+
+    def test_all_funcs(self):
+        sub = Subscriber("/empty", String)
+        cache = Cache(sub, 5)
+
+        msg = AnonymMsg()
+        msg.header.stamp = rospy.Time(0)
+        cache.add(msg)
+
+        msg = AnonymMsg()
+        msg.header.stamp = rospy.Time(1)
+        cache.add(msg)
+
+        msg = AnonymMsg()
+        msg.header.stamp = rospy.Time(2)
+        cache.add(msg)
+
+        msg = AnonymMsg()
+        msg.header.stamp = rospy.Time(3)
+        cache.add(msg)
+
+        msg = AnonymMsg()
+        msg.header.stamp = rospy.Time(4)
+        cache.add(msg)
+
+        l = len(cache.getInterval(rospy.Time(2.5),
+                                  rospy.Time(3.5)))
+        self.assertEquals(l, 1, "invalid number of messages" +
+                                " returned in getInterval 1")
+
+        l = len(cache.getInterval(rospy.Time(2),
+                                  rospy.Time(3)))
+        self.assertEquals(l, 2, "invalid number of messages" +
+                                " returned in getInterval 2")
+
+        l = len(cache.getInterval(rospy.Time(0),
+                                  rospy.Time(4)))
+        self.assertEquals(l, 5, "invalid number of messages" +
+                                " returned in getInterval 3")
+
+        s = cache.getElemAfterTime(rospy.Time(0)).header.stamp
+        self.assertEqual(s, rospy.Time(0),
+                         "invalid msg return by getElemAfterTime")
+
+        s = cache.getElemBeforeTime(rospy.Time(3.5)).header.stamp
+        self.assertEqual(s, rospy.Time(3),
+                         "invalid msg return by getElemBeforeTime")
+
+        s = cache.getLatestTime()
+        self.assertEqual(s, rospy.Time(4),
+                         "invalid stamp return by getLatestTime")
+        self.assertEqual(s, cache.getLastestTime(),
+                         "stamps returned by getLatestTime and getLastestTime don't match")
+
+        s = cache.getOldestTime()
+        self.assertEqual(s, rospy.Time(0),
+                         "invalid stamp return by getOldestTime")
+
+        # Add another msg to fill the buffer
+        msg = AnonymMsg()
+        msg.header.stamp = rospy.Time(5)
+        cache.add(msg)
+
+        # Test that it discarded the right one
+        s = cache.getOldestTime()
+        self.assertEqual(s, rospy.Time(1),
+                         "wrong message discarded")
+
+    def test_headerless(self):
+        sub = Subscriber("/empty", String)
+        cache = Cache(sub, 5, allow_headerless=False)
+
+        msg = String()
+        cache.add(msg)
+
+        self.assertIsNone(cache.getElemAfterTime(rospy.Time(0)),
+                          "Headerless message invalidly added.")
+
+        cache = Cache(sub, 5, allow_headerless=True)
+
+        rospy.rostime.set_rostime_initialized(True)
+
+        rospy.rostime._set_rostime(rospy.Time(0))
+        cache.add(msg)
+
+        s = cache.getElemAfterTime(rospy.Time(0))
+        self.assertEqual(s, msg,
+                         "invalid msg returned in headerless scenario")
+
+        s = cache.getElemAfterTime(rospy.Time(1))
+        self.assertIsNone(s, "invalid msg returned in headerless scenario")
+
+        rospy.rostime._set_rostime(rospy.Time(2))
+        cache.add(msg)
+
+        s = cache.getInterval(rospy.Time(0), rospy.Time(1))
+        self.assertEqual(s, [msg],
+                         "invalid msg returned in headerless scenario")
+
+        s = cache.getInterval(rospy.Time(0), rospy.Time(2))
+        self.assertEqual(s, [msg, msg],
+                         "invalid msg returned in headerless scenario")
+
+
+if __name__ == '__main__':
+    import rosunit
+    rosunit.unitrun(PKG, 'test_message_filters_cache', TestCache)

thirdparty/ros/ros_comm/utilities/message_filters/test/test_simple.cpp

@@ -0,0 +1,158 @@
+/*********************************************************************
+* 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&)
+  {
+    ++counts_[0];
+  }
+
+  void cb1(const Msg&)
+  {
+    ++counts_[1];
+  }
+
+  void cb2(MsgConstPtr)
+  {
+    ++counts_[2];
+  }
+
+  void cb3(const ros::MessageEvent<Msg const>&)
+  {
+    ++counts_[3];
+  }
+
+  void cb4(Msg)
+  {
+    ++counts_[4];
+  }
+
+  void cb5(const MsgPtr&)
+  {
+    ++counts_[5];
+  }
+
+  void cb6(MsgPtr)
+  {
+    ++counts_[6];
+  }
+
+  void cb7(const ros::MessageEvent<Msg>&)
+  {
+    ++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(boost::make_shared<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&)>&)
+  {
+    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();
+}
+
+
+

thirdparty/ros/ros_comm/utilities/message_filters/test/test_subscriber.cpp

@@ -0,0 +1,209 @@
+/*********************************************************************
+* 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::make_shared<Subscriber<Msg> >(boost::ref(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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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();
+}
+
+

thirdparty/ros/ros_comm/utilities/message_filters/test/test_subscriber.xml

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

thirdparty/ros/ros_comm/utilities/message_filters/test/test_synchronizer.cpp

@@ -0,0 +1,463 @@
+/*********************************************************************
+* 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"
+#include <boost/array.hpp>
+
+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*)
+  {
+  }
+
+  template<int i>
+  void add(const typename mpl::at_c<Events, i>::type&)
+  {
+    ++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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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();
+}
+
+

thirdparty/ros/ros_comm/utilities/message_filters/test/time_sequencer_unittest.cpp

@@ -0,0 +1,156 @@
+/*********************************************************************
+* 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(boost::make_shared<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(boost::make_shared<Msg const>(), 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();
+}
+
+

thirdparty/ros/ros_comm/utilities/message_filters/test/time_sequencer_unittest.xml

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

thirdparty/ros/ros_comm/utilities/message_filters/test/time_synchronizer_unittest.cpp

@@ -0,0 +1,553 @@
+/*********************************************************************
+* 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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<Msg>());
+  m->header.stamp = ros::Time();
+
+  sync.add0(m);
+  ASSERT_EQ(h.count_, 0);
+
+  m = boost::make_shared<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(boost::make_shared<Msg>());
+  m->header.stamp = ros::Time();
+
+  sync.add0(m);
+  ASSERT_EQ(h.count_, 0);
+  sync.add1(m);
+  ASSERT_EQ(h.count_, 0);
+
+  m = boost::make_shared<Msg>();
+  m->header.stamp = ros::Time(0.1);
+  sync.add1(m);
+  ASSERT_EQ(h.count_, 0);
+
+  m = boost::make_shared<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(boost::make_shared<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(boost::make_shared<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(boost::make_shared<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();
+}
+
+
+

thirdparty/ros/ros_comm/utilities/roslz4/CHANGELOG.rst

@@ -0,0 +1,114 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package roslz4
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.14 (2018-08-23)
+--------------------
+
+1.12.13 (2018-02-21)
+--------------------
+* adding decompress to free(state) before return (`#1313 <https://github.com/ros/ros_comm/issues/1313>`_)
+
+1.12.12 (2017-11-16)
+--------------------
+
+1.12.11 (2017-11-07)
+--------------------
+* revert replace deprecated lz4 function call (`#1220 <https://github.com/ros/ros_comm/issues/1220>`_, regression from 1.12.8 on Debian Jessie)
+
+1.12.10 (2017-11-06)
+--------------------
+
+1.12.9 (2017-11-06)
+-------------------
+
+1.12.8 (2017-11-06)
+-------------------
+* replace deprecated lz4 function call (`#1136 <https://github.com/ros/ros_comm/issues/1136>`_)
+
+1.12.7 (2017-02-17)
+-------------------
+
+1.12.6 (2016-10-26)
+-------------------
+
+1.12.5 (2016-09-30)
+-------------------
+
+1.12.4 (2016-09-19)
+-------------------
+
+1.12.3 (2016-09-17)
+-------------------
+* set lz4_FOUND in order to continue using it with catkin_package(DEPENDS) (`ros/catkin#813 <https://github.com/ros/catkin/issues/813>`_)
+
+1.12.2 (2016-06-03)
+-------------------
+
+1.12.1 (2016-04-18)
+-------------------
+* use directory specific compiler flags (`#785 <https://github.com/ros/ros_comm/pull/785>`_)
+
+1.12.0 (2016-03-18)
+-------------------
+
+1.11.18 (2016-03-17)
+--------------------
+* fix compiler warnings
+
+1.11.17 (2016-03-11)
+--------------------
+
+1.11.16 (2015-11-09)
+--------------------
+
+1.11.15 (2015-10-13)
+--------------------
+
+1.11.14 (2015-09-19)
+--------------------
+
+1.11.13 (2015-04-28)
+--------------------
+
+1.11.12 (2015-04-27)
+--------------------
+
+1.11.11 (2015-04-16)
+--------------------
+* fix import of compiled library with Python 3.x (`#563 <https://github.com/ros/ros_comm/pull/563>`_)
+
+1.11.10 (2014-12-22)
+--------------------
+* disable lz4 Python bindings on Android (`#521 <https://github.com/ros/ros_comm/pull/521>`_)
+
+1.11.9 (2014-08-18)
+-------------------
+
+1.11.8 (2014-08-04)
+-------------------
+
+1.11.7 (2014-07-18)
+-------------------
+
+1.11.6 (2014-07-10)
+-------------------
+* fix finding specific version of PythonLibs with CMake 3
+
+1.11.5 (2014-06-24)
+-------------------
+
+1.11.4 (2014-06-16)
+-------------------
+
+1.11.3 (2014-05-21)
+-------------------
+
+1.11.2 (2014-05-08)
+-------------------
+* fix symbol problem on OSX (`#405 <https://github.com/ros/ros_comm/issues/405>`_)
+* fix return value in the Python module (`#406 <https://github.com/ros/ros_comm/issues/406>`_)
+
+1.11.1 (2014-05-07)
+-------------------
+* initial release

thirdparty/ros/ros_comm/utilities/roslz4/CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(roslz4)
+
+if(NOT WIN32)
+  set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
+endif()
+
+find_package(catkin REQUIRED)
+
+find_path(lz4_INCLUDE_DIRS NAMES lz4.h)
+if (NOT lz4_INCLUDE_DIRS)
+  message(FATAL_ERROR "lz4 includes not found")
+endif()
+
+find_library(lz4_LIBRARIES NAMES lz4)
+if (NOT lz4_LIBRARIES)
+  message(FATAL_ERROR "lz4 library not found")
+endif()
+set(lz4_FOUND TRUE)
+
+catkin_python_setup()
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES roslz4
+  DEPENDS lz4)
+
+include_directories(include ${lz4_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+add_library(roslz4 src/lz4s.c src/xxhash.c)
+set_source_files_properties(
+  src/lz4s.c
+PROPERTIES COMPILE_FLAGS "-Wno-sign-compare")
+target_link_libraries(roslz4 ${lz4_LIBRARIES} ${catkin_LIBRARIES})
+
+if(NOT ANDROID)
+# Python bindings
+set(Python_ADDITIONAL_VERSIONS "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}")
+find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIRED)
+include_directories(${PYTHON_INCLUDE_PATH})
+
+add_library(roslz4_py src/_roslz4module.c)
+set_source_files_properties(
+  src/_roslz4module.c
+PROPERTIES COMPILE_FLAGS "-Wno-missing-field-initializers -Wno-unused-variable -Wno-strict-aliasing")
+set_target_properties(
+  roslz4_py PROPERTIES OUTPUT_NAME roslz4 PREFIX "_" SUFFIX ".so"
+  LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/roslz4)
+target_link_libraries(roslz4_py roslz4 ${catkin_LIBRARIES} ${PYTHON_LIBRARIES})
+endif()
+
+install(TARGETS roslz4
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+if(NOT ANDROID)
+install(TARGETS roslz4_py
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION})
+endif()
+
+install(DIRECTORY include/${PROJECT_NAME}
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)
+
+# Testing
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(test_roslz4 test/roslz4_test.cpp)
+  if (TARGET test_roslz4)
+    target_link_libraries(test_roslz4 roslz4 ${catkin_LIBRARIES})
+  endif()
+endif()

thirdparty/ros/ros_comm/utilities/roslz4/include/roslz4/lz4s.h

@@ -0,0 +1,95 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2014, Ben Charrow
+*  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 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.
+********************************************************************/
+
+#ifndef ROSLZ4_LZ4S_H
+#define ROSLZ4_LZ4S_H
+
+#include <lz4.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Return codes
+const int ROSLZ4_MEMORY_ERROR = -5;
+const int ROSLZ4_PARAM_ERROR = -4;
+const int ROSLZ4_DATA_ERROR = -3;
+const int ROSLZ4_OUTPUT_SMALL = -2;
+const int ROSLZ4_ERROR = -1;
+const int ROSLZ4_OK = 0;
+const int ROSLZ4_STREAM_END = 2;
+
+// Actions
+const int ROSLZ4_RUN = 0;
+const int ROSLZ4_FINISH = 1;
+
+typedef struct {
+  char *input_next;
+  int input_left;
+
+  char *output_next;
+  int output_left;
+
+  int total_in;
+  int total_out;
+
+  int block_size_id;
+
+  // Internal use
+  void *state;
+} roslz4_stream;
+
+// Low level functions
+int roslz4_blockSizeFromIndex(int block_id);
+
+int roslz4_compressStart(roslz4_stream *stream, int block_size_id);
+int roslz4_compress(roslz4_stream *stream, int action);
+void roslz4_compressEnd(roslz4_stream *stream);
+
+int roslz4_decompressStart(roslz4_stream *stream);
+int roslz4_decompress(roslz4_stream *stream);
+void roslz4_decompressEnd(roslz4_stream *str);
+
+// Oneshot compression / decompression
+int roslz4_buffToBuffCompress(char *input, unsigned int input_size,
+                              char *output, unsigned int *output_size,
+                              int block_size_id);
+int roslz4_buffToBuffDecompress(char *input, unsigned int input_size,
+                                char *output, unsigned int *output_size);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // ROSLZ4_LZ4S_H

thirdparty/ros/ros_comm/utilities/roslz4/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package>
+  <name>roslz4</name>
+  <version>1.12.14</version>
+  <description>
+    A Python and C++ implementation of the LZ4 streaming format.  Large data
+    streams are split into blocks which are compressed using the very fast LZ4
+    compression algorithm.
+  </description>
+
+  <maintainer email="bcharrow@seas.upenn.edu">Ben Charrow</maintainer>
+  <license>BSD</license>
+  <author email="bcharrow@seas.upenn.edu">Ben Charrow</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>lz4</build_depend>
+
+  <run_depend>lz4</run_depend>
+
+  <test_depend>rosunit</test_depend>
+</package>

thirdparty/ros/ros_comm/utilities/roslz4/setup.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup, Extension
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['roslz4'],
+    package_dir={'': 'src'},
+    requires=[],
+)
+
+setup(**d)

thirdparty/ros/ros_comm/utilities/roslz4/src/_roslz4module.c

@@ -0,0 +1,452 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2014, Ben Charrow
+*  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 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.
+********************************************************************/
+
+#include "Python.h"
+
+#include "roslz4/lz4s.h"
+
+struct module_state {
+  PyObject *error;
+};
+
+#if PY_MAJOR_VERSION >= 3
+#define GETSTATE(m) ((struct module_state*)PyModule_GetState(m))
+#else
+#define GETSTATE(m) (&_state)
+static struct module_state _state;
+#endif
+
+/* Taken from Python's _bz2module.c */
+static int
+grow_buffer(PyObject **buf)
+{
+    /* Expand the buffer by an amount proportional to the current size,
+       giving us amortized linear-time behavior. Use a less-than-double
+       growth factor to avoid excessive allocation. */
+    size_t size = PyBytes_GET_SIZE(*buf);
+    size_t new_size = size + (size >> 3) + 6;
+    if (new_size > size) {
+        return _PyBytes_Resize(buf, new_size);
+    } else {  /* overflow */
+        PyErr_SetString(PyExc_OverflowError,
+                        "Unable to allocate buffer - output too large");
+        return -1;
+    }
+}
+
+/*============================== LZ4Compressor ==============================*/
+
+typedef struct {
+    PyObject_HEAD
+    roslz4_stream stream;
+} LZ4Compressor;
+
+static void
+LZ4Compressor_dealloc(LZ4Compressor *self)
+{
+  roslz4_compressEnd(&self->stream);
+  Py_TYPE(self)->tp_free((PyObject*)self);
+}
+
+static int
+LZ4Compressor_init(LZ4Compressor *self, PyObject *args, PyObject *kwds)
+{
+  (void)kwds;
+  if (!PyArg_ParseTuple(args, ":__init__")) {
+    return -1;
+  }
+
+  int ret = roslz4_compressStart(&self->stream, 6);
+  if (ret != ROSLZ4_OK) {
+    PyErr_SetString(PyExc_RuntimeError, "error initializing roslz4 stream");
+    return -1;
+  }
+  return 0;
+}
+
+static PyObject *
+compress_impl(LZ4Compressor *self, Py_buffer *input, PyObject *output)
+{
+  /* Allocate output string */
+  int initial_size = roslz4_blockSizeFromIndex(self->stream.block_size_id) + 64;
+  output = PyBytes_FromStringAndSize(NULL, initial_size);
+  if (!output) {
+    if (input != NULL) { PyBuffer_Release(input); }
+    return NULL;
+  }
+
+  /* Setup stream */
+  int action;
+  if (input != NULL) {
+    action = ROSLZ4_RUN;
+    self->stream.input_next = input->buf;
+    self->stream.input_left = input->len;
+  } else {
+    action = ROSLZ4_FINISH;
+    self->stream.input_next = NULL;
+    self->stream.input_left = 0;
+  }
+  self->stream.output_next = PyBytes_AS_STRING(output);
+  self->stream.output_left = PyBytes_GET_SIZE(output);
+
+  /* Compress data */
+  int status;
+  int output_written = 0;
+  while ((action == ROSLZ4_FINISH) ||
+         (action == ROSLZ4_RUN && self->stream.input_left > 0)) {
+    int out_start = self->stream.total_out;
+    status = roslz4_compress(&self->stream, action);
+    output_written += self->stream.total_out - out_start;
+    if (status == ROSLZ4_OK) {
+      continue;
+    } else if (status == ROSLZ4_STREAM_END) {
+      break;
+    } else if (status == ROSLZ4_OUTPUT_SMALL) {
+      if (grow_buffer(&output) < 0) {
+        goto error;
+      }
+      self->stream.output_next = PyBytes_AS_STRING(output) + output_written;
+      self->stream.output_left = PyBytes_GET_SIZE(output) - output_written;
+    } else if (status == ROSLZ4_PARAM_ERROR) {
+      PyErr_SetString(PyExc_IOError, "bad block size parameter");
+      goto error;
+    } else if (status == ROSLZ4_ERROR) {
+      PyErr_SetString(PyExc_IOError, "error compressing");
+      goto error;
+    } else {
+      PyErr_Format(PyExc_RuntimeError, "unhandled return code %i", status);
+      goto error;
+    }
+  }
+
+  /* Shrink return buffer */
+  if (output_written != PyBytes_GET_SIZE(output)) {
+    _PyBytes_Resize(&output, output_written);
+  }
+
+  if (input != NULL) { PyBuffer_Release(input); }
+  return output;
+
+error:
+  if (input != NULL) { PyBuffer_Release(input); }
+  Py_XDECREF(output);
+  return NULL;
+}
+
+static PyObject *
+LZ4Compressor_compress(LZ4Compressor *self, PyObject *args)
+{
+  Py_buffer input;
+  PyObject *output = NULL;
+
+  /* TODO: Keyword argument */
+  if (!PyArg_ParseTuple(args, "s*:compress", &input)) {
+    return NULL;
+  }
+
+  return compress_impl(self, &input, output);
+}
+
+static PyObject *
+LZ4Compressor_flush(LZ4Compressor *self, PyObject *args)
+{
+  PyObject *output = NULL;
+
+  if (!PyArg_ParseTuple(args, ":flush")) {
+    return NULL;
+  }
+
+  return compress_impl(self, NULL, output);
+}
+
+
+static PyMethodDef LZ4Compressor_methods[] = {
+  {"compress", (PyCFunction)LZ4Compressor_compress, METH_VARARGS, "method doc"},
+  {"flush", (PyCFunction)LZ4Compressor_flush, METH_VARARGS, "method doc"},
+  {NULL} /* Sentinel */
+};
+
+static PyTypeObject LZ4Compressor_Type = {
+    PyVarObject_HEAD_INIT(NULL, 0)
+    "_roslz4.LZ4Compressor",           /* tp_name */
+    sizeof(LZ4Compressor),             /* tp_basicsize */
+    0,                                 /* tp_itemsize */
+    (destructor)LZ4Compressor_dealloc, /* tp_dealloc */
+    0,                                 /* tp_print */
+    0,                                 /* tp_getattr */
+    0,                                 /* tp_setattr */
+    0,                                 /* tp_compare */
+    0,                                 /* tp_repr */
+    0,                                 /* tp_as_number */
+    0,                                 /* tp_as_sequence */
+    0,                                 /* tp_as_mapping */
+    0,                                 /* tp_hash */
+    0,                                 /* tp_call */
+    0,                                 /* tp_str */
+    0,                                 /* tp_getattro */
+    0,                                 /* tp_setattro */
+    0,                                 /* tp_as_buffer */
+    Py_TPFLAGS_DEFAULT,                /* tp_flags */
+    "LZ4Compressor objects",           /* tp_doc */
+    0,                                 /* tp_traverse */
+    0,                                 /* tp_clear */
+    0,                                 /* tp_richcompare */
+    0,                                 /* tp_weaklistoffset */
+    0,                                 /* tp_iter */
+    0,                                 /* tp_iternext */
+    LZ4Compressor_methods,             /* tp_methods */
+    0,                                 /* tp_members */
+    0,                                 /* tp_getset */
+    0,                                 /* tp_base */
+    0,                                 /* tp_dict */
+    0,                                 /* tp_descr_get */
+    0,                                 /* tp_descr_set */
+    0,                                 /* tp_dictoffset */
+    (initproc)LZ4Compressor_init       /* tp_init */
+};
+
+/*============================= LZ4Decompressor =============================*/
+
+typedef struct {
+    PyObject_HEAD
+    roslz4_stream stream;
+} LZ4Decompressor;
+
+static void
+LZ4Decompressor_dealloc(LZ4Decompressor *self)
+{
+  roslz4_decompressEnd(&self->stream);
+  Py_TYPE(self)->tp_free((PyObject*)self);
+}
+
+static int
+LZ4Decompressor_init(LZ4Decompressor *self, PyObject *args, PyObject *kwds)
+{
+  (void)kwds;
+  if (!PyArg_ParseTuple(args, ":__init__")) {
+    return -1;
+  }
+
+  int ret = roslz4_decompressStart(&self->stream);
+  if (ret != ROSLZ4_OK) {
+    PyErr_SetString(PyExc_RuntimeError, "error initializing roslz4 stream");
+    return -1;
+  }
+  return 0;
+}
+
+static PyObject *
+LZ4Decompressor_decompress(LZ4Decompressor *self, PyObject *args)
+{
+  Py_buffer input;
+  PyObject *output = NULL;
+
+  /* TODO: Keyword argument */
+  if (!PyArg_ParseTuple(args, "s*:decompress", &input)) {
+    return NULL;
+  }
+
+  /* Allocate 1 output block. If header not read, use compression block size */
+  int block_size;
+  if (self->stream.block_size_id == -1 ) {
+    block_size = roslz4_blockSizeFromIndex(6);
+  } else {
+    block_size = roslz4_blockSizeFromIndex(self->stream.block_size_id);
+  }
+
+  output = PyBytes_FromStringAndSize(NULL, block_size);
+  if (!output) {
+    PyBuffer_Release(&input);
+    return NULL;
+  }
+
+  /* Setup stream */
+  self->stream.input_next = input.buf;
+  self->stream.input_left = input.len;
+  self->stream.output_next = PyBytes_AS_STRING(output);
+  self->stream.output_left = PyBytes_GET_SIZE(output);
+
+  int output_written = 0;
+  while (self->stream.input_left > 0) {
+    int out_start = self->stream.total_out;
+    int status = roslz4_decompress(&self->stream);
+    output_written += self->stream.total_out - out_start;
+    if (status == ROSLZ4_OK) {
+      continue;
+    } else if (status == ROSLZ4_STREAM_END) {
+      break;
+    } else if (status == ROSLZ4_OUTPUT_SMALL) {
+      if (grow_buffer(&output) < 0) {
+        goto error;
+      }
+      self->stream.output_next = PyBytes_AS_STRING(output) + output_written;
+      self->stream.output_left = PyBytes_GET_SIZE(output) - output_written;
+    } else if (status == ROSLZ4_ERROR) {
+      PyErr_SetString(PyExc_IOError, "error decompressing");
+      goto error;
+    } else if (status == ROSLZ4_DATA_ERROR) {
+      PyErr_SetString(PyExc_IOError, "malformed data to decompress");
+      goto error;
+    } else {
+      PyErr_Format(PyExc_RuntimeError, "unhandled return code %i", status);
+      goto error;
+    }
+  }
+
+  if (output_written != PyBytes_GET_SIZE(output)) {
+    _PyBytes_Resize(&output, output_written);
+  }
+
+  PyBuffer_Release(&input);
+  return output;
+
+error:
+  PyBuffer_Release(&input);
+  Py_XDECREF(output);
+  return NULL;
+}
+
+static PyMethodDef LZ4Decompressor_methods[] = {
+  {"decompress", (PyCFunction)LZ4Decompressor_decompress, METH_VARARGS, "method doc"},
+  {NULL} /* Sentinel */
+};
+
+static PyTypeObject LZ4Decompressor_Type = {
+    PyVarObject_HEAD_INIT(NULL, 0)
+    "_roslz4.LZ4Decompressor",           /* tp_name */
+    sizeof(LZ4Decompressor),             /* tp_basicsize */
+    0,                                   /* tp_itemsize */
+    (destructor)LZ4Decompressor_dealloc, /* tp_dealloc */
+    0,                                   /* tp_print */
+    0,                                   /* tp_getattr */
+    0,                                   /* tp_setattr */
+    0,                                   /* tp_compare */
+    0,                                   /* tp_repr */
+    0,                                   /* tp_as_number */
+    0,                                   /* tp_as_sequence */
+    0,                                   /* tp_as_mapping */
+    0,                                   /* tp_hash */
+    0,                                   /* tp_call */
+    0,                                   /* tp_str */
+    0,                                   /* tp_getattro */
+    0,                                   /* tp_setattro */
+    0,                                   /* tp_as_buffer */
+    Py_TPFLAGS_DEFAULT,                  /* tp_flags */
+    "LZ4Decompressor objects",           /* tp_doc */
+    0,                                   /* tp_traverse */
+    0,                                   /* tp_clear */
+    0,                                   /* tp_richcompare */
+    0,                                   /* tp_weaklistoffset */
+    0,                                   /* tp_iter */
+    0,                                   /* tp_iternext */
+    LZ4Decompressor_methods,             /* tp_methods */
+    0,                                   /* tp_members */
+    0,                                   /* tp_getset */
+    0,                                   /* tp_base */
+    0,                                   /* tp_dict */
+    0,                                   /* tp_descr_get */
+    0,                                   /* tp_descr_set */
+    0,                                   /* tp_dictoffset */
+    (initproc)LZ4Decompressor_init       /* tp_init */
+};
+
+
+/*========================== Module initialization ==========================*/
+
+#if PY_MAJOR_VERSION >= 3
+
+static int roslz4_traverse(PyObject *m, visitproc visit, void *arg) {
+  Py_VISIT(GETSTATE(m)->error);
+  return 0;
+}
+
+static int roslz4_clear(PyObject *m) {
+  Py_CLEAR(GETSTATE(m)->error);
+  return 0;
+}
+
+static struct PyModuleDef moduledef = {
+  PyModuleDef_HEAD_INIT,
+  "_roslz4",
+  NULL,
+  sizeof(struct module_state),
+  NULL,
+  NULL,
+  roslz4_traverse,
+  roslz4_clear,
+  NULL
+};
+#define INITERROR return NULL
+
+PyObject *
+PyInit__roslz4(void)
+
+#else
+#define INITERROR return
+
+void
+init_roslz4(void)
+#endif
+{
+  PyObject *m;
+
+  LZ4Compressor_Type.tp_new = PyType_GenericNew;
+  if (PyType_Ready(&LZ4Compressor_Type) < 0) {
+    INITERROR;
+  }
+
+  LZ4Decompressor_Type.tp_new = PyType_GenericNew;
+  if (PyType_Ready(&LZ4Decompressor_Type) < 0) {
+    INITERROR;
+  }
+
+#if PY_MAJOR_VERSION >= 3
+  m = PyModule_Create(&moduledef);
+#else
+  m = Py_InitModule("_roslz4", NULL);
+#endif
+
+  if (m == NULL) {
+    INITERROR;
+  }
+
+  Py_INCREF(&LZ4Compressor_Type);
+  PyModule_AddObject(m, "LZ4Compressor", (PyObject *)&LZ4Compressor_Type);
+  Py_INCREF(&LZ4Decompressor_Type);
+  PyModule_AddObject(m, "LZ4Decompressor", (PyObject *)&LZ4Decompressor_Type);
+
+#if PY_MAJOR_VERSION >= 3
+  return m;
+#endif
+}

thirdparty/ros/ros_comm/utilities/roslz4/src/lz4s.c

@@ -0,0 +1,627 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2014, Ben Charrow
+*  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 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.
+********************************************************************/
+
+#include "roslz4/lz4s.h"
+
+#include "xxhash.h"
+
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#if 0
+#define DEBUG(...) fprintf(stderr, __VA_ARGS__)
+#else
+#define DEBUG(...)
+#endif
+
+// magic numbers
+const uint32_t kMagicNumber = 0x184D2204;
+const uint32_t kEndOfStream = 0x00000000;
+
+// Bitmasks
+const uint8_t k1Bits = 0x01;
+const uint8_t k2Bits = 0x03;
+const uint8_t k3Bits = 0x07;
+const uint8_t k4Bits = 0x0F;
+const uint8_t k8Bits = 0xFF;
+
+uint32_t readUInt32(unsigned char *buffer) {
+  return ((buffer[0] <<  0) | (buffer[1] <<  8) |
+          (buffer[2] << 16) | (buffer[3] << 24));
+}
+
+void writeUInt32(unsigned char *buffer, uint32_t val) {
+  buffer[0] = val & 0xFF;
+  buffer[1] = (val >> 8) & 0xFF;
+  buffer[2] = (val >> 16) & 0xFF;
+  buffer[3] = (val >> 24) & 0xFF;
+}
+
+int min(int a, int b) {
+  return a < b ? a : b;
+}
+
+/*========================== Low level compression ==========================*/
+
+typedef struct {
+  int block_independence_flag;
+  int block_checksum_flag;
+  int stream_checksum_flag;
+
+  char *buffer;
+  int buffer_size;
+  int buffer_offset;
+
+  int finished; // 1 if done compressing/decompressing; 0 otherwise
+
+  void* xxh32_state;
+
+  // Compression state
+  int wrote_header;
+
+  // Decompression state
+  char header[10];
+  uint32_t block_size; // Size of current block
+  int block_size_read; // # of bytes read for current block_size
+  int block_uncompressed; // 1 if block is uncompressed, 0 otherwise
+  uint32_t stream_checksum; // Storage for checksum
+  int stream_checksum_read; // # of bytes read for stream_checksum
+} stream_state;
+
+void advanceInput(roslz4_stream *str, int nbytes) {
+  str->input_next += nbytes;
+  str->input_left -= nbytes;
+  str->total_in += nbytes;
+}
+
+void advanceOutput(roslz4_stream *str, int nbytes) {
+  str->output_next += nbytes;
+  str->output_left -= nbytes;
+  str->total_out += nbytes;
+}
+
+void fillUInt32(roslz4_stream *str, uint32_t *dest_val, int *offset) {
+  char *dest = (char*) dest_val;
+  int to_copy = min(4 - *offset, str->input_left);
+  memcpy(dest + *offset, str->input_next, to_copy);
+  advanceInput(str, to_copy);
+  *offset += to_copy;
+}
+
+int writeHeader(roslz4_stream *str) {
+  if (str->output_left < 7) {
+    return ROSLZ4_OUTPUT_SMALL; // Output must have 7 bytes
+  }
+
+  stream_state *state = str->state;
+  writeUInt32((unsigned char*) str->output_next, kMagicNumber);
+  int version = 1;
+  char *out = str->output_next;
+  *(out+4)  = ((unsigned)version                        & k2Bits) << 6;
+  *(out+4) |= ((unsigned)state->block_independence_flag & k1Bits) << 5;
+  *(out+4) |= ((unsigned)state->block_checksum_flag     & k1Bits) << 4;
+  *(out+4) |= ((unsigned)state->stream_checksum_flag    & k1Bits) << 2;
+  *(out+5)  = ((unsigned)str->block_size_id             & k3Bits) << 4;
+
+  // Checksum: 2nd byte of hash of header flags
+  unsigned char checksum = (XXH32(str->output_next + 4, 2, 0) >> 8) & k8Bits;
+  *(str->output_next+6) = checksum;
+
+  advanceOutput(str, 7);
+  DEBUG("writeHeader() Put 7 bytes in output\n");
+
+  return ROSLZ4_OK;
+}
+
+int writeEOS(roslz4_stream *str) {
+  if (str->output_left < 8) {
+    return ROSLZ4_OUTPUT_SMALL;
+  }
+
+  stream_state *state = str->state;
+  state->finished = 1;
+  writeUInt32((unsigned char*) str->output_next, kEndOfStream);
+  advanceOutput(str, 4);
+
+  uint32_t stream_checksum = XXH32_digest(state->xxh32_state);
+  writeUInt32((unsigned char*) str->output_next, stream_checksum);
+  advanceOutput(str, 4);
+  state->xxh32_state = NULL;
+
+  DEBUG("writeEOS() Wrote 8 bytes to output %i\n", str->output_left);
+  return ROSLZ4_STREAM_END;
+}
+
+// If successfull, number of bytes written to output
+// If error, LZ4 return code
+int bufferToOutput(roslz4_stream *str) {
+  stream_state *state = str->state;
+  uint32_t uncomp_size = state->buffer_offset;
+  if (state->buffer_offset == 0) {
+    return 0; // No data to flush
+  } else if (str->output_left - 4 < uncomp_size) {
+    DEBUG("bufferToOutput() Not enough space left in output\n");
+    return ROSLZ4_OUTPUT_SMALL;
+  }
+
+  DEBUG("bufferToOutput() Flushing %i bytes, %i left in output\n",
+        state->buffer_offset, str->output_left);
+
+  // Shrink output by 1 to detect if data is not compressible
+  uint32_t comp_size = LZ4_compress_limitedOutput(state->buffer,
+                                                  str->output_next + 4,
+                                                  (int) state->buffer_offset,
+                                                  (int) uncomp_size - 1);
+  uint32_t wrote;
+  if (comp_size > 0) {
+    DEBUG("bufferToOutput() Compressed to %i bytes\n", comp_size);
+    // Write compressed data size
+    wrote = 4 + comp_size;
+    writeUInt32((unsigned char*)str->output_next, comp_size);
+  } else {
+    // Write uncompressed data
+    DEBUG("bufferToOutput() Can't compress, copying input\n");
+    memcpy(str->output_next + 4, state->buffer, uncomp_size);
+    // Write uncompressed data size.  Signal data is uncompressed with high
+    // order bit; won't confuse decompression because max block size is < 2GB
+    wrote = 4 + uncomp_size;
+    writeUInt32((unsigned char*) str->output_next, uncomp_size | 0x80000000);
+  }
+
+  advanceOutput(str, wrote);
+  state->buffer_offset -= uncomp_size;
+
+  DEBUG("bufferToOutput() Ate %i from buffer, wrote %i to output (%i)\n",
+        uncomp_size, wrote, str->output_left);
+  return wrote;
+}
+
+// Copy as much data as possible from input to internal buffer
+// Return number of bytes written if successful, LZ4 error code on error
+int inputToBuffer(roslz4_stream *str) {
+  stream_state *state = str->state;
+  if (str->input_left == 0 ||
+      state->buffer_size == state->buffer_offset) {
+    return 0;
+  }
+  int buffer_left = state->buffer_size - state->buffer_offset;
+  int to_copy = min(str->input_left, buffer_left);
+
+  int ret = XXH32_update(state->xxh32_state, str->input_next, to_copy);
+  if (ret == XXH_ERROR) { return ROSLZ4_ERROR; }
+
+  memcpy(state->buffer + state->buffer_offset, str->input_next, to_copy);
+  advanceInput(str, to_copy);
+  state->buffer_offset += to_copy;
+
+  DEBUG("inputToBuffer() Wrote % 5i bytes to buffer (size=% 5i)\n",
+        to_copy, state->buffer_offset);
+  return to_copy;
+}
+
+int streamStateAlloc(roslz4_stream *str) {
+  stream_state *state = (stream_state*) malloc(sizeof(stream_state));
+  if (state == NULL) {
+    return ROSLZ4_MEMORY_ERROR; // Allocation of state failed
+  }
+  str->state = state;
+
+  str->block_size_id = -1;
+  state->block_independence_flag = 1;
+  state->block_checksum_flag = 0;
+  state->stream_checksum_flag = 1;
+
+  state->finished = 0;
+
+  state->xxh32_state = XXH32_init(0);
+  state->stream_checksum = 0;
+  state->stream_checksum_read = 0;
+
+  state->wrote_header = 0;
+
+  state->buffer_offset = 0;
+  state->buffer_size = 0;
+  state->buffer = NULL;
+
+  state->block_size = 0;
+  state->block_size_read = 0;
+  state->block_uncompressed = 0;
+
+  str->total_in = 0;
+  str->total_out = 0;
+
+  return ROSLZ4_OK;
+}
+
+int streamResizeBuffer(roslz4_stream *str, int block_size_id) {
+  stream_state *state = str->state;
+  if (!(4 <= block_size_id && block_size_id <= 7)) {
+    return ROSLZ4_PARAM_ERROR; // Invalid block size
+  }
+
+  str->block_size_id = block_size_id;
+  state->buffer_offset = 0;
+  state->buffer_size = roslz4_blockSizeFromIndex(str->block_size_id);
+  state->buffer = (char*) malloc(sizeof(char) * state->buffer_size);
+  if (state->buffer == NULL) {
+    return ROSLZ4_MEMORY_ERROR; // Allocation of buffer failed
+  }
+  return ROSLZ4_OK;
+}
+
+void streamStateFree(roslz4_stream *str) {
+  stream_state *state = str->state;
+  if (state != NULL) {
+    if (state->buffer != NULL) {
+      free(state->buffer);
+    }
+    if (state->xxh32_state != NULL) {
+      XXH32_digest(state->xxh32_state);
+    }
+    free(state);
+    str->state = NULL;
+  }
+}
+
+int roslz4_blockSizeFromIndex(int block_id) {
+  return (1 << (8 + (2 * block_id)));
+}
+
+int roslz4_compressStart(roslz4_stream *str, int block_size_id) {
+  int ret = streamStateAlloc(str);
+  if (ret < 0) { return ret; }
+  return streamResizeBuffer(str, block_size_id);
+}
+
+int roslz4_compress(roslz4_stream *str, int action) {
+  int ret;
+  stream_state *state = str->state;
+  if (action != ROSLZ4_RUN && action != ROSLZ4_FINISH) {
+    return ROSLZ4_PARAM_ERROR; // Unrecognized compression action
+  } else if (state->finished) {
+    return ROSLZ4_ERROR; // Cannot call action on finished stream
+  }
+
+  if (!state->wrote_header) {
+    ret = writeHeader(str);
+    if (ret < 0) { return ret; }
+    state->wrote_header = 1;
+  }
+
+  // Copy input to internal buffer, compressing when full or finishing stream
+  int read = 0, wrote = 0;
+  do {
+    read = inputToBuffer(str);
+    if (read < 0) { return read; }
+
+    wrote = 0;
+    if (action == ROSLZ4_FINISH || state->buffer_offset == state->buffer_size) {
+      wrote = bufferToOutput(str);
+      if (wrote < 0) { return wrote; }
+    }
+  } while (read > 0 || wrote > 0);
+
+  // Signal end of stream if finishing up, otherwise done
+  if (action == ROSLZ4_FINISH) {
+    return writeEOS(str);
+  } else {
+    return ROSLZ4_OK;
+  }
+}
+
+void roslz4_compressEnd(roslz4_stream *str) {
+  streamStateFree(str);
+}
+
+/*========================= Low level decompression =========================*/
+
+int roslz4_decompressStart(roslz4_stream *str) {
+  return streamStateAlloc(str);
+  // Can't allocate internal buffer, block size is unknown until header is read
+}
+
+// Return 1 if header is present, 0 if more data is needed,
+// LZ4 error code (< 0) if error
+int processHeader(roslz4_stream *str) {
+  stream_state *state = str->state;
+  if (str->total_in >= 7) {
+    return 1;
+  }
+  // Populate header buffer
+  int to_copy = min(7 - str->total_in, str->input_left);
+  memcpy(state->header + str->total_in, str->input_next, to_copy);
+  advanceInput(str, to_copy);
+  if (str->total_in < 7) {
+    return 0;
+  }
+
+  // Parse header buffer
+  unsigned char *header = (unsigned char*) state->header;
+  uint32_t magic_number = readUInt32(header);
+  if (magic_number != kMagicNumber) {
+    return ROSLZ4_DATA_ERROR; // Stream does not start with magic number
+  }
+  // Check descriptor flags
+  int version                 = (header[4] >> 6) & k2Bits;
+  int block_independence_flag = (header[4] >> 5) & k1Bits;
+  int block_checksum_flag     = (header[4] >> 4) & k1Bits;
+  int stream_size_flag        = (header[4] >> 3) & k1Bits;
+  int stream_checksum_flag    = (header[4] >> 2) & k1Bits;
+  int reserved1               = (header[4] >> 1) & k1Bits;
+  int preset_dictionary_flag  = (header[4] >> 0) & k1Bits;
+
+  int reserved2               = (header[5] >> 7) & k1Bits;
+  int block_max_id            = (header[5] >> 4) & k3Bits;
+  int reserved3               = (header[5] >> 0) & k4Bits;
+
+  // LZ4 standard requirements
+  if (version != 1) {
+    return ROSLZ4_DATA_ERROR; // Wrong version number
+  }
+  if (reserved1 != 0 || reserved2 != 0 || reserved3 != 0) {
+    return ROSLZ4_DATA_ERROR; // Reserved bits must be 0
+  }
+  if (!(4 <= block_max_id && block_max_id <= 7)) {
+    return ROSLZ4_DATA_ERROR; // Invalid block size
+  }
+
+  // Implementation requirements
+  if (stream_size_flag != 0) {
+    return ROSLZ4_DATA_ERROR; // Stream size not supported
+  }
+  if (preset_dictionary_flag != 0) {
+    return ROSLZ4_DATA_ERROR; // Dictionary not supported
+  }
+  if (block_independence_flag != 1) {
+    return ROSLZ4_DATA_ERROR; // Block dependence not supported
+  }
+  if (block_checksum_flag != 0) {
+    return ROSLZ4_DATA_ERROR; // Block checksums not supported
+  }
+  if (stream_checksum_flag != 1) {
+    return ROSLZ4_DATA_ERROR; // Must have stream checksum
+  }
+
+  int header_checksum = (XXH32(header + 4, 2, 0) >> 8) & k8Bits;
+  int stored_header_checksum = (header[6] >> 0) & k8Bits;
+  if (header_checksum != stored_header_checksum) {
+    return ROSLZ4_DATA_ERROR; // Header checksum doesn't match
+  }
+
+  int ret = streamResizeBuffer(str, block_max_id);
+  if (ret == ROSLZ4_OK) {
+    return 1;
+  } else {
+    return ret;
+  }
+}
+
+// Read block size, return 1 if value is stored in state->block_size 0 otherwise
+int readBlockSize(roslz4_stream *str) {
+  stream_state *state = str->state;
+  if (state->block_size_read < 4) {
+    fillUInt32(str, &state->block_size, &state->block_size_read);
+    if (state->block_size_read == 4) {
+      state->block_size = readUInt32((unsigned char*)&state->block_size);
+      state->block_uncompressed = ((unsigned)state->block_size >> 31) & k1Bits;
+      state->block_size &= 0x7FFFFFFF;
+      DEBUG("readBlockSize() Block size = %i uncompressed = %i\n",
+            state->block_size, state->block_uncompressed);
+      return 1;
+    } else {
+      return 0;
+    }
+  }
+  return 1;
+}
+
+// Copy at most one blocks worth of data from input to internal buffer.
+// Return 1 if whole block has been read, 0 if not, LZ4 error otherwise
+int readBlock(roslz4_stream *str) {
+  stream_state *state = str->state;
+  if (state->block_size_read != 4 || state->block_size == kEndOfStream) {
+    return ROSLZ4_ERROR;
+  }
+
+  int block_left = state->block_size - state->buffer_offset;
+  int to_copy = min(str->input_left, block_left);
+  memcpy(state->buffer + state->buffer_offset, str->input_next, to_copy);
+  advanceInput(str, to_copy);
+  state->buffer_offset += to_copy;
+  DEBUG("readBlock() Read %i bytes from input (block = %i/%i)\n",
+        to_copy, state->buffer_offset, state->block_size);
+  return state->buffer_offset == state->block_size;
+}
+
+int decompressBlock(roslz4_stream *str) {
+  stream_state *state = str->state;
+  if (state->block_size_read != 4 || state->block_size != state->buffer_offset) {
+    // Internal error: Can't decompress block, it's not in buffer
+    return ROSLZ4_ERROR;
+  }
+
+  if (state->block_uncompressed) {
+    if (str->output_left >= state->block_size) {
+      memcpy(str->output_next, state->buffer, state->block_size);
+      int ret = XXH32_update(state->xxh32_state, str->output_next,
+                             state->block_size);
+      if (ret == XXH_ERROR) { return ROSLZ4_ERROR; }
+      advanceOutput(str, state->block_size);
+      state->block_size_read = 0;
+      state->buffer_offset = 0;
+      return ROSLZ4_OK;
+    } else {
+      return ROSLZ4_OUTPUT_SMALL;
+    }
+  } else {
+    int decomp_size;
+    decomp_size = LZ4_decompress_safe(state->buffer, str->output_next,
+                                      state->block_size, str->output_left);
+    if (decomp_size < 0) {
+      if (str->output_left >= state->buffer_size) {
+        return ROSLZ4_DATA_ERROR; // Must be a problem with the data stream
+      } else {
+        // Data error or output is small; increase output to disambiguate
+        return ROSLZ4_OUTPUT_SMALL;
+      }
+    } else {
+      int ret = XXH32_update(state->xxh32_state, str->output_next, decomp_size);
+      if (ret == XXH_ERROR) { return ROSLZ4_ERROR; }
+      advanceOutput(str, decomp_size);
+      state->block_size_read = 0;
+      state->buffer_offset = 0;
+      return ROSLZ4_OK;
+    }
+  }
+}
+
+int readChecksum(roslz4_stream *str) {
+  stream_state *state = str->state;
+  fillUInt32(str, &state->stream_checksum, &state->stream_checksum_read);
+  if (state->stream_checksum_read == 4) {
+    state->finished = 1;
+    state->stream_checksum = readUInt32((unsigned char*)&state->stream_checksum);
+    uint32_t checksum = XXH32_digest(state->xxh32_state);
+    state->xxh32_state = NULL;
+    if (checksum == state->stream_checksum) {
+      return ROSLZ4_STREAM_END;
+    } else {
+      return ROSLZ4_DATA_ERROR;
+    }
+  }
+  return ROSLZ4_OK;
+}
+
+int roslz4_decompress(roslz4_stream *str) {
+  stream_state *state = str->state;
+  if (state->finished) {
+    return ROSLZ4_ERROR; // Already reached end of stream
+  }
+
+  // Return if header isn't present or error was encountered
+  int ret = processHeader(str);
+  if (ret <= 0) {
+    return ret;
+  }
+
+  // Read in blocks and decompress them as long as there's data to be processed
+  while (str->input_left > 0) {
+    ret = readBlockSize(str);
+    if (ret == 0) { return ROSLZ4_OK; }
+
+    if (state->block_size == kEndOfStream) {
+      return readChecksum(str);
+    }
+
+    ret = readBlock(str);
+    if (ret == 0) { return ROSLZ4_OK; }
+    else if (ret < 0) { return ret; }
+
+    ret = decompressBlock(str);
+    if (ret < 0) { return ret; }
+  }
+  return ROSLZ4_OK;
+}
+
+void roslz4_decompressEnd(roslz4_stream *str) {
+  streamStateFree(str);
+}
+
+/*=================== Oneshot compression / decompression ===================*/
+
+int roslz4_buffToBuffCompress(char *input, unsigned int input_size,
+                              char *output, unsigned int *output_size,
+                              int block_size_id) {
+  roslz4_stream stream;
+  stream.input_next = input;
+  stream.input_left = input_size;
+  stream.output_next = output;
+  stream.output_left = *output_size;
+
+  int ret;
+  ret = roslz4_compressStart(&stream, block_size_id);
+  if (ret != ROSLZ4_OK) {
+    roslz4_compressEnd(&stream);
+    return ret;
+  }
+
+  while (stream.input_left > 0 && ret != ROSLZ4_STREAM_END) {
+    ret = roslz4_compress(&stream, ROSLZ4_FINISH);
+    if (ret == ROSLZ4_ERROR || ret == ROSLZ4_OUTPUT_SMALL) {
+      roslz4_compressEnd(&stream);
+      return ret;
+    }
+  }
+
+  *output_size = *output_size - stream.output_left;
+  roslz4_compressEnd(&stream);
+
+  if (stream.input_left == 0 && ret == ROSLZ4_STREAM_END) {
+    return ROSLZ4_OK; // Success
+  } else {
+    return ROSLZ4_ERROR; // User did not provide exact buffer
+  }
+}
+
+int roslz4_buffToBuffDecompress(char *input, unsigned int input_size,
+                                char *output, unsigned int *output_size) {
+  roslz4_stream stream;
+  stream.input_next = input;
+  stream.input_left = input_size;
+  stream.output_next = output;
+  stream.output_left = *output_size;
+
+  int ret;
+  ret = roslz4_decompressStart(&stream);
+  if (ret != ROSLZ4_OK) { return ret; }
+
+  while (stream.input_left > 0 && ret != ROSLZ4_STREAM_END) {
+    ret = roslz4_decompress(&stream);
+    if (ret < 0) {
+      roslz4_decompressEnd(&stream);
+      return ret;
+    }
+  }
+
+  *output_size = *output_size - stream.output_left;
+  roslz4_decompressEnd(&stream);
+
+  if (stream.input_left == 0 && ret == ROSLZ4_STREAM_END) {
+    return ROSLZ4_OK; // Success
+  } else {
+    return ROSLZ4_ERROR; // User did not provide exact buffer
+  }
+}

thirdparty/ros/ros_comm/utilities/roslz4/src/roslz4/__init__.py

@@ -0,0 +1,42 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2014, Ben Charrow
+# 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 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.
+
+from ._roslz4 import *
+
+def compress(data):
+    compressor = LZ4Compressor()
+    return compressor.compress(data) + compressor.flush()
+
+def decompress(data):
+    decompressor = LZ4Decompressor()
+    output = decompressor.decompress(data)
+    return output

thirdparty/ros/ros_comm/utilities/roslz4/src/xxhash.c

@@ -0,0 +1,475 @@
+/*
+xxHash - Fast Hash algorithm
+Copyright (C) 2012-2014, Yann Collet.
+BSD 2-Clause License (http://www.opensource.org/licenses/bsd-license.php)
+
+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.
+
+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.
+
+You can contact the author at :
+- xxHash source repository : http://code.google.com/p/xxhash/
+*/
+
+
+//**************************************
+// Tuning parameters
+//**************************************
+// Unaligned memory access is automatically enabled for "common" CPU, such as x86.
+// For others CPU, the compiler will be more cautious, and insert extra code to ensure aligned access is respected.
+// If you know your target CPU supports unaligned memory access, you want to force this option manually to improve performance.
+// You can also enable this parameter if you know your input data will always be aligned (boundaries of 4, for U32).
+#if defined(__ARM_FEATURE_UNALIGNED) || defined(__i386) || defined(_M_IX86) || defined(__x86_64__) || defined(_M_X64)
+#  define XXH_USE_UNALIGNED_ACCESS 1
+#endif
+
+// XXH_ACCEPT_NULL_INPUT_POINTER :
+// If the input pointer is a null pointer, xxHash default behavior is to trigger a memory access error, since it is a bad pointer.
+// When this option is enabled, xxHash output for null input pointers will be the same as a null-length input.
+// This option has a very small performance cost (only measurable on small inputs).
+// By default, this option is disabled. To enable it, uncomment below define :
+//#define XXH_ACCEPT_NULL_INPUT_POINTER 1
+
+// XXH_FORCE_NATIVE_FORMAT :
+// By default, xxHash library provides endian-independant Hash values, based on little-endian convention.
+// Results are therefore identical for little-endian and big-endian CPU.
+// This comes at a performance cost for big-endian CPU, since some swapping is required to emulate little-endian format.
+// Should endian-independance be of no importance for your application, you may set the #define below to 1.
+// It will improve speed for Big-endian CPU.
+// This option has no impact on Little_Endian CPU.
+#define XXH_FORCE_NATIVE_FORMAT 0
+
+
+//**************************************
+// Compiler Specific Options
+//**************************************
+// Disable some Visual warning messages
+#ifdef _MSC_VER  // Visual Studio
+#  pragma warning(disable : 4127)      // disable: C4127: conditional expression is constant
+#endif
+
+#ifdef _MSC_VER    // Visual Studio
+#  define FORCE_INLINE static __forceinline
+#else 
+#  ifdef __GNUC__
+#    define FORCE_INLINE static inline __attribute__((always_inline))
+#  else
+#    define FORCE_INLINE static inline
+#  endif
+#endif
+
+
+//**************************************
+// Includes & Memory related functions
+//**************************************
+#include "xxhash.h"
+// Modify the local functions below should you wish to use some other memory related routines
+// for malloc(), free()
+#include <stdlib.h>
+FORCE_INLINE void* XXH_malloc(size_t s) { return malloc(s); }
+FORCE_INLINE void  XXH_free  (void* p)  { free(p); }
+// for memcpy()
+#include <string.h>
+FORCE_INLINE void* XXH_memcpy(void* dest, const void* src, size_t size) { return memcpy(dest,src,size); }
+
+
+//**************************************
+// Basic Types
+//**************************************
+#if defined (__STDC_VERSION__) && __STDC_VERSION__ >= 199901L   // C99
+# include <stdint.h>
+  typedef uint8_t  BYTE;
+  typedef uint16_t U16;
+  typedef uint32_t U32;
+  typedef  int32_t S32;
+  typedef uint64_t U64;
+#else
+  typedef unsigned char      BYTE;
+  typedef unsigned short     U16;
+  typedef unsigned int       U32;
+  typedef   signed int       S32;
+  typedef unsigned long long U64;
+#endif
+
+#if defined(__GNUC__)  && !defined(XXH_USE_UNALIGNED_ACCESS)
+#  define _PACKED __attribute__ ((packed))
+#else
+#  define _PACKED
+#endif
+
+#if !defined(XXH_USE_UNALIGNED_ACCESS) && !defined(__GNUC__)
+#  ifdef __IBMC__
+#    pragma pack(1)
+#  else
+#    pragma pack(push, 1)
+#  endif
+#endif
+
+typedef struct _U32_S { U32 v; } _PACKED U32_S;
+
+#if !defined(XXH_USE_UNALIGNED_ACCESS) && !defined(__GNUC__)
+#  pragma pack(pop)
+#endif
+
+#define A32(x) (((U32_S *)(x))->v)
+
+
+//***************************************
+// Compiler-specific Functions and Macros
+//***************************************
+#define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__)
+
+// Note : although _rotl exists for minGW (GCC under windows), performance seems poor
+#if defined(_MSC_VER)
+#  define XXH_rotl32(x,r) _rotl(x,r)
+#else
+#  define XXH_rotl32(x,r) ((x << r) | (x >> (32 - r)))
+#endif
+
+#if defined(_MSC_VER)     // Visual Studio
+#  define XXH_swap32 _byteswap_ulong
+#elif GCC_VERSION >= 403
+#  define XXH_swap32 __builtin_bswap32
+#else
+static inline U32 XXH_swap32 (U32 x) {
+    return  ((x << 24) & 0xff000000 ) |
+        ((x <<  8) & 0x00ff0000 ) |
+        ((x >>  8) & 0x0000ff00 ) |
+        ((x >> 24) & 0x000000ff );}
+#endif
+
+
+//**************************************
+// Constants
+//**************************************
+#define PRIME32_1   2654435761U
+#define PRIME32_2   2246822519U
+#define PRIME32_3   3266489917U
+#define PRIME32_4    668265263U
+#define PRIME32_5    374761393U
+
+
+//**************************************
+// Architecture Macros
+//**************************************
+typedef enum { XXH_bigEndian=0, XXH_littleEndian=1 } XXH_endianess;
+#ifndef XXH_CPU_LITTLE_ENDIAN   // It is possible to define XXH_CPU_LITTLE_ENDIAN externally, for example using a compiler switch
+    static const int one = 1;
+#   define XXH_CPU_LITTLE_ENDIAN   (*(char*)(&one))
+#endif
+
+
+//**************************************
+// Macros
+//**************************************
+#define XXH_STATIC_ASSERT(c)   { enum { XXH_static_assert = 1/(!!(c)) }; }    // use only *after* variable declarations
+
+
+//****************************
+// Memory reads
+//****************************
+typedef enum { XXH_aligned, XXH_unaligned } XXH_alignment;
+
+FORCE_INLINE U32 XXH_readLE32_align(const U32* ptr, XXH_endianess endian, XXH_alignment align)
+{ 
+    if (align==XXH_unaligned)
+        return endian==XXH_littleEndian ? A32(ptr) : XXH_swap32(A32(ptr)); 
+    else
+        return endian==XXH_littleEndian ? *ptr : XXH_swap32(*ptr); 
+}
+
+FORCE_INLINE U32 XXH_readLE32(const U32* ptr, XXH_endianess endian) { return XXH_readLE32_align(ptr, endian, XXH_unaligned); }
+
+
+//****************************
+// Simple Hash Functions
+//****************************
+FORCE_INLINE U32 XXH32_endian_align(const void* input, int len, U32 seed, XXH_endianess endian, XXH_alignment align)
+{
+    const BYTE* p = (const BYTE*)input;
+    const BYTE* const bEnd = p + len;
+    U32 h32;
+
+#ifdef XXH_ACCEPT_NULL_INPUT_POINTER
+    if (p==NULL) { len=0; p=(const BYTE*)(size_t)16; }
+#endif
+
+    if (len>=16)
+    {
+        const BYTE* const limit = bEnd - 16;
+        U32 v1 = seed + PRIME32_1 + PRIME32_2;
+        U32 v2 = seed + PRIME32_2;
+        U32 v3 = seed + 0;
+        U32 v4 = seed - PRIME32_1;
+
+        do
+        {
+            v1 += XXH_readLE32_align((const U32*)p, endian, align) * PRIME32_2; v1 = XXH_rotl32(v1, 13); v1 *= PRIME32_1; p+=4;
+            v2 += XXH_readLE32_align((const U32*)p, endian, align) * PRIME32_2; v2 = XXH_rotl32(v2, 13); v2 *= PRIME32_1; p+=4;
+            v3 += XXH_readLE32_align((const U32*)p, endian, align) * PRIME32_2; v3 = XXH_rotl32(v3, 13); v3 *= PRIME32_1; p+=4;
+            v4 += XXH_readLE32_align((const U32*)p, endian, align) * PRIME32_2; v4 = XXH_rotl32(v4, 13); v4 *= PRIME32_1; p+=4;
+        } while (p<=limit);
+
+        h32 = XXH_rotl32(v1, 1) + XXH_rotl32(v2, 7) + XXH_rotl32(v3, 12) + XXH_rotl32(v4, 18);
+    }
+    else
+    {
+        h32  = seed + PRIME32_5;
+    }
+
+    h32 += (U32) len;
+
+    while (p<=bEnd-4)
+    {
+        h32 += XXH_readLE32_align((const U32*)p, endian, align) * PRIME32_3;
+        h32  = XXH_rotl32(h32, 17) * PRIME32_4 ;
+        p+=4;
+    }
+
+    while (p<bEnd)
+    {
+        h32 += (*p) * PRIME32_5;
+        h32 = XXH_rotl32(h32, 11) * PRIME32_1 ;
+        p++;
+    }
+
+    h32 ^= h32 >> 15;
+    h32 *= PRIME32_2;
+    h32 ^= h32 >> 13;
+    h32 *= PRIME32_3;
+    h32 ^= h32 >> 16;
+
+    return h32;
+}
+
+
+U32 XXH32(const void* input, int len, U32 seed)
+{
+#if 0
+    // Simple version, good for code maintenance, but unfortunately slow for small inputs
+    void* state = XXH32_init(seed);
+    XXH32_update(state, input, len);
+    return XXH32_digest(state);
+#else
+    XXH_endianess endian_detected = (XXH_endianess)XXH_CPU_LITTLE_ENDIAN;
+
+#  if !defined(XXH_USE_UNALIGNED_ACCESS)
+    if ((((size_t)input) & 3))   // Input is aligned, let's leverage the speed advantage
+    {
+        if ((endian_detected==XXH_littleEndian) || XXH_FORCE_NATIVE_FORMAT)
+            return XXH32_endian_align(input, len, seed, XXH_littleEndian, XXH_aligned);
+        else
+            return XXH32_endian_align(input, len, seed, XXH_bigEndian, XXH_aligned);
+    }
+#  endif
+
+    if ((endian_detected==XXH_littleEndian) || XXH_FORCE_NATIVE_FORMAT)
+        return XXH32_endian_align(input, len, seed, XXH_littleEndian, XXH_unaligned);
+    else
+        return XXH32_endian_align(input, len, seed, XXH_bigEndian, XXH_unaligned);
+#endif
+}
+
+
+//****************************
+// Advanced Hash Functions
+//****************************
+
+struct XXH_state32_t
+{
+    U64 total_len;
+    U32 seed;
+    U32 v1;
+    U32 v2;
+    U32 v3;
+    U32 v4;
+    int memsize;
+    char memory[16];
+};
+
+
+int XXH32_sizeofState() 
+{
+    XXH_STATIC_ASSERT(XXH32_SIZEOFSTATE >= sizeof(struct XXH_state32_t));   // A compilation error here means XXH32_SIZEOFSTATE is not large enough
+    return sizeof(struct XXH_state32_t); 
+}
+
+
+XXH_errorcode XXH32_resetState(void* state_in, U32 seed)
+{ 
+    struct XXH_state32_t * state = (struct XXH_state32_t *) state_in;
+    state->seed = seed;
+    state->v1 = seed + PRIME32_1 + PRIME32_2;
+    state->v2 = seed + PRIME32_2;
+    state->v3 = seed + 0;
+    state->v4 = seed - PRIME32_1;
+    state->total_len = 0;
+    state->memsize = 0;
+    return XXH_OK;
+}
+
+
+void* XXH32_init (U32 seed)
+{
+    void* state = XXH_malloc (sizeof(struct XXH_state32_t));
+    XXH32_resetState(state, seed);
+    return state;
+}
+
+
+FORCE_INLINE XXH_errorcode XXH32_update_endian (void* state_in, const void* input, int len, XXH_endianess endian)
+{
+    struct XXH_state32_t * state = (struct XXH_state32_t *) state_in;
+    const BYTE* p = (const BYTE*)input;
+    const BYTE* const bEnd = p + len;
+
+#ifdef XXH_ACCEPT_NULL_INPUT_POINTER
+    if (input==NULL) return XXH_ERROR;
+#endif
+
+    state->total_len += len;
+
+    if (state->memsize + len < 16)   // fill in tmp buffer
+    {
+        XXH_memcpy(state->memory + state->memsize, input, len);
+        state->memsize +=  len;
+        return XXH_OK;
+    }
+
+    if (state->memsize)   // some data left from previous update
+    {
+        XXH_memcpy(state->memory + state->memsize, input, 16-state->memsize);
+        {
+            const U32* p32 = (const U32*)state->memory;
+            state->v1 += XXH_readLE32(p32, endian) * PRIME32_2; state->v1 = XXH_rotl32(state->v1, 13); state->v1 *= PRIME32_1; p32++;
+            state->v2 += XXH_readLE32(p32, endian) * PRIME32_2; state->v2 = XXH_rotl32(state->v2, 13); state->v2 *= PRIME32_1; p32++; 
+            state->v3 += XXH_readLE32(p32, endian) * PRIME32_2; state->v3 = XXH_rotl32(state->v3, 13); state->v3 *= PRIME32_1; p32++;
+            state->v4 += XXH_readLE32(p32, endian) * PRIME32_2; state->v4 = XXH_rotl32(state->v4, 13); state->v4 *= PRIME32_1; p32++;
+        }
+        p += 16-state->memsize;
+        state->memsize = 0;
+    }
+
+    if (p <= bEnd-16)
+    {
+        const BYTE* const limit = bEnd - 16;
+        U32 v1 = state->v1;
+        U32 v2 = state->v2;
+        U32 v3 = state->v3;
+        U32 v4 = state->v4;
+
+        do
+        {
+            v1 += XXH_readLE32((const U32*)p, endian) * PRIME32_2; v1 = XXH_rotl32(v1, 13); v1 *= PRIME32_1; p+=4;
+            v2 += XXH_readLE32((const U32*)p, endian) * PRIME32_2; v2 = XXH_rotl32(v2, 13); v2 *= PRIME32_1; p+=4;
+            v3 += XXH_readLE32((const U32*)p, endian) * PRIME32_2; v3 = XXH_rotl32(v3, 13); v3 *= PRIME32_1; p+=4;
+            v4 += XXH_readLE32((const U32*)p, endian) * PRIME32_2; v4 = XXH_rotl32(v4, 13); v4 *= PRIME32_1; p+=4;
+        } while (p<=limit);
+
+        state->v1 = v1;
+        state->v2 = v2;
+        state->v3 = v3;
+        state->v4 = v4;
+    }
+
+    if (p < bEnd)
+    {
+        XXH_memcpy(state->memory, p, bEnd-p);
+        state->memsize = (int)(bEnd-p);
+    }
+
+    return XXH_OK;
+}
+
+XXH_errorcode XXH32_update (void* state_in, const void* input, int len)
+{
+    XXH_endianess endian_detected = (XXH_endianess)XXH_CPU_LITTLE_ENDIAN;
+    
+    if ((endian_detected==XXH_littleEndian) || XXH_FORCE_NATIVE_FORMAT)
+        return XXH32_update_endian(state_in, input, len, XXH_littleEndian);
+    else
+        return XXH32_update_endian(state_in, input, len, XXH_bigEndian);
+}
+
+
+
+FORCE_INLINE U32 XXH32_intermediateDigest_endian (void* state_in, XXH_endianess endian)
+{
+    struct XXH_state32_t * state = (struct XXH_state32_t *) state_in;
+    const BYTE * p = (const BYTE*)state->memory;
+    BYTE* bEnd = (BYTE*)state->memory + state->memsize;
+    U32 h32;
+
+    if (state->total_len >= 16)
+    {
+        h32 = XXH_rotl32(state->v1, 1) + XXH_rotl32(state->v2, 7) + XXH_rotl32(state->v3, 12) + XXH_rotl32(state->v4, 18);
+    }
+    else
+    {
+        h32  = state->seed + PRIME32_5;
+    }
+
+    h32 += (U32) state->total_len;
+
+    while (p<=bEnd-4)
+    {
+        h32 += XXH_readLE32((const U32*)p, endian) * PRIME32_3;
+        h32  = XXH_rotl32(h32, 17) * PRIME32_4;
+        p+=4;
+    }
+
+    while (p<bEnd)
+    {
+        h32 += (*p) * PRIME32_5;
+        h32 = XXH_rotl32(h32, 11) * PRIME32_1;
+        p++;
+    }
+
+    h32 ^= h32 >> 15;
+    h32 *= PRIME32_2;
+    h32 ^= h32 >> 13;
+    h32 *= PRIME32_3;
+    h32 ^= h32 >> 16;
+
+    return h32;
+}
+
+
+U32 XXH32_intermediateDigest (void* state_in)
+{
+    XXH_endianess endian_detected = (XXH_endianess)XXH_CPU_LITTLE_ENDIAN;
+    
+    if ((endian_detected==XXH_littleEndian) || XXH_FORCE_NATIVE_FORMAT)
+        return XXH32_intermediateDigest_endian(state_in, XXH_littleEndian);
+    else
+        return XXH32_intermediateDigest_endian(state_in, XXH_bigEndian);
+}
+
+
+U32 XXH32_digest (void* state_in)
+{
+    U32 h32 = XXH32_intermediateDigest(state_in);
+
+    XXH_free(state_in);
+
+    return h32;
+}

thirdparty/ros/ros_comm/utilities/roslz4/src/xxhash.h

@@ -0,0 +1,164 @@
+/*
+   xxHash - Fast Hash algorithm
+   Header File
+   Copyright (C) 2012-2014, Yann Collet.
+   BSD 2-Clause License (http://www.opensource.org/licenses/bsd-license.php)
+
+   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.
+  
+   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.
+
+   You can contact the author at :
+   - xxHash source repository : http://code.google.com/p/xxhash/
+*/
+
+/* Notice extracted from xxHash homepage :
+
+xxHash is an extremely fast Hash algorithm, running at RAM speed limits.
+It also successfully passes all tests from the SMHasher suite.
+
+Comparison (single thread, Windows Seven 32 bits, using SMHasher on a Core 2 Duo @3GHz)
+
+Name            Speed       Q.Score   Author
+xxHash          5.4 GB/s     10
+CrapWow         3.2 GB/s      2       Andrew
+MumurHash 3a    2.7 GB/s     10       Austin Appleby
+SpookyHash      2.0 GB/s     10       Bob Jenkins
+SBox            1.4 GB/s      9       Bret Mulvey
+Lookup3         1.2 GB/s      9       Bob Jenkins
+SuperFastHash   1.2 GB/s      1       Paul Hsieh
+CityHash64      1.05 GB/s    10       Pike & Alakuijala
+FNV             0.55 GB/s     5       Fowler, Noll, Vo
+CRC32           0.43 GB/s     9
+MD5-32          0.33 GB/s    10       Ronald L. Rivest
+SHA1-32         0.28 GB/s    10
+
+Q.Score is a measure of quality of the hash function. 
+It depends on successfully passing SMHasher test set. 
+10 is a perfect score.
+*/
+
+#pragma once
+
+#if defined (__cplusplus)
+extern "C" {
+#endif
+
+
+//****************************
+// Type
+//****************************
+typedef enum { XXH_OK=0, XXH_ERROR } XXH_errorcode;
+
+
+
+//****************************
+// Simple Hash Functions
+//****************************
+
+unsigned int XXH32 (const void* input, int len, unsigned int seed);
+
+/*
+XXH32() :
+    Calculate the 32-bits hash of sequence of length "len" stored at memory address "input".
+    The memory between input & input+len must be valid (allocated and read-accessible).
+    "seed" can be used to alter the result predictably.
+    This function successfully passes all SMHasher tests.
+    Speed on Core 2 Duo @ 3 GHz (single thread, SMHasher benchmark) : 5.4 GB/s
+    Note that "len" is type "int", which means it is limited to 2^31-1.
+    If your data is larger, use the advanced functions below.
+*/
+
+
+
+//****************************
+// Advanced Hash Functions
+//****************************
+
+void*         XXH32_init   (unsigned int seed);
+XXH_errorcode XXH32_update (void* state, const void* input, int len);
+unsigned int  XXH32_digest (void* state);
+
+/*
+These functions calculate the xxhash of an input provided in several small packets,
+as opposed to an input provided as a single block.
+
+It must be started with :
+void* XXH32_init()
+The function returns a pointer which holds the state of calculation.
+
+This pointer must be provided as "void* state" parameter for XXH32_update().
+XXH32_update() can be called as many times as necessary.
+The user must provide a valid (allocated) input.
+The function returns an error code, with 0 meaning OK, and any other value meaning there is an error.
+Note that "len" is type "int", which means it is limited to 2^31-1. 
+If your data is larger, it is recommended to chunk your data into blocks 
+of size for example 2^30 (1GB) to avoid any "int" overflow issue.
+
+Finally, you can end the calculation anytime, by using XXH32_digest().
+This function returns the final 32-bits hash.
+You must provide the same "void* state" parameter created by XXH32_init().
+Memory will be freed by XXH32_digest().
+*/
+
+
+int           XXH32_sizeofState();
+XXH_errorcode XXH32_resetState(void* state, unsigned int seed);
+
+#define       XXH32_SIZEOFSTATE 48
+typedef struct { long long ll[(XXH32_SIZEOFSTATE+(sizeof(long long)-1))/sizeof(long long)]; } XXH32_stateSpace_t;
+/*
+These functions allow user application to make its own allocation for state.
+
+XXH32_sizeofState() is used to know how much space must be allocated for the xxHash 32-bits state.
+Note that the state must be aligned to access 'long long' fields. Memory must be allocated and referenced by a pointer.
+This pointer must then be provided as 'state' into XXH32_resetState(), which initializes the state.
+
+For static allocation purposes (such as allocation on stack, or freestanding systems without malloc()),
+use the structure XXH32_stateSpace_t, which will ensure that memory space is large enough and correctly aligned to access 'long long' fields.
+*/
+
+
+unsigned int XXH32_intermediateDigest (void* state);
+/*
+This function does the same as XXH32_digest(), generating a 32-bit hash,
+but preserve memory context.
+This way, it becomes possible to generate intermediate hashes, and then continue feeding data with XXH32_update().
+To free memory context, use XXH32_digest(), or free().
+*/
+
+
+
+//****************************
+// Deprecated function names
+//****************************
+// The following translations are provided to ease code transition
+// You are encouraged to no longer this function names
+#define XXH32_feed   XXH32_update
+#define XXH32_result XXH32_digest
+#define XXH32_getIntermediateResult XXH32_intermediateDigest
+
+
+
+#if defined (__cplusplus)
+}
+#endif

thirdparty/ros/ros_comm/utilities/roslz4/test/roslz4_test.cpp

@@ -0,0 +1,133 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2014, Ben Charrow
+*  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 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.
+********************************************************************/
+
+#include <gtest/gtest.h>
+
+#include <roslz4/lz4s.h>
+
+class CompressATest :public testing::Test {
+protected:
+  void SetUp() {
+    for (size_t i = 0; i < sizeof(input); ++i) {
+      input[i] = 'a';
+    }
+    for (size_t i = 0; i < sizeof(output); ++i) {
+      output[i] = 0;
+    }
+    for (size_t i = 0; i < sizeof(other); ++i) {
+      other[i] = 0;
+    }
+  }
+
+  char input[1024];
+  char output[1048];
+  char other[1024];
+};
+
+TEST_F(CompressATest, Stream) {
+  // Compression
+  roslz4_stream stream;
+  int ret;
+  ret = roslz4_compressStart(&stream, 4);
+  ASSERT_EQ(ROSLZ4_OK, ret);
+
+  stream.input_left = sizeof(input);
+  stream.input_next = input;
+  stream.output_left = sizeof(output);
+  stream.output_next = output;
+
+  int counter;
+  for (counter = 0; ret == ROSLZ4_OK; ++counter) {
+    ret = roslz4_compress(&stream, ROSLZ4_FINISH);
+  }
+  ASSERT_EQ(ROSLZ4_STREAM_END, ret);
+
+  int output_size = stream.output_next - output;
+  roslz4_compressEnd(&stream);
+
+  // Decompression
+  stream.input_left = output_size;
+  stream.input_next = output;
+  stream.output_left = sizeof(other);
+  stream.output_next = other;
+
+  ret = roslz4_decompressStart(&stream);
+  ASSERT_EQ(ROSLZ4_OK, ret);
+
+  ret = roslz4_decompress(&stream);
+  ASSERT_EQ(ROSLZ4_STREAM_END, ret);
+
+  roslz4_decompressEnd(&stream);
+
+  for (size_t i = 0; i < sizeof(other); ++i) {
+    ASSERT_EQ(input[i], other[i]) <<  "Original and uncompressed data differ at index " << i;
+  }
+}
+
+TEST_F(CompressATest, Oneshot) {
+  // Compression
+  unsigned int comp_size = sizeof(output);
+  int ret = roslz4_buffToBuffCompress(input, sizeof(input), output, &comp_size,
+                                      4);
+  ASSERT_EQ(ROSLZ4_OK, ret);
+
+  // Decompression
+  unsigned int decomp_size = sizeof(other);
+  ret = roslz4_buffToBuffDecompress(output, comp_size, other, &decomp_size);
+  ASSERT_EQ(ROSLZ4_OK, ret);
+  ASSERT_EQ(sizeof(input), decomp_size);
+
+  for (size_t i = 0; i < sizeof(other); ++i) {
+    ASSERT_EQ(input[i], other[i]) << "Original and uncompressed data differ at index " << i;
+  }
+}
+
+TEST_F(CompressATest, OneshotDataCorruption) {
+  unsigned int comp_size = sizeof(output);
+  int ret = roslz4_buffToBuffCompress(input, sizeof(input), output, &comp_size,
+                                      4);
+  ASSERT_EQ(ROSLZ4_OK, ret);
+
+  output[20] += 1;
+
+  unsigned int decomp_size = sizeof(other);
+  ret = roslz4_buffToBuffDecompress(output, comp_size, other, &decomp_size);
+  ASSERT_EQ(ROSLZ4_DATA_ERROR, ret);
+}
+
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

thirdparty/ros/ros_comm/utilities/roswtf/CHANGELOG.rst

@@ -0,0 +1,172 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package roswtf
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.14 (2018-08-23)
+--------------------
+
+1.12.13 (2018-02-21)
+--------------------
+
+1.12.12 (2017-11-16)
+--------------------
+
+1.12.11 (2017-11-07)
+--------------------
+
+1.12.10 (2017-11-06)
+--------------------
+
+1.12.9 (2017-11-06)
+-------------------
+
+1.12.8 (2017-11-06)
+-------------------
+* improve roswtf tests (`#1102 <https://github.com/ros/ros_comm/pull/1102>`_)
+
+1.12.7 (2017-02-17)
+-------------------
+
+1.12.6 (2016-10-26)
+-------------------
+
+1.12.5 (2016-09-30)
+-------------------
+
+1.12.4 (2016-09-19)
+-------------------
+
+1.12.3 (2016-09-17)
+-------------------
+
+1.12.2 (2016-06-03)
+-------------------
+
+1.12.1 (2016-04-18)
+-------------------
+
+1.12.0 (2016-03-18)
+-------------------
+
+1.11.18 (2016-03-17)
+--------------------
+
+1.11.17 (2016-03-11)
+--------------------
+
+1.11.16 (2015-11-09)
+--------------------
+
+1.11.15 (2015-10-13)
+--------------------
+
+1.11.14 (2015-09-19)
+--------------------
+* add optional dependency on geneus to make roswtf tests pass in jade
+
+1.11.13 (2015-04-28)
+--------------------
+
+1.11.12 (2015-04-27)
+--------------------
+
+1.11.11 (2015-04-16)
+--------------------
+* support IPv6 addresses containing percentage symbols (`#585 <https://github.com/ros/ros_comm/issues/585>`_)
+
+1.11.10 (2014-12-22)
+--------------------
+
+1.11.9 (2014-08-18)
+-------------------
+
+1.11.8 (2014-08-04)
+-------------------
+
+1.11.7 (2014-07-18)
+-------------------
+
+1.11.6 (2014-07-10)
+-------------------
+
+1.11.5 (2014-06-24)
+-------------------
+
+1.11.4 (2014-06-16)
+-------------------
+* Python 3 compatibility (`#426 <https://github.com/ros/ros_comm/issues/426>`_, `#427 <https://github.com/ros/ros_comm/issues/427>`_)
+
+1.11.3 (2014-05-21)
+-------------------
+
+1.11.2 (2014-05-08)
+-------------------
+
+1.11.1 (2014-05-07)
+-------------------
+* update roswtf test for upcoming rospack 2.2.3
+* add architecture_independent flag in package.xml (`#391 <https://github.com/ros/ros_comm/issues/391>`_)
+
+1.11.0 (2014-03-04)
+-------------------
+* make rostest in CMakeLists optional (`ros/rosdistro#3010 <https://github.com/ros/rosdistro/issues/3010>`_)
+
+1.10.0 (2014-02-11)
+-------------------
+
+1.9.54 (2014-01-27)
+-------------------
+* fix roswtf checks to not require release-only python packages to be installed
+* add missing run/test dependencies on rosbuild to get ROS_ROOT environment variable
+
+1.9.53 (2014-01-14)
+-------------------
+
+1.9.52 (2014-01-08)
+-------------------
+
+1.9.51 (2014-01-07)
+-------------------
+* do not warn about not existing stacks folder in a catkin workspace
+
+1.9.50 (2013-10-04)
+-------------------
+
+1.9.49 (2013-09-16)
+-------------------
+
+1.9.48 (2013-08-21)
+-------------------
+
+1.9.47 (2013-07-03)
+-------------------
+* check for CATKIN_ENABLE_TESTING to enable configure without tests
+
+1.9.46 (2013-06-18)
+-------------------
+
+1.9.45 (2013-06-06)
+-------------------
+
+1.9.44 (2013-03-21)
+-------------------
+* fix ROS_ROOT check to access trailing 'rosbuild'
+
+1.9.43 (2013-03-13)
+-------------------
+
+1.9.42 (2013-03-08)
+-------------------
+
+1.9.41 (2013-01-24)
+-------------------
+
+1.9.40 (2013-01-13)
+-------------------
+* add checks for pip packages and rosdep
+* fix check for catkin_pkg
+* fix for thread race condition causes incorrect graph connectivity analysis
+
+1.9.39 (2012-12-29)
+-------------------
+* first public release for Groovy

thirdparty/ros/ros_comm/utilities/roswtf/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(roswtf)
+find_package(catkin REQUIRED)
+catkin_package()
+catkin_python_setup()
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest)
+  add_rostest(test/roswtf.test)
+  catkin_add_nosetests(test)
+endif()

thirdparty/ros/ros_comm/utilities/roswtf/package.xml

@@ -0,0 +1,32 @@
+<package>
+  <name>roswtf</name>
+  <version>1.12.14</version>
+  <description>
+     roswtf is a tool for diagnosing issues with a running ROS system. Think of it as a FAQ implemented in code.
+  </description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>BSD</license>
+
+  <url>http://ros.org/wiki/roswtf</url>
+  <author>Ken Conley</author>
+
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>rostest</build_depend>
+
+  <run_depend>python-paramiko</run_depend>
+  <run_depend>python-rospkg</run_depend>
+  <run_depend>rosbuild</run_depend>
+  <run_depend>rosgraph</run_depend>
+  <run_depend>roslaunch</run_depend>
+  <run_depend>roslib</run_depend>
+  <run_depend>rosnode</run_depend>
+  <run_depend>rosservice</run_depend>
+
+  <test_depend>cmake_modules</test_depend>  <!-- since the other packages recursively depend on it roswtf needs to find it during its own tests -->
+
+  <export>
+    <rosdoc config="rosdoc.yaml"/>
+    <architecture_independent/>
+  </export>
+</package>

thirdparty/ros/ros_comm/utilities/roswtf/rosdoc.yaml

@@ -0,0 +1 @@
+ - builder: epydoc

thirdparty/ros/ros_comm/utilities/roswtf/scripts/roswtf

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+# 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 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.
+
+import roswtf
+roswtf.roswtf_main()

thirdparty/ros/ros_comm/utilities/roswtf/setup.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['roswtf'],
+    package_dir={'': 'src'},
+    scripts=['scripts/roswtf'],
+    requires=['genmsg', 'genpy', 'roslib', 'rospkg']
+)
+
+setup(**d)

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/__init__.py

@@ -0,0 +1,242 @@
+# 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 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.
+#
+# Revision $Id$
+
+"""
+roswtf command-line tool.
+"""
+
+from __future__ import print_function
+
+import os
+import socket
+import sys
+import traceback
+
+import rospkg
+import rosgraph.names
+    
+def yaml_results(ctx):
+    cd = ctx.as_dictionary()
+    d = {}
+    d['warnings'] = {}
+    d['errors'] = {}
+    wd = d['warnings']
+    for warn in ctx.warnings:
+        wd[warn.format_msg%cd] = warn.return_val
+    ed = d['warnings']        
+    for err in ctx.warnings:
+        ed[err.format_msg%cd] = err.return_val
+    import yaml
+    print(yaml.dump(d))
+
+def print_results(ctx):
+    if not ctx.warnings and not ctx.errors:
+        print("No errors or warnings")
+    else:
+        if ctx.warnings:
+            print("Found %s warning(s).\nWarnings are things that may be just fine, but are sometimes at fault\n" % len(ctx.warnings))
+            for warn in ctx.warnings:
+                print('\033[1mWARNING\033[0m', warn.msg)
+            print('')
+
+        if ctx.errors:
+            print("Found %s error(s).\n"%len(ctx.errors))
+            for e in ctx.errors:
+                print('\033[31m\033[1mERROR\033[0m', e.msg)
+                #print("ERROR:", e.msg
+    
+def roswtf_main():
+    try:
+        import std_msgs.msg
+        import rosgraph_msgs.msg
+    except ImportError:
+        print("ERROR: The core ROS message libraries (std_msgs and rosgraph_msgs) have not been built.")
+        sys.exit(1)
+    
+    from roswtf.context import WtfException
+    try:
+        _roswtf_main()
+    except WtfException as e:
+        print(str(e), file=sys.stderr)
+        
+def _roswtf_main():
+    launch_files = names = None
+    # performance optimization
+    rospack = rospkg.RosPack()
+    all_pkgs = rospack.list()
+
+    import optparse
+    parser = optparse.OptionParser(usage="usage: roswtf [launch file]", description="roswtf is a tool for verifying a ROS installation and running system. Checks provided launchfile if provided, else current stack or package.")
+    # #2268
+    parser.add_option("--all", 
+                      dest="all_packages", default=False,
+                      action="store_true",
+                      help="run roswtf against all packages")
+    # #2270
+    parser.add_option("--no-plugins", 
+                      dest="disable_plugins", default=False,
+                      action="store_true",
+                      help="disable roswtf plugins")
+
+    parser.add_option("--offline", 
+                      dest="offline", default=False,
+                      action="store_true",
+                      help="only run offline tests")
+
+    #TODO: --all-pkgs option
+    options, args = parser.parse_args()
+    if args:
+        launch_files = args
+        if 0:
+            # disable names for now as don't have any rules yet
+            launch_files = [a for a in args if os.path.isfile(a)]
+            names = [a for a in args if not a in launch_files]
+            names = [rosgraph.names.script_resolve_name('/roswtf', n) for n in names]
+
+    from roswtf.context import WtfContext
+    from roswtf.environment import wtf_check_environment, invalid_url, ros_root_check
+    from roswtf.graph import wtf_check_graph
+    import roswtf.rosdep_db
+    import roswtf.py_pip_deb_checks
+    import roswtf.network
+    import roswtf.packages
+    import roswtf.roslaunchwtf
+    import roswtf.stacks    
+    import roswtf.plugins
+    if not options.disable_plugins:
+        static_plugins, online_plugins = roswtf.plugins.load_plugins()
+    else:
+        static_plugins, online_plugins = [], []
+        
+    # - do a ros_root check first and abort if it fails as rest of tests are useless after that
+    error = ros_root_check(None, ros_root=os.environ['ROS_ROOT'])
+    if error:
+        print("ROS_ROOT is invalid: "+str(error))
+        sys.exit(1)
+
+    all_warnings = []
+    all_errors = []
+    
+    if launch_files:
+        ctx = WtfContext.from_roslaunch(launch_files)
+        #TODO: allow specifying multiple roslaunch files
+    else:
+        curr_package = rospkg.get_package_name('.')
+        if curr_package:
+            print("Package:", curr_package)
+            ctx = WtfContext.from_package(curr_package)
+            #TODO: load all .launch files in package
+        elif os.path.isfile('stack.xml'):
+            curr_stack = os.path.basename(os.path.abspath('.'))
+            print("Stack:", curr_stack)
+            ctx = WtfContext.from_stack(curr_stack)
+        else:
+            print("No package or stack in context")
+            ctx = WtfContext.from_env()
+        if options.all_packages:
+            print("roswtf will run against all packages")
+            ctx.pkgs = all_pkgs
+
+    # static checks
+    wtf_check_environment(ctx)
+    roswtf.rosdep_db.wtf_check(ctx)
+    roswtf.py_pip_deb_checks.wtf_check(ctx)
+    roswtf.network.wtf_check(ctx)
+    roswtf.packages.wtf_check(ctx)
+    roswtf.stacks.wtf_check(ctx)    
+    roswtf.roslaunchwtf.wtf_check_static(ctx)
+    for p in static_plugins:
+        p(ctx)
+
+    print("="*80)
+    print("Static checks summary:\n")
+    print_results(ctx)
+
+    # Save static results and start afresh for online checks
+    all_warnings.extend(ctx.warnings)
+    all_errors.extend(ctx.errors)
+    del ctx.warnings[:]
+    del ctx.errors[:]    
+
+    # test online
+    print("="*80)
+
+    try:
+
+        if options.offline or not ctx.ros_master_uri or invalid_url(ctx.ros_master_uri) or not rosgraph.is_master_online():
+            online_checks = False
+        else:
+            online_checks = True
+        if online_checks:
+            online_checks = True
+            print("Beginning tests of your ROS graph. These may take awhile...")
+            
+            # online checks
+            wtf_check_graph(ctx, names=names)
+        elif names:
+            # TODO: need to rework this logic
+            print("\nCannot communicate with master, unable to diagnose [%s]"%(', '.join(names)))
+            return
+        else:
+            print("\nROS Master does not appear to be running.\nOnline graph checks will not be run.\nROS_MASTER_URI is [%s]"%(ctx.ros_master_uri))
+            return
+
+        # spin up a roswtf node so we can subscribe to messages
+        import rospy
+        rospy.init_node('roswtf', anonymous=True)
+
+        online_checks = True
+        roswtf.roslaunchwtf.wtf_check_online(ctx)
+
+        for p in online_plugins:
+            online_checks = True
+            p(ctx)
+
+        if online_checks:
+            # done
+            print("\nOnline checks summary:\n")
+            print_results(ctx)
+            
+    except roswtf.context.WtfException as e:
+        print(str(e), file=sys.stderr)
+        print("\nAborting checks, partial results summary:\n")
+        print_results(ctx)
+    except Exception as e:
+        traceback.print_exc()
+        print(str(e), file=sys.stderr)
+        print("\nAborting checks, partial results summary:\n")
+        print_results(ctx)
+
+    #TODO: print results in YAML if run remotely
+    #yaml_results(ctx)

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/context.py

@@ -0,0 +1,306 @@
+# 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 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.
+#
+# Revision $Id$
+
+"""
+L{WtfContext} object, which is commonly used throughout the roswtf
+APIs to pass state.
+"""
+
+import os
+import sys
+
+import rospkg
+import rospkg.environment
+
+import rosgraph
+
+import roslaunch.depends
+import roslaunch.substitution_args
+
+from roswtf.model import WtfWarning
+
+class WtfException(Exception):
+    """
+    Base exception class of roswtf-related issues.
+    """
+    pass
+
+class WtfContext(object):
+    """
+    WtfContext stores common state about the ROS filesystem and online
+    environment. The primary use of this is for convenience (not
+    having to load this state manually) and performance (not having to
+    do the same calculation repeatedly).
+    """
+    __slots__ = ['pkg', 'pkg_dir', 'pkgs',
+                 'stack', 'stack_dir', 'stacks',
+                 'manifest_file', 'manifest',
+                 'env', 'ros_root', 'ros_package_path', 'pythonpath',
+                 'ros_master_uri',
+                 'roslaunch_uris',
+                 'launch_files',
+                 'launch_file_deps',
+                 'launch_file_missing_deps',
+                 'system_state',
+                 'service_providers',
+                 'topics', 'services',
+                 'nodes', 'uri_node_map',
+                 'expected_edges',
+                 'actual_edges',
+                 'unconnected_subscriptions',
+                 'use_sim_time',
+                 'warnings', 'errors',
+                 'rospack', 'rosstack']
+    
+    def __init__(self):
+        # main package we are running 
+        self.pkg = None
+        self.pkg_dir = None
+        # main stack we are running 
+        self.stack = None
+        self.stack_dir = None
+        
+        # - list of all packages involved in this check
+        self.pkgs = []
+        # - list of all stacks involved in this check        
+        self.stacks = []        
+
+        # manifest location of package that we are running 
+        self.manifest_file = None
+        # manifest of package that we are running 
+        self.manifest = None
+
+        # environment variables 
+        self.env = {}
+
+        # provide these for convenience
+        self.ros_root = None
+        self.ros_package_path = None
+        self.pythonpath = None
+        
+        # launch file that is being run
+        self.launch_files = None
+        self.launch_file_deps = None
+        self.launch_file_missing_deps = None        
+        
+        # online state
+        self.roslaunch_uris = None 
+        self.system_state = None #master.getSystemState
+        self.topics = None
+        self.services = None
+        self.service_providers = None #names of nodes with services
+        self.nodes = None
+        self.uri_node_map = {}
+        self.expected_edges = None
+        self.actual_edges = None
+        self.unconnected_subscriptions = None
+        self.use_sim_time = None
+        
+        # caching rospack instance
+        self.rospack = self.rosstack = None
+        
+        # warnings that we have collected so far
+        self.warnings = []
+        # errors that we have collected so far
+        self.errors = []
+
+    def as_dictionary(self):
+        """
+        @return: dictionary representation of context, which is
+        useful for producing error messages
+        @rtype: dict
+        """
+        return dict((s, getattr(self, s)) for s in self.__slots__)
+
+    @staticmethod
+    def from_roslaunch(roslaunch_files, env=None):
+        """
+        @param roslaunch_file: roslaunch_file to load from
+        @type  roslaunch_file: str
+        """
+        if env is None:
+            env = os.environ
+
+        # can't go any further if launch file doesn't validate
+        l, c = roslaunch.XmlLoader(), roslaunch.ROSLaunchConfig()
+        for f in roslaunch_files:
+            try:
+                l.load(f, c, verbose=False) 
+            except roslaunch.RLException as e:
+                raise WtfException("Unable to load roslaunch file [%s]: %s"%(f, str(e)))
+
+        ctx = WtfContext()
+        ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
+        ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
+
+        ctx.launch_files = roslaunch_files
+        _load_roslaunch(ctx, roslaunch_files)
+        # ctx.pkg and ctx.stack initialized by _load_roslaunch
+        _load_pkg(ctx, ctx.pkg)
+        if ctx.stack:
+            _load_stack(ctx, ctx.stack)        
+        _load_env(ctx, env)
+        return ctx
+
+    @staticmethod
+    def from_stack(stack, env=None):
+        """
+        Initialize WtfContext from stack.
+        @param stack: stack name
+        @type  stack: str
+        @raise WtfException: if context state cannot be initialized
+        """
+        if env is None:
+            env = os.environ
+
+        ctx = WtfContext()
+        ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
+        ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
+
+        _load_stack(ctx, stack)
+        try:
+            ctx.pkgs = ctx.rosstack.packages_of(stack)
+        except rospkg.ResourceNotFound:
+            # this should be handled elsewhere
+            ctx.pkgs = []
+        _load_env(ctx, env)
+        return ctx
+    
+    @staticmethod
+    def from_package(pkg, env=None):
+        """
+        Initialize WtfContext from package name.
+
+        @param pkg: package name
+        @type  pkg: str
+        @raise WtfException: if context state cannot be initialized
+        """
+        if env is None:
+            env = os.environ
+
+        ctx = WtfContext()
+        ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
+        ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
+        
+        _load_pkg(ctx, pkg)
+        stack = ctx.rospack.stack_of(pkg)
+        if stack:
+            _load_stack(ctx, stack)
+        _load_env(ctx, env)
+        return ctx
+
+    @staticmethod
+    def from_env(env=None):
+        """
+        Initialize WtfContext from environment.
+        
+        @raise WtfException: if context state cannot be initialized
+        """
+        if env is None:
+            env = os.environ
+
+        ctx = WtfContext()
+        ctx.rospack = rospkg.RosPack(rospkg.get_ros_paths(env))
+        ctx.rosstack = rospkg.RosStack(rospkg.get_ros_paths(env))
+
+        _load_env(ctx, env)
+        return ctx
+    
+def _load_roslaunch(ctx, roslaunch_files):
+    """
+    Utility for initializing WtfContext state from roslaunch file
+    """
+    try:
+        base_pkg, file_deps, missing = roslaunch.depends.roslaunch_deps(roslaunch_files)
+        ctx.pkg = base_pkg
+        ctx.launch_file_deps = file_deps
+        ctx.launch_file_missing_deps = missing
+    except roslaunch.substitution_args.SubstitutionException as se:
+        raise WtfException("Cannot load roslaunch file(s): "+str(se))
+    except roslaunch.depends.RoslaunchDepsException as e:
+        raise WtfException(str(e))
+
+def _load_pkg(ctx, pkg):
+    """
+    Utility for initializing WtfContext state
+    @raise WtfException: if context state cannot be initialized
+    """
+    r = ctx.rospack
+    ctx.pkg = pkg
+    try:
+        ctx.pkgs = [pkg] + r.get_depends(pkg)
+    except rospkg.ResourceNotFound as e:
+        raise WtfException("Cannot find dependencies for package [%s]: missing %s"%(pkg, e))
+    try:
+        ctx.pkg_dir = r.get_path(pkg)
+        ctx.manifest_file = os.path.join(ctx.pkg_dir, 'manifest.xml')
+        ctx.manifest = r.get_manifest(pkg)
+    except rospkg.ResourceNotFound:
+        raise WtfException("Cannot locate manifest file for package [%s]"%pkg)
+    except rospkg.InvalidManifest as e:
+        raise WtfException("Package [%s] has an invalid manifest: %s"%(pkg, e))
+
+def _load_stack(ctx, stack):
+    """
+    Utility for initializing WtfContext state
+    @raise WtfException: if context state cannot be initialized
+    """
+    r = ctx.rosstack
+    ctx.stack = stack
+    try:
+        ctx.stacks = [stack] + r.get_depends(stack, implicit=True)
+    except rospkg.ResourceNotFound as e:
+        raise WtfException("Cannot load dependencies of stack [%s]: %s"%(stack, e))
+    try:
+        ctx.stack_dir = r.get_path(stack)
+    except rospkg.ResourceNotFound:
+        raise WtfException("[%s] appears to be a stack, but it's not on your ROS_PACKAGE_PATH"%stack)
+    
+    
+def _load_env(ctx, env):
+    """
+    Utility for initializing WtfContext state
+
+    @raise WtfException: if context state cannot be initialized
+    """
+    ctx.env = env
+    try:
+        ctx.ros_root = env[rospkg.environment.ROS_ROOT]
+    except KeyError:
+        raise WtfException("ROS_ROOT is not set")
+    ctx.ros_package_path = env.get(rospkg.environment.ROS_PACKAGE_PATH, None)
+    ctx.pythonpath = env.get('PYTHONPATH', None)
+    ctx.ros_master_uri = env.get(rosgraph.ROS_MASTER_URI, None)
+
+    

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/environment.py

@@ -0,0 +1,220 @@
+# 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 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.
+#
+# Revision $Id$
+
+"""
+Rules for checking ROS environment state.
+"""
+
+import os
+import socket
+import stat
+import string
+import sys
+
+from os.path import isdir, isfile
+from roswtf.rules import warning_rule, error_rule
+
+#TODO: unit tests
+
+def paths(path):
+    """
+    @return: paths contained in path variable. path must conform to OS
+    conventions for path separation (i.e. colon-separated on Unix)
+    @rtype: [str]
+    """
+    if path:
+        return path.split(os.pathsep)
+    return []
+
+def is_executable(path):
+    """
+    @return: True if path has executable permissions
+    @rtype: bool
+    """
+    mode = os.stat(path)[stat.ST_MODE]
+    return mode & (stat.S_IXUSR|stat.S_IXGRP|stat.S_IXOTH)
+
+try:
+    from urllib.parse import urlparse #Python 3
+except ImportError:
+    from urlparse import urlparse #Python 2
+
+def invalid_url(url):
+    """
+    @return: error message if \a url is not a valid url. \a url is
+    allowed to be empty as that check is considered separately.
+    @rtype: str
+    """
+    if not url:
+        return #caught by different rule
+    p = urlparse(url)
+    if p[0] != 'http':
+        return "protocol is not 'http'"
+    if not p[1]:
+        return "address is missing"
+    if not ':' in p[1]:
+        return "port number is missing"
+    try:
+        splits = p[1].split(':')
+        if len(splits) != 2:
+            return "invalid address string [%s]"%p[1]
+        int(splits[1])
+    except ValueError:
+        return "port number [%s] is invalid"%(splits[1])
+    
+# Error-checking functions for more advanced checks
+
+def ros_root_check(ctx, ros_root=None):
+    """
+    @param ros_root: override ctx, useful for when ctx is not created yet
+    @type  ros_root: str
+    """
+    if ros_root is not None:
+        path = ros_root
+    else:
+        path = ctx.ros_root
+    if os.path.basename(os.path.normpath(path)) not in ['ros', 'rosbuild']:
+        return "ROS_ROOT [%s] must end in directory named 'ros'"%path      
+    
+def _writable_dir_check(ctx, path, name):
+    """
+    If path is not None, validate that it is a writable directory
+    """
+    if path is None:
+        return
+    if isfile(path):
+        return "%s [%s] must point to a directory, not a file"%(name, path)
+    if not os.access(path, os.W_OK):
+        return "%s [%s] is not writable"%(name, path)
+
+def ros_home_check(ctx):
+    return _writable_dir_check(ctx, ctx.env.get('ROS_HOME', None), 'ROS_HOME')
+def ros_log_dir_check(ctx):
+    return _writable_dir_check(ctx, ctx.env.get('ROS_LOG_DIR', None), 'ROS_LOG_DIR')
+def ros_test_results_dir_check(ctx):
+    return _writable_dir_check(ctx, ctx.env.get('ROS_TEST_RESULTS_DIR', None), 'ROS_TEST_RESULTS_DIR')
+
+def pythonpath_check(ctx):
+    # used to have a lot more checks here, but trying to phase out need for roslib on custom PYTHONPATH
+    path = ctx.pythonpath
+    roslib_count = len(set([p for p in paths(path) if 'roslib' in p.split(os.sep)]))
+    if roslib_count > 1:
+        return "Multiple roslib directories in PYTHONPATH (there should only be one)"
+
+def rosconsole_config_file_check(ctx):
+    if 'ROSCONSOLE_CONFIG_FILE' in ctx.env:
+        return not isfile(ctx.env['ROSCONSOLE_CONFIG_FILE'])
+    
+def path_check(ctx):
+    # rosdeb setup can clobber local ros stuff, so try and detect this
+    path = ctx.env['PATH']
+    idx = path.find('/usr/bin')
+    if idx < 0:
+        return
+    if os.path.exists('/usr/lib/ros/'):
+        rr_idx = path.find(ctx.ros_root)
+        if rr_idx > -1 and rr_idx > idx:
+            return True
+        
+def ros_master_uri_hostname(ctx):
+    uri = ctx.ros_master_uri
+    parsed = urlparse(uri)
+    p = urlparse(uri)
+    if not p[1]:
+        return #caught by different rule
+    if not ':' in p[1]:
+        return #caught by different rule
+    try:
+        splits = p[1].split(':')
+        if len(splits) != 2:
+            return #caught by different rule
+        #TODO IPV6: only check for IPv6 when IPv6 is enabled
+        socket.getaddrinfo(splits[0], 0, 0, 0, socket.SOL_TCP)
+
+    except socket.gaierror:
+        return "Unknown host %s"%splits[0]
+    
+# Error/Warning Rules
+
+environment_warnings = [
+    (path_check,
+     "PATH has /usr/bin set before ROS_ROOT/bin, which can cause problems as there is system install of ROS on this machine. You may wish to put ROS_ROOT/bin first"),
+    (lambda ctx: ctx.ros_package_path is None, 
+     "ROS_PACKAGE_PATH is not set. This is not required, but is unusual"),
+    (lambda ctx: len(paths(ctx.ros_package_path)) == 0, 
+     "ROS_PACKAGE_PATH is empty. This is not required, but is unusual"),
+    (lambda ctx: not ctx.ros_master_uri,
+     "ROS_MASTER_URI is empty. This is not required, but is unusual"),
+    (ros_master_uri_hostname,
+     "Cannot resolve hostname in ROS_MASTER_URI [%(ros_master_uri)s]"),
+    (rosconsole_config_file_check,
+     "ROS_CONSOLE_CONFIG_FILE does not point to an existing file"),
+    ]
+
+environment_errors = [
+    # ROS_ROOT
+    (lambda ctx: not isdir(ctx.ros_root),
+     "ROS_ROOT [%(ros_root)s] does not point to a directory"),
+    (ros_root_check,
+     "ROS_ROOT is invalid: "),
+    
+    # ROS_PACKAGE_PATH
+    (lambda ctx: [d for d in paths(ctx.ros_package_path) if d and isfile(d)],
+     "Path(s) in ROS_PACKAGE_PATH [%(ros_package_path)s] points to a file instead of a directory: "),
+    (lambda ctx: [d for d in paths(ctx.ros_package_path) if d and not isdir(d) and not (os.path.basename(d) == 'stacks' and os.path.exists(os.path.join(os.path.dirname(d), '.catkin')))],
+     "Not all paths in ROS_PACKAGE_PATH [%(ros_package_path)s] point to an existing directory: "),
+    
+    # PYTHONPATH
+    (lambda ctx: [d for d in paths(ctx.pythonpath) if d and not isdir(d)],
+     "Not all paths in PYTHONPATH [%(pythonpath)s] point to a directory: "),
+    (pythonpath_check,
+     "PYTHONPATH [%(pythonpath)s] is invalid: "),
+
+    # ROS_HOME, ROS_LOG_DIR, ROS_TEST_RESULTS_DIR
+    (ros_home_check, "ROS_HOME is invalid: "),
+    (ros_log_dir_check, "ROS_LOG_DIR is invalid: "),    
+    (ros_test_results_dir_check, "ROS_TEST_RESULTS_DIR is invalid: "),    
+
+    (lambda ctx: invalid_url(ctx.ros_master_uri),
+     "ROS_MASTER_URI [%(ros_master_uri)s] is not a valid URL: "),
+
+    ]
+
+def wtf_check_environment(ctx):
+    #TODO: check ROS_BOOST_ROOT
+    for r in environment_warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in environment_errors:
+        error_rule(r, r[0](ctx), ctx)
+        

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/graph.py

@@ -0,0 +1,417 @@
+# 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 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.
+#
+# Revision $Id$
+
+from __future__ import print_function
+from __future__ import with_statement
+
+import os
+import itertools
+import socket
+import sys
+import time
+try:
+    from xmlrpc.client import ServerProxy
+except ImportError:
+    from xmlrpclib import ServerProxy
+
+import rospkg.environment
+
+import rosgraph
+import rosgraph.rosenv
+import rosgraph.network
+
+import rosnode
+import rosservice
+
+from roswtf.context import WtfException
+from roswtf.environment import paths, is_executable
+from roswtf.model import WtfWarning, WtfError
+from roswtf.rules import warning_rule, error_rule
+
+def _businfo(ctx, node, bus_info):
+    # [[connectionId1, destinationId1, direction1, transport1, ...]... ]
+    edges = []
+    for info in bus_info:
+        #connection_id = info[0]
+        dest_id       = info[1]
+        if dest_id.startswith('http://'):
+            if dest_id in ctx.uri_node_map:
+                dest_id = ctx.uri_node_map[dest_id]
+            else:
+                dest_id = 'unknown (%s)'%dest_id
+        direction     = info[2]
+        #transport     = info[3]
+        topic         = info[4]
+        if len(info) > 5:
+            connected = info[5]
+        else:
+            connected = True #backwards compatibility
+
+        if connected:
+            if direction == 'i':
+                edges.append((topic, dest_id, node))
+            elif direction == 'o':
+                edges.append((topic, node, dest_id))
+            elif direction == 'b':
+                print("cannot handle bidirectional edges", file=sys.stderr)
+            else:
+                raise Exception()
+
+    return edges
+
+def unexpected_edges(ctx):
+    if not ctx.system_state or not ctx.nodes:
+        return
+    unexpected = set(ctx.actual_edges) - set(ctx.expected_edges)
+    return ["%s->%s (%s)"%(p, s, t) for (t, p, s) in unexpected]
+
+def missing_edges(ctx):
+    if not ctx.system_state or not ctx.nodes:
+        return
+    missing = set(ctx.expected_edges) - set(ctx.actual_edges)
+    return ["%s->%s (%s)"%(p, s, t) for (t, p, s) in missing]
+    
+def ping_check(ctx):
+    if not ctx.system_state or not ctx.nodes:
+        return
+    _, unpinged = rosnode.rosnode_ping_all()
+    return unpinged
+
+def simtime_check(ctx):
+    if ctx.use_sim_time:
+        master = rosgraph.Master('/roswtf')
+        try:
+            pubtopics = master.getPublishedTopics('/')
+        except rosgraph.MasterException:
+            ctx.errors.append(WtfError("Cannot talk to ROS master"))
+            raise WtfException("roswtf lost connection to the ROS Master at %s"%rosgraph.rosenv.get_master_uri())
+
+        for topic, _ in pubtopics:
+            if topic in ['/time', '/clock']:
+                return
+        return True
+    
+## contact each service and make sure it returns a header
+def probe_all_services(ctx):
+    master = rosgraph.Master('/roswtf')
+    errors = []
+    for service_name in ctx.services:
+        try:
+            service_uri = master.lookupService(service_name)
+        except:
+            ctx.errors.append(WtfError("cannot contact ROS Master at %s"%rosgraph.rosenv.get_master_uri()))
+            raise WtfException("roswtf lost connection to the ROS Master at %s"%rosgraph.rosenv.get_master_uri())
+        
+        try:
+            headers = rosservice.get_service_headers(service_name, service_uri)
+            if not headers:
+                errors.append("service [%s] did not return service headers"%service_name)
+        except rosgraph.network.ROSHandshakeException as e:
+            errors.append("service [%s] appears to be malfunctioning"%service_name)
+        except Exception as e:
+            errors.append("service [%s] appears to be malfunctioning: %s"%(service_name, e))
+    return errors
+                
+def unconnected_subscriptions(ctx):
+    ret = ''
+    whitelist = ['/reset_time']
+    if ctx.use_sim_time:
+        for sub, l in ctx.unconnected_subscriptions.items():
+            l = [t for t in l if t not in whitelist]
+            if l:
+                ret += ' * %s:\n'%sub
+                ret += ''.join(["   * %s\n"%t for t in l])
+    else:
+        for sub, l in ctx.unconnected_subscriptions.items():
+            l = [t for t in l if t not in ['/time', '/clock']]
+            if l:
+                ret += ' * %s:\n'%sub
+                ret += ''.join(["   * %s\n"%t for t in l])
+    return ret
+
+graph_warnings = [
+    (unconnected_subscriptions, "The following node subscriptions are unconnected:\n"),
+    (unexpected_edges, "The following nodes are unexpectedly connected:"),
+    ]
+
+graph_errors = [
+    (simtime_check, "/use_simtime is set but no publisher of /clock is present"),
+    (ping_check, "Could not contact the following nodes:"),
+    (missing_edges, "The following nodes should be connected but aren't:"),
+    (probe_all_services, "Errors connecting to the following services:"),
+    ]
+
+def topic_timestamp_drift(ctx, t):
+    #TODO: get msg_class, if msg_class has header, receive a message
+    # and compare its time to ros time
+    if 0:
+        rospy.Subscriber(t, msg_class)
+
+#TODO: these are mainly future enhancements. It's unclear to me whether or not this will be
+#useful as most of the generic rules are capable of targetting these problems as well.
+#The only rule that in particular seems useful is the timestamp drift. It may be too
+#expensive otherwise to run, though it would be interesting to attempt to receive a
+#message from every single topic.
+        
+#TODO: parameter audit?
+service_errors = [
+    ]
+service_warnings = [
+    ]
+topic_errors = [
+    (topic_timestamp_drift, "Timestamp drift:")
+    ]
+topic_warnings = [
+    ]
+node_errors = [
+    ]
+node_warnings = [
+    ]
+
+## cache sim_time calculation sot that multiple rules can use
+def _compute_sim_time(ctx):
+    param_server = rosgraph.Master('/roswtf')
+    ctx.use_sim_time = False        
+    try:
+        val = simtime = param_server.getParam('/use_sim_time')
+        if val:
+            ctx.use_sim_time = True
+    except:
+        pass
+    
+def _compute_system_state(ctx):
+    socket.setdefaulttimeout(3.0)
+    master = rosgraph.Master('/roswtf')
+
+    # store system state
+    try:
+        val = master.getSystemState()
+    except rosgraph.MasterException:
+        return
+    ctx.system_state = val
+
+    pubs, subs, srvs = val
+    
+    # compute list of topics and services
+    topics = []
+    for t, _ in itertools.chain(pubs, subs):
+        topics.append(t)
+    services = []
+    service_providers = []
+    for s, l in srvs:
+        services.append(s)
+        service_providers.extend(l)
+    ctx.topics = topics
+    ctx.services = services
+    ctx.service_providers = service_providers
+
+    # compute list of nodes
+    nodes = []
+    for s in val:
+        for t, l in s:
+            nodes.extend(l)
+    ctx.nodes = list(set(nodes)) #uniq
+
+    # - compute reverse mapping of URI->nodename
+    count = 0
+    start = time.time()
+    for n in ctx.nodes:
+        count += 1
+        try:
+            val = master.lookupNode(n)
+        except socket.error:
+            ctx.errors.append(WtfError("cannot contact ROS Master at %s"%rosgraph.rosenv.get_master_uri()))
+            raise WtfException("roswtf lost connection to the ROS Master at %s"%rosgraph.rosenv.get_master_uri())
+        ctx.uri_node_map[val] = n
+    end = time.time()
+    # - time thresholds currently very arbitrary
+    if count:
+        if ((end - start) / count) > 1.:
+            ctx.warnings.append(WtfError("Communication with master is very slow (>1s average)"))        
+        elif (end - start) / count > .5:
+            ctx.warnings.append(WtfWarning("Communication with master is very slow (>0.5s average)")) 
+
+import threading
+class NodeInfoThread(threading.Thread):
+    def __init__(self, n, ctx, master, actual_edges, lock):
+        threading.Thread.__init__(self)
+        self.master = master
+        self.actual_edges = actual_edges
+        self.lock = lock
+        self.n = n
+        self.done = False
+        self.ctx = ctx
+
+    def run(self):
+        ctx = self.ctx
+        master = self.master
+        actual_edges = self.actual_edges
+        lock = self.lock
+        n = self.n
+
+        try:
+            socket.setdefaulttimeout(3.0)
+            with lock: #Apparently get_api_uri is not thread safe...
+                node_api = rosnode.get_api_uri(master, n)
+
+
+            if not node_api:
+                with lock:
+                    ctx.errors.append(WtfError("Master does not have lookup information for node [%s]"%n))
+                return
+                
+            node = ServerProxy(node_api)
+            start = time.time()
+            socket.setdefaulttimeout(3.0)            
+            code, msg, bus_info = node.getBusInfo('/roswtf')
+            end = time.time()
+            with lock:
+                if (end-start) > 1.:
+                    ctx.warnings.append(WtfWarning("Communication with node [%s] is very slow"%n))
+                if code != 1:
+                    ctx.warnings.append(WtfWarning("Node [%s] would not return bus info"%n))
+                elif not bus_info:
+                    if not n in ctx.service_providers:
+                        ctx.warnings.append(WtfWarning("Node [%s] is not connected to anything"%n))
+                else:
+                    edges = _businfo(ctx, n, bus_info)
+                    actual_edges.extend(edges)
+        except socket.error:
+            pass #ignore as we have rules to catch this
+        except Exception as e:
+            ctx.errors.append(WtfError("Communication with [%s] raised an error: %s"%(n, str(e))))
+        finally:
+            self.done = True
+        
+        
+## retrieve graph state from master and related nodes once so we don't overload
+## the network
+def _compute_connectivity(ctx):
+    socket.setdefaulttimeout(3.0)
+    master = rosgraph.Master('/roswtf')
+
+    # Compute list of expected edges and unconnected subscriptions
+    pubs, subs, _ = ctx.system_state
+    expected_edges = [] # [(topic, publisher, subscriber),]
+    unconnected_subscriptions = {} # { subscriber : [topics] }
+
+    # - build up a dictionary of publishers keyed by topic
+    pub_dict = {}
+    for t, pub_list in pubs:
+        pub_dict[t] = pub_list
+    # - iterate through subscribers and add edge to each publisher of topic
+    for t, sub_list in subs:
+        for sub in sub_list:
+            if t in pub_dict:
+                expected_edges.extend([(t, pub, sub) for pub in pub_dict[t]])
+            elif sub in unconnected_subscriptions:
+                unconnected_subscriptions[sub].append(t)
+            else:
+                unconnected_subscriptions[sub] = [t]
+                    
+    # compute actual edges
+    actual_edges = []
+    lock = threading.Lock()
+    threads = []
+    for n in ctx.nodes:
+        t =NodeInfoThread(n, ctx, master, actual_edges, lock)
+        threads.append(t)
+        t.start()
+        
+    # spend up to a minute waiting for threads to complete. each
+    # thread has a 3-second timeout, but this will spike load
+    timeout_t = time.time() + 60.0
+    while time.time() < timeout_t and [t for t in threads if not t.done]:
+        time.sleep(0.5)
+    
+    ctx.expected_edges = expected_edges
+    ctx.actual_edges = actual_edges
+    ctx.unconnected_subscriptions = unconnected_subscriptions
+            
+def _compute_online_context(ctx):
+    # have to compute sim time first
+    _compute_sim_time(ctx)
+    _compute_system_state(ctx)
+    _compute_connectivity(ctx)
+    
+def wtf_check_graph(ctx, names=None):
+    master_uri = ctx.ros_master_uri
+    #TODO: master rules
+    # - check for stale master state
+    
+    # TODO: get the type for each topic from each publisher and see if they match up
+
+    master = rosgraph.Master('/roswtf')
+    try:
+        master.getPid()
+    except rospkg.MasterException:
+        warning_rule((True, "Cannot communicate with master, ignoring online checks"), True, ctx)
+        return
+            
+    # fill in ctx info so we only have to compute once
+    print("analyzing graph...")
+    _compute_online_context(ctx)
+    print("... done analyzing graph")
+    
+    if names:
+        check_topics = [t for t in names if t in ctx.topics]
+        check_services = [t for t in names if t in ctx.services]
+        check_nodes = [t for t in names if t in ctx.nodes]
+        unknown = [t for t in names if t not in check_topics + check_services + check_nodes]
+        if unknown:
+            raise WtfException("The following names were not found in the list of nodes, topics, or services:\n%s"%(''.join([" * %s\n"%t for t in unknown])))
+
+        for t in check_topics:
+            for r in topic_warnings:
+                warning_rule(r, r[0](ctx, t), ctx)            
+            for r in topic_errors:
+                error_rule(r, r[0](ctx, t), ctx)            
+        for s in check_services:
+            for r in service_warnings:
+                warning_rule(r, r[0](ctx, s), ctx)            
+            for r in service_errors:
+                error_rule(r, r[0](ctx, s), ctx)            
+        for n in check_nodes:
+            for r in node_warnings:
+                warning_rule(r, r[0](ctx, n), ctx)            
+            for r in node_errors:
+                error_rule(r, r[0](ctx, n), ctx)            
+
+
+    print("running graph rules...")
+    for r in graph_warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in graph_errors:
+        error_rule(r, r[0](ctx), ctx)
+    print("... done running graph rules")

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/model.py

@@ -0,0 +1,63 @@
+# 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 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.
+#
+# Revision $Id$
+
+"""
+Error and Warning representation. The roswtf.rules module simplifies
+the process of creating instances of L{WtfError}s and L{WtfWarning}s,
+but these objects can be used directly if desired.
+"""
+
+class WtfError(object):
+    def __init__(self, msg, format_msg=None, return_val=None):
+        self.msg = msg
+        if format_msg is None:
+            self.format_msg = msg
+        else:
+            self.format_msg = format_msg
+        if return_val is None:
+            self.return_val = True
+        else:
+            self.return_val = return_val
+
+class WtfWarning(object):
+    def __init__(self, msg, format_msg=None, return_val=None):
+        self.msg = msg
+        if format_msg is None:
+            self.format_msg = msg
+        else:
+            self.format_msg = format_msg
+        if return_val is None:
+            self.return_val = True
+        else:
+            self.return_val = return_val

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/network.py

@@ -0,0 +1,114 @@
+# 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 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.
+#
+# Revision $Id: environment.py 4428 2009-05-05 05:48:36Z jfaustwg $
+
+import os
+import socket
+import stat
+import string
+import sys
+
+import rosgraph
+import rosgraph.network
+
+from roswtf.rules import warning_rule, error_rule
+
+# #1220
+def ip_check(ctx):
+    # best we can do is compare roslib's routine against socket resolution and make sure they agree
+    local_addrs = rosgraph.network.get_local_addresses()
+
+    resolved_ips = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, 0, 0, socket.SOL_TCP)]
+    global_ips = [ ip for ip in resolved_ips if not ip.startswith('127.') and not ip == '::1']
+
+    remote_ips = list(set(global_ips) - set(local_addrs))
+    if remote_ips:
+        retval = "Local hostname [%s] resolves to [%s], which does not appear to be a local IP address %s." % (socket.gethostname(), ','.join(remote_ips), str(local_addrs))
+        # IPv6 support % to denote zone/scope ids. The value is expanded
+        # in other functions, this is why we are using replace command in
+        # the return. For more info https://github.com/ros/ros_comm/pull/598
+        return retval.replace('%', '%%')
+
+# suggestion by mquigley based on laptop dhcp issues    
+def ros_hostname_check(ctx):
+    """Make sure that ROS_HOSTNAME resolves to a local IP address"""
+    if not rosgraph.ROS_HOSTNAME in ctx.env:
+        return
+
+    hostname = ctx.env[rosgraph.ROS_HOSTNAME]
+    try:
+        resolved_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, 0, 0, socket.SOL_TCP)]
+    except socket.gaierror:
+        return "ROS_HOSTNAME [%s] cannot be resolved to an IP address"%(hostname)
+    
+    # best we can do is compare roslib's routine against socket resolution and make sure they agree
+    local_addrs = rosgraph.network.get_local_addresses()
+
+    remote_ips = list(set(resolved_ips) - set(local_addrs))
+    if remote_ips:
+        return "ROS_HOSTNAME [%s] resolves to [%s], which does not appear to be a local IP address %s."%(hostname, ','.join(remote_ips), str(local_addrs))
+
+def ros_ip_check(ctx):
+    """Make sure that ROS_IP is a local IP address"""
+    if not rosgraph.ROS_IP in ctx.env:
+        return
+
+    ip = ctx.env[rosgraph.ROS_IP]
+    
+    # best we can do is compare roslib's routine against socket resolution and make sure they agree
+    addrs = rosgraph.network.get_local_addresses()
+
+    if ip not in addrs:
+        return "ROS_IP [%s] does not appear to be a local IP address %s."%(ip, str(addrs))
+    
+# Error/Warning Rules
+
+warnings = [
+    (ros_hostname_check,
+     "ROS_HOSTNAME may be incorrect: "),
+    (ros_ip_check,
+     "ROS_IP may be incorrect: "),
+
+    ]
+
+errors = [
+    (ip_check,
+     "Local network configuration is invalid: "),
+    ]
+
+def wtf_check(ctx):
+    for r in warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in errors:
+        error_rule(r, r[0](ctx), ctx)
+        

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/packages.py

@@ -0,0 +1,199 @@
+# 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 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.
+#
+# Revision $Id$
+
+import os
+import time
+
+from roswtf.environment import paths, is_executable
+from roswtf.rules import warning_rule, error_rule
+
+import roslib.msgs
+import roslib.srvs
+from roslib.packages import get_pkg_dir, InvalidROSPkgException, PACKAGE_FILE
+
+## look for unknown tags in manifest
+def manifest_valid(ctx):
+    errors = []
+    if ctx.manifest is not None:
+        errors = ["<%s>"%t.tagName for t in ctx.manifest.unknown_tags]
+    return errors
+    
+def _manifest_msg_srv_export(ctx, type_):
+    exist = []
+    for pkg in ctx.pkgs:
+        pkg_dir = roslib.packages.get_pkg_dir(pkg)
+        d = os.path.join(pkg_dir, type_)
+        if os.path.isdir(d):
+            files = os.listdir(d)
+            if filter(lambda x: x.endswith('.'+type_), files):
+                try:
+                    m_file = roslib.manifest.manifest_file(pkg, True)
+                except InvalidROSPkgException:
+                    # ignore wet package from further investigation
+                    env = os.environ
+                    pkg_path = get_pkg_dir(pkg, True, ros_root=env['ROS_ROOT'])
+                    if os.path.exists(os.path.join(pkg_path, PACKAGE_FILE)):
+                        continue
+                    raise
+                m = roslib.manifest.parse_file(m_file)
+                cflags = m.get_export('cpp', 'cflags')
+                include = '-I${prefix}/%s/cpp'%type_
+                if filter(lambda x: include in x, cflags):
+                    exist.append(pkg)
+    return exist
+    
+def manifest_msg_srv_export(ctx):
+    msgs = set(_manifest_msg_srv_export(ctx, 'msg'))
+    srvs = set(_manifest_msg_srv_export(ctx, 'srv'))
+    errors = []
+
+    for pkg in msgs & srvs:
+        errors.append('%s: -I${prefix}/msg/cpp -I${prefix}/srv/cpp'%pkg)
+    for pkg in msgs - srvs:
+        errors.append('%s: -I${prefix}/msg/cpp'%pkg)
+    for pkg in srvs - msgs:
+        errors.append('%s: -I${prefix}/srv/cpp'%pkg)
+    return errors
+        
+
+def _check_for_rpath_flags(pkg, lflags):
+    if not lflags:
+        return
+    L_arg = '-L'
+    Wl_arg = '-Wl'
+    rpath_arg = '-rpath'
+    lflags_args = lflags.split()
+    # Collect the args we care about
+    L_args = []
+    rpath_args = []
+    i = 0
+    while i < len(lflags_args):
+        f = lflags_args[i]
+        if f.startswith(L_arg) and len(f) > len(L_arg):
+            # normpath avoids problems with trailing slash vs. no trailing
+            # slash, #2284
+            L_args.append(os.path.normpath(f[len(L_arg):]))
+        elif f == L_arg and (i+1) < len(lflags_args):
+            i += 1
+            # normpath avoids problems with trailing slash vs. no trailing
+            # slash, #2284
+            L_args.append(os.path.normpath(lflags_args[i]))
+        elif f.startswith(Wl_arg) and len(f) > len(Wl_arg):
+            # -Wl can be followed by multiple, comma-separated arguments,
+            # #2284.
+            args = f.split(',')
+            j = 1
+            while j < (len(args) - 1):
+                if args[j] == rpath_arg:
+                   # normpath avoids problems with trailing slash vs. no trailing
+                   # slash, #2284
+                    rpath_args.append(os.path.normpath(args[j+1]))
+                    j += 2
+                else:
+                    j += 1
+        i += 1
+    # Check for parallelism; not efficient, but these strings are short
+    for f in L_args:
+        if f not in rpath_args:
+            return '%s: found flag "-L%s", but no matching "-Wl,-rpath,%s"'%(pkg, f,f)
+    for f in rpath_args:
+        if f not in L_args:
+            return '%s: found flag "-Wl,-rpath,%s", but no matching "-L%s"'%(pkg, f,f)
+
+def manifest_rpath_flags(ctx):
+    warn = []
+    for pkg in ctx.pkgs:
+        # Use rospack to get lflags, so that they can be bash-expanded
+        # first, #2286.
+        import subprocess
+        lflags = subprocess.Popen(['rospack', 'export', '--lang=cpp', '--attrib=lflags', pkg], stdout=subprocess.PIPE).communicate()[0]
+        err_msg = _check_for_rpath_flags(pkg, lflags)
+        if err_msg:
+            warn.append(err_msg)
+    return warn
+
+def cmakelists_package_valid(ctx):
+    missing = []
+    for pkg in ctx.pkgs:
+        found = False
+        pkg_dir = roslib.packages.get_pkg_dir(pkg)
+        p = os.path.join(pkg_dir, 'CMakeLists.txt')
+        if not os.path.isfile(p):
+            continue #covered by cmakelists_exists
+        f = open(p)
+        try:
+            for l in f:
+                # ignore all whitespace
+                l = l.strip().replace(' ', '')
+                
+                if l.startswith('rospack('):
+                    found = True
+                    if not l.startswith('rospack(%s)'%pkg):
+                        missing.append(pkg)
+                        break
+                    # there may be more than 1 rospack() declaration, so scan through entire
+                    # CMakeLists
+                elif l.startswith("rosbuild_init()"):
+                    found = True
+        finally:
+            f.close()
+    # rospack exists outside our build system
+    if 'rospack' in missing:
+        missing.remove('rospack')
+    return missing
+
+warnings = [
+    # disabling as it is too common and regular
+    (cmakelists_package_valid,
+     "The following packages have incorrect rospack() declarations in CMakeLists.txt.\nPlease switch to using rosbuild_init():"),
+    
+    (manifest_msg_srv_export,
+     'The following packages have msg/srv-related cflags exports that are no longer necessary\n\t<export>\n\t\t<cpp cflags="..."\n\t</export>:'),
+    (manifest_valid, "%(pkg)s/manifest.xml has unrecognized tags:"),
+
+    ]
+errors = [
+    (manifest_rpath_flags, "The following packages have rpath issues in manifest.xml:"),
+    ]
+
+def wtf_check(ctx):
+    # no package in context to verify
+    if not ctx.pkgs:
+        return
+    
+    for r in warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in errors:
+        error_rule(r, r[0](ctx), ctx)
+

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/plugins.py

@@ -0,0 +1,95 @@
+# 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 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.
+#
+# Revision $Id$
+
+"""
+Plugin loader for roswtf.
+"""
+
+from __future__ import print_function
+
+import os
+import sys
+
+import roslib
+import rospkg
+
+def load_plugins():
+    """
+    @return: list of static roswtf plugins, list of online
+    roswtf plugins
+    @rtype: [fn], [fn]
+    """
+    rospack = rospkg.RosPack()
+    to_check = rospack.get_depends_on('roswtf', implicit=False)
+    static_plugins = []
+    online_plugins = []
+    for pkg in to_check:
+        m = rospack.get_manifest(pkg)
+        p_module = m.get_export('roswtf', 'plugin')
+        if not p_module:
+            continue
+        elif len(p_module) != 1:
+            print("Cannot load plugin [%s]: invalid 'plugin' attribute"%(pkg), file=sys.stderr)
+            continue
+        p_module = p_module[0]
+        try:
+            # load that packages namespace
+            roslib.load_manifest(pkg)
+            
+            # import the specified plugin module
+            mod = __import__(p_module)
+            for sub_mod in p_module.split('.')[1:]:
+                mod = getattr(mod, sub_mod)
+
+            # retrieve the roswtf_plugin_static and roswtf_plugin_online functions
+            s_attr = o_attr = None
+            try:
+                s_attr = getattr(mod, 'roswtf_plugin_static')
+            except AttributeError: pass
+            try:
+                o_attr = getattr(mod, 'roswtf_plugin_online')
+            except AttributeError: pass
+            if s_attr:
+                static_plugins.append(s_attr)
+            if o_attr:
+                online_plugins.append(o_attr)
+            if s_attr is None and o_attr is None:
+                print("Cannot load plugin [%s]: no 'roswtf_plugin_static' or 'roswtf_plugin_online' attributes [%s]"%(p_module), file=sys.stderr)
+            else:
+                print("Loaded plugin", p_module)
+
+        except Exception:
+            print("Unable to load plugin [%s] from package [%s]"%(p_module, pkg), file=sys.stderr)
+    return static_plugins, online_plugins
+

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/py_pip_deb_checks.py

@@ -0,0 +1,170 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, 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 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.
+
+"""
+Checks to see if core Python scripts have:
+1) Been installed
+2) Have been installed via Debians on Ubuntu
+3) Have not been installed via pip on Ubuntu
+"""
+
+from __future__ import print_function
+
+import subprocess
+import importlib
+import os
+import sys
+
+python_prefix = 'python'
+if sys.version_info[0] == 3:
+    python_prefix += '3'
+
+#A dictionary of core ROS python packages and their corresponding .deb packages
+py_to_deb_core_packages = {
+    'catkin_pkg': '%s-catkin-pkg' % python_prefix,
+    'rospkg': '%s-rospkg' % python_prefix,
+    'rosdep2': '%s-rosdep' % python_prefix,
+}
+# optional ROS python packages and their corresponding .deb packages
+py_to_deb_optional_packages = {
+    'rosinstall': '%s-rosinstall' % python_prefix,
+}
+
+#A dictionary of release ROS python packages and their corresponding .deb packages
+py_to_deb_release_packages = {
+    'bloom': '%s-bloom' % python_prefix,
+    'rosrelease': '%s-rosrelease' % python_prefix,
+}
+
+
+def get_host_os():
+    """Determines the name of the host operating system"""
+    import rospkg.os_detect
+    os_detector = rospkg.os_detect.OsDetect()
+    return (os_detector.detect_os())[0]
+
+
+def is_host_os_ubuntu():
+    """Indicates if the host operating system is Ubuntu"""
+    return (get_host_os() == 'ubuntu')
+
+
+def is_debian_package_installed(deb_pkg):
+    """Uses dpkg to determine if a package has been installed"""
+    return (subprocess.call(
+        'dpkg -l ' + deb_pkg,
+        shell=True,
+        stdout=subprocess.PIPE,
+        stderr=subprocess.PIPE) == 0)
+
+
+def is_a_pip_path_on_ubuntu(path):
+    """Indicates if a path (either directory or file) is in the same place
+    pip installs Python code"""
+    return ('/usr/local' in path)
+
+
+def is_python_package_installed(python_pkg):
+    """Indicates if a Python package is importable in the current
+    environment."""
+    try:
+        importlib.import_module(python_pkg)
+        return True
+    except ImportError:
+        return False
+
+
+def is_python_package_installed_via_pip_on_ubuntu(python_pkg):
+    """Indicates if am importable package has been installed through pip on
+    Ubuntu"""
+    try:
+        pkg_handle = importlib.import_module(python_pkg)
+        return is_a_pip_path_on_ubuntu(pkg_handle.__file__)
+    except ImportError:
+        return False
+
+
+# Error/Warning Rules
+def python_module_install_check(ctx):
+    """Make sure core Python modules are installed"""
+    warn_str = ''
+    for py_pkg in py_to_deb_core_packages:
+        if not is_python_package_installed(py_pkg):
+            warn_str = warn_str + py_pkg + ' -- '
+    if (warn_str != ''):
+        return warn_str
+
+
+def deb_install_check_on_ubuntu(ctx):
+    """Make sure on Debian python packages are installed"""
+    if (is_host_os_ubuntu()):
+        warn_str = ''
+        for py_pkg in py_to_deb_core_packages:
+            deb_pkg = py_to_deb_core_packages[py_pkg]
+            if not is_debian_package_installed(deb_pkg):
+                warn_str = warn_str + py_pkg + ' (' + deb_pkg + ') -- '
+        if (warn_str != ''):
+            return warn_str
+
+
+def pip_install_check_on_ubuntu(ctx):
+    """Make sure on Ubuntu, Python packages are install with apt and not pip"""
+    if (is_host_os_ubuntu()):
+        warn_str = ''
+        pt_to_deb_package_names = list(py_to_deb_core_packages.keys()) + \
+            list(py_to_deb_optional_packages.keys()) + list(py_to_deb_release_packages.keys())
+        for py_pkg in pt_to_deb_package_names:
+            if is_python_package_installed_via_pip_on_ubuntu(py_pkg):
+                warn_str = warn_str + py_pkg + ' -- '
+        if (warn_str != ''):
+            return warn_str
+
+warnings = [
+    (python_module_install_check,
+     "You are missing core ROS Python modules: "),
+    (pip_install_check_on_ubuntu,
+     "You have pip installed packages on Ubuntu, "
+     "remove and install using Debian packages: "),
+    (deb_install_check_on_ubuntu,
+     "You are missing Debian packages for core ROS Python modules: "),
+    ]
+
+errors = []
+
+
+def wtf_check(ctx):
+    """Check implementation function for roswtf"""
+    from roswtf.rules import warning_rule, error_rule
+    for r in warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in errors:
+        error_rule(r, r[0](ctx), ctx)

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/rosdep_db.py

@@ -0,0 +1,63 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, 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 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.
+
+"""
+Checks if rosdep database has been initialized
+"""
+import os
+
+
+def get_user_home_directory():
+    """Returns cross-platform user home directory """
+    return os.path.expanduser("~")
+
+
+def rosdep_database_updated_check(ctx):
+    """Makes sure rosdep database is updated"""
+    if not os.path.exists((os.path.join(get_user_home_directory(), '.ros', 'rosdep', 'sources.cache', 'index'))):
+        return "Please update rosdep database with 'rosdep update'."
+
+
+warnings = []
+
+errors = [(rosdep_database_updated_check,
+           "ROS Dep database not updated: "),
+         ]
+
+
+def wtf_check(ctx):
+    """Check implementation function for roswtf"""
+    from roswtf.rules import warning_rule, error_rule
+    for r in warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in errors:
+        error_rule(r, r[0](ctx), ctx)

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/roslaunchwtf.py

@@ -0,0 +1,299 @@
+# 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 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.
+#
+# Revision $Id$
+
+import os
+import itertools
+import socket
+import stat
+import sys
+try:
+    from xmlrpc.client import ServerProxy
+except ImportError:
+    from xmlrpclib import ServerProxy
+
+from os.path import isfile, isdir
+
+import roslib.packages
+import roslaunch
+import roslaunch.netapi
+
+from roswtf.environment import paths, is_executable
+from roswtf.rules import warning_rule, error_rule
+
+## check if node is cannot be located in package
+def roslaunch_missing_node_check(ctx):
+    nodes = []
+    for filename, rldeps in ctx.launch_file_deps.items():
+        nodes.extend(rldeps.nodes)
+    errors = []
+    for pkg, node_type in nodes:
+        paths = roslib.packages.find_node(pkg, node_type)
+        if not paths:
+            errors.append("node [%s] in package [%s]"%(node_type, pkg))
+    return errors
+    
+## check if two nodes with same name in package
+def roslaunch_duplicate_node_check(ctx):
+    nodes = []
+    for filename, rldeps in ctx.launch_file_deps.items():
+        nodes.extend(rldeps.nodes)
+    warnings = []
+    for pkg, node_type in nodes:
+        paths = roslib.packages.find_node(pkg, node_type)
+        if len(paths) > 1:
+            warnings.append("node [%s] in package [%s]\n"%(node_type, pkg))
+    return warnings
+
+def pycrypto_check(ctx):
+    try:
+        import Crypto
+    except ImportError as e:
+        return True
+
+def paramiko_check(ctx):
+    try:
+        import paramiko
+    except ImportError as e:
+        return True
+def paramiko_system_keys(ctx):
+    try:
+        import paramiko
+        ssh = paramiko.SSHClient()
+        try:
+            ssh.load_system_host_keys() #default location
+        except:
+            return True
+    except: pass
+
+def paramiko_ssh(ctx, address, port, username, password):
+    try:
+        import paramiko
+        ssh = paramiko.SSHClient()
+
+        import roslaunch.remoteprocess
+        err_msg = roslaunch.remoteprocess.ssh_check_known_hosts(ssh, address, port, username=username)
+        if err_msg:
+            return err_msg
+        
+        if not password: #use SSH agent
+            ssh.connect(address, port, username)
+        else: #use SSH with login/pass
+            ssh.connect(address, port, username, password)
+
+    except paramiko.BadHostKeyException:
+        return  "Unable to verify host key for [%s:%s]"%(address, port)
+    except paramiko.AuthenticationException:
+        return "Authentication to [%s:%s] failed"%(address, port)
+    except paramiko.SSHException as e:
+        return "[%s:%s]: %s"%(address, port, e)
+    except ImportError:
+        pass
+
+def _load_roslaunch_config(ctx):
+    config = roslaunch.ROSLaunchConfig()
+    loader = roslaunch.XmlLoader()
+    # TODO load roscore
+    for launch_file in ctx.launch_files:
+        loader.load(launch_file, config, verbose=False)
+    try:
+        config.assign_machines()
+    except roslaunch.RLException as e:
+        return config, []
+    machines = []
+    for n in itertools.chain(config.nodes, config.tests):
+        if n.machine not in machines:
+            machines.append(n.machine)
+    return config, machines
+
+def roslaunch_load_check(ctx):
+    config = roslaunch.ROSLaunchConfig()
+    loader = roslaunch.XmlLoader()
+    # TODO load roscore
+    for launch_file in ctx.launch_files:
+        loader.load(launch_file, config, verbose=False)
+    try:
+        config.assign_machines()
+    except roslaunch.RLException as e:
+        return str(e)
+    
+def roslaunch_machine_name_check(ctx):
+    config, machines = _load_roslaunch_config(ctx)
+    bad = []
+    for m in machines:
+        try:
+            #TODO IPV6: only check for IPv6 when IPv6 is enabled
+            socket.getaddrinfo(m.address, 0, 0, 0, socket.SOL_TCP)
+        except socket.gaierror:
+            bad.append(m.address)
+    return ''.join([' * %s\n'%b for b in bad])
+
+def roslaunch_ssh_check(ctx):
+    import roslaunch.core
+    if not ctx.launch_files:
+        return # not relevant
+    config, machines = _load_roslaunch_config(ctx)
+    err_msgs = []
+    for m in machines:
+        socket.setdefaulttimeout(3.)
+        # only check if the machine requires ssh to connect
+        if not roslaunch.core.is_machine_local(m):
+            err_msg = paramiko_ssh(ctx, m.address, m.ssh_port, m.user, m.password)
+            if err_msg:
+                err_msgs.append(err_msg)
+    return err_msgs
+
+def roslaunch_missing_pkgs_check(ctx):
+    # rospack depends does not return depends that it cannot find, so
+    # we have to manually determine this
+    config, machines = _load_roslaunch_config(ctx)
+    missing = []
+    for n in config.nodes:
+        pkg = n.package
+        try:
+            roslib.packages.get_pkg_dir(pkg, required=True)
+        except:
+            missing.append(pkg)
+    return missing
+
+def roslaunch_config_errors(ctx):
+    config, machines = _load_roslaunch_config(ctx)
+    return config.config_errors
+
+def roslaunch_missing_deps_check(ctx):
+    missing = []
+    for pkg, miss in ctx.launch_file_missing_deps.items():
+        if miss:
+            missing.append("%s/manifest.xml: %s"%(pkg, ', '.join(miss)))
+    return missing
+
+def roslaunch_respawn_check(ctx):
+    respawn = []
+    for uri in ctx.roslaunch_uris:
+        try:
+            r = ServerProxy(uri)
+            code, msg, val = r.list_processes()
+            active, _ = val
+            respawn.extend([a for a in active if a[1] > 1])
+            #TODO: children processes
+            #code, msg, val = r.list_children()
+        except:
+            pass # error for another rule
+    return ["%s (%s)"%(a[0], a[1]) for a in respawn]
+
+def roslaunch_uris_check(ctx):
+    # check for any roslaunch processes that cannot be contacted
+    bad = []
+    # uris only contains the parent launches
+    for uri in ctx.roslaunch_uris:
+        try:
+            r = ServerProxy(uri)
+            code, msg, val = r.list_children()
+            # check the children launches
+            if code == 1:
+                for child_uri in val:
+                    try:
+                        r = ServerProxy(uri)
+                        code, msg, val = r.get_pid()
+                    except:
+                        bad.append(child_uri)
+        except:
+            bad.append(uri)
+    return bad
+
+def roslaunch_dead_check(ctx):
+    dead = []
+    for uri in ctx.roslaunch_uris:
+        try:
+            r = ServerProxy(uri)
+            code, msg, val = r.list_processes()
+            _, dead_list = val
+            dead.extend([d[0] for d in dead_list])
+            #TODO: children processes
+            #code, msg, val = r.list_children()
+        except:
+            pass # error for another rule
+    return dead
+
+online_roslaunch_warnings = [
+    (roslaunch_respawn_check,"These nodes have respawned at least once:"),
+    (roslaunch_dead_check,"These nodes have died:"),
+    # disabling for now as roslaunches don't do cleanup
+    #(roslaunch_uris_check,"These roslaunch processes can no longer be contacted and may have exited:"),    
+    ]
+
+online_roslaunch_errors = [
+    (roslaunch_ssh_check,"SSH failures:"),
+    ]
+
+static_roslaunch_warnings = [
+    (roslaunch_duplicate_node_check, "Multiple nodes of same name in packages:"),
+    (pycrypto_check, "pycrypto is not installed"),
+    (paramiko_check, "paramiko is not installed"),
+    (paramiko_system_keys, "cannot load SSH host keys -- your known_hosts file may be corrupt") ,
+    (roslaunch_config_errors, "Loading your launch files reported the following configuration errors:"),
+    ]
+static_roslaunch_errors = [
+    # Disabling, because we've removed package dependencies from manifests.
+    #(roslaunch_missing_deps_check, 
+    # "Package %(pkg)s is missing roslaunch dependencies.\nPlease add the following tags to %(pkg)s/manifest.xml:"),
+    (roslaunch_missing_pkgs_check, 
+     "Cannot find the following required packages:"),
+    (roslaunch_missing_node_check, "Several nodes in your launch file could not be located. These are either typed incorrectly or need to be built:"),
+    (roslaunch_machine_name_check,"Cannot resolve the following hostnames:"),
+    (roslaunch_load_check, "roslaunch load failed"),
+    ]
+
+def wtf_check_static(ctx):
+    if not ctx.launch_files:
+        return
+
+    #NOTE: roslaunch files are already loaded separately into context
+
+    #TODO: check each machine name
+    #TODO: bidirectional ping for each machine
+
+    for r in static_roslaunch_warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in static_roslaunch_errors:
+        error_rule(r, r[0](ctx), ctx)
+        
+def _load_online_ctx(ctx):
+    ctx.roslaunch_uris = roslaunch.netapi.get_roslaunch_uris()
+    
+def wtf_check_online(ctx):
+    _load_online_ctx(ctx)
+    for r in online_roslaunch_warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in online_roslaunch_errors:
+        error_rule(r, r[0](ctx), ctx)

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/rules.py

@@ -0,0 +1,101 @@
+# 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 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.
+#
+# Revision $Id$
+
+"""
+Common library for writing rule-style checks for generating warnings
+and errors.  Use of this style streamlines reporting.
+
+The pattern for rules is simple: a rule provides a function that
+implements the rule and a format string. If the function returns a
+non-zero value, that value is combined with the format string to
+produced an error reporting string. There are other conveniences as
+well. If the rule returns a list or a tuple, that will be transformed
+into a human-readable list.
+
+This library is a layer on top of the base L{WtfWarning} and
+L{WtfError} representation in roswtf.model.
+"""
+
+from roswtf.model import WtfWarning, WtfError
+
+def _check_rule(rule, ret, ctx, ctx_list, level):
+    if ret:
+        d = ctx.as_dictionary()
+
+        def isstring(s):
+            """Small helper version to check an object is a string in
+            a way that works for both Python 2 and 3
+            """
+            try:
+                return isinstance(s, basestring)
+            except NameError:
+                return isinstance(s, str)
+
+        if type(ret) in (tuple, list):
+            f_msg = rule[1]
+            ret_str = '\n'.join([" * %s"%r for r in ret])
+            ctx_list.append(level(f_msg%d + "\n" + ret_str+'\n', f_msg, ret))
+        elif isstring(ret):
+            f_msg = rule[1]
+            ctx_list.append(level(f_msg%d + ret%d, f_msg, ret))
+        else:
+            f_msg = rule[1]
+            ctx_list.append(level(f_msg%d, f_msg, ret))
+    
+def warning_rule(rule, ret, ctx):
+    """
+    Check return value of rule and update ctx if rule failed.
+    
+    @param rule: Rule/message pair.
+    @type  rule: (rule_fn, format_msg)
+    @param ret: return value of rule. If value is non-zero, rule failed
+    @param ret: Any
+    @param ctx: context for which rule failed
+    @param ctx: L{WtfContext}
+    """
+    _check_rule(rule, ret, ctx, ctx.warnings, WtfWarning)
+    
+def error_rule(rule, ret, ctx):
+    """
+    Check return value of rule and update ctx if rule failed.
+    
+    @param rule: Rule/message pair.
+    @type  rule: (rule_fn, format_msg)
+    @param ret: return value of rule. If value is non-zero, rule failed
+    @type  ret: Any
+    @param ctx: context for which rule failed
+    @type  ctx: L{WtfContext}
+    """
+    _check_rule(rule, ret, ctx, ctx.errors, WtfError)
+    

thirdparty/ros/ros_comm/utilities/roswtf/src/roswtf/stacks.py

@@ -0,0 +1,101 @@
+# 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 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.
+#
+# Revision $Id: packages.py 6258 2009-09-22 22:08:20Z kwc $
+
+import os
+import time
+
+from roswtf.environment import paths, is_executable
+from roswtf.rules import warning_rule, error_rule
+
+import rospkg
+
+_packages_of_cache = {}
+def _packages_of(rosstack, d):
+    if d in _packages_of_cache:
+        return _packages_of_cache[d]
+    else:
+        _packages_of_cache[d] = pkgs = rosstack.packages_of(d)
+        return pkgs
+                
+def manifest_depends(ctx):
+    # This rule should probably be cache optimized
+    errors = []
+    rospack = rospkg.RosPack()
+    rosstack = rospkg.RosStack()
+
+    stack_list = rosstack.list()
+    #print stack_list
+    for s in ctx.stacks:
+        try:
+            s_deps = []
+            s_pkgs = _packages_of(rosstack, s)
+            for p in s_pkgs:
+                s_deps.extend(rospack.get_depends(p, implicit=False))
+            m = rosstack.get_manifest(s)
+            m_file = os.path.join(rosstack.get_path(s), 'stack.xml')
+            for d in m.depends:
+                if not d.name in stack_list:
+                    errors.append("%s (%s does not exist)"%(m_file, d))
+                elif d.name in ['ros', 'ros_comm']:
+                    # ros dependency always exists. ros_comm
+                    # dependency has implicit connections (msggen), so
+                    # we ignore.
+                    continue
+                else:
+                    pkgs = _packages_of(rosstack, d.name)
+                    # check no longer works due to rosdeps chains
+                    #if not [p for p in pkgs if p in s_deps]:
+                    #    errors.append("%s (%s appears to be an unnecessary depend)"%(m_file, d))
+        except rospkg.ResourceNotFound:
+            # report with a different rule
+            pass
+    return errors
+
+warnings = [
+    (manifest_depends, "The following stack.xml file list invalid dependencies:"),
+    
+    ]
+errors = [
+    ]
+
+def wtf_check(ctx):
+    # no package in context to verify
+    if not ctx.stacks:
+        return
+    
+    for r in warnings:
+        warning_rule(r, r[0](ctx), ctx)
+    for r in errors:
+        error_rule(r, r[0](ctx), ctx)
+

thirdparty/ros/ros_comm/utilities/roswtf/test/check_roswtf_command_line_online.py

@@ -0,0 +1,141 @@
+#!/usr/bin/env python
+# 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 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.
+
+PKG = 'roswtf'
+NAME = 'test_roswtf_command_line_online'
+
+import os
+import signal
+import sys 
+import time
+import unittest
+
+import rospkg
+import rospy
+import rostest
+
+import std_msgs.msg
+
+from subprocess import Popen, PIPE, check_call, call
+
+def run_for(cmd, secs):
+    popen = Popen(cmd, stdout=PIPE, stderr=PIPE, close_fds=True)
+    timeout_t = time.time() + secs
+    while time.time() < timeout_t:
+        time.sleep(0.1)
+    os.kill(popen.pid, signal.SIGKILL)
+    
+class TestRostopicOnline(unittest.TestCase):
+
+    def setUp(self):
+        self.vals = set()
+        self.msgs = {}
+
+    ## test that the rosmsg command works
+    def test_cmd_help(self):
+        cmd = 'roswtf'
+        output = Popen([cmd, '-h'], stdout=PIPE).communicate()[0]
+        self.assert_('Options' in output)
+            
+    def test_offline(self):
+        # this test is disabled for now; now that test_roswtf is part
+        # of ros_comm, the tricks before that were used no longer work
+        cmd = 'roswtf'
+
+        # pass in special test key to roswtf for ROS_PACKAGE_PATH
+        env = os.environ.copy()
+
+        rospack = rospkg.RosPack()
+        # add all dependencies to ros package path
+        pkgs = ['roswtf',
+            'rosgraph', 'roslaunch', 'roslib', 'rosnode', 'rosservice',
+            'rosbag', 'rosbag_storage', 'roslz4', 'rosconsole', 'roscpp', 'rosgraph_msgs', 'roslang', 'rosmaster', 'rosmsg', 'rosout', 'rosparam', 'rospy', 'rostest', 'rostopic', 'topic_tools', 'xmlrpcpp',
+            'cpp_common', 'roscpp_serialization', 'roscpp_traits', 'rostime',  # roscpp_core
+            'rosbuild', 'rosclean', 'rosunit',  # ros
+            'rospack', 'std_msgs', 'message_runtime', 'message_generation', 'gencpp', 'genlisp', 'genpy', 'genmsg', 'catkin',
+        ]
+        paths = [rospack.get_path(pkg) for pkg in pkgs]
+        try:
+            path = rospack.get_path('cmake_modules')
+        except rospkg.ResourceNotFound:
+            pass
+        else:
+            paths.append(path)
+        try:
+            path = rospack.get_path('geneus')
+        except rospkg.ResourceNotFound:
+            pass
+        else:
+            paths.append(path)
+        try:
+            path = rospack.get_path('gennodejs')
+        except rospkg.ResourceNotFound:
+            pass
+        else:
+            paths.append(path)
+        env['ROS_PACKAGE_PATH'] = os.pathsep.join(paths)
+
+        cwd  = rospack.get_path('roswtf')
+        kwds = { 'env': env, 'stdout': PIPE, 'stderr': PIPE, 'cwd': cwd}
+
+        # run roswtf nakedly in the roswtf directory. Running in
+        # ROS_ROOT effectively make roswtf have dependencies on
+        # every package in the ROS stack, which doesn't work.
+
+        output, err = Popen([cmd], **kwds).communicate()
+        self._check_output([cmd], output, err)
+
+        # run roswtf on a simple launch file online
+        rospack = rospkg.RosPack()
+        p = os.path.join(rospack.get_path('roswtf'), 'test', 'min.launch')
+        output = Popen([cmd, p], **kwds).communicate()[0]
+        self._check_output([cmd, p], output)
+
+    def _check_output(self, cmd, output, error=None):
+        # do both a positive and negative test
+        self.assert_(
+            'No errors or warnings' in output or 'Found 1 error' in output,
+            'CMD[%s] OUTPUT[%s]%s' %
+            (' '.join(cmd), output, '\nstderr[%s]' % error if error else ''))
+        allowed_errors = 0
+        if 'Found 1 error' in output:
+            self.assert_(output.count('ERROR') == 1, 'OUTPUT[%s]' % output)
+            self.assert_(
+                'ROS Dep database not updated' in output,
+                'OUTPUT[%s]' % output)
+            allowed_errors += 1
+        if 'No errors or warnings' in output:
+            self.assert_(output.count('ERROR') <= allowed_errors, 'OUTPUT[%s]' % output)
+
+if __name__ == '__main__':
+    rostest.run(PKG, NAME, TestRostopicOnline, sys.argv)

thirdparty/ros/ros_comm/utilities/roswtf/test/min.launch

@@ -0,0 +1,4 @@
+<launch>
+  <!-- $(anon talker) creates an anonymous name for this node -->
+  <node name="$(anon talker)" pkg="rospy" type="talker.py" />
+</launch>

thirdparty/ros/ros_comm/utilities/roswtf/test/roswtf.test

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="roswtf_command_line_online" pkg="roswtf" type="check_roswtf_command_line_online.py" />
+</launch>

thirdparty/ros/ros_comm/utilities/roswtf/test/test_roswtf_command_line_offline.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python
+# 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 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.
+
+import os
+import sys 
+import unittest
+import time
+        
+from subprocess import Popen, PIPE, check_call, call
+
+import rospkg
+
+def get_test_path():
+    return os.path.abspath(os.path.dirname(__file__))
+def get_roswtf_path():
+    return os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))
+
+class TestRoswtfOffline(unittest.TestCase):
+
+    def setUp(self):
+        pass
+
+    ## test that the rosmsg command works
+    def test_cmd_help(self):
+        cmd = 'roswtf'
+        output = Popen([cmd, '-h'], stdout=PIPE).communicate()[0]
+        output = output.decode()
+        self.assert_('Options' in output)
+            
+    def test_offline(self):
+        cmd = 'roswtf'
+
+        # point at a different 'master'
+        env = os.environ.copy()
+        env['ROS_MASTER_URI'] = 'http://localhost:11312'
+
+        rospack = rospkg.RosPack()
+        # add all dependencies to ros package path
+        pkgs = ['roswtf',
+            'rosgraph', 'roslaunch', 'roslib', 'rosnode', 'rosservice',
+            'rosbag', 'rosbag_storage', 'roslz4', 'rosconsole', 'roscpp', 'rosgraph_msgs', 'roslang', 'rosmaster', 'rosmsg', 'rosout', 'rosparam', 'rospy', 'rostest', 'rostopic', 'topic_tools', 'xmlrpcpp',
+            'cpp_common', 'roscpp_serialization', 'roscpp_traits', 'rostime',  # roscpp_core
+            'rosbuild', 'rosclean', 'rosunit',  # ros
+            'rospack', 'std_msgs', 'message_runtime', 'message_generation', 'gencpp', 'genlisp', 'genpy', 'genmsg', 'catkin',
+        ]
+        paths = [rospack.get_path(pkg) for pkg in pkgs]
+        try:
+            path = rospack.get_path('cmake_modules')
+        except rospkg.ResourceNotFound:
+            pass
+        else:
+            paths.append(path)
+        try:
+            path = rospack.get_path('geneus')
+        except rospkg.ResourceNotFound:
+            pass
+        else:
+            paths.append(path)
+        try:
+            path = rospack.get_path('gennodejs')
+        except rospkg.ResourceNotFound:
+            pass
+        else:
+            paths.append(path)
+        env['ROS_PACKAGE_PATH'] = os.pathsep.join(paths)
+
+        cwd  = get_roswtf_path()
+        kwds = { 'env': env, 'stdout': PIPE, 'stderr': PIPE, 'cwd': cwd}
+
+        # run roswtf nakedly
+        output = Popen([cmd], **kwds).communicate()
+        output = [o.decode() for o in output]
+
+        # there should either be no errors or warnings or
+        # there should be exactly one error about rosdep not being initialized
+        self._check_output(output[0])
+
+        # run roswtf on a simple launch file offline
+        p = os.path.join(get_test_path(), 'min.launch')
+        output = Popen([cmd, p], **kwds).communicate()[0]
+        output = output.decode()
+        self._check_output(output)
+
+    def _check_output(self, output):
+        # do both a positive and negative test
+        self.assert_(
+            'No errors or warnings' in output or 'Found 1 error' in output,
+            'OUTPUT[%s]' % output)
+        if 'No errors or warnings' in output:
+            self.assert_('ERROR' not in output, 'OUTPUT[%s]' % output)
+        if 'Found 1 error' in output:
+            self.assert_(output.count('ERROR') == 1, 'OUTPUT[%s]' % output)
+            self.assert_(
+                'Error: the rosdep view is empty' not in output,
+                'OUTPUT[%s]' % output)

thirdparty/ros/ros_comm/utilities/xmlrpcpp/CHANGELOG.rst

@@ -0,0 +1,170 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package xmlrpcpp
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.14 (2018-08-23)
+--------------------
+* take XmlRpcValue by *const* ref. in operator<< (`#1350 <https://github.com/ros/ros_comm/issues/1350>`_)
+
+1.12.13 (2018-02-21)
+--------------------
+* fix xmlrpc timeout using monotonic clock (`#1249 <https://github.com/ros/ros_comm/issues/1249>`_)
+* make xmlrpcpp specific include directory work in devel space (`#1261 <https://github.com/ros/ros_comm/issues/1261>`_)
+
+1.12.12 (2017-11-16)
+--------------------
+
+1.12.11 (2017-11-07)
+--------------------
+
+1.12.10 (2017-11-06)
+--------------------
+
+1.12.9 (2017-11-06)
+-------------------
+
+1.12.8 (2017-11-06)
+-------------------
+* use poll() in favor of select() in the XmlRPCDispatcher (`#833 <https://github.com/ros/ros_comm/issues/833>`_)
+
+1.12.7 (2017-02-17)
+-------------------
+* move headers to include/xmlrpcpp (`#962 <https://github.com/ros/ros_comm/issues/962>`_)
+
+1.12.6 (2016-10-26)
+-------------------
+
+1.12.5 (2016-09-30)
+-------------------
+
+1.12.4 (2016-09-19)
+-------------------
+
+1.12.3 (2016-09-17)
+-------------------
+
+1.12.2 (2016-06-03)
+-------------------
+
+1.12.1 (2016-04-18)
+-------------------
+* use directory specific compiler flags (`#785 <https://github.com/ros/ros_comm/pull/785>`_)
+
+1.12.0 (2016-03-18)
+-------------------
+
+1.11.18 (2016-03-17)
+--------------------
+
+1.11.17 (2016-03-11)
+--------------------
+
+1.11.16 (2015-11-09)
+--------------------
+
+1.11.15 (2015-10-13)
+--------------------
+
+1.11.14 (2015-09-19)
+--------------------
+
+1.11.13 (2015-04-28)
+--------------------
+
+1.11.12 (2015-04-27)
+--------------------
+
+1.11.11 (2015-04-16)
+--------------------
+
+1.11.10 (2014-12-22)
+--------------------
+* improve Android support (`#537 <https://github.com/ros/ros_comm/pull/537>`_)
+* fix various defects reported by coverity
+
+1.11.9 (2014-08-18)
+-------------------
+
+1.11.8 (2014-08-04)
+-------------------
+
+1.11.7 (2014-07-18)
+-------------------
+
+1.11.6 (2014-07-10)
+-------------------
+
+1.11.5 (2014-06-24)
+-------------------
+
+1.11.4 (2014-06-16)
+-------------------
+
+1.11.3 (2014-05-21)
+-------------------
+
+1.11.2 (2014-05-08)
+-------------------
+
+1.11.1 (2014-05-07)
+-------------------
+* fix day comparison for rpc value of type timestamp (`#395 <https://github.com/ros/ros_comm/issues/395>`_)
+
+1.11.0 (2014-03-04)
+-------------------
+* output error message when hostname lookup fails (`#364 <https://github.com/ros/ros_comm/issues/364>`_)
+
+1.10.0 (2014-02-11)
+-------------------
+
+1.9.54 (2014-01-27)
+-------------------
+
+1.9.53 (2014-01-14)
+-------------------
+
+1.9.52 (2014-01-08)
+-------------------
+
+1.9.51 (2014-01-07)
+-------------------
+* fix compilation and warnings with clang (`#291 <https://github.com/ros/ros_comm/issues/291>`_)
+
+1.9.50 (2013-10-04)
+-------------------
+
+1.9.49 (2013-09-16)
+-------------------
+
+1.9.48 (2013-08-21)
+-------------------
+
+1.9.47 (2013-07-03)
+-------------------
+
+1.9.46 (2013-06-18)
+-------------------
+
+1.9.45 (2013-06-06)
+-------------------
+
+1.9.44 (2013-03-21)
+-------------------
+* fix install destination for dll's under Windows
+
+1.9.43 (2013-03-13)
+-------------------
+
+1.9.42 (2013-03-08)
+-------------------
+* refine license description to LGPL-2.1
+
+1.9.41 (2013-01-24)
+-------------------
+
+1.9.40 (2013-01-13)
+-------------------
+
+1.9.39 (2012-12-29)
+-------------------
+* first public release for Groovy

thirdparty/ros/ros_comm/utilities/xmlrpcpp/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(xmlrpcpp)
+
+if(NOT WIN32)
+  set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
+endif()
+
+find_package(catkin REQUIRED COMPONENTS cpp_common rostime)
+
+# The CFG_EXTRAS is only for compatibility, to be removed in Lunar.
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES xmlrpcpp
+  CATKIN_DEPENDS cpp_common rostime
+  CFG_EXTRAS xmlrpcpp-extras.cmake
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+link_directories(${catkin_LIBRARY_DIRS})
+
+if(WIN32)
+  add_definitions(-D_WINDOWS)
+endif()
+
+add_library(xmlrpcpp
+  src/XmlRpcClient.cpp
+  src/XmlRpcDispatch.cpp
+  src/XmlRpcServer.cpp
+  src/XmlRpcServerConnection.cpp
+  src/XmlRpcServerMethod.cpp
+  src/XmlRpcSocket.cpp
+  src/XmlRpcSource.cpp
+  src/XmlRpcUtil.cpp
+  src/XmlRpcValue.cpp
+  )
+
+target_link_libraries(xmlrpcpp ${catkin_LIBRARIES})
+if(WIN32)
+  target_link_libraries(xmlrpcpp ws2_32)
+endif()
+
+install(DIRECTORY include/${PROJECT_NAME}
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h")
+
+install(TARGETS xmlrpcpp
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})

thirdparty/ros/ros_comm/utilities/xmlrpcpp/COPYING

@@ -0,0 +1,504 @@
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+     59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+		     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the library's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  <signature of Ty Coon>, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
+
+

thirdparty/ros/ros_comm/utilities/xmlrpcpp/README.html

@@ -0,0 +1,109 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html>
+	<head>
+		<title>XmlRpc++ Library</title>
+		<meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
+		<meta http-equiv="Content-language" content="en-US">
+		<meta name="author" content="Chris Morley">
+		<meta name="copyright" content="Copyright © 2003 by Chris Morley">
+	</head>
+	<body>
+		<H3>XmlRpc++ Library</H3>
+		<P>This is version 0.7 of XmlRpc++, an implementation of the <A HREF="http://www.xmlrpc.org">
+				XmlRpc protocol</A> written in C++, based upon Shilad Sen's excellent <A HREF="http://py-xmlrpc.sourceforge.net">
+				py-xmlrpc library</A>. XmlRpc++ is designed to make it easy to incorporate 
+			XmlRpc client and server support into C++ applications. Or use both client and 
+			server objects in your app for easy peer-to-peer support.
+		</P>
+		<H3>Features</H3>
+		<UL>
+			<li>
+				<STRONG>Easy</STRONG> &nbsp; This library is easy to incorporate into C++ 
+				applications. No other libraries are required, other than your system's socket 
+				libraries. Simple XML parsing and HTTP support are built in.<br>
+			<li>
+				<STRONG>Fast</STRONG> &nbsp; All IO is non-blocking, so a slow client or 
+				network will not slow down the server.<br>
+			<li>
+				<STRONG>Portable</STRONG> Written in standard C++ to the POSIX and Windows 
+				sockets APIs. You do need a fairly recent compiler (g++ 3.1 or MSVC++ .Net or 
+				MSVC++ 6 with the <A href="http://www.dinkumware.com/vc_fixes.html">STL patches</A>.)
+			</li>
+			<li>
+				<STRONG>Free</STRONG> &nbsp; This library is released under the <a href="http://www.gnu.org/">
+					GNU</a> <a href="http://www.gnu.org/copyleft/lesser.html">LGPL</a>.<br>
+				<br>
+			</li>
+		</UL>
+		<P>&nbsp;</P>
+		<h3>Changes</h3>
+		<UL>
+			<li>
+				Better handling of fault responses: server methods can throw an 
+				XmlRpcException to return a fault and XmlRpcClient has a new method to 
+				test whether the last response was a fault.</li>
+			<li>
+				Support for system.listMethods and system.methodHelp from the introspection 
+				API.</li>
+			<li>
+				Support for system.multicall to process multiple requests in a single transaction.</li>
+			<li>
+				Fixed a problem in the XmlRpcServer destructor (it should not have been deleting the methods).</li>
+			<li>
+				The server ensures a valid result value is returned even if the method does not
+				set the result. The default result is an empty string.</li>
+			<li>
+				Doxygen comments in header files and a doc target in the makefile.</li>
+		</UL>
+		<P>
+		<P>&nbsp;</P>
+		<h3>Installation</h3>
+		<P>
+			There are VC++ 6 and VC++ .Net project files building on Windows. If you are 
+			using VC++ 6, you should apply SP3 and the fixes at <A href="http://www.dinkumware.com/vc_fixes.html">
+				http://www.dinkumware.com/vc_fixes.html</A>. Be sure to set the appropriate 
+			code generation switches. In particular, ensure that the runtime library 
+			(single/multi-threaded, static library/DLL) used is the same for the XmlRpc++ 
+			code and whatever application it will be linked to.</P>
+		<P>
+			For Linux, Solaris, and other Unix-like platforms there is a GNU Makefile which 
+			can be edited to suit your system. Specify your C++ compiler, compiler flags, 
+			and your system's socket libraries.
+		</P>
+		<p>In the test directory there are various test programs that are built by default. 
+			To verify that the library built correctly, you can start the HelloServer 
+			example:<br>
+			<pre>HelloServer 8000
+			</pre>
+			and the HelloClient example in another terminal window:<br>
+			<pre>HelloClient localhost 8000
+			</pre>
+		<P>
+			You should see two Hello messages and a sum displayed (amongst a bunch of debug 
+			output). You can also try the XML server validator program (eg, "Validator 80") 
+			and then attempt to connect to it from <A href="http://validator.xmlrpc.com">http://validator.xmlrpc.com</A>
+			(if you have access to the internet and are not behind a firewall etc).
+		</P>
+		<H3>Author</H3>
+		<P><A href="mailto:cmorley@users.sourceforge.net">Chris Morley</A>
+		</P>
+		<P>Although no code was re-used, the design and structure of the library is based 
+			upon the py-xmlrpc library implementation.<BR>
+			The base64 decoder/encoder is by <A href="mailto:lostd@ukr.net">Konstantin 
+				Pilipchuk</A>.</P>
+		<P></P>
+		<H3>License</H3>
+		<p>A full copy of the LGPL license is included in the file COPYING. The source code 
+			is Copyright (c) 2002-2003 by Chris Morley. This library is free software; you 
+			can redistribute it and/or modify it under the terms of the GNU Lesser General 
+			Public License as published by the Free Software Foundation; either version 2.1 
+			of the License, or (at your option) any later version. This library is 
+			distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; 
+			without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 
+			PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 
+			You should have received a copy of the GNU Lesser General Public License along 
+			with this library; if not, write to the Free Software Foundation, Inc., 59 
+			Temple Place, Suite 330, Boston, MA 02111-1307 USA
+		</p>
+	</body>
+</html>

thirdparty/ros/ros_comm/utilities/xmlrpcpp/cmake/xmlrpcpp-extras.cmake.develspace.in

@@ -0,0 +1 @@
+list(APPEND @PROJECT_NAME@_INCLUDE_DIRS "@xmlrpcpp_SOURCE_DIR@/include/xmlrpcpp")

thirdparty/ros/ros_comm/utilities/xmlrpcpp/cmake/xmlrpcpp-extras.cmake.installspace.in

@@ -0,0 +1 @@
+list(APPEND @PROJECT_NAME@_INCLUDE_DIRS "${@PROJECT_NAME@_DIR}/../../../@CATKIN_GLOBAL_INCLUDE_DESTINATION@/xmlrpcpp")

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpc.h

@@ -0,0 +1,102 @@
+// this file modified by Morgan Quigley on 22 April 2008 to add 
+// a std::exception-derived class
+#ifndef _XMLRPC_H_
+#define _XMLRPC_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+// 
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Lesser General Public License for more details.
+// 
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
+// 
+
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#ifndef MAKEDEPEND
+# include <string>
+#endif
+
+#include "xmlrpcpp/XmlRpcClient.h"
+#include "xmlrpcpp/XmlRpcException.h"
+#include "xmlrpcpp/XmlRpcServer.h"
+#include "xmlrpcpp/XmlRpcServerMethod.h"
+#include "xmlrpcpp/XmlRpcValue.h"
+#include "xmlrpcpp/XmlRpcUtil.h"
+
+#include <stdexcept>
+
+namespace XmlRpc {
+
+
+  //! An interface allowing custom handling of error message reporting.
+  class XmlRpcErrorHandler {
+  public:
+    virtual ~XmlRpcErrorHandler() { }
+
+    //! Returns a pointer to the currently installed error handling object.
+    static XmlRpcErrorHandler* getErrorHandler() 
+    { return _errorHandler; }
+
+    //! Specifies the error handler.
+    static void setErrorHandler(XmlRpcErrorHandler* eh)
+    { _errorHandler = eh; }
+
+    //! Report an error. Custom error handlers should define this method.
+    virtual void error(const char* msg) = 0;
+
+  protected:
+    static XmlRpcErrorHandler* _errorHandler;
+  };
+
+  //! An interface allowing custom handling of informational message reporting.
+  class XmlRpcLogHandler {
+  public:
+    virtual ~XmlRpcLogHandler() { }
+
+    //! Returns a pointer to the currently installed message reporting object.
+    static XmlRpcLogHandler* getLogHandler() 
+    { return _logHandler; }
+
+    //! Specifies the message handler.
+    static void setLogHandler(XmlRpcLogHandler* lh)
+    { _logHandler = lh; }
+
+    //! Returns the level of verbosity of informational messages. 0 is no output, 5 is very verbose.
+    static int getVerbosity() 
+    { return _verbosity; }
+
+    //! Specify the level of verbosity of informational messages. 0 is no output, 5 is very verbose.
+    static void setVerbosity(int v) 
+    { _verbosity = v; }
+
+    //! Output a message. Custom error handlers should define this method.
+    virtual void log(int level, const char* msg) = 0;
+
+  protected:
+    static XmlRpcLogHandler* _logHandler;
+    static int _verbosity;
+  };
+
+  //! Returns log message verbosity. This is short for XmlRpcLogHandler::getVerbosity()
+  int getVerbosity();
+  //! Sets log message verbosity. This is short for XmlRpcLogHandler::setVerbosity(level)
+  void setVerbosity(int level);
+
+
+  //! Version identifier
+  extern const char XMLRPC_VERSION[];
+
+} // namespace XmlRpc
+
+#endif // _XMLRPC_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcClient.h

@@ -0,0 +1,133 @@
+
+#ifndef _XMLRPCCLIENT_H_
+#define _XMLRPCCLIENT_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+
+#ifndef MAKEDEPEND
+# include <string>
+#endif
+
+#include "xmlrpcpp/XmlRpcDispatch.h"
+#include "xmlrpcpp/XmlRpcSource.h"
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+namespace XmlRpc {
+
+  // Arguments and results are represented by XmlRpcValues
+  class XmlRpcValue;
+
+  //! A class to send XML RPC requests to a server and return the results.
+  class XMLRPCPP_DECL XmlRpcClient : public XmlRpcSource {
+  public:
+    // Static data
+    static const char REQUEST_BEGIN[];
+    static const char REQUEST_END_METHODNAME[];
+    static const char PARAMS_TAG[];
+    static const char PARAMS_ETAG[];
+    static const char PARAM_TAG[];
+    static const char PARAM_ETAG[];
+    static const char REQUEST_END[];
+    // Result tags
+    static const char METHODRESPONSE_TAG[];
+    static const char FAULT_TAG[];
+
+    //! Construct a client to connect to the server at the specified host:port address
+    //!  @param host The name of the remote machine hosting the server
+    //!  @param port The port on the remote machine where the server is listening
+    //!  @param uri  An optional string to be sent as the URI in the HTTP GET header
+    XmlRpcClient(const char* host, int port, const char* uri=0);
+
+    //! Destructor
+    virtual ~XmlRpcClient();
+
+    //! Execute the named procedure on the remote server.
+    //!  @param method The name of the remote procedure to execute
+    //!  @param params An array of the arguments for the method
+    //!  @param result The result value to be returned to the client
+    //!  @return true if the request was sent and a result received 
+    //!   (although the result might be a fault).
+    //!
+    //! Currently this is a synchronous (blocking) implementation (execute
+    //! does not return until it receives a response or an error). Use isFault()
+    //! to determine whether the result is a fault response.
+    bool execute(const char* method, XmlRpcValue const& params, XmlRpcValue& result);
+
+    bool executeNonBlock(const char* method, XmlRpcValue const& params);
+    bool executeCheckDone(XmlRpcValue& result);
+
+    //! Returns true if the result of the last execute() was a fault response.
+    bool isFault() const { return _isFault; }
+
+
+    // XmlRpcSource interface implementation
+    //! Close the connection
+    virtual void close();
+
+    //! Handle server responses. Called by the event dispatcher during execute.
+    //!  @param eventType The type of event that occurred. 
+    //!  @see XmlRpcDispatch::EventType
+    virtual unsigned handleEvent(unsigned eventType);
+
+  protected:
+    // Execution processing helpers
+    virtual bool doConnect();
+    virtual bool setupConnection();
+
+    virtual bool generateRequest(const char* method, XmlRpcValue const& params);
+    virtual std::string generateHeader(std::string const& body);
+    virtual bool writeRequest();
+    virtual bool readHeader();
+    virtual bool readResponse();
+    virtual bool parseResponse(XmlRpcValue& result);
+
+    // Possible IO states for the connection
+    enum ClientConnectionState { NO_CONNECTION, CONNECTING, WRITE_REQUEST, READ_HEADER, READ_RESPONSE, IDLE };
+    ClientConnectionState _connectionState;
+
+    // Server location
+    std::string _host;
+    std::string _uri;
+    int _port;
+  public:
+    const std::string &getHost() { return _host; }
+    const std::string &getUri()  { return _uri; }
+    int getPort() const { return _port; }
+    
+    // The xml-encoded request, http header of response, and response xml
+    std::string _request;
+    std::string _header;
+    std::string _response;
+
+    // Number of times the client has attempted to send the request
+    int _sendAttempts;
+
+    // Number of bytes of the request that have been written to the socket so far
+    int _bytesWritten;
+
+    // True if we are currently executing a request. If you want to multithread,
+    // each thread should have its own client.
+    bool _executing;
+
+    // True if the server closed the connection
+    bool _eof;
+
+    // True if a fault response was returned by the server
+    bool _isFault;
+
+    // Number of bytes expected in the response body (parsed from response header)
+    int _contentLength;
+
+    // Event dispatcher
+    XmlRpcDispatch _disp;
+
+  };	// class XmlRpcClient
+
+}	// namespace XmlRpc
+
+#endif	// _XMLRPCCLIENT_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcDecl.h

@@ -0,0 +1,55 @@
+/*********************************************************************
+*
+* 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.
+*
+*********************************************************************/
+/*
+ * Cross platform macros.
+ *
+ */
+#ifndef XMLRPCPP_DECL_H_INCLUDED
+#define XMLRPCPP_DECL_H_INCLUDED
+
+#include <ros/macros.h>
+
+#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
+  #ifdef xmlrpcpp_EXPORTS // we are building a shared lib/dll
+    #define XMLRPCPP_DECL ROS_HELPER_EXPORT
+  #else // we are using shared lib/dll
+    #define XMLRPCPP_DECL ROS_HELPER_IMPORT
+  #endif
+#else // ros is being built around static libraries
+  #define XMLRPCPP_DECL
+#endif
+
+#endif /* XMLRPCPP_DECL_H_INCLUDED */

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcDispatch.h

@@ -0,0 +1,89 @@
+
+#ifndef _XMLRPCDISPATCH_H_
+#define _XMLRPCDISPATCH_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+#ifndef MAKEDEPEND
+# include <list>
+#endif
+
+namespace XmlRpc {
+
+  // An RPC source represents a file descriptor to monitor
+  class XmlRpcSource;
+
+  //! An object which monitors file descriptors for events and performs
+  //! callbacks when interesting events happen.
+  class XMLRPCPP_DECL XmlRpcDispatch {
+  public:
+    //! Constructor
+    XmlRpcDispatch();
+    ~XmlRpcDispatch();
+
+    //! Values indicating the type of events a source is interested in
+    enum EventType {
+      ReadableEvent = 1,    //!< data available to read
+      WritableEvent = 2,    //!< connected/data can be written without blocking
+      Exception     = 4     //!< uh oh
+    };
+    
+    //! Monitor this source for the event types specified by the event mask
+    //! and call its event handler when any of the events occur.
+    //!  @param source The source to monitor
+    //!  @param eventMask Which event types to watch for. \see EventType
+    void addSource(XmlRpcSource* source, unsigned eventMask);
+
+    //! Stop monitoring this source.
+    //!  @param source The source to stop monitoring
+    void removeSource(XmlRpcSource* source);
+
+    //! Modify the types of events to watch for on this source
+    void setSourceEvents(XmlRpcSource* source, unsigned eventMask);
+
+
+    //! Watch current set of sources and process events for the specified
+    //! duration (in ms, -1 implies wait forever, or until exit is called)
+    void work(double msTime);
+
+    //! Exit from work routine
+    void exit();
+
+    //! Clear all sources from the monitored sources list. Sources are closed.
+    void clear();
+
+    // helper returning current steady/monotonic time
+    double getTime();
+
+    // A source to monitor and what to monitor it for
+    struct MonitoredSource {
+      MonitoredSource(XmlRpcSource* src, unsigned mask) : _src(src), _mask(mask) {}
+      XmlRpcSource* getSource() const { return _src; }
+      unsigned& getMask() { return _mask; }
+      XmlRpcSource* _src;
+      unsigned _mask;
+    };
+
+    // A list of sources to monitor
+    typedef std::list< MonitoredSource > SourceList; 
+
+    // Sources being monitored
+    SourceList _sources;
+  protected:
+
+    // When work should stop (-1 implies wait forever, or until exit is called)
+    double _endTime;
+
+    bool _doClear;
+    bool _inWork;
+
+  };
+} // namespace XmlRpc
+
+#endif  // _XMLRPCDISPATCH_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcException.h

@@ -0,0 +1,44 @@
+
+#ifndef _XMLRPCEXCEPTION_H_
+#define _XMLRPCEXCEPTION_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#ifndef MAKEDEPEND
+# include <string>
+#endif
+
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+
+namespace XmlRpc {
+
+  //! A class representing an error.
+  //! If server methods throw this exception, a fault response is returned
+  //! to the client.
+  class XMLRPCPP_DECL XmlRpcException {
+  public:
+    //! Constructor
+    //!   @param message  A descriptive error message
+    //!   @param code     An integer error code
+    XmlRpcException(const std::string& message, int code=-1) :
+        _message(message), _code(code) {}
+
+    //! Return the error message.
+    const std::string& getMessage() const { return _message; }
+
+    //! Return the error code.
+    int getCode() const { return _code; }
+
+  private:
+    std::string _message;
+    int _code;
+  };
+
+}
+
+#endif	// _XMLRPCEXCEPTION_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcServer.h

@@ -0,0 +1,114 @@
+// this file modified by Morgan Quigley on 22 Apr 2008.
+// added features: server can be opened on port 0 and you can read back
+// what port the OS gave you
+
+#ifndef _XMLRPCSERVER_H_
+#define _XMLRPCSERVER_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#ifndef MAKEDEPEND
+# include <map>
+# include <string>
+#endif
+
+#include "xmlrpcpp/XmlRpcDispatch.h"
+#include "xmlrpcpp/XmlRpcSource.h"
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+namespace XmlRpc {
+
+
+  // An abstract class supporting XML RPC methods
+  class XmlRpcServerMethod;
+
+  // Class representing connections to specific clients
+  class XmlRpcServerConnection;
+
+  // Class representing argument and result values
+  class XmlRpcValue;
+
+
+  //! A class to handle XML RPC requests
+  class XMLRPCPP_DECL XmlRpcServer : public XmlRpcSource {
+  public:
+    //! Create a server object.
+    XmlRpcServer();
+    //! Destructor.
+    virtual ~XmlRpcServer();
+
+    //! Specify whether introspection is enabled or not. Default is not enabled.
+    void enableIntrospection(bool enabled=true);
+
+    //! Add a command to the RPC server
+    void addMethod(XmlRpcServerMethod* method);
+
+    //! Remove a command from the RPC server
+    void removeMethod(XmlRpcServerMethod* method);
+
+    //! Remove a command from the RPC server by name
+    void removeMethod(const std::string& methodName);
+
+    //! Look up a method by name
+    XmlRpcServerMethod* findMethod(const std::string& name) const;
+
+    //! Create a socket, bind to the specified port, and
+    //! set it in listen mode to make it available for clients.
+    bool bindAndListen(int port, int backlog = 5);
+
+    //! Process client requests for the specified time
+    void work(double msTime);
+
+    //! Temporarily stop processing client requests and exit the work() method.
+    void exit();
+
+    //! Close all connections with clients and the socket file descriptor
+    void shutdown();
+
+    //! Introspection support
+    void listMethods(XmlRpcValue& result);
+
+    // XmlRpcSource interface implementation
+
+    //! Handle client connection requests
+    virtual unsigned handleEvent(unsigned eventType);
+
+    //! Remove a connection from the dispatcher
+    virtual void removeConnection(XmlRpcServerConnection*);
+
+    inline int get_port() { return _port; }
+
+    XmlRpcDispatch *get_dispatch() { return &_disp; }
+
+  protected:
+
+    //! Accept a client connection request
+    virtual void acceptConnection();
+
+    //! Create a new connection object for processing requests from a specific client.
+    virtual XmlRpcServerConnection* createConnection(int socket);
+
+    // Whether the introspection API is supported by this server
+    bool _introspectionEnabled;
+
+    // Event dispatcher
+    XmlRpcDispatch _disp;
+
+    // Collection of methods. This could be a set keyed on method name if we wanted...
+    typedef std::map< std::string, XmlRpcServerMethod* > MethodMap;
+    MethodMap _methods;
+
+    // system methods
+    XmlRpcServerMethod* _listMethods;
+    XmlRpcServerMethod* _methodHelp;
+
+    int _port;
+
+  };
+} // namespace XmlRpc
+
+#endif //_XMLRPCSERVER_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcServerConnection.h

@@ -0,0 +1,103 @@
+#ifndef _XMLRPCSERVERCONNECTION_H_
+#define _XMLRPCSERVERCONNECTION_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#ifndef MAKEDEPEND
+# include <string>
+#endif
+
+#include "xmlrpcpp/XmlRpcValue.h"
+#include "xmlrpcpp/XmlRpcSource.h"
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+namespace XmlRpc {
+
+
+  // The server waits for client connections and provides methods
+  class XmlRpcServer;
+  class XmlRpcServerMethod;
+
+  //! A class to handle XML RPC requests from a particular client
+  class XMLRPCPP_DECL XmlRpcServerConnection : public XmlRpcSource {
+  public:
+    // Static data
+    static const char METHODNAME_TAG[];
+    static const char PARAMS_TAG[];
+    static const char PARAMS_ETAG[];
+    static const char PARAM_TAG[];
+    static const char PARAM_ETAG[];
+
+    static const std::string SYSTEM_MULTICALL;
+    static const std::string METHODNAME;
+    static const std::string PARAMS;
+
+    static const std::string FAULTCODE;
+    static const std::string FAULTSTRING;
+
+    //! Constructor
+    XmlRpcServerConnection(int fd, XmlRpcServer* server, bool deleteOnClose = false);
+    //! Destructor
+    virtual ~XmlRpcServerConnection();
+
+    // XmlRpcSource interface implementation
+    //! Handle IO on the client connection socket.
+    //!   @param eventType Type of IO event that occurred. @see XmlRpcDispatch::EventType.
+    virtual unsigned handleEvent(unsigned eventType);
+
+  protected:
+
+    bool readHeader();
+    bool readRequest();
+    bool writeResponse();
+
+    // Parses the request, runs the method, generates the response xml.
+    virtual void executeRequest();
+
+    // Parse the methodName and parameters from the request.
+    std::string parseRequest(XmlRpcValue& params);
+
+    // Execute a named method with the specified params.
+    bool executeMethod(const std::string& methodName, XmlRpcValue& params, XmlRpcValue& result);
+
+    // Execute multiple calls and return the results in an array.
+    bool executeMulticall(const std::string& methodName, XmlRpcValue& params, XmlRpcValue& result);
+
+    // Construct a response from the result XML.
+    void generateResponse(std::string const& resultXml);
+    void generateFaultResponse(std::string const& msg, int errorCode = -1);
+    std::string generateHeader(std::string const& body);
+
+
+    // The XmlRpc server that accepted this connection
+    XmlRpcServer* _server;
+
+    // Possible IO states for the connection
+    enum ServerConnectionState { READ_HEADER, READ_REQUEST, WRITE_RESPONSE };
+    ServerConnectionState _connectionState;
+
+    // Request headers
+    std::string _header;
+
+    // Number of bytes expected in the request body (parsed from header)
+    int _contentLength;
+
+    // Request body
+    std::string _request;
+
+    // Response
+    std::string _response;
+
+    // Number of bytes of the response written so far
+    int _bytesWritten;
+
+    // Whether to keep the current client connection open for further requests
+    bool _keepAlive;
+  };
+} // namespace XmlRpc
+
+#endif // _XMLRPCSERVERCONNECTION_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcServerMethod.h

@@ -0,0 +1,49 @@
+
+#ifndef _XMLRPCSERVERMETHOD_H_
+#define _XMLRPCSERVERMETHOD_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+#ifndef MAKEDEPEND
+# include <string>
+#endif
+
+namespace XmlRpc {
+
+  // Representation of a parameter or result value
+  class XmlRpcValue;
+
+  // The XmlRpcServer processes client requests to call RPCs
+  class XmlRpcServer;
+
+  //! Abstract class representing a single RPC method
+  class XMLRPCPP_DECL XmlRpcServerMethod {
+  public:
+    //! Constructor
+    XmlRpcServerMethod(std::string const& name, XmlRpcServer* server = 0);
+    //! Destructor
+    virtual ~XmlRpcServerMethod();
+
+    //! Returns the name of the method
+    std::string& name() { return _name; }
+
+    //! Execute the method. Subclasses must provide a definition for this method.
+    virtual void execute(XmlRpcValue& params, XmlRpcValue& result) = 0;
+
+    //! Returns a help string for the method.
+    //! Subclasses should define this method if introspection is being used.
+    virtual std::string help() { return std::string(); }
+
+  protected:
+    std::string _name;
+    XmlRpcServer* _server;
+  };
+} // namespace XmlRpc
+
+#endif // _XMLRPCSERVERMETHOD_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcSocket.h

@@ -0,0 +1,80 @@
+// this file modified by Morgan Quigley on 22 Apr 2008.
+// added features: server can be opened on port 0 and you can read back
+// what port the OS gave you
+
+#ifndef _XMLRPCSOCKET_H_
+#define _XMLRPCSOCKET_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#ifndef MAKEDEPEND
+# include <string>
+#endif
+
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+namespace XmlRpc {
+
+  //! A platform-independent socket API.
+  class XMLRPCPP_DECL XmlRpcSocket {
+  public:
+
+    static bool s_use_ipv6_;
+
+    //! Creates a stream (TCP) socket. Returns -1 on failure.
+    static int socket();
+
+    //! Closes a socket.
+    static void close(int socket);
+
+
+    //! Sets a stream (TCP) socket to perform non-blocking IO. Returns false on failure.
+    static bool setNonBlocking(int socket);
+
+    //! Read text from the specified socket. Returns false on error.
+    static bool nbRead(int socket, std::string& s, bool *eof);
+
+    //! Write text to the specified socket. Returns false on error.
+    static bool nbWrite(int socket, std::string& s, int *bytesSoFar);
+
+
+    // The next four methods are appropriate for servers.
+
+    //! Allow the port the specified socket is bound to to be re-bound immediately so 
+    //! server re-starts are not delayed. Returns false on failure.
+    static bool setReuseAddr(int socket);
+
+    //! Bind to a specified port
+    static bool bind(int socket, int port);
+    
+    static int get_port(int socket);
+
+    //! Set socket in listen mode
+    static bool listen(int socket, int backlog);
+
+    //! Accept a client connection request
+    static int accept(int socket);
+
+
+
+    //! Connect a socket to a server (from a client)
+    static bool connect(int socket, std::string& host, int port);
+
+
+    //! Returns last errno
+    static int getError();
+
+    //! Returns message corresponding to last error
+    static std::string getErrorMsg();
+
+    //! Returns message corresponding to error
+    static std::string getErrorMsg(int error);
+  };
+
+} // namespace XmlRpc
+
+#endif

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcSource.h

@@ -0,0 +1,57 @@
+
+#ifndef _XMLRPCSOURCE_H_
+#define _XMLRPCSOURCE_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+namespace XmlRpc {
+
+  //! An RPC source represents a file descriptor to monitor
+  class XMLRPCPP_DECL XmlRpcSource {
+  public:
+    //! Constructor
+    //!  @param fd The socket file descriptor to monitor.
+    //!  @param deleteOnClose If true, the object deletes itself when close is called.
+    XmlRpcSource(int fd = -1, bool deleteOnClose = false);
+
+    //! Destructor
+    virtual ~XmlRpcSource();
+
+    //! Return the file descriptor being monitored.
+    int getfd() const { return _fd; }
+    //! Specify the file descriptor to monitor.
+    void setfd(int fd) { _fd = fd; }
+
+    //! Return whether the file descriptor should be kept open if it is no longer monitored.
+    bool getKeepOpen() const { return _keepOpen; }
+    //! Specify whether the file descriptor should be kept open if it is no longer monitored.
+    void setKeepOpen(bool b=true) { _keepOpen = b; }
+
+    //! Close the owned fd. If deleteOnClose was specified at construction, the object is deleted.
+    virtual void close();
+
+    //! Return true to continue monitoring this source
+    virtual unsigned handleEvent(unsigned eventType) = 0;
+
+  private:
+
+    // Socket. This should really be a SOCKET (an alias for unsigned int*) on windows...
+    int _fd;
+
+    // In the server, a new source (XmlRpcServerConnection) is created
+    // for each connected client. When each connection is closed, the
+    // corresponding source object is deleted.
+    bool _deleteOnClose;
+
+    // In the client, keep connections open if you intend to make multiple calls.
+    bool _keepOpen;
+  };
+} // namespace XmlRpc
+
+#endif //_XMLRPCSOURCE_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcUtil.h

@@ -0,0 +1,63 @@
+#ifndef _XMLRPCUTIL_H_
+#define _XMLRPCUTIL_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#ifndef MAKEDEPEND
+# include <string>
+#endif
+
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+#if defined(_MSC_VER)
+# define snprintf	    _snprintf_s
+# define vsnprintf    _vsnprintf_s
+# define strcasecmp	  _stricmp
+# define strncasecmp	_strnicmp
+#elif defined(__BORLANDC__)
+# define strcasecmp stricmp
+# define strncasecmp strnicmp
+#endif
+
+namespace XmlRpc {
+
+  //! Utilities for XML parsing, encoding, and decoding and message handlers.
+  class XMLRPCPP_DECL XmlRpcUtil {
+  public:
+    // hokey xml parsing
+    //! Returns contents between <tag> and </tag>, updates offset to char after </tag>
+    static std::string parseTag(const char* tag, std::string const& xml, int* offset);
+
+    //! Returns true if the tag is found and updates offset to the char after the tag
+    static bool findTag(const char* tag, std::string const& xml, int* offset);
+
+    //! Returns the next tag and updates offset to the char after the tag, or empty string
+    //! if the next non-whitespace character is not '<'
+    static std::string getNextTag(std::string const& xml, int* offset);
+
+    //! Returns true if the tag is found at the specified offset (modulo any whitespace)
+    //! and updates offset to the char after the tag
+    static bool nextTagIs(const char* tag, std::string const& xml, int* offset);
+
+
+    //! Convert raw text to encoded xml.
+    static std::string xmlEncode(const std::string& raw);
+
+    //! Convert encoded xml to raw text
+    static std::string xmlDecode(const std::string& encoded);
+
+
+    //! Dump messages somewhere
+    static void log(int level, const char* fmt, ...);
+
+    //! Dump error messages somewhere
+    static void error(const char* fmt, ...);
+
+  };
+} // namespace XmlRpc
+
+#endif // _XMLRPCUTIL_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/XmlRpcValue.h

@@ -0,0 +1,195 @@
+
+#ifndef _XMLRPCVALUE_H_
+#define _XMLRPCVALUE_H_
+//
+// XmlRpc++ Copyright (c) 2002-2003 by Chris Morley
+//
+#if defined(_MSC_VER)
+# pragma warning(disable:4786)    // identifier was truncated in debug info
+#endif
+
+#include "xmlrpcpp/XmlRpcDecl.h"
+
+#ifndef MAKEDEPEND
+# include <map>
+# include <string>
+# include <vector>
+# include <time.h>
+#endif
+
+namespace XmlRpc {
+
+  //! RPC method arguments and results are represented by Values
+  //   should probably refcount them...
+  class XMLRPCPP_DECL XmlRpcValue {
+  public:
+
+
+    enum Type {
+      TypeInvalid,
+      TypeBoolean,
+      TypeInt,
+      TypeDouble,
+      TypeString,
+      TypeDateTime,
+      TypeBase64,
+      TypeArray,
+      TypeStruct
+    };
+
+    // Non-primitive types
+    typedef std::vector<char> BinaryData;
+    typedef std::vector<XmlRpcValue> ValueArray;
+    typedef std::map<std::string, XmlRpcValue> ValueStruct;
+    typedef ValueStruct::iterator iterator;
+
+
+    //! Constructors
+    XmlRpcValue() : _type(TypeInvalid) { _value.asBinary = 0; }
+    XmlRpcValue(bool value) : _type(TypeBoolean) { _value.asBool = value; }
+    XmlRpcValue(int value)  : _type(TypeInt) { _value.asInt = value; }
+    XmlRpcValue(double value)  : _type(TypeDouble) { _value.asDouble = value; }
+
+    XmlRpcValue(std::string const& value) : _type(TypeString) 
+    { _value.asString = new std::string(value); }
+
+    XmlRpcValue(const char* value)  : _type(TypeString)
+    { _value.asString = new std::string(value); }
+
+    XmlRpcValue(struct tm* value)  : _type(TypeDateTime) 
+    { _value.asTime = new struct tm(*value); }
+
+
+    XmlRpcValue(void* value, int nBytes)  : _type(TypeBase64)
+    {
+      _value.asBinary = new BinaryData((char*)value, ((char*)value)+nBytes);
+    }
+
+    //! Construct from xml, beginning at *offset chars into the string, updates offset
+    XmlRpcValue(std::string const& xml, int* offset) : _type(TypeInvalid)
+    { if ( ! fromXml(xml,offset)) _type = TypeInvalid; }
+
+    //! Copy
+    XmlRpcValue(XmlRpcValue const& rhs) : _type(TypeInvalid) { *this = rhs; }
+
+    //! Destructor (make virtual if you want to subclass)
+    /*virtual*/ ~XmlRpcValue() { invalidate(); }
+
+    //! Erase the current value
+    void clear() { invalidate(); }
+
+    // Operators
+    XmlRpcValue& operator=(XmlRpcValue const& rhs);
+    XmlRpcValue& operator=(int const& rhs) { return operator=(XmlRpcValue(rhs)); }
+    XmlRpcValue& operator=(double const& rhs) { return operator=(XmlRpcValue(rhs)); }
+    XmlRpcValue& operator=(const char* rhs) { return operator=(XmlRpcValue(std::string(rhs))); }
+
+    bool operator==(XmlRpcValue const& other) const;
+    bool operator!=(XmlRpcValue const& other) const;
+
+    operator bool&()          { assertTypeOrInvalid(TypeBoolean); return _value.asBool; }
+    operator int&()           { assertTypeOrInvalid(TypeInt); return _value.asInt; }
+    operator double&()        { assertTypeOrInvalid(TypeDouble); return _value.asDouble; }
+    operator std::string&()   { assertTypeOrInvalid(TypeString); return *_value.asString; }
+    operator BinaryData&()    { assertTypeOrInvalid(TypeBase64); return *_value.asBinary; }
+    operator struct tm&()     { assertTypeOrInvalid(TypeDateTime); return *_value.asTime; }
+
+    XmlRpcValue const& operator[](int i) const { assertArray(i+1); return _value.asArray->at(i); }
+    XmlRpcValue& operator[](int i)             { assertArray(i+1); return _value.asArray->at(i); }
+
+    XmlRpcValue& operator[](std::string const& k) { assertStruct(); return (*_value.asStruct)[k]; }
+    XmlRpcValue& operator[](const char* k) { assertStruct(); std::string s(k); return (*_value.asStruct)[s]; }
+
+    iterator begin() {assertStruct(); return (*_value.asStruct).begin(); }
+    iterator end() {assertStruct(); return (*_value.asStruct).end(); }
+
+    // Accessors
+    //! Return true if the value has been set to something.
+    bool valid() const { return _type != TypeInvalid; }
+
+    //! Return the type of the value stored. \see Type.
+    Type const &getType() const { return _type; }
+
+    //! Return the size for string, base64, array, and struct values.
+    int size() const;
+
+    //! Specify the size for array values. Array values will grow beyond this size if needed.
+    void setSize(int size)    { assertArray(size); }
+
+    //! Check for the existence of a struct member by name.
+    bool hasMember(const std::string& name) const;
+
+    //! Decode xml. Destroys any existing value.
+    bool fromXml(std::string const& valueXml, int* offset);
+
+    //! Encode the Value in xml
+    std::string toXml() const;
+
+    //! Write the value (no xml encoding)
+    std::ostream& write(std::ostream& os) const;
+
+    // Formatting
+    //! Return the format used to write double values.
+    static std::string const& getDoubleFormat() { return _doubleFormat; }
+
+    //! Specify the format used to write double values.
+    static void setDoubleFormat(const char* f) { _doubleFormat = f; }
+
+
+  protected:
+    // Clean up
+    void invalidate();
+
+    // Type checking
+    void assertTypeOrInvalid(Type t);
+    void assertArray(int size) const;
+    void assertArray(int size);
+    void assertStruct();
+
+    // XML decoding
+    bool boolFromXml(std::string const& valueXml, int* offset);
+    bool intFromXml(std::string const& valueXml, int* offset);
+    bool doubleFromXml(std::string const& valueXml, int* offset);
+    bool stringFromXml(std::string const& valueXml, int* offset);
+    bool timeFromXml(std::string const& valueXml, int* offset);
+    bool binaryFromXml(std::string const& valueXml, int* offset);
+    bool arrayFromXml(std::string const& valueXml, int* offset);
+    bool structFromXml(std::string const& valueXml, int* offset);
+
+    // XML encoding
+    std::string boolToXml() const;
+    std::string intToXml() const;
+    std::string doubleToXml() const;
+    std::string stringToXml() const;
+    std::string timeToXml() const;
+    std::string binaryToXml() const;
+    std::string arrayToXml() const;
+    std::string structToXml() const;
+
+    // Format strings
+    static std::string _doubleFormat;
+
+    // Type tag and values
+    Type _type;
+
+    // At some point I will split off Arrays and Structs into
+    // separate ref-counted objects for more efficient copying.
+    union {
+      bool          asBool;
+      int           asInt;
+      double        asDouble;
+      struct tm*    asTime;
+      std::string*  asString;
+      BinaryData*   asBinary;
+      ValueArray*   asArray;
+      ValueStruct*  asStruct;
+    } _value;
+    
+  };
+} // namespace XmlRpc
+
+
+std::ostream& operator<<(std::ostream& os, const XmlRpc::XmlRpcValue& v);
+
+
+#endif // _XMLRPCVALUE_H_

thirdparty/ros/ros_comm/utilities/xmlrpcpp/include/xmlrpcpp/base64.h

@@ -0,0 +1,380 @@
+
+
+//  base64.hpp 
+//  Autor Konstantin Pilipchuk
+//  mailto:lostd@ukr.net
+//
+//
+
+#if !defined(__BASE64_H_INCLUDED__)
+#define __BASE64_H_INCLUDED__ 1
+
+#ifndef MAKEDEPEND
+# include <iterator>
+#endif
+
+#include <ios>
+
+static
+int _base64Chars[]= {'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P','Q','R','S','T','U','V','W','X','Y','Z',
+				     'a','b','c','d','e','f','g','h','i','j','k','l','m','n','o','p','q','r','s','t','u','v','w','x','y','z',
+			         '0','1','2','3','4','5','6','7','8','9',
+			         '+','/' };
+
+
+#define _0000_0011 0x03
+#define _1111_1100 0xFC
+#define _1111_0000 0xF0
+#define _0011_0000 0x30
+#define _0011_1100 0x3C
+#define _0000_1111 0x0F
+#define _1100_0000 0xC0
+#define _0011_1111 0x3F
+
+#define _EQUAL_CHAR   (-1)
+#define _UNKNOWN_CHAR (-2)
+
+#define _IOS_FAILBIT   std::ios_base::failbit
+#define _IOS_EOFBIT    std::ios_base::eofbit
+#define _IOS_BADBIT    std::ios_base::badbit
+#define _IOS_GOODBIT   std::ios_base::goodbit
+
+// TEMPLATE CLASS base64_put
+template<class _E = char, class _Tr = std::char_traits<_E> >
+class base64
+{
+public:
+
+	typedef unsigned char byte_t;
+	typedef _E            char_type;
+	typedef _Tr           traits_type; 
+
+	// base64 requires max line length <= 72 characters
+	// you can fill end of line
+	// it may be crlf, crlfsp, noline or other class like it
+
+
+	struct crlf
+	{
+		template<class _OI>
+			_OI operator()(_OI _To) const{
+			*_To = _Tr::to_char_type('\r'); ++_To;
+			*_To = _Tr::to_char_type('\n'); ++_To;
+
+			return (_To);
+		}
+	};
+
+
+	struct crlfsp
+	{
+		template<class _OI>
+			_OI operator()(_OI _To) const{
+			*_To = _Tr::to_char_type('\r'); ++_To;
+			*_To = _Tr::to_char_type('\n'); ++_To;
+			*_To = _Tr::to_char_type(' '); ++_To;
+
+			return (_To);
+		}
+	};
+
+	struct noline
+	{
+		template<class _OI>
+			_OI operator()(_OI _To) const{
+			return (_To);
+		}
+	};
+
+	struct three2four
+	{
+		void zero()
+		{
+			_data[0] = 0;
+			_data[1] = 0;
+			_data[2] = 0;
+		}
+
+		byte_t get_0()	const
+		{
+			return _data[0];
+		}
+		byte_t get_1()	const
+		{
+			return _data[1];
+		}
+		byte_t get_2()	const
+		{
+			return _data[2];
+		}
+
+		void set_0(byte_t _ch)
+		{
+			_data[0] = _ch;
+		}
+
+		void set_1(byte_t _ch)
+		{
+			_data[1] = _ch;
+		}
+
+		void set_2(byte_t _ch)
+		{
+			_data[2] = _ch;
+		}
+
+		// 0000 0000  1111 1111  2222 2222
+		// xxxx xxxx  xxxx xxxx  xxxx xxxx
+		// 0000 0011  1111 2222  2233 3333
+
+		int b64_0()	const	{return (_data[0] & _1111_1100) >> 2;}
+		int b64_1()	const	{return ((_data[0] & _0000_0011) << 4) + ((_data[1] & _1111_0000)>>4);}
+		int b64_2()	const	{return ((_data[1] & _0000_1111) << 2) + ((_data[2] & _1100_0000)>>6);}
+		int b64_3()	const	{return (_data[2] & _0011_1111);}
+
+		void b64_0(int _ch)	{_data[0] = ((_ch & _0011_1111) << 2) | (_0000_0011 & _data[0]);}
+
+		void b64_1(int _ch)	{
+			_data[0] = ((_ch & _0011_0000) >> 4) | (_1111_1100 & _data[0]);
+			_data[1] = ((_ch & _0000_1111) << 4) | (_0000_1111 & _data[1]);	}
+
+		void b64_2(int _ch)	{
+			_data[1] = ((_ch & _0011_1100) >> 2) | (_1111_0000 & _data[1]);
+			_data[2] = ((_ch & _0000_0011) << 6) | (_0011_1111 & _data[2]);	}
+
+		void b64_3(int _ch){
+			_data[2] = (_ch & _0011_1111) | (_1100_0000 & _data[2]);}
+
+	private:
+		byte_t _data[3];
+
+	};
+
+
+
+
+	template<class _II, class _OI, class _State, class _Endline>
+        _II put(_II _First, _II _Last, _OI _To, _State&, _Endline)  const
+	{
+		three2four _3to4;
+		int line_octets = 0;
+
+		while(_First != _Last)
+		{
+			_3to4.zero();
+
+			_3to4.set_0(*_First);
+			_First++;
+
+			if(_First == _Last)
+			{
+				*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_0()]); ++_To;
+				*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_1()]); ++_To;
+				*_To = _Tr::to_char_type('='); ++_To;
+				*_To = _Tr::to_char_type('='); ++_To;
+				goto __end;
+			}
+
+			_3to4.set_1(*_First);
+			_First++;
+
+			if(_First == _Last)
+			{
+				*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_0()]); ++_To;
+				*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_1()]); ++_To;
+				*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_2()]); ++_To;
+				*_To = _Tr::to_char_type('='); ++_To;
+				goto __end;
+			}
+
+			_3to4.set_2(*_First);
+			_First++;
+
+			*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_0()]); ++_To;
+			*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_1()]); ++_To;
+			*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_2()]); ++_To;
+			*_To = _Tr::to_char_type(_base64Chars[_3to4.b64_3()]); ++_To;
+
+			if(line_octets == 17)
+			{
+                                //_To = _Endl(_To);
+                                *_To = '\n'; ++_To;
+                                line_octets = 0;
+			}
+			else
+				++line_octets;
+		}
+
+		__end: ;
+
+		return (_First);
+
+	}
+
+
+	template<class _II, class _OI, class _State>
+		_II get(_II _First, _II _Last, _OI _To, _State& _St) const
+	{
+		three2four _3to4;
+		int _Char;
+
+		while(_First != _Last)
+		{
+
+			// Take octet
+			_3to4.zero();
+
+			// -- 0 --
+			// Search next valid char... 
+			while((_Char =  _getCharType(*_First)) < 0 && _Char == _UNKNOWN_CHAR)
+			{
+				if(++_First == _Last)
+				{
+					_St |= _IOS_FAILBIT|_IOS_EOFBIT; return _First; // unexpected EOF
+				}
+			}
+
+			if(_Char == _EQUAL_CHAR){
+				// Error! First character in octet can't be '='
+				_St |= _IOS_FAILBIT; 
+				return _First; 
+			}
+			else
+				_3to4.b64_0(_Char);
+
+
+			// -- 1 --
+			// Search next valid char... 
+			while(++_First != _Last)
+				if((_Char = _getCharType(*_First)) != _UNKNOWN_CHAR)
+					break;
+
+			if(_First == _Last)	{
+				_St |= _IOS_FAILBIT|_IOS_EOFBIT; // unexpected EOF 
+				return _First;
+			}
+
+			if(_Char == _EQUAL_CHAR){
+				// Error! Second character in octet can't be '='
+				_St |= _IOS_FAILBIT; 
+				return _First; 
+			}
+			else
+				_3to4.b64_1(_Char);
+
+
+			// -- 2 --
+			// Search next valid char... 
+			while(++_First != _Last)
+				if((_Char = _getCharType(*_First)) != _UNKNOWN_CHAR)
+					break;
+
+			if(_First == _Last)	{
+				// Error! Unexpected EOF. Must be '=' or base64 character
+				_St |= _IOS_FAILBIT|_IOS_EOFBIT; 
+				return _First; 
+			}
+
+			if(_Char == _EQUAL_CHAR){
+				// OK!
+				_3to4.b64_2(0); 
+				_3to4.b64_3(0); 
+
+				// chek for EOF
+				if(++_First == _Last)
+				{
+					// Error! Unexpected EOF. Must be '='. Ignore it.
+					//_St |= _IOS_BADBIT|_IOS_EOFBIT;
+					_St |= _IOS_EOFBIT;
+				}
+				else 
+					if(_getCharType(*_First) != _EQUAL_CHAR)
+					{
+						// Error! Must be '='. Ignore it.
+						//_St |= _IOS_BADBIT;
+					}
+				else
+					++_First; // Skip '='
+
+				// write 1 byte to output
+				*_To = (byte_t) _3to4.get_0();
+				return _First;
+			}
+			else
+				_3to4.b64_2(_Char);
+
+
+			// -- 3 --
+			// Search next valid char... 
+			while(++_First != _Last)
+				if((_Char = _getCharType(*_First)) != _UNKNOWN_CHAR)
+					break;
+
+			if(_First == _Last)	{
+				// Unexpected EOF. It's error. But ignore it.
+				//_St |= _IOS_FAILBIT|_IOS_EOFBIT; 
+					_St |= _IOS_EOFBIT; 
+				
+				return _First; 
+			}
+
+			if(_Char == _EQUAL_CHAR)
+			{
+				// OK!
+				_3to4.b64_3(0); 
+
+				// write to output 2 bytes
+				*_To = (byte_t) _3to4.get_0();
+				*_To = (byte_t) _3to4.get_1();
+
+				++_First; // set position to next character
+
+				return _First;
+			}
+			else
+				_3to4.b64_3(_Char);
+
+
+			// write to output 3 bytes
+			*_To = (byte_t) _3to4.get_0();
+			*_To = (byte_t) _3to4.get_1();
+			*_To = (byte_t) _3to4.get_2();
+
+			++_First;
+			
+
+		} // while(_First != _Last)
+
+		return (_First);
+	}
+
+protected:
+	
+	int _getCharType(int _Ch) const
+	{
+		if(_base64Chars[62] == _Ch)
+			return 62;
+
+		if(_base64Chars[63] == _Ch)
+			return 63;
+
+		if((_base64Chars[0] <= _Ch) && (_base64Chars[25] >= _Ch))
+			return _Ch - _base64Chars[0];
+
+		if((_base64Chars[26] <= _Ch) && (_base64Chars[51] >= _Ch))
+			return _Ch - _base64Chars[26] + 26;
+
+		if((_base64Chars[52] <= _Ch) && (_base64Chars[61] >= _Ch))
+			return _Ch - _base64Chars[52] + 52;
+
+		if(_Ch == _Tr::to_int_type('='))
+			return _EQUAL_CHAR;
+
+		return _UNKNOWN_CHAR;
+	}
+
+
+};
+
+
+#endif

thirdparty/ros/ros_comm/utilities/xmlrpcpp/package.xml

@@ -0,0 +1,26 @@
+<package format="2">
+  <name>xmlrpcpp</name>
+  <version>1.12.14</version>
+  <description>
+    XmlRpc++ is a C++ implementation of the XML-RPC protocol. This version is
+    heavily modified from the package available on SourceForge in order to
+    support roscpp's threading model. As such, we are maintaining our
+    own fork.
+  </description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>LGPL-2.1</license>
+
+  <url>http://xmlrpcpp.sourceforge.net</url>
+  <author>Chris Morley</author>
+  <author>Konstantin Pilipchuk</author>
+  <author>Morgan Quigley</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>cpp_common</depend>
+  <depend version_gte="0.6.9">rostime</depend>
+
+  <export>
+    <rosdoc external="http://xmlrpcpp.sourceforge.net/doc/hierarchy.html"/>
+  </export>
+</package>

thirdparty/ros/ros_comm/utilities/xmlrpcpp/src/XmlRpcClient.cpp

@@ -0,0 +1,472 @@
+
+#include "xmlrpcpp/XmlRpcClient.h"
+
+#include "xmlrpcpp/XmlRpcSocket.h"
+#include "xmlrpcpp/XmlRpc.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#ifndef _WINDOWS
+	# include <strings.h>
+#endif
+#include <string.h>
+
+
+using namespace XmlRpc;
+
+// Static data
+const char XmlRpcClient::REQUEST_BEGIN[] = 
+  "<?xml version=\"1.0\"?>\r\n"
+  "<methodCall><methodName>";
+const char XmlRpcClient::REQUEST_END_METHODNAME[] = "</methodName>\r\n";
+const char XmlRpcClient::PARAMS_TAG[] = "<params>";
+const char XmlRpcClient::PARAMS_ETAG[] = "</params>";
+const char XmlRpcClient::PARAM_TAG[] = "<param>";
+const char XmlRpcClient::PARAM_ETAG[] =  "</param>";
+const char XmlRpcClient::REQUEST_END[] = "</methodCall>\r\n";
+const char XmlRpcClient::METHODRESPONSE_TAG[] = "<methodResponse>";
+const char XmlRpcClient::FAULT_TAG[] = "<fault>";
+
+
+
+XmlRpcClient::XmlRpcClient(const char* host, int port, const char* uri/*=0*/)
+{
+  XmlRpcUtil::log(1, "XmlRpcClient new client: host %s, port %d.", host, port);
+
+  _host = host;
+  _port = port;
+  if (uri)
+    _uri = uri;
+  else
+    _uri = "/RPC2";
+  _connectionState = NO_CONNECTION;
+  _executing = false;
+  _eof = false;
+
+  // Default to keeping the connection open until an explicit close is done
+  setKeepOpen();
+}
+
+
+XmlRpcClient::~XmlRpcClient()
+{
+  this->close();
+}
+
+// Close the owned fd
+void
+XmlRpcClient::close()
+{
+  XmlRpcUtil::log(4, "XmlRpcClient::close: fd %d.", getfd());
+  _connectionState = NO_CONNECTION;
+  _disp.exit();
+  _disp.removeSource(this);
+  XmlRpcSource::close();
+}
+
+
+// Clear the referenced flag even if exceptions or errors occur.
+struct ClearFlagOnExit {
+  ClearFlagOnExit(bool& flag) : _flag(flag) {}
+  ~ClearFlagOnExit() { _flag = false; }
+  bool& _flag;
+};
+
+// Execute the named procedure on the remote server.
+// Params should be an array of the arguments for the method.
+// Returns true if the request was sent and a result received (although the result
+// might be a fault).
+bool
+XmlRpcClient::execute(const char* method, XmlRpcValue const& params, XmlRpcValue& result)
+{
+  XmlRpcUtil::log(1, "XmlRpcClient::execute: method %s (_connectionState %d).", method, _connectionState);
+
+  // This is not a thread-safe operation, if you want to do multithreading, use separate
+  // clients for each thread. If you want to protect yourself from multiple threads
+  // accessing the same client, replace this code with a real mutex.
+  if (_executing)
+    return false;
+
+  _executing = true;
+  ClearFlagOnExit cf(_executing);
+
+  _sendAttempts = 0;
+  _isFault = false;
+
+  if ( ! setupConnection())
+    return false;
+
+  if ( ! generateRequest(method, params))
+    return false;
+
+  result.clear();
+  double msTime = -1.0;   // Process until exit is called
+  _disp.work(msTime);
+
+  if (_connectionState != IDLE || ! parseResponse(result))
+    return false;
+
+  XmlRpcUtil::log(1, "XmlRpcClient::execute: method %s completed.", method);
+  _response = "";
+  return true;
+}
+
+// Execute the named procedure on the remote server, non-blocking.
+// Params should be an array of the arguments for the method.
+// Returns true if the request was sent and a result received (although the result
+// might be a fault).
+bool
+XmlRpcClient::executeNonBlock(const char* method, XmlRpcValue const& params)
+{
+  XmlRpcUtil::log(1, "XmlRpcClient::execute: method %s (_connectionState %d).", method, _connectionState);
+
+  // This is not a thread-safe operation, if you want to do multithreading, use separate
+  // clients for each thread. If you want to protect yourself from multiple threads
+  // accessing the same client, replace this code with a real mutex.
+  if (_executing)
+    return false;
+
+  _executing = true;
+  ClearFlagOnExit cf(_executing);
+
+  _sendAttempts = 0;
+  _isFault = false;
+
+  if ( ! setupConnection())
+    return false;
+
+  if ( ! generateRequest(method, params))
+    return false;
+
+  return true;
+}
+
+bool
+XmlRpcClient::executeCheckDone(XmlRpcValue& result)
+{
+  result.clear();
+  // Are we done yet?
+  if (_connectionState != IDLE)
+    return false;
+  if (! parseResponse(result))
+  {
+    // Hopefully the caller can determine that parsing failed.
+  }
+  //XmlRpcUtil::log(1, "XmlRpcClient::execute: method %s completed.", method);
+  _response = "";
+  return true;
+}
+
+// XmlRpcSource interface implementation
+// Handle server responses. Called by the event dispatcher during execute.
+unsigned
+XmlRpcClient::handleEvent(unsigned eventType)
+{
+  if (eventType == XmlRpcDispatch::Exception)
+  {
+    if (_connectionState == WRITE_REQUEST && _bytesWritten == 0)
+      XmlRpcUtil::error("Error in XmlRpcClient::handleEvent: could not connect to server (%s).", 
+                       XmlRpcSocket::getErrorMsg().c_str());
+    else
+      XmlRpcUtil::error("Error in XmlRpcClient::handleEvent (state %d): %s.", 
+                        _connectionState, XmlRpcSocket::getErrorMsg().c_str());
+    return 0;
+  }
+
+  if (_connectionState == WRITE_REQUEST)
+    if ( ! writeRequest()) return 0;
+
+  if (_connectionState == READ_HEADER)
+    if ( ! readHeader()) return 0;
+
+  if (_connectionState == READ_RESPONSE)
+    if ( ! readResponse()) return 0;
+
+  // This should probably always ask for Exception events too
+  return (_connectionState == WRITE_REQUEST) 
+        ? XmlRpcDispatch::WritableEvent : XmlRpcDispatch::ReadableEvent;
+}
+
+
+// Create the socket connection to the server if necessary
+bool
+XmlRpcClient::setupConnection()
+{
+  // If an error occurred last time through, or if the server closed the connection, close our end
+  if ((_connectionState != NO_CONNECTION && _connectionState != IDLE) || _eof)
+    close();
+
+  _eof = false;
+  if (_connectionState == NO_CONNECTION)
+    if (! doConnect()) 
+      return false;
+
+  // Prepare to write the request
+  _connectionState = WRITE_REQUEST;
+  _bytesWritten = 0;
+
+  // Notify the dispatcher to listen on this source (calls handleEvent when the socket is writable)
+  _disp.removeSource(this);       // Make sure nothing is left over
+  _disp.addSource(this, XmlRpcDispatch::WritableEvent | XmlRpcDispatch::Exception);
+
+  return true;
+}
+
+
+// Connect to the xmlrpc server
+bool
+XmlRpcClient::doConnect()
+{
+  int fd = XmlRpcSocket::socket();
+  if (fd < 0)
+  {
+    XmlRpcUtil::error("Error in XmlRpcClient::doConnect: Could not create socket (%s).", XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  XmlRpcUtil::log(3, "XmlRpcClient::doConnect: fd %d.", fd);
+  this->setfd(fd);
+
+  // Don't block on connect/reads/writes
+  if ( ! XmlRpcSocket::setNonBlocking(fd))
+  {
+    this->close();
+    XmlRpcUtil::error("Error in XmlRpcClient::doConnect: Could not set socket to non-blocking IO mode (%s).", XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  if ( ! XmlRpcSocket::connect(fd, _host, _port))
+  {
+    this->close();
+    XmlRpcUtil::error("Error in XmlRpcClient::doConnect: Could not connect to server (%s).", XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  return true;
+}
+
+// Encode the request to call the specified method with the specified parameters into xml
+bool
+XmlRpcClient::generateRequest(const char* methodName, XmlRpcValue const& params)
+{
+  std::string body = REQUEST_BEGIN;
+  body += methodName;
+  body += REQUEST_END_METHODNAME;
+
+  // If params is an array, each element is a separate parameter
+  if (params.valid()) {
+    body += PARAMS_TAG;
+    if (params.getType() == XmlRpcValue::TypeArray)
+    {
+      for (int i=0; i<params.size(); ++i) {
+        body += PARAM_TAG;
+        body += params[i].toXml();
+        body += PARAM_ETAG;
+      }
+    }
+    else
+    {
+      body += PARAM_TAG;
+      body += params.toXml();
+      body += PARAM_ETAG;
+    }
+      
+    body += PARAMS_ETAG;
+  }
+  body += REQUEST_END;
+
+  std::string header = generateHeader(body);
+  XmlRpcUtil::log(4, "XmlRpcClient::generateRequest: header is %d bytes, content-length is %d.", 
+                  header.length(), body.length());
+
+  _request = header + body;
+  return true;
+}
+
+// Prepend http headers
+std::string
+XmlRpcClient::generateHeader(std::string const& body)
+{
+  std::string header = 
+    "POST " + _uri + " HTTP/1.1\r\n"
+    "User-Agent: ";
+  header += XMLRPC_VERSION;
+  header += "\r\nHost: ";
+  header += _host;
+
+  char buff[40];
+#ifdef _MSC_VER
+  sprintf_s(buff,40,":%d\r\n", _port);
+#else
+  sprintf(buff,":%d\r\n", _port);
+#endif
+
+  header += buff;
+  header += "Content-Type: text/xml\r\nContent-length: ";
+
+#ifdef _MSC_VER
+  sprintf_s(buff,40,"%d\r\n\r\n", (int)body.size());
+#else
+  sprintf(buff,"%d\r\n\r\n", (int)body.size());
+#endif
+
+  return header + buff;
+}
+
+bool
+XmlRpcClient::writeRequest()
+{
+  if (_bytesWritten == 0)
+    XmlRpcUtil::log(5, "XmlRpcClient::writeRequest (attempt %d):\n%s\n", _sendAttempts+1, _request.c_str());
+
+  // Try to write the request
+  if ( ! XmlRpcSocket::nbWrite(this->getfd(), _request, &_bytesWritten)) {
+    XmlRpcUtil::error("Error in XmlRpcClient::writeRequest: write error (%s).",XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+    
+  XmlRpcUtil::log(3, "XmlRpcClient::writeRequest: wrote %d of %d bytes.", _bytesWritten, _request.length());
+
+  // Wait for the result
+  if (_bytesWritten == int(_request.length())) {
+    _header = "";
+    _response = "";
+    _connectionState = READ_HEADER;
+  }
+  return true;
+}
+
+
+// Read the header from the response
+bool
+XmlRpcClient::readHeader()
+{
+  // Read available data
+  if ( ! XmlRpcSocket::nbRead(this->getfd(), _header, &_eof) ||
+       (_eof && _header.length() == 0)) {
+
+    // If we haven't read any data yet and this is a keep-alive connection, the server may
+    // have timed out, so we try one more time.
+    if (getKeepOpen() && _header.length() == 0 && _sendAttempts++ == 0) {
+      XmlRpcUtil::log(4, "XmlRpcClient::readHeader: re-trying connection");
+      XmlRpcSource::close();
+      _connectionState = NO_CONNECTION;
+      _eof = false;
+      return setupConnection();
+    }
+
+    XmlRpcUtil::error("Error in XmlRpcClient::readHeader: error while reading header (%s) on fd %d.",
+                      XmlRpcSocket::getErrorMsg().c_str(), getfd());
+    return false;
+  }
+
+  XmlRpcUtil::log(4, "XmlRpcClient::readHeader: client has read %d bytes", _header.length());
+
+  char *hp = (char*)_header.c_str();  // Start of header
+  char *ep = hp + _header.length();   // End of string
+  char *bp = 0;                       // Start of body
+  char *lp = 0;                       // Start of content-length value
+
+  for (char *cp = hp; (bp == 0) && (cp < ep); ++cp) {
+    if ((ep - cp > 16) && (strncasecmp(cp, "Content-length: ", 16) == 0))
+      lp = cp + 16;
+    else if ((ep - cp > 4) && (strncmp(cp, "\r\n\r\n", 4) == 0))
+      bp = cp + 4;
+    else if ((ep - cp > 2) && (strncmp(cp, "\n\n", 2) == 0))
+      bp = cp + 2;
+  }
+
+  // If we haven't gotten the entire header yet, return (keep reading)
+  if (bp == 0) {
+    if (_eof)          // EOF in the middle of a response is an error
+    {
+      XmlRpcUtil::error("Error in XmlRpcClient::readHeader: EOF while reading header");
+      return false;   // Close the connection
+    }
+    
+    return true;  // Keep reading
+  }
+
+  // Decode content length
+  if (lp == 0) {
+    XmlRpcUtil::error("Error XmlRpcClient::readHeader: No Content-length specified");
+    return false;   // We could try to figure it out by parsing as we read, but for now...
+  }
+
+  _contentLength = atoi(lp);
+  if (_contentLength <= 0) {
+    XmlRpcUtil::error("Error in XmlRpcClient::readHeader: Invalid Content-length specified (%d).", _contentLength);
+    return false;
+  }
+  	
+  XmlRpcUtil::log(4, "client read content length: %d", _contentLength);
+
+  // Otherwise copy non-header data to response buffer and set state to read response.
+  _response = bp;
+  _header = "";   // should parse out any interesting bits from the header (connection, etc)...
+  _connectionState = READ_RESPONSE;
+  return true;    // Continue monitoring this source
+}
+
+    
+bool
+XmlRpcClient::readResponse()
+{
+  // If we dont have the entire response yet, read available data
+  if (int(_response.length()) < _contentLength) {
+    if ( ! XmlRpcSocket::nbRead(this->getfd(), _response, &_eof)) {
+      XmlRpcUtil::error("Error in XmlRpcClient::readResponse: read error (%s).",XmlRpcSocket::getErrorMsg().c_str());
+      return false;
+    }
+
+    // If we haven't gotten the entire _response yet, return (keep reading)
+    if (int(_response.length()) < _contentLength) {
+      if (_eof) {
+        XmlRpcUtil::error("Error in XmlRpcClient::readResponse: EOF while reading response");
+        return false;
+      }
+      return true;
+    }
+  }
+
+  // Otherwise, parse and return the result
+  XmlRpcUtil::log(3, "XmlRpcClient::readResponse (read %d bytes)", _response.length());
+  XmlRpcUtil::log(5, "response:\n%s", _response.c_str());
+
+  _connectionState = IDLE;
+
+  return false;    // Stop monitoring this source (causes return from work)
+}
+
+
+// Convert the response xml into a result value
+bool
+XmlRpcClient::parseResponse(XmlRpcValue& result)
+{
+  // Parse response xml into result
+  int offset = 0;
+  if ( ! XmlRpcUtil::findTag(METHODRESPONSE_TAG,_response,&offset)) {
+    XmlRpcUtil::error("Error in XmlRpcClient::parseResponse: Invalid response - no methodResponse. Response:\n%s", _response.c_str());
+    return false;
+  }
+
+  // Expect either <params><param>... or <fault>...
+  if ((XmlRpcUtil::nextTagIs(PARAMS_TAG,_response,&offset) &&
+       XmlRpcUtil::nextTagIs(PARAM_TAG,_response,&offset)) ||
+      (XmlRpcUtil::nextTagIs(FAULT_TAG,_response,&offset) && (_isFault = true)))
+  {
+    if ( ! result.fromXml(_response, &offset)) {
+      XmlRpcUtil::error("Error in XmlRpcClient::parseResponse: Invalid response value. Response:\n%s", _response.c_str());
+      _response = "";
+      return false;
+    }
+  } else {
+    XmlRpcUtil::error("Error in XmlRpcClient::parseResponse: Invalid response - no param or fault tag. Response:\n%s", _response.c_str());
+    _response = "";
+    return false;
+  }
+      
+  _response = "";
+  return result.valid();
+}
+

thirdparty/ros/ros_comm/utilities/xmlrpcpp/src/XmlRpcDispatch.cpp

@@ -0,0 +1,231 @@
+
+#include "xmlrpcpp/XmlRpcDispatch.h"
+#include "xmlrpcpp/XmlRpcSource.h"
+#include "xmlrpcpp/XmlRpcUtil.h"
+
+#include "ros/time.h"
+
+#include <math.h>
+#include <errno.h>
+#include <sys/timeb.h>
+#include <sys/poll.h>
+
+#if defined(_WINDOWS)
+# include <winsock2.h>
+static inline int poll( struct pollfd *pfd, int nfds, int timeout)
+{
+  return WSAPoll(pfd, nfds, timeout);
+}
+
+# define USE_FTIME
+# if defined(_MSC_VER)
+#  define timeb _timeb
+#  define ftime _ftime_s
+# endif
+#else
+# include <sys/time.h>
+#endif  // _WINDOWS
+
+
+using namespace XmlRpc;
+
+
+XmlRpcDispatch::XmlRpcDispatch()
+{
+  _endTime = -1.0;
+  _doClear = false;
+  _inWork = false;
+}
+
+
+XmlRpcDispatch::~XmlRpcDispatch()
+{
+}
+
+// Monitor this source for the specified events and call its event handler
+// when the event occurs
+void
+XmlRpcDispatch::addSource(XmlRpcSource* source, unsigned mask)
+{
+  _sources.push_back(MonitoredSource(source, mask));
+}
+
+// Stop monitoring this source. Does not close the source.
+void
+XmlRpcDispatch::removeSource(XmlRpcSource* source)
+{
+  for (SourceList::iterator it=_sources.begin(); it!=_sources.end(); ++it)
+    if (it->getSource() == source)
+    {
+      _sources.erase(it);
+      break;
+    }
+}
+
+
+// Modify the types of events to watch for on this source
+void
+XmlRpcDispatch::setSourceEvents(XmlRpcSource* source, unsigned eventMask)
+{
+  for (SourceList::iterator it=_sources.begin(); it!=_sources.end(); ++it)
+    if (it->getSource() == source)
+    {
+      it->getMask() = eventMask;
+      break;
+    }
+}
+
+
+
+// Watch current set of sources and process events
+void
+XmlRpcDispatch::work(double timeout)
+{
+  // Loosely based on `man select` > Correspondence between select() and poll() notifications
+  // and cloudius-systems/osv#35, cloudius-systems/osv@b53d39a using poll to emulate select
+  const unsigned POLLIN_REQ = POLLIN; // Request read
+  const unsigned POLLIN_CHK = (POLLIN | POLLHUP | POLLERR); // Readable or connection lost
+  const unsigned POLLOUT_REQ = POLLOUT; // Request write
+  const unsigned POLLOUT_CHK = (POLLOUT | POLLERR); // Writable or connection lost
+  const unsigned POLLEX_CHK = (POLLPRI | POLLNVAL); // Exception or invalid fd
+
+  // Compute end time
+  _endTime = (timeout < 0.0) ? -1.0 : (getTime() + timeout);
+  _doClear = false;
+  _inWork = true;
+  int timeout_ms = static_cast<int>(floor(timeout * 1000.));
+
+  // Only work while there is something to monitor
+  while (_sources.size() > 0) {
+
+    // Construct the sets of descriptors we are interested in
+    const unsigned source_cnt = _sources.size();
+    pollfd fds[source_cnt];
+    XmlRpcSource * sources[source_cnt];
+
+    SourceList::iterator it;
+    std::size_t i = 0;
+    for (it=_sources.begin(); it!=_sources.end(); ++it, ++i) {
+      sources[i] = it->getSource();
+      fds[i].fd = sources[i]->getfd();
+      fds[i].revents = 0; // some platforms may not clear this in poll()
+      fds[i].events = 0;
+      if (it->getMask() & ReadableEvent) fds[i].events |= POLLIN_REQ;
+      if (it->getMask() & WritableEvent) fds[i].events |= POLLOUT_REQ;
+    }
+
+    // Check for events
+    int nEvents = poll(fds, source_cnt, (timeout_ms < 0) ? -1 : timeout_ms);
+
+    if (nEvents < 0)
+    {
+      if(errno != EINTR)
+        XmlRpcUtil::error("Error in XmlRpcDispatch::work: error in poll (%d).", nEvents);
+      _inWork = false;
+      return;
+    }
+
+    // Process events
+    for (i=0; i < source_cnt; ++i)
+    {
+      XmlRpcSource* src = sources[i];
+      pollfd & pfd = fds[i];
+      unsigned newMask = (unsigned) -1;
+      // Only handle requested events to avoid being prematurely removed from dispatch
+      bool readable = (pfd.events & POLLIN_REQ) == POLLIN_REQ;
+      bool writable = (pfd.events & POLLOUT_REQ) == POLLOUT_REQ;
+      if (readable && (pfd.revents & POLLIN_CHK))
+        newMask &= src->handleEvent(ReadableEvent);
+      if (writable && (pfd.revents & POLLOUT_CHK))
+        newMask &= src->handleEvent(WritableEvent);
+      if (pfd.revents & POLLEX_CHK)
+        newMask &= src->handleEvent(Exception);
+
+      // Find the source iterator. It may have moved as a result of the way
+      // that sources are removed and added in the call stack starting
+      // from the handleEvent() calls above.
+      SourceList::iterator thisIt;
+      for (thisIt = _sources.begin(); thisIt != _sources.end(); thisIt++)
+      {
+        if(thisIt->getSource() == src)
+          break;
+      }
+      if(thisIt == _sources.end())
+      {
+        XmlRpcUtil::error("Error in XmlRpcDispatch::work: couldn't find source iterator");
+        continue;
+      }
+
+      if ( ! newMask) {
+        _sources.erase(thisIt);  // Stop monitoring this one
+        if ( ! src->getKeepOpen())
+          src->close();
+      } else if (newMask != (unsigned) -1) {
+        thisIt->getMask() = newMask;
+      }
+    }
+
+    // Check whether to clear all sources
+    if (_doClear)
+    {
+      SourceList closeList = _sources;
+      _sources.clear();
+      for (SourceList::iterator it=closeList.begin(); it!=closeList.end(); ++it) {
+	XmlRpcSource *src = it->getSource();
+        src->close();
+      }
+
+      _doClear = false;
+    }
+
+    // Check whether end time has passed
+    if (0 <= _endTime && getTime() > _endTime)
+      break;
+  }
+
+  _inWork = false;
+}
+
+
+// Exit from work routine. Presumably this will be called from
+// one of the source event handlers.
+void
+XmlRpcDispatch::exit()
+{
+  _endTime = 0.0;   // Return from work asap
+}
+
+// Clear all sources from the monitored sources list
+void
+XmlRpcDispatch::clear()
+{
+  if (_inWork)
+    _doClear = true;  // Finish reporting current events before clearing
+  else
+  {
+    SourceList closeList = _sources;
+    _sources.clear();
+    for (SourceList::iterator it=closeList.begin(); it!=closeList.end(); ++it)
+      it->getSource()->close();
+  }
+}
+
+
+double
+XmlRpcDispatch::getTime()
+{
+#ifdef USE_FTIME
+  struct timeb	tbuff;
+
+  ftime(&tbuff);
+  return ((double) tbuff.time + ((double)tbuff.millitm / 1000.0) +
+	  ((double) tbuff.timezone * 60));
+#else
+  uint32_t sec, nsec;
+
+  ros::ros_steadytime(sec, nsec);
+  return ((double)sec + (double)nsec / 1e9);
+#endif /* USE_FTIME */
+}
+
+

thirdparty/ros/ros_comm/utilities/xmlrpcpp/src/XmlRpcServer.cpp

@@ -0,0 +1,290 @@
+// this file modified by Morgan Quigley on 22 Apr 2008.
+// added features: server can be opened on port 0 and you can read back
+// what port the OS gave you
+
+#include "xmlrpcpp/XmlRpcServer.h"
+#include "xmlrpcpp/XmlRpcServerConnection.h"
+#include "xmlrpcpp/XmlRpcServerMethod.h"
+#include "xmlrpcpp/XmlRpcSocket.h"
+#include "xmlrpcpp/XmlRpcUtil.h"
+#include "xmlrpcpp/XmlRpcException.h"
+
+
+using namespace XmlRpc;
+
+
+XmlRpcServer::XmlRpcServer()
+{
+  _introspectionEnabled = false;
+  _listMethods = 0;
+  _methodHelp = 0;
+  _port = 0;
+}
+
+
+XmlRpcServer::~XmlRpcServer()
+{
+  this->shutdown();
+  _methods.clear();
+  delete _listMethods;
+  delete _methodHelp;
+}
+
+
+// Add a command to the RPC server
+void 
+XmlRpcServer::addMethod(XmlRpcServerMethod* method)
+{
+  _methods[method->name()] = method;
+}
+
+// Remove a command from the RPC server
+void 
+XmlRpcServer::removeMethod(XmlRpcServerMethod* method)
+{
+  MethodMap::iterator i = _methods.find(method->name());
+  if (i != _methods.end())
+    _methods.erase(i);
+}
+
+// Remove a command from the RPC server by name
+void 
+XmlRpcServer::removeMethod(const std::string& methodName)
+{
+  MethodMap::iterator i = _methods.find(methodName);
+  if (i != _methods.end())
+    _methods.erase(i);
+}
+
+
+// Look up a method by name
+XmlRpcServerMethod* 
+XmlRpcServer::findMethod(const std::string& name) const
+{
+  MethodMap::const_iterator i = _methods.find(name);
+  if (i == _methods.end())
+    return 0;
+  return i->second;
+}
+
+
+// Create a socket, bind to the specified port, and
+// set it in listen mode to make it available for clients.
+bool 
+XmlRpcServer::bindAndListen(int port, int backlog /*= 5*/)
+{
+  int fd = XmlRpcSocket::socket();
+  if (fd < 0)
+  {
+    XmlRpcUtil::error("XmlRpcServer::bindAndListen: Could not create socket (%s).", XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  this->setfd(fd);
+
+  // Don't block on reads/writes
+  if ( ! XmlRpcSocket::setNonBlocking(fd))
+  {
+    this->close();
+    XmlRpcUtil::error("XmlRpcServer::bindAndListen: Could not set socket to non-blocking input mode (%s).", XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  // Allow this port to be re-bound immediately so server re-starts are not delayed
+  if ( ! XmlRpcSocket::setReuseAddr(fd))
+  {
+    this->close();
+    XmlRpcUtil::error("XmlRpcServer::bindAndListen: Could not set SO_REUSEADDR socket option (%s).", XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  // Bind to the specified port on the default interface
+  if ( ! XmlRpcSocket::bind(fd, port))
+  {
+    this->close();
+    XmlRpcUtil::error("XmlRpcServer::bindAndListen: Could not bind to specified port (%s).", XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  // Set in listening mode
+  if ( ! XmlRpcSocket::listen(fd, backlog))
+  {
+    this->close();
+    XmlRpcUtil::error("XmlRpcServer::bindAndListen: Could not set socket in listening mode (%s).", XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  _port = XmlRpcSocket::get_port(fd);
+
+  XmlRpcUtil::log(2, "XmlRpcServer::bindAndListen: server listening on port %d fd %d", _port, fd);
+
+  // Notify the dispatcher to listen on this source when we are in work()
+  _disp.addSource(this, XmlRpcDispatch::ReadableEvent);
+
+  return true;
+}
+
+
+// Process client requests for the specified time
+void 
+XmlRpcServer::work(double msTime)
+{
+  XmlRpcUtil::log(2, "XmlRpcServer::work: waiting for a connection");
+  _disp.work(msTime);
+}
+
+
+
+// Handle input on the server socket by accepting the connection
+// and reading the rpc request.
+unsigned
+XmlRpcServer::handleEvent(unsigned)
+{
+  acceptConnection();
+  return XmlRpcDispatch::ReadableEvent;		// Continue to monitor this fd
+}
+
+
+// Accept a client connection request and create a connection to
+// handle method calls from the client.
+void
+XmlRpcServer::acceptConnection()
+{
+  int s = XmlRpcSocket::accept(this->getfd());
+  XmlRpcUtil::log(2, "XmlRpcServer::acceptConnection: socket %d", s);
+  if (s < 0)
+  {
+    //this->close();
+    XmlRpcUtil::error("XmlRpcServer::acceptConnection: Could not accept connection (%s).", XmlRpcSocket::getErrorMsg().c_str());
+  }
+  else if ( ! XmlRpcSocket::setNonBlocking(s))
+  {
+    XmlRpcSocket::close(s);
+    XmlRpcUtil::error("XmlRpcServer::acceptConnection: Could not set socket to non-blocking input mode (%s).", XmlRpcSocket::getErrorMsg().c_str());
+  }
+  else  // Notify the dispatcher to listen for input on this source when we are in work()
+  {
+    XmlRpcUtil::log(2, "XmlRpcServer::acceptConnection: creating a connection");
+    _disp.addSource(this->createConnection(s), XmlRpcDispatch::ReadableEvent);
+  }
+}
+
+
+// Create a new connection object for processing requests from a specific client.
+XmlRpcServerConnection*
+XmlRpcServer::createConnection(int s)
+{
+  // Specify that the connection object be deleted when it is closed
+  return new XmlRpcServerConnection(s, this, true);
+}
+
+
+void 
+XmlRpcServer::removeConnection(XmlRpcServerConnection* sc)
+{
+  _disp.removeSource(sc);
+}
+
+
+// Stop processing client requests
+void 
+XmlRpcServer::exit()
+{
+  _disp.exit();
+}
+
+
+// Close the server socket file descriptor and stop monitoring connections
+void 
+XmlRpcServer::shutdown()
+{
+  // This closes and destroys all connections as well as closing this socket
+  _disp.clear();
+}
+
+
+// Introspection support
+static const std::string LIST_METHODS("system.listMethods");
+static const std::string METHOD_HELP("system.methodHelp");
+static const std::string MULTICALL("system.multicall");
+
+
+// List all methods available on a server
+class ListMethods : public XmlRpcServerMethod
+{
+public:
+  ListMethods(XmlRpcServer* s) : XmlRpcServerMethod(LIST_METHODS, s) {}
+
+  void execute(XmlRpcValue&, XmlRpcValue& result)
+  {
+    _server->listMethods(result);
+  }
+
+  std::string help() { return std::string("List all methods available on a server as an array of strings"); }
+};
+
+
+// Retrieve the help string for a named method
+class MethodHelp : public XmlRpcServerMethod
+{
+public:
+  MethodHelp(XmlRpcServer* s) : XmlRpcServerMethod(METHOD_HELP, s) {}
+
+  void execute(XmlRpcValue& params, XmlRpcValue& result)
+  {
+    if (params[0].getType() != XmlRpcValue::TypeString)
+      throw XmlRpcException(METHOD_HELP + ": Invalid argument type");
+
+    XmlRpcServerMethod* m = _server->findMethod(params[0]);
+    if ( ! m)
+      throw XmlRpcException(METHOD_HELP + ": Unknown method name");
+
+    result = m->help();
+  }
+
+  std::string help() { return std::string("Retrieve the help string for a named method"); }
+};
+
+    
+// Specify whether introspection is enabled or not. Default is enabled.
+void 
+XmlRpcServer::enableIntrospection(bool enabled)
+{
+  if (_introspectionEnabled == enabled)
+    return;
+
+  _introspectionEnabled = enabled;
+
+  if (enabled)
+  {
+    if ( ! _listMethods)
+    {
+      _listMethods = new ListMethods(this);
+      _methodHelp = new MethodHelp(this);
+    } else {
+      addMethod(_listMethods);
+      addMethod(_methodHelp);
+    }
+  }
+  else
+  {
+    removeMethod(LIST_METHODS);
+    removeMethod(METHOD_HELP);
+  }
+}
+
+
+void
+XmlRpcServer::listMethods(XmlRpcValue& result)
+{
+  int i = 0;
+  result.setSize(_methods.size()+1);
+  for (MethodMap::iterator it=_methods.begin(); it != _methods.end(); ++it)
+    result[i++] = it->first;
+
+  // Multicall support is built into XmlRpcServerConnection
+  result[i] = MULTICALL;
+}
+
+
+

thirdparty/ros/ros_comm/utilities/xmlrpcpp/src/XmlRpcServerConnection.cpp

@@ -0,0 +1,381 @@
+
+#include "xmlrpcpp/XmlRpcServerConnection.h"
+
+#include "xmlrpcpp/XmlRpcSocket.h"
+#include "xmlrpcpp/XmlRpc.h"
+#ifndef MAKEDEPEND
+# include <stdio.h>
+# include <stdlib.h>
+#ifndef _WINDOWS
+	# include <strings.h>
+#endif
+# include <string.h>
+#endif
+
+using namespace XmlRpc;
+
+// Static data
+const char XmlRpcServerConnection::METHODNAME_TAG[] = "<methodName>";
+const char XmlRpcServerConnection::PARAMS_TAG[] = "<params>";
+const char XmlRpcServerConnection::PARAMS_ETAG[] = "</params>";
+const char XmlRpcServerConnection::PARAM_TAG[] = "<param>";
+const char XmlRpcServerConnection::PARAM_ETAG[] = "</param>";
+
+const std::string XmlRpcServerConnection::SYSTEM_MULTICALL = "system.multicall";
+const std::string XmlRpcServerConnection::METHODNAME = "methodName";
+const std::string XmlRpcServerConnection::PARAMS = "params";
+
+const std::string XmlRpcServerConnection::FAULTCODE = "faultCode";
+const std::string XmlRpcServerConnection::FAULTSTRING = "faultString";
+
+
+
+// The server delegates handling client requests to a serverConnection object.
+XmlRpcServerConnection::XmlRpcServerConnection(int fd, XmlRpcServer* server, bool deleteOnClose /*= false*/) :
+  XmlRpcSource(fd, deleteOnClose)
+{
+  XmlRpcUtil::log(2,"XmlRpcServerConnection: new socket %d.", fd);
+  _server = server;
+  _connectionState = READ_HEADER;
+  _contentLength = 0;
+  _bytesWritten = 0;
+  _keepAlive = true;
+}
+
+
+XmlRpcServerConnection::~XmlRpcServerConnection()
+{
+  XmlRpcUtil::log(4,"XmlRpcServerConnection dtor.");
+  _server->removeConnection(this);
+}
+
+
+// Handle input on the server socket by accepting the connection
+// and reading the rpc request. Return true to continue to monitor
+// the socket for events, false to remove it from the dispatcher.
+unsigned
+XmlRpcServerConnection::handleEvent(unsigned /*eventType*/)
+{
+  if (_connectionState == READ_HEADER)
+    if ( ! readHeader()) return 0;
+
+  if (_connectionState == READ_REQUEST)
+    if ( ! readRequest()) return 0;
+
+  if (_connectionState == WRITE_RESPONSE)
+    if ( ! writeResponse()) return 0;
+
+  return (_connectionState == WRITE_RESPONSE) 
+        ? XmlRpcDispatch::WritableEvent : XmlRpcDispatch::ReadableEvent;
+}
+
+
+bool
+XmlRpcServerConnection::readHeader()
+{
+  // Read available data
+  bool eof;
+  if ( ! XmlRpcSocket::nbRead(this->getfd(), _header, &eof)) {
+    // Its only an error if we already have read some data
+    if (_header.length() > 0)
+      XmlRpcUtil::error("XmlRpcServerConnection::readHeader: error while reading header (%s).",XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+
+  XmlRpcUtil::log(4, "XmlRpcServerConnection::readHeader: read %d bytes.", _header.length());
+  char *hp = (char*)_header.c_str();  // Start of header
+  char *ep = hp + _header.length();   // End of string
+  char *bp = 0;                       // Start of body
+  char *lp = 0;                       // Start of content-length value
+  char *kp = 0;                       // Start of connection value
+
+  for (char *cp = hp; (bp == 0) && (cp < ep); ++cp) {
+	if ((ep - cp > 16) && (strncasecmp(cp, "Content-length: ", 16) == 0))
+	  lp = cp + 16;
+	else if ((ep - cp > 12) && (strncasecmp(cp, "Connection: ", 12) == 0))
+	  kp = cp + 12;
+	else if ((ep - cp > 4) && (strncmp(cp, "\r\n\r\n", 4) == 0))
+	  bp = cp + 4;
+	else if ((ep - cp > 2) && (strncmp(cp, "\n\n", 2) == 0))
+	  bp = cp + 2;
+  }
+
+  // If we haven't gotten the entire header yet, return (keep reading)
+  if (bp == 0) {
+    // EOF in the middle of a request is an error, otherwise its ok
+    if (eof) {
+      XmlRpcUtil::log(4, "XmlRpcServerConnection::readHeader: EOF");
+      if (_header.length() > 0)
+        XmlRpcUtil::error("XmlRpcServerConnection::readHeader: EOF while reading header");
+      return false;   // Either way we close the connection
+    }
+    
+    return true;  // Keep reading
+  }
+
+  // Decode content length
+  if (lp == 0) {
+    XmlRpcUtil::error("XmlRpcServerConnection::readHeader: No Content-length specified");
+    return false;   // We could try to figure it out by parsing as we read, but for now...
+  }
+
+  _contentLength = atoi(lp);
+  if (_contentLength <= 0) {
+    XmlRpcUtil::error("XmlRpcServerConnection::readHeader: Invalid Content-length specified (%d).", _contentLength);
+    return false;
+  }
+  	
+  XmlRpcUtil::log(3, "XmlRpcServerConnection::readHeader: specified content length is %d.", _contentLength);
+
+  // Otherwise copy non-header data to request buffer and set state to read request.
+  _request = bp;
+
+  // Parse out any interesting bits from the header (HTTP version, connection)
+  _keepAlive = true;
+  if (_header.find("HTTP/1.0") != std::string::npos) {
+    if (kp == 0 || strncasecmp(kp, "keep-alive", 10) != 0)
+      _keepAlive = false;           // Default for HTTP 1.0 is to close the connection
+  } else {
+    if (kp != 0 && strncasecmp(kp, "close", 5) == 0)
+      _keepAlive = false;
+  }
+  XmlRpcUtil::log(3, "KeepAlive: %d", _keepAlive);
+
+
+  _header = ""; 
+  _connectionState = READ_REQUEST;
+  return true;    // Continue monitoring this source
+}
+
+bool
+XmlRpcServerConnection::readRequest()
+{
+  // If we dont have the entire request yet, read available data
+  if (int(_request.length()) < _contentLength) {
+    bool eof;
+    if ( ! XmlRpcSocket::nbRead(this->getfd(), _request, &eof)) {
+      XmlRpcUtil::error("XmlRpcServerConnection::readRequest: read error (%s).",XmlRpcSocket::getErrorMsg().c_str());
+      return false;
+    }
+
+    // If we haven't gotten the entire request yet, return (keep reading)
+    if (int(_request.length()) < _contentLength) {
+      if (eof) {
+        XmlRpcUtil::error("XmlRpcServerConnection::readRequest: EOF while reading request");
+        return false;   // Either way we close the connection
+      }
+      return true;
+    }
+  }
+
+  // Otherwise, parse and dispatch the request
+  XmlRpcUtil::log(3, "XmlRpcServerConnection::readRequest read %d bytes.", _request.length());
+  //XmlRpcUtil::log(5, "XmlRpcServerConnection::readRequest:\n%s\n", _request.c_str());
+
+  _connectionState = WRITE_RESPONSE;
+
+  return true;    // Continue monitoring this source
+}
+
+
+bool
+XmlRpcServerConnection::writeResponse()
+{
+  if (_response.length() == 0) {
+    executeRequest();
+    _bytesWritten = 0;
+    if (_response.length() == 0) {
+      XmlRpcUtil::error("XmlRpcServerConnection::writeResponse: empty response.");
+      return false;
+    }
+  }
+
+  // Try to write the response
+  if ( ! XmlRpcSocket::nbWrite(this->getfd(), _response, &_bytesWritten)) {
+    XmlRpcUtil::error("XmlRpcServerConnection::writeResponse: write error (%s).",XmlRpcSocket::getErrorMsg().c_str());
+    return false;
+  }
+  XmlRpcUtil::log(3, "XmlRpcServerConnection::writeResponse: wrote %d of %d bytes.", _bytesWritten, _response.length());
+
+  // Prepare to read the next request
+  if (_bytesWritten == int(_response.length())) {
+    _header = "";
+    _request = "";
+    _response = "";
+    _connectionState = READ_HEADER;
+  }
+
+  return _keepAlive;    // Continue monitoring this source if true
+}
+
+// Run the method, generate _response string
+void
+XmlRpcServerConnection::executeRequest()
+{
+  XmlRpcValue params, resultValue;
+  std::string methodName = parseRequest(params);
+  XmlRpcUtil::log(2, "XmlRpcServerConnection::executeRequest: server calling method '%s'", 
+                    methodName.c_str());
+
+  try {
+
+    if ( ! executeMethod(methodName, params, resultValue) &&
+         ! executeMulticall(methodName, params, resultValue))
+      generateFaultResponse(methodName + ": unknown method name");
+    else
+      generateResponse(resultValue.toXml());
+
+  } catch (const XmlRpcException& fault) {
+    XmlRpcUtil::log(2, "XmlRpcServerConnection::executeRequest: fault %s.",
+                    fault.getMessage().c_str()); 
+    generateFaultResponse(fault.getMessage(), fault.getCode());
+  }
+}
+
+// Parse the method name and the argument values from the request.
+std::string
+XmlRpcServerConnection::parseRequest(XmlRpcValue& params)
+{
+  int offset = 0;   // Number of chars parsed from the request
+
+  std::string methodName = XmlRpcUtil::parseTag(METHODNAME_TAG, _request, &offset);
+
+  if (methodName.size() > 0 && XmlRpcUtil::findTag(PARAMS_TAG, _request, &offset))
+  {
+    int nArgs = 0;
+    while (XmlRpcUtil::nextTagIs(PARAM_TAG, _request, &offset)) {
+      params[nArgs++] = XmlRpcValue(_request, &offset);
+      (void) XmlRpcUtil::nextTagIs(PARAM_ETAG, _request, &offset);
+    }
+
+    (void) XmlRpcUtil::nextTagIs(PARAMS_ETAG, _request, &offset);
+  }
+
+  return methodName;
+}
+
+// Execute a named method with the specified params.
+bool
+XmlRpcServerConnection::executeMethod(const std::string& methodName, 
+                                      XmlRpcValue& params, XmlRpcValue& result)
+{
+  XmlRpcServerMethod* method = _server->findMethod(methodName);
+
+  if ( ! method) return false;
+
+  method->execute(params, result);
+
+  // Ensure a valid result value
+  if ( ! result.valid())
+      result = std::string();
+
+  return true;
+}
+
+// Execute multiple calls and return the results in an array.
+bool
+XmlRpcServerConnection::executeMulticall(const std::string& methodName, 
+                                         XmlRpcValue& params, XmlRpcValue& result)
+{
+  if (methodName != SYSTEM_MULTICALL) return false;
+
+  // There ought to be 1 parameter, an array of structs
+  if (params.size() != 1 || params[0].getType() != XmlRpcValue::TypeArray)
+    throw XmlRpcException(SYSTEM_MULTICALL + ": Invalid argument (expected an array)");
+
+  int nc = params[0].size();
+  result.setSize(nc);
+
+  for (int i=0; i<nc; ++i) {
+
+    if ( ! params[0][i].hasMember(METHODNAME) ||
+         ! params[0][i].hasMember(PARAMS)) {
+      result[i][FAULTCODE] = -1;
+      result[i][FAULTSTRING] = SYSTEM_MULTICALL +
+              ": Invalid argument (expected a struct with members methodName and params)";
+      continue;
+    }
+
+    const std::string& methodName = params[0][i][METHODNAME];
+    XmlRpcValue& methodParams = params[0][i][PARAMS];
+
+    XmlRpcValue resultValue;
+    resultValue.setSize(1);
+    try {
+      if ( ! executeMethod(methodName, methodParams, resultValue[0]) &&
+           ! executeMulticall(methodName, params, resultValue[0]))
+      {
+        result[i][FAULTCODE] = -1;
+        result[i][FAULTSTRING] = methodName + ": unknown method name";
+      }
+      else
+        result[i] = resultValue;
+
+    } catch (const XmlRpcException& fault) {
+        result[i][FAULTCODE] = fault.getCode();
+        result[i][FAULTSTRING] = fault.getMessage();
+    }
+  }
+
+  return true;
+}
+
+
+// Create a response from results xml
+void
+XmlRpcServerConnection::generateResponse(std::string const& resultXml)
+{
+  const char RESPONSE_1[] = 
+    "<?xml version=\"1.0\"?>\r\n"
+    "<methodResponse><params><param>\r\n\t";
+  const char RESPONSE_2[] =
+    "\r\n</param></params></methodResponse>\r\n";
+
+  std::string body = RESPONSE_1 + resultXml + RESPONSE_2;
+  std::string header = generateHeader(body);
+
+  _response = header + body;
+  XmlRpcUtil::log(5, "XmlRpcServerConnection::generateResponse:\n%s\n", _response.c_str()); 
+}
+
+// Prepend http headers
+std::string
+XmlRpcServerConnection::generateHeader(std::string const& body)
+{
+  std::string header = 
+    "HTTP/1.1 200 OK\r\n"
+    "Server: ";
+  header += XMLRPC_VERSION;
+  header += "\r\n"
+    "Content-Type: text/xml\r\n"
+    "Content-length: ";
+
+  char buffLen[40];
+#ifdef _MSC_VER
+  sprintf_s(buffLen,40,"%d\r\n\r\n", (int)body.size());
+#else
+  sprintf(buffLen,"%d\r\n\r\n", (int)body.size());
+#endif
+
+  return header + buffLen;
+}
+
+
+void
+XmlRpcServerConnection::generateFaultResponse(std::string const& errorMsg, int errorCode)
+{
+  const char RESPONSE_1[] = 
+    "<?xml version=\"1.0\"?>\r\n"
+    "<methodResponse><fault>\r\n\t";
+  const char RESPONSE_2[] =
+    "\r\n</fault></methodResponse>\r\n";
+
+  XmlRpcValue faultStruct;
+  faultStruct[FAULTCODE] = errorCode;
+  faultStruct[FAULTSTRING] = errorMsg;
+  std::string body = RESPONSE_1 + faultStruct.toXml() + RESPONSE_2;
+  std::string header = generateHeader(body);
+
+  _response = header + body;
+}
+

thirdparty/ros/ros_comm/utilities/xmlrpcpp/src/XmlRpcServerMethod.cpp

@@ -0,0 +1,21 @@
+
+#include "xmlrpcpp/XmlRpcServerMethod.h"
+#include "xmlrpcpp/XmlRpcServer.h"
+
+namespace XmlRpc {
+
+
+  XmlRpcServerMethod::XmlRpcServerMethod(std::string const& name, XmlRpcServer* server)
+  {
+    _name = name;
+    _server = server;
+    if (_server) _server->addMethod(this);
+  }
+
+  XmlRpcServerMethod::~XmlRpcServerMethod()
+  {
+    if (_server) _server->removeMethod(this);
+  }
+
+
+} // namespace XmlRpc