diff --git a/CMakeLists.txt b/CMakeLists.txt index ec7dda2..787f8ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,17 +57,11 @@ ament_export_targets(${PROJECT_NAME}) ament_export_dependencies(rclcpp rcutils std_msgs) if(BUILD_TESTING) - find_package(ament_cmake_copyright REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_lint_auto REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(sensor_msgs REQUIRED) - ament_cpplint() - ament_copyright() - ament_uncrustify(LANGUAGE "c++") - ament_lint_cmake() + ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest) diff --git a/doc/conf.py b/doc/conf.py index 2edb392..52059c3 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -39,18 +39,20 @@ # All configuration values have a default; values that are commented out # serve to show the default. -import sys, os +# import sys, os +from datetime import datetime # 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('.')) +# 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.imgmath', 'sphinx_rtd_theme'] +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', + 'sphinx.ext.imgmath', 'sphinx_rtd_theme'] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] @@ -59,15 +61,14 @@ source_suffix = '.rst' # The encoding of source files. -#source_encoding = 'utf-8' +# source_encoding = 'utf-8' # The master toctree document. master_doc = 'index' # General information about the project. -from datetime import datetime -now = datetime.now() # current date and time -year = now.strftime("%Y") +now = datetime.now() # current date and time +year = now.strftime('%Y') project = 'message_filters' copyright = '2008-' + year + ', Open Source Robotics Foundation, Inc.' # noqa @@ -84,40 +85,40 @@ # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. -#language = None +# language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: -#today = '' +# today = '' # Else, today_fmt is used as the format for a strftime call. -#today_fmt = '%B %d, %Y' +# today_fmt = '%B %d, %Y' # List of documents that shouldn't be included in the build. -#unused_docs = [] +# unused_docs = [] # List of directories, relative to source directory, that shouldn't be searched # for source files. exclude_trees = ['_build'] # The reST default role (used for this markup: `text`) to use for all documents. -#default_role = None +# default_role = None # If true, '()' will be appended to :func: etc. cross-reference text. -#add_function_parentheses = True +# 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 +# add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. -#show_authors = False +# 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 = [] +# modindex_common_prefix = [] # -- Options for HTML output --------------------------------------------------- @@ -129,66 +130,66 @@ # 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 = {} +# html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. -#html_theme_path = [] +# html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". -#html_title = None +# html_title = None # A shorter title for the navigation bar. Default is the same as html_title. -#html_short_title = None +# 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 +# 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 +# html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -#html_static_path = ['_static'] +# html_static_path = ['_static'] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. -#html_last_updated_fmt = '%b %d, %Y' +# 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 +# html_use_smartypants = True # Custom sidebar templates, maps document names to template names. -#html_sidebars = {} +# html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. -#html_additional_pages = {} +# html_additional_pages = {} # If false, no module index is generated. -#html_use_modindex = True +# html_use_modindex = True # If false, no index is generated. -#html_use_index = True +# html_use_index = True # If true, the index is split into individual pages for each letter. -#html_split_index = False +# html_split_index = False # If true, links to the reST sources are added to the pages. -#html_show_sourcelink = True +# html_show_sourcelink = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. -#html_use_opensearch = '' +# html_use_opensearch = '' # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). -#html_file_suffix = '' +# html_file_suffix = '' # Output file base name for HTML help builder. htmlhelp_basename = 'message_filters_doc' @@ -197,10 +198,10 @@ # -- Options for LaTeX output -------------------------------------------------- # The paper size ('letter' or 'a4'). -#latex_paper_size = 'letter' +# latex_paper_size = 'letter' # The font size ('10pt', '11pt' or '12pt'). -#latex_font_size = '10pt' +# latex_font_size = '10pt' # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, author, documentclass [howto/manual]). @@ -211,20 +212,20 @@ # The name of an image file (relative to this directory) to place at the top of # the title page. -#latex_logo = None +# latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. -#latex_use_parts = False +# latex_use_parts = False # Additional stuff for the LaTeX preamble. -#latex_preamble = '' +# latex_preamble = '' # Documents to append as an appendix to all manuals. -#latex_appendices = [] +# latex_appendices = [] # If false, no module index is generated. -#latex_use_modindex = True +# latex_use_modindex = True # -- Options for manual page output ------------------------------------------ @@ -247,7 +248,7 @@ ] -autoclass_content = "both" +autoclass_content = 'both' autodoc_default_options = { 'members': True, # document members diff --git a/package.xml b/package.xml index 247e800..162e24c 100644 --- a/package.xml +++ b/package.xml @@ -10,7 +10,7 @@ Geoffrey Biggs BSD - https://github.com/intel/ros2_message_filters + https://github.com/ros2/message_filters Dirk Thomas Ethan Gao @@ -29,10 +29,8 @@ builtin_interfaces rclpy - ament_cmake_cpplint - ament_cmake_copyright - ament_cmake_uncrustify - ament_cmake_lint_cmake + ament_lint_auto + ament_lint_common ament_cmake_gtest ament_cmake_pytest sensor_msgs diff --git a/setup.py b/setup.py index ee1ea46..f700539 100644 --- a/setup.py +++ b/setup.py @@ -1,6 +1,7 @@ #!/usr/bin/env python from distutils.core import setup + from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( diff --git a/src/message_filters/__init__.py b/src/message_filters/__init__.py index 559e417..6a4aca4 100644 --- a/src/message_filters/__init__.py +++ b/src/message_filters/__init__.py @@ -26,17 +26,15 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -""" -Message Filter Objects -====================== -""" + +"""Message Filter Objects.""" from functools import reduce import itertools import threading -import rclpy +# import builtin_interfaces -import builtin_interfaces +import rclpy from rclpy.clock import ROSClock from rclpy.duration import Duration from rclpy.logging import LoggingSeverity @@ -50,12 +48,12 @@ def __init__(self): 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``. - """ + Register a callback `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 @@ -64,8 +62,8 @@ def signalMessage(self, *msg): for (cb, args) in self.callbacks.values(): cb(*(msg + args)) + class Subscriber(SimpleFilter): - """ ROS 2 subscription filter, takes identical arguments as :class:`rclpy.Subscriber`. @@ -73,6 +71,7 @@ class Subscriber(SimpleFilter): from a ROS 2 subscription through to the filters which have connected to it. """ + def __init__(self, *args, **kwargs): SimpleFilter.__init__(self) self.node = args[0] @@ -87,11 +86,11 @@ def getTopic(self): return self.topic def __getattr__(self, key): - """Serve same API as rospy.Subscriber""" + """Serve same API as rospy.Subscriber.""" return self.sub.__getattribute__(key) -class Cache(SimpleFilter): +class Cache(SimpleFilter): """ Stores a time history of messages. @@ -124,12 +123,12 @@ def add(self, msg): if not self.allow_headerless: msg_filters_logger = rclpy.logging.get_logger('message_filters_cache') msg_filters_logger.set_level(LoggingSeverity.INFO) - msg_filters_logger.warn("can not use message filters messages " - "without timestamp infomation when " - "'allow_headerless' is disabled. " - "auto assign ROSTIME to headerless " - "messages once enabling constructor " - "option of 'allow_headerless'.") + msg_filters_logger.warn('can not use message filters messages ' + 'without timestamp infomation when ' + '"allow_headerless" is disabled. ' + 'auto assign ROSTIME to headerless ' + 'messages once enabling constructor ' + 'option of "allow_headerless".') return @@ -184,7 +183,7 @@ def getOldestTime(self): if not self.cache_times: return None return self.cache_times[0] - + def getLast(self): if self.getLastestTime() is None: return None @@ -192,7 +191,6 @@ def getLast(self): class TimeSynchronizer(SimpleFilter): - """ Synchronizes messages by their timestamps. @@ -240,8 +238,8 @@ def add(self, msg, my_queue, my_queue_index=None): del q[t] self.lock.release() -class ApproximateTimeSynchronizer(TimeSynchronizer): +class ApproximateTimeSynchronizer(TimeSynchronizer): """ Approximately synchronizes messages by their timestamps. @@ -264,11 +262,12 @@ def add(self, msg, my_queue, my_queue_index=None): if not self.allow_headerless: msg_filters_logger = rclpy.logging.get_logger('message_filters_approx') msg_filters_logger.set_level(LoggingSeverity.INFO) - msg_filters_logger.warn("can not use message filters for " - "messages without timestamp infomation when " - "'allow_headerless' is disabled. auto assign " - "ROSTIME to headerless messages once enabling " - "constructor option of 'allow_headerless'.") + msg_filters_logger.warn('can not use message filters messages ' + 'without timestamp infomation when ' + '"allow_headerless" is disabled. ' + 'auto assign ROSTIME to headerless ' + 'messages once enabling constructor ' + 'option of "allow_headerless".') return stamp = ROSClock().now() @@ -296,7 +295,8 @@ def add(self, msg, my_queue, my_queue_index=None): if stamp_delta > self.slop: continue # far over the slop topic_stamps.append(((Time(nanoseconds=s, - clock_type=stamp.clock_type)), stamp_delta)) + clock_type=stamp.clock_type)), + stamp_delta)) if not topic_stamps: self.lock.release() return @@ -308,11 +308,11 @@ def add(self, msg, my_queue, my_queue_index=None): 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.nanoseconds not in q]) == 0) ): - msgs = [q[t.nanoseconds] for q,t in qt] + if (((max(vv) - min(vv)) < self.slop) and + (len([1 for q, t in qt if t.nanoseconds not in q]) == 0)): + msgs = [q[t.nanoseconds] for q, t in qt] self.signalMessage(*msgs) - for q,t in qt: + for q, t in qt: del q[t.nanoseconds] break # fast finish after the synchronization self.lock.release() diff --git a/test/directed.py b/test/directed.py index b01ce9e..d93a60d 100644 --- a/test/directed.py +++ b/test/directed.py @@ -51,15 +51,19 @@ class MockHeader: pass + class MockMessage: + def __init__(self, stamp, data): self.header = MockHeader() self.header.stamp = TimeMsg(sec=stamp) self.data = data + class MockFilter(SimpleFilter): pass + class TestDirected(unittest.TestCase): def cb_collector_2msg(self, msg1, msg2): @@ -101,6 +105,7 @@ def test_synchronizer(self): m1.signalMessage(msg) self.assertEqual(set(self.collector), set(zip(seq0, seq1))) + if __name__ == '__main__': suite = unittest.TestSuite() suite.addTest(TestDirected('test_synchronizer')) diff --git a/test/test_approxsync.py b/test/test_approxsync.py index 1c3f27d..693150c 100644 --- a/test/test_approxsync.py +++ b/test/test_approxsync.py @@ -26,31 +26,39 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +import random +import unittest + import message_filters from message_filters import ApproximateTimeSynchronizer -import random from rclpy.clock import ROSClock from rclpy.duration import Duration from rclpy.time import Time -import unittest + 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): @@ -79,9 +87,9 @@ def test_approx(self): for N in range(1, 10): m0 = MockFilter() m1 = MockFilter() - seq0 = [MockMessage(Time(seconds=t), random.random())\ + seq0 = [MockMessage(Time(seconds=t), random.random()) for t in range(N)] - seq1 = [MockMessage(Time(seconds=t), random.random())\ + seq1 = [MockMessage(Time(seconds=t), random.random()) for t in range(N)] # random.shuffle(seq0) ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1) @@ -117,6 +125,7 @@ def test_approx(self): m1.signalMessage(msg) self.assertEqual(set(self.collector), set(zip(seq0, seq1))) + if __name__ == '__main__': suite = unittest.TestSuite() suite.addTest(TestApproxSync('test_approx')) diff --git a/test/test_message_filters_cache.py b/test/test_message_filters_cache.py index 332674a..16d4aee 100644 --- a/test/test_message_filters_cache.py +++ b/test/test_message_filters_cache.py @@ -27,24 +27,25 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +# import time +import unittest + from message_filters import Cache, Subscriber import rclpy -from rclpy.time import Time -from rclpy.clock import ClockType -from rclpy.clock import ROSClock +from rclpy.clock import ClockType, ROSClock from rclpy.duration import Duration +from rclpy.time import Time from std_msgs.msg import String -import time -import unittest PKG = 'message_filters' + class AnonymMsg: class AnonymHeader: stamp = None def __init__(self): - stamp = Time() + self.stamp = Time() header = None @@ -53,6 +54,7 @@ def __init__(self): class TestCache(unittest.TestCase): + @classmethod def setUpClass(cls): rclpy.init() @@ -64,7 +66,7 @@ def tearDownClass(cls): rclpy.shutdown() def test_all_funcs(self): - sub = Subscriber(self.node, String, "/empty") + sub = Subscriber(self.node, String, '/empty') cache = Cache(sub, 5) msg = AnonymMsg() @@ -87,35 +89,35 @@ def test_all_funcs(self): msg.header.stamp = Time(seconds=4) cache.add(msg) - l = len(cache.getInterval(Time(seconds=2.5), - Time(seconds=3.5))) - self.assertEqual(l, 1, "invalid number of messages" + - " returned in getInterval 1") + length = len(cache.getInterval(Time(seconds=2.5), + Time(seconds=3.5))) + self.assertEqual(length, 1, 'invalid number of messages' + + ' returned in getInterval 1') - l = len(cache.getInterval(Time(seconds=2), - Time(seconds=3))) - self.assertEqual(l, 2, "invalid number of messages" + - " returned in getInterval 2") + length = len(cache.getInterval(Time(seconds=2), + Time(seconds=3))) + self.assertEqual(length, 2, 'invalid number of messages' + + ' returned in getInterval 2') - l = len(cache.getInterval(Time(), - Time(seconds=4))) - self.assertEqual(l, 5, "invalid number of messages" + - " returned in getInterval 5") + length = len(cache.getInterval(Time(), + Time(seconds=4))) + self.assertEqual(length, 5, 'invalid number of messages' + + ' returned in getInterval 5') s = cache.getElemAfterTime(Time()).header.stamp - self.assertEqual(s, Time(), "invalid msg return by getElemAfterTime") + self.assertEqual(s, Time(), 'invalid msg return by getElemAfterTime') s = cache.getElemBeforeTime(Time(seconds=3.5)).header.stamp self.assertEqual(s, Time(seconds=3), - "invalid msg return by getElemBeforeTime") + 'invalid msg return by getElemBeforeTime') s = cache.getLastestTime() self.assertEqual(s, Time(seconds=4), - "invalid stamp return by getLastestTime") + 'invalid stamp return by getLastestTime') s = cache.getOldestTime() self.assertEqual(s, Time(), - "invalid stamp return by getOldestTime") + 'invalid stamp return by getOldestTime') # Add another msg to fill the buffer msg = AnonymMsg() @@ -124,41 +126,40 @@ def test_all_funcs(self): # Test that it discarded the right one s = cache.getOldestTime() - self.assertEqual(s, Time(seconds=1), "wrong message discarded") + self.assertEqual(s, Time(seconds=1), 'wrong message discarded') def test_headerless(self): - sub = Subscriber(self.node, String, "/empty") + sub = Subscriber(self.node, String, '/empty') cache = Cache(sub, 5, allow_headerless=False) msg = String() cache.add(msg) self.assertIsNone(cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME)), - "Headerless message invalidly added.") + 'Headerless message invalidly added.') cache = Cache(sub, 5, allow_headerless=True) cache.add(msg) s = cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME)) self.assertEqual(s, msg, - "invalid msg returned in headerless scenario") + 'invalid msg returned in headerless scenario') currentRosTime = ROSClock().now() s = cache.getElemAfterTime(currentRosTime) - self.assertIsNone(s, "invalid msg returned in headerless scenario") - + self.assertIsNone(s, 'invalid msg returned in headerless scenario') cache.add(msg) s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME), currentRosTime) self.assertEqual(s, [msg], - "invalid msg returned in headerless scenario") + 'invalid msg returned in headerless scenario') s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME), (currentRosTime + Duration(seconds=2))) self.assertEqual(s, [msg, msg], - "invalid msg returned in headerless scenario") + 'invalid msg returned in headerless scenario') if __name__ == '__main__':