├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── conf.py ├── include └── message_filters │ ├── cache.h │ ├── chain.h │ ├── connection.h │ ├── macros.h │ ├── message_event.h │ ├── message_traits.h │ ├── null_types.h │ ├── parameter_adapter.h │ ├── pass_through.h │ ├── signal1.h │ ├── signal9.h │ ├── simple_filter.h │ ├── subscriber.h │ ├── sync_policies │ ├── approximate_time.h │ └── exact_time.h │ ├── synchronizer.h │ ├── time_sequencer.h │ └── time_synchronizer.h ├── index.rst ├── mainpage.dox ├── package.xml ├── rosdoc.yaml ├── src ├── connection.cpp └── message_filters │ └── __init__.py └── test ├── directed.py ├── msg_cache_unittest.cpp ├── test_approximate_time_policy.cpp ├── test_approxsync.py ├── test_chain.cpp ├── test_exact_time_policy.cpp ├── test_fuzz.cpp ├── test_message_filters_cache.py ├── test_simple.cpp ├── test_subscriber.cpp ├── test_subscriber.xml ├── test_synchronizer.cpp ├── time_sequencer_unittest.cpp ├── time_sequencer_unittest.xml └── time_synchronizer_unittest.cpp /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package message_filters 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2018-06-06) 6 | ------------------ 7 | * Change build tool from catkin to ament in CMakeLists.txt 8 | * ROS APIs move to ROS2 APIs 9 | * Drop boost dependency, using C++14 instead 10 | * Add message_event.h message_trait.h parameter_adapter.h which are missing in ROS2 core 11 | * Update existing test cases and add a fuzz test(default disable) 12 | * Drop unused setup.py 13 | * Add a README 14 | * Bug fixes 15 | * Contributor: Jing Wang, Ethan Gao 16 | 17 | 1.13.5 (2017-11-09) 18 | ------------------- 19 | 20 | 1.13.4 (2017-11-02) 21 | ------------------- 22 | 23 | 1.13.3 (2017-10-25) 24 | ------------------- 25 | 26 | 1.13.2 (2017-08-15) 27 | ------------------- 28 | 29 | 1.13.1 (2017-07-27) 30 | ------------------- 31 | 32 | 1.13.0 (2017-02-22) 33 | ------------------- 34 | 35 | 1.12.7 (2017-02-17) 36 | ------------------- 37 | 38 | 1.12.6 (2016-10-26) 39 | ------------------- 40 | * use boost::bind to bind the callback function (`#906 `_) 41 | 42 | 1.12.5 (2016-09-30) 43 | ------------------- 44 | 45 | 1.12.4 (2016-09-19) 46 | ------------------- 47 | 48 | 1.12.3 (2016-09-17) 49 | ------------------- 50 | * add fast approximate time synchronization in message_filters (in pure Python) (`#802 `_) 51 | 52 | 1.12.2 (2016-06-03) 53 | ------------------- 54 | * allow saving timestamp-less messages to Cache, add getLast method (`#806 `_) 55 | 56 | 1.12.1 (2016-04-18) 57 | ------------------- 58 | * use directory specific compiler flags (`#785 `_) 59 | 60 | 1.12.0 (2016-03-18) 61 | ------------------- 62 | 63 | 1.11.18 (2016-03-17) 64 | -------------------- 65 | * fix compiler warnings 66 | 67 | 1.11.17 (2016-03-11) 68 | -------------------- 69 | * use boost::make_shared instead of new for constructing boost::shared_ptr (`#740 `_) 70 | * add __getattr_\_ to handle sub in message_filters as standard one (`#700 `_) 71 | 72 | 1.11.16 (2015-11-09) 73 | -------------------- 74 | 75 | 1.11.15 (2015-10-13) 76 | -------------------- 77 | * add unregister() method to message_filter.Subscriber (`#683 `_) 78 | 79 | 1.11.14 (2015-09-19) 80 | -------------------- 81 | 82 | 1.11.13 (2015-04-28) 83 | -------------------- 84 | 85 | 1.11.12 (2015-04-27) 86 | -------------------- 87 | 88 | 1.11.11 (2015-04-16) 89 | -------------------- 90 | * implement message filter cache in Python (`#599 `_) 91 | 92 | 1.11.10 (2014-12-22) 93 | -------------------- 94 | 95 | 1.11.9 (2014-08-18) 96 | ------------------- 97 | 98 | 1.11.8 (2014-08-04) 99 | ------------------- 100 | 101 | 1.11.7 (2014-07-18) 102 | ------------------- 103 | 104 | 1.11.6 (2014-07-10) 105 | ------------------- 106 | 107 | 1.11.5 (2014-06-24) 108 | ------------------- 109 | 110 | 1.11.4 (2014-06-16) 111 | ------------------- 112 | * add approximate Python time synchronizer (used to be in camera_calibration) (`#424 `_) 113 | 114 | 1.11.3 (2014-05-21) 115 | ------------------- 116 | 117 | 1.11.2 (2014-05-08) 118 | ------------------- 119 | 120 | 1.11.1 (2014-05-07) 121 | ------------------- 122 | * update API to use boost::signals2 (`#267 `_) 123 | 124 | 1.11.0 (2014-03-04) 125 | ------------------- 126 | * suppress boost::signals deprecation warning (`#362 `_) 127 | 128 | 1.10.0 (2014-02-11) 129 | ------------------- 130 | 131 | 1.9.54 (2014-01-27) 132 | ------------------- 133 | 134 | 1.9.53 (2014-01-14) 135 | ------------------- 136 | * add kwargs for message_filters.Subscriber 137 | 138 | 1.9.52 (2014-01-08) 139 | ------------------- 140 | 141 | 1.9.51 (2014-01-07) 142 | ------------------- 143 | * update code after refactoring into rosbag_storage and roscpp_core (`#299 `_) 144 | * fix segmentation fault on OS X 10.9 (clang / libc++) 145 | 146 | 1.9.50 (2013-10-04) 147 | ------------------- 148 | 149 | 1.9.49 (2013-09-16) 150 | ------------------- 151 | 152 | 1.9.48 (2013-08-21) 153 | ------------------- 154 | 155 | 1.9.47 (2013-07-03) 156 | ------------------- 157 | * check for CATKIN_ENABLE_TESTING to enable configure without tests 158 | 159 | 1.9.46 (2013-06-18) 160 | ------------------- 161 | 162 | 1.9.45 (2013-06-06) 163 | ------------------- 164 | * fix template syntax for signal\_.template addCallback() to work with Intel compiler 165 | 166 | 1.9.44 (2013-03-21) 167 | ------------------- 168 | * fix install destination for dll's under Windows 169 | 170 | 1.9.43 (2013-03-13) 171 | ------------------- 172 | * fix exports of message filter symbols for Windows 173 | 174 | 1.9.42 (2013-03-08) 175 | ------------------- 176 | 177 | 1.9.41 (2013-01-24) 178 | ------------------- 179 | 180 | 1.9.40 (2013-01-13) 181 | ------------------- 182 | 183 | 1.9.39 (2012-12-29) 184 | ------------------- 185 | * first public release for Groovy 186 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(message_filters) 4 | 5 | # Default to C++14 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 14) 8 | endif() 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | find_package(ament_cmake_python REQUIRED) 15 | find_package(ament_cmake REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rcutils) 18 | find_package(builtin_interfaces REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | find_package(sensor_msgs REQUIRED) 21 | 22 | include_directories(include) 23 | add_library(${PROJECT_NAME} SHARED src/connection.cpp) 24 | ament_target_dependencies(${PROJECT_NAME} 25 | "rclcpp" 26 | "rcutils" 27 | "std_msgs" 28 | "builtin_interfaces" 29 | "sensor_msgs") 30 | 31 | install( 32 | TARGETS ${PROJECT_NAME} 33 | ARCHIVE DESTINATION lib 34 | LIBRARY DESTINATION lib 35 | RUNTIME DESTINATION bin 36 | ) 37 | 38 | install( 39 | DIRECTORY "include/" 40 | DESTINATION include 41 | ) 42 | 43 | find_package(python_cmake_module REQUIRED) 44 | _ament_cmake_python_register_environment_hook() 45 | install( 46 | DIRECTORY "src/${PROJECT_NAME}/" 47 | DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" 48 | PATTERN "*.pyc" EXCLUDE 49 | PATTERN "__pycache__" EXCLUDE 50 | ) 51 | 52 | ament_export_include_directories(include) 53 | ament_export_libraries(${PROJECT_NAME}) 54 | 55 | if(BUILD_TESTING) 56 | find_package(ament_lint_auto REQUIRED) 57 | ament_lint_auto_find_test_dependencies() 58 | 59 | find_package(ament_cmake_gtest) 60 | 61 | ament_add_gtest(${PROJECT_NAME}-test_simple test/test_simple.cpp) 62 | if(TARGET ${PROJECT_NAME}-test_simple) 63 | target_link_libraries(${PROJECT_NAME}-test_simple ${PROJECT_NAME}) 64 | endif() 65 | 66 | 67 | ament_add_gtest(${PROJECT_NAME}-msg_cache_unittest test/msg_cache_unittest.cpp) 68 | if(TARGET ${PROJECT_NAME}-msg_cache_unittest) 69 | target_link_libraries(${PROJECT_NAME}-msg_cache_unittest ${PROJECT_NAME}) 70 | endif() 71 | 72 | ament_add_gtest(${PROJECT_NAME}-test_chain test/test_chain.cpp) 73 | if(TARGET ${PROJECT_NAME}-test_chain) 74 | target_link_libraries(${PROJECT_NAME}-test_chain ${PROJECT_NAME}) 75 | endif() 76 | 77 | ament_add_gtest(${PROJECT_NAME}-test_subscriber test/test_subscriber.cpp) 78 | if(TARGET ${PROJECT_NAME}-test_subscriber) 79 | target_link_libraries(${PROJECT_NAME}-test_subscriber ${PROJECT_NAME}) 80 | endif() 81 | 82 | ament_add_gtest(${PROJECT_NAME}-test_synchronizer test/test_synchronizer.cpp) 83 | if(TARGET ${PROJECT_NAME}-test_synchronizer) 84 | target_link_libraries(${PROJECT_NAME}-test_synchronizer ${PROJECT_NAME}) 85 | endif() 86 | 87 | ament_add_gtest(${PROJECT_NAME}-time_synchronizer_unittest test/time_synchronizer_unittest.cpp) 88 | if(TARGET ${PROJECT_NAME}-time_synchronizer_unittest) 89 | target_link_libraries(${PROJECT_NAME}-time_synchronizer_unittest ${PROJECT_NAME}) 90 | endif() 91 | 92 | ament_add_gtest(${PROJECT_NAME}-test_exact_time_policy test/test_exact_time_policy.cpp) 93 | if(TARGET ${PROJECT_NAME}-test_exact_time_policy) 94 | target_link_libraries(${PROJECT_NAME}-test_exact_time_policy ${PROJECT_NAME}) 95 | endif() 96 | 97 | ament_add_gtest(${PROJECT_NAME}-test_approximate_time_policy test/test_approximate_time_policy.cpp) 98 | if(TARGET ${PROJECT_NAME}-test_approximate_time_policy) 99 | target_link_libraries(${PROJECT_NAME}-test_approximate_time_policy ${PROJECT_NAME}) 100 | endif() 101 | 102 | ament_add_gtest(${PROJECT_NAME}-test_fuzz test/test_fuzz.cpp SKIP_TEST) 103 | if(TARGET ${PROJECT_NAME}-test_fuzz) 104 | target_link_libraries(${PROJECT_NAME}-test_fuzz ${PROJECT_NAME}) 105 | endif() 106 | 107 | find_package(ament_cmake_pytest REQUIRED) 108 | ament_add_pytest_test(msfpytests test 109 | PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" 110 | APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path} 111 | PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} 112 | TIMEOUT 90 113 | ) 114 | endif() 115 | ament_package() 116 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | DISCONTINUATION OF PROJECT. 2 | 3 | This project will no longer be maintained by Intel. 4 | 5 | Intel has ceased development and contributions including, but not limited to, maintenance, bug fixes, new releases, or updates, to this project. 6 | 7 | Intel no longer accepts patches to this project. 8 | 9 | If you have an ongoing need to use this project, are interested in independently developing it, or would like to maintain patches for the open source software community, please create your own fork of this project. 10 | # ROS2 Message Filters 11 | 12 | `ros2_message_filters` blends various messages based on the conditions that filter needs to met and derives from ROS2 porting of [ROS message_filters](http://wiki.ros.org/message_filters). It collects commonly used message "filtering" algorithms into a common space. A message filter is defined as something which a message arrives into and may or may not be spit back out of at a later point in time. 13 | 14 | 15 | **The API is a combination of parts:** 16 | - Filter Pattern 17 | - `registerCallback(cb)` 18 | - `connectInput(filter)` 19 | - Subscriber 20 | - `subscribe(nodep, topic, 1, myCallback)` 21 | - Time Synchronizer 22 | - Input : 23 | - C++: Up to 9 separate filters, each of which is of the form `void callback(const std::shared_ptr&)`. The number of filters supported is determined by the number of template arguments the class was created with. 24 | - Python: N separate filters, each of which has signature `callback(msg)`. 25 | - Output : 26 | - C++: For message types M0..M8, `void callback(const std::shared_ptr&`, ..., `const std::shared_ptr&)`. The number of parameters is determined by the number of template arguments the class was created with. 27 | - Python: `callback(msg0.. msgN)`. The number of parameters is determined by the number of template arguments the class was created with. 28 | 29 | ``` 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | using namespace sensor_msgs; 36 | using namespace message_filters; 37 | 38 | void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info) 39 | { 40 | // Solve all of perception here... 41 | } 42 | 43 | int main(int argc, char** argv) 44 | { 45 | rclcpp::init(argc, argv); 46 | auto nh = std::make_shared("test_node"); 47 | Subscriber image_sub(nh.get(), "image"); 48 | Subscriber info_sub(nh.get(), "camera"); 49 | TimeSynchronizer sync(image_sub, info_sub, 10); 50 | sync.registerCallback(std::bind(&callback, _1, _2)); 51 | rclcpp::spin(nh); 52 | return 0; 53 | } 54 | ``` 55 | 56 | **Environment** 57 | 58 | * Hardware: Compliant to ROS2 HW requirements 59 | * Software: ROS2 + Supported OS(Linux, Win, Mac) 60 | 61 | **How to build and test ROS2 message filters** 62 | - Clone and build ROS2 source code under ros2 workspace, please refer to [ROS2 Installation](http://github.com/ros2/ros2/wiki/Installation) for more details and the following is the example in Ubuntu 16.04 LTS. 63 | ``` 64 | $cd ~/ 65 | $src/ament/ament_tools/scripts/ament.py build --build-tests --symlink-install 66 | ``` 67 | - Build message filters 68 | ``` 69 | $cd src/ros2 70 | $git clone https://github.com/intel/ros2_message_filters.git 71 | $source /install/local_setup.bash 72 | $ament build src/ros2/ros2_message_filters 73 | ``` 74 | - Run unit tests 75 | ``` 76 | $ament test src/ros2/ros2_message_filters 77 | 78 | 1: Test timeout computed to be: 60 79 | 1: -- run_test.py: invoking following command in '/home/jwang-robot/ros2_ws/src/ros2/message_filters': 80 | 1: - /home/jwang-robot/ros2_ws/build/message_filters/message_filters-test_simple --gtest_output=xml:/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-test_simple.gtest.xml 81 | 1: [==========] Running 2 tests from 1 test case. 82 | 1: [----------] Global test environment set-up. 83 | 1: [----------] 2 tests from SimpleFilter 84 | 1: [ RUN ] SimpleFilter.callbackTypes 85 | 1: [ OK ] SimpleFilter.callbackTypes (0 ms) 86 | 1: [ RUN ] SimpleFilter.oldRegisterWithNewFilter 87 | 1: [ OK ] SimpleFilter.oldRegisterWithNewFilter (0 ms) 88 | 1: [----------] 2 tests from SimpleFilter (0 ms total) 89 | 1: 90 | 1: [----------] Global test environment tear-down 91 | 1: [==========] 2 tests from 1 test case ran. (0 ms total) 92 | 1: [ PASSED ] 2 tests. 93 | 1: -- run_test.py: return code 0 94 | 1: -- run_test.py: inject classname prefix into gtest result file '/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-test_simple.gtest.xml' 95 | 1: -- run_test.py: verify result file '/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-test_simple.gtest.xml' 96 | 1/10 Test #1: message_filters-test_simple .................... Passed 0.09 sec 97 | test 2 98 | Start 2: message_filters-msg_cache_unittest 99 | 100 | 2: Test timeout computed to be: 60 101 | 2: -- run_test.py: invoking following command in '/home/jwang-robot/ros2_ws/src/ros2/message_filters': 102 | 2: - /home/jwang-robot/ros2_ws/build/message_filters/message_filters-msg_cache_unittest --gtest_output=xml:/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-msg_cache_unittest.gtest.xml 103 | 2: [==========] Running 5 tests from 1 test case. 104 | 2: [----------] Global test environment set-up. 105 | 2: [----------] 5 tests from Cache 106 | 2: [ RUN ] Cache.easyInterval 107 | 2: [ OK ] Cache.easyInterval (0 ms) 108 | 2: [ RUN ] Cache.easySurroundingInterval 109 | 2: [ OK ] Cache.easySurroundingInterval (0 ms) 110 | 2: [ RUN ] Cache.easyUnsorted 111 | 2: [ OK ] Cache.easyUnsorted (0 ms) 112 | 2: [ RUN ] Cache.easyElemBeforeAfter 113 | 2: [ OK ] Cache.easyElemBeforeAfter (0 ms) 114 | 2: [ RUN ] Cache.eventInEventOut 115 | 2: [ OK ] Cache.eventInEventOut (0 ms) 116 | 2: [----------] 5 tests from Cache (0 ms total) 117 | 2: 118 | 2: [----------] Global test environment tear-down 119 | 2: [==========] 5 tests from 1 test case ran. (0 ms total) 120 | 2: [ PASSED ] 5 tests. 121 | 2: -- run_test.py: return code 0 122 | 2: -- run_test.py: inject classname prefix into gtest result file '/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-msg_cache_unittest.gtest.xml' 123 | 2: -- run_test.py: verify result file '/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-msg_cache_unittest.gtest.xml' 124 | 2/10 Test #2: message_filters-msg_cache_unittest ............. Passed 0.09 sec 125 | test 3 126 | Start 3: message_filters-test_chain 127 | 128 | 3: Test timeout computed to be: 60 129 | 3: -- run_test.py: invoking following command in '/home/jwang-robot/ros2_ws/src/ros2/message_filters': 130 | 3: - /home/jwang-robot/ros2_ws/build/message_filters/message_filters-test_chain --gtest_output=xml:/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-test_chain.gtest.xml 131 | 3: [==========] Running 8 tests from 1 test case. 132 | 3: [----------] Global test environment set-up. 133 | 3: [----------] 8 tests from Chain 134 | 3: [ RUN ] Chain.simple 135 | 3: [ OK ] Chain.simple (0 ms) 136 | 3: [ RUN ] Chain.multipleFilters 137 | 3: [ OK ] Chain.multipleFilters (0 ms) 138 | 3: [ RUN ] Chain.addingFilters 139 | 3: [ OK ] Chain.addingFilters (1 ms) 140 | 3: [ RUN ] Chain.inputFilter 141 | 3: [ OK ] Chain.inputFilter (0 ms) 142 | 3: [ RUN ] Chain.nonSharedPtrFilter 143 | 3: [ OK ] Chain.nonSharedPtrFilter (0 ms) 144 | 3: [ RUN ] Chain.retrieveFilter 145 | 3: [ OK ] Chain.retrieveFilter (0 ms) 146 | 3: [ RUN ] Chain.retrieveFilterThroughBaseClass 147 | 3: [ OK ] Chain.retrieveFilterThroughBaseClass (0 ms) 148 | 3: [ RUN ] Chain.retrieveBaseClass 149 | 3: [ OK ] Chain.retrieveBaseClass (0 ms) 150 | 3: [----------] 8 tests from Chain (1 ms total) 151 | 3: 152 | 3: [----------] Global test environment tear-down 153 | 3: [==========] 8 tests from 1 test case ran. (1 ms total) 154 | 3: [ PASSED ] 8 tests. 155 | 3: -- run_test.py: return code 0 156 | 3: -- run_test.py: inject classname prefix into gtest result file '/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-test_chain.gtest.xml' 157 | 3: -- run_test.py: verify result file '/home/jwang-robot/ros2_ws/build/message_filters/test_results/message_filters/message_filters-test_chain.gtest.xml' 158 | 3/10 Test #3: message_filters-test_chain ..................... Passed 0.09 sec 159 | test 4 160 | ... 161 | ... 162 | ... 163 | 100% tests passed, 0 tests failed out of 10 164 | 165 | Label Time Summary: 166 | gtest = 16.15 sec (9 tests) 167 | pytest = 0.54 sec (1 test) 168 | 169 | Total Test time (real) = 16.69 sec 170 | 171 | ``` 172 | 173 | - Run fuzz tests (`disabled by default`) 174 | ``` 175 | $./build/message_filters/message_filters-test_fuzz 176 | 177 | [==========] Running 3 tests from 3 test cases. 178 | [----------] Global test environment set-up. 179 | [----------] 1 test from TimeSequencer 180 | [ RUN ] TimeSequencer.fuzz_sequencer 181 | [ OK ] TimeSequencer.fuzz_sequencer (5118 ms) 182 | [----------] 1 test from TimeSequencer (5118 ms total) 183 | 184 | [----------] 1 test from TimeSynchronizer 185 | [ RUN ] TimeSynchronizer.fuzz_synchronizer 186 | [ OK ] TimeSynchronizer.fuzz_synchronizer (5013 ms) 187 | [----------] 1 test from TimeSynchronizer (5013 ms total) 188 | 189 | [----------] 1 test from Subscriber 190 | [ RUN ] Subscriber.fuzz_subscriber 191 | [ OK ] Subscriber.fuzz_subscriber (5010 ms) 192 | [----------] 1 test from Subscriber (5010 ms total) 193 | 194 | [----------] Global test environment tear-down 195 | [==========] 3 tests from 3 test cases ran. (15142 ms total) 196 | [ PASSED ] 3 tests. 197 | ``` 198 | 199 | ## Known issue 200 | * python not support headless message 201 | * Not verify with Windows and OS X environment and there may be some errors 202 | 203 | ## Security check 204 | If there is any security issue, it should be reported using process at https://01.org/security. 205 | -------------------------------------------------------------------------------- /conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # message_filters documentation build configuration file, created by 4 | # sphinx-quickstart on Mon Jun 1 14:21:53 2009. 5 | # 6 | # This file is execfile()d with the current directory set to its containing dir. 7 | # 8 | # Note that not all possible configuration values are present in this 9 | # autogenerated file. 10 | # 11 | # All configuration values have a default; values that are commented out 12 | # serve to show the default. 13 | 14 | import sys 15 | import os 16 | 17 | # If extensions (or modules to document with autodoc) are in another directory, 18 | # add these directories to sys.path here. If the directory is relative to the 19 | # documentation root, use os.path.abspath to make it absolute, like shown here. 20 | #sys.path.append(os.path.abspath('.')) 21 | 22 | # -- General configuration ----------------------------------------------------- 23 | 24 | # Add any Sphinx extension module names here, as strings. They can be extensions 25 | # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. 26 | extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] 27 | 28 | # Add any paths that contain templates here, relative to this directory. 29 | templates_path = ['_templates'] 30 | 31 | # The suffix of source filenames. 32 | source_suffix = '.rst' 33 | 34 | # The encoding of source files. 35 | #source_encoding = 'utf-8' 36 | 37 | # The master toctree document. 38 | master_doc = 'index' 39 | 40 | # General information about the project. 41 | project = u'message_filters' 42 | copyright = u'2009, Willow Garage, Inc.' 43 | 44 | # The version info for the project you're documenting, acts as replacement for 45 | # |version| and |release|, also used in various other places throughout the 46 | # built documents. 47 | # 48 | # The short X.Y version. 49 | version = '0.1' 50 | # The full version, including alpha/beta/rc tags. 51 | release = '0.1.0' 52 | 53 | # The language for content autogenerated by Sphinx. Refer to documentation 54 | # for a list of supported languages. 55 | #language = None 56 | 57 | # There are two options for replacing |today|: either, you set today to some 58 | # non-false value, then it is used: 59 | #today = '' 60 | # Else, today_fmt is used as the format for a strftime call. 61 | #today_fmt = '%B %d, %Y' 62 | 63 | # List of documents that shouldn't be included in the build. 64 | #unused_docs = [] 65 | 66 | exclude_patterns = ['CHANGELOG.rst'] 67 | 68 | # List of directories, relative to source directory, that shouldn't be searched 69 | # for source files. 70 | exclude_trees = ['_build'] 71 | 72 | # The reST default role (used for this markup: `text`) to use for all documents. 73 | #default_role = None 74 | 75 | # If true, '()' will be appended to :func: etc. cross-reference text. 76 | #add_function_parentheses = True 77 | 78 | # If true, the current module name will be prepended to all description 79 | # unit titles (such as .. function::). 80 | #add_module_names = True 81 | 82 | # If true, sectionauthor and moduleauthor directives will be shown in the 83 | # output. They are ignored by default. 84 | #show_authors = False 85 | 86 | # The name of the Pygments (syntax highlighting) style to use. 87 | pygments_style = 'sphinx' 88 | 89 | # A list of ignored prefixes for module index sorting. 90 | #modindex_common_prefix = [] 91 | 92 | 93 | # -- Options for HTML output --------------------------------------------------- 94 | 95 | # The theme to use for HTML and HTML Help pages. Major themes that come with 96 | # Sphinx are currently 'default' and 'sphinxdoc'. 97 | html_theme = 'default' 98 | 99 | # Theme options are theme-specific and customize the look and feel of a theme 100 | # further. For a list of options available for each theme, see the 101 | # documentation. 102 | #html_theme_options = {} 103 | 104 | # Add any paths that contain custom themes here, relative to this directory. 105 | #html_theme_path = [] 106 | 107 | # The name for this set of Sphinx documents. If None, it defaults to 108 | # " v documentation". 109 | #html_title = None 110 | 111 | # A shorter title for the navigation bar. Default is the same as html_title. 112 | #html_short_title = None 113 | 114 | # The name of an image file (relative to this directory) to place at the top 115 | # of the sidebar. 116 | #html_logo = None 117 | 118 | # The name of an image file (within the static path) to use as favicon of the 119 | # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 120 | # pixels large. 121 | #html_favicon = None 122 | 123 | # Add any paths that contain custom static files (such as style sheets) here, 124 | # relative to this directory. They are copied after the builtin static files, 125 | # so a file named "default.css" will overwrite the builtin "default.css". 126 | html_static_path = [] 127 | 128 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 129 | # using the given strftime format. 130 | #html_last_updated_fmt = '%b %d, %Y' 131 | 132 | # If true, SmartyPants will be used to convert quotes and dashes to 133 | # typographically correct entities. 134 | #html_use_smartypants = True 135 | 136 | # Custom sidebar templates, maps document names to template names. 137 | #html_sidebars = {} 138 | 139 | # Additional templates that should be rendered to pages, maps page names to 140 | # template names. 141 | #html_additional_pages = {} 142 | 143 | # If false, no module index is generated. 144 | #html_use_modindex = True 145 | 146 | # If false, no index is generated. 147 | #html_use_index = True 148 | 149 | # If true, the index is split into individual pages for each letter. 150 | #html_split_index = False 151 | 152 | # If true, links to the reST sources are added to the pages. 153 | #html_show_sourcelink = True 154 | 155 | # If true, an OpenSearch description file will be output, and all pages will 156 | # contain a tag referring to it. The value of this option must be the 157 | # base URL from which the finished HTML is served. 158 | #html_use_opensearch = '' 159 | 160 | # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). 161 | #html_file_suffix = '' 162 | 163 | # Output file base name for HTML help builder. 164 | htmlhelp_basename = 'tfdoc' 165 | 166 | 167 | # -- Options for LaTeX output -------------------------------------------------- 168 | 169 | # The paper size ('letter' or 'a4'). 170 | #latex_paper_size = 'letter' 171 | 172 | # The font size ('10pt', '11pt' or '12pt'). 173 | #latex_font_size = '10pt' 174 | 175 | # Grouping the document tree into LaTeX files. List of tuples 176 | # (source start file, target name, title, author, documentclass [howto/manual]). 177 | latex_documents = [ 178 | ('index', 'message_filters.tex', u'stereo\\_utils Documentation', 179 | u'James Bowman', 'manual'), 180 | ] 181 | 182 | # The name of an image file (relative to this directory) to place at the top of 183 | # the title page. 184 | #latex_logo = None 185 | 186 | # For "manual" documents, if this is true, then toplevel headings are parts, 187 | # not chapters. 188 | #latex_use_parts = False 189 | 190 | # Additional stuff for the LaTeX preamble. 191 | #latex_preamble = '' 192 | 193 | # Documents to append as an appendix to all manuals. 194 | #latex_appendices = [] 195 | 196 | # If false, no module index is generated. 197 | #latex_use_modindex = True 198 | 199 | # Example configuration for intersphinx: refer to the Python standard library. 200 | intersphinx_mapping = { 201 | 'http://docs.python.org/': None, 202 | 'http://docs.scipy.org/doc/numpy' : None 203 | } 204 | -------------------------------------------------------------------------------- /include/message_filters/cache.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_CACHE_H_ 36 | #define MESSAGE_FILTERS_CACHE_H_ 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include "connection.h" 45 | #include "simple_filter.h" 46 | #include "message_traits.h" 47 | 48 | namespace message_filters 49 | { 50 | using namespace std::placeholders; 51 | /** 52 | * \brief Stores a time history of messages 53 | * 54 | * Given a stream of messages, the most recent N messages are cached in a ring buffer, 55 | * from which time intervals of the cache can then be retrieved by the client. 56 | * 57 | * Cache immediately passes messages through to its output connections. 58 | * 59 | * \section connections CONNECTIONS 60 | * 61 | * Cache's input and output connections are both of the same signature as roscpp subscription callbacks, ie. 62 | \verbatim 63 | void callback(const std::shared_ptr&); 64 | \endverbatim 65 | */ 66 | template 67 | class Cache : public SimpleFilter 68 | { 69 | public: 70 | typedef std::shared_ptr MConstPtr; 71 | typedef MessageEvent EventType; 72 | 73 | template 74 | Cache(F& f, unsigned int cache_size = 1) 75 | { 76 | setCacheSize(cache_size) ; 77 | connectInput(f) ; 78 | } 79 | 80 | /** 81 | * Initializes a Message Cache without specifying a parent filter. This implies that in 82 | * order to populate the cache, the user then has to call add themselves, or connectInput() is 83 | * called later 84 | */ 85 | Cache(unsigned int cache_size = 1) 86 | { 87 | setCacheSize(cache_size); 88 | } 89 | 90 | template 91 | void connectInput(F& f) 92 | { 93 | incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback(std::bind(&Cache::callback, this, _1))); 94 | } 95 | 96 | ~Cache() 97 | { 98 | incoming_connection_.disconnect(); 99 | } 100 | 101 | /** 102 | * Set the size of the cache. 103 | * \param cache_size The new size the cache should be. Must be > 0 104 | */ 105 | void setCacheSize(unsigned int cache_size) 106 | { 107 | if (cache_size == 0) 108 | { 109 | //ROS_ERROR("Cannot set max_size to 0") ; 110 | return ; 111 | } 112 | 113 | cache_size_ = cache_size ; 114 | } 115 | 116 | /** 117 | * \brief Add a message to the cache, and pop off any elements that are too old. 118 | * This method is registered with a data provider when connectTo is called. 119 | */ 120 | void add(const MConstPtr& msg) 121 | { 122 | add(EventType(msg)); 123 | } 124 | 125 | /** 126 | * \brief Add a message to the cache, and pop off any elements that are too old. 127 | * This method is registered with a data provider when connectTo is called. 128 | */ 129 | void add(const EventType& evt) 130 | { 131 | // namespace mt = ros::message_traits; 132 | namespace mt = message_filters::message_traits; 133 | 134 | //printf(" Cache Size: %u\n", cache_.size()) ; 135 | { 136 | std::lock_guard lock(cache_lock_); 137 | 138 | while (cache_.size() >= cache_size_) // Keep popping off old data until we have space for a new msg 139 | cache_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it 140 | 141 | // No longer naively pushing msgs to back. Want to make sure they're sorted correctly 142 | //cache_.push_back(msg) ; // Add the newest message to the back of the deque 143 | 144 | typename std::deque::reverse_iterator rev_it = cache_.rbegin(); 145 | 146 | // Keep walking backwards along deque until we hit the beginning, 147 | // or until we find a timestamp that's smaller than (or equal to) msg's timestamp 148 | rclcpp::Time evt_stamp = mt::TimeStamp::value(*evt.getMessage()); 149 | while(rev_it != cache_.rend() && mt::TimeStamp::value(*(*rev_it).getMessage()) > evt_stamp) 150 | rev_it++; 151 | 152 | // Add msg to the cache 153 | cache_.insert(rev_it.base(), evt); 154 | 155 | } 156 | 157 | this->signalMessage(evt); 158 | } 159 | 160 | /** 161 | * \brief Receive a vector of messages that occur between a start and end time (inclusive). 162 | * 163 | * This call is non-blocking, and only aggregates messages it has already received. 164 | * It will not wait for messages have not yet been received, but occur in the interval. 165 | * \param start The start of the requested interval 166 | * \param end The end of the requested interval 167 | */ 168 | std::vector getInterval(const rclcpp::Time& start, const rclcpp::Time& end) const 169 | { 170 | namespace mt = message_filters::message_traits; 171 | std::lock_guard lock(cache_lock_); 172 | 173 | // Find the starting index. (Find the first index after [or at] the start of the interval) 174 | unsigned int start_index = 0 ; 175 | while(start_index < cache_.size() && 176 | mt::TimeStamp::value(*cache_[start_index].getMessage()) < start) 177 | { 178 | start_index++ ; 179 | } 180 | 181 | // Find the ending index. (Find the first index after the end of interval) 182 | unsigned int end_index = start_index ; 183 | while(end_index < cache_.size() && 184 | mt::TimeStamp::value(*cache_[end_index].getMessage()) <= end) 185 | { 186 | end_index++ ; 187 | } 188 | 189 | std::vector interval_elems ; 190 | interval_elems.reserve(end_index - start_index) ; 191 | for (unsigned int i=start_index; i getSurroundingInterval(const rclcpp::Time& start, const rclcpp::Time& end) const 207 | { 208 | namespace mt = message_filters::message_traits; 209 | 210 | std::lock_guard lock(cache_lock_); 211 | // Find the starting index. (Find the first index after [or at] the start of the interval) 212 | unsigned int start_index = cache_.size()-1; 213 | while(start_index > 0 && 214 | mt::TimeStamp::value(*cache_[start_index].getMessage()) > start) 215 | { 216 | start_index--; 217 | } 218 | unsigned int end_index = start_index; 219 | while(end_index < cache_.size()-1 && 220 | mt::TimeStamp::value(*cache_[end_index].getMessage()) < end) 221 | { 222 | end_index++; 223 | } 224 | 225 | std::vector interval_elems; 226 | interval_elems.reserve(end_index - start_index + 1) ; 227 | for (unsigned int i=start_index; i<=end_index; i++) 228 | { 229 | interval_elems.push_back(cache_[i].getMessage()) ; 230 | } 231 | 232 | return interval_elems; 233 | } 234 | 235 | /** 236 | * \brief Grab the newest element that occurs right before the specified time. 237 | * \param time Time that must occur right after the returned elem 238 | * \returns shared_ptr to the newest elem that occurs before 'time'. NULL if doesn't exist 239 | */ 240 | MConstPtr getElemBeforeTime(const rclcpp::Time& time) const 241 | { 242 | namespace mt = message_filters::message_traits; 243 | 244 | std::lock_guard lock(cache_lock_); 245 | 246 | MConstPtr out ; 247 | 248 | unsigned int i=0 ; 249 | int elem_index = -1 ; 250 | while (i::value(*cache_[i].getMessage()) < time) 252 | { 253 | elem_index = i ; 254 | i++ ; 255 | } 256 | 257 | if (elem_index >= 0) 258 | out = cache_[elem_index].getMessage() ; 259 | 260 | return out ; 261 | } 262 | 263 | /** 264 | * \brief Grab the oldest element that occurs right after the specified time. 265 | * \param time Time that must occur right before the returned elem 266 | * \returns shared_ptr to the oldest elem that occurs after 'time'. NULL if doesn't exist 267 | */ 268 | MConstPtr getElemAfterTime(const rclcpp::Time& time) const 269 | { 270 | namespace mt = message_filters::message_traits; 271 | 272 | std::lock_guard lock(cache_lock_); 273 | 274 | MConstPtr out ; 275 | 276 | int i=cache_.size()-1 ; 277 | int elem_index = -1 ; 278 | while (i>=0 && 279 | mt::TimeStamp::value(*cache_[i].getMessage()) > time) 280 | { 281 | elem_index = i ; 282 | i-- ; 283 | } 284 | 285 | if (elem_index >= 0) 286 | out = cache_[elem_index].getMessage() ; 287 | else 288 | out.reset() ; 289 | 290 | return out ; 291 | } 292 | 293 | /** 294 | * \brief Returns the timestamp associated with the newest packet cache 295 | */ 296 | rclcpp::Time getLatestTime() const 297 | { 298 | namespace mt = message_filters::message_traits; 299 | 300 | std::lock_guard lock(cache_lock_); 301 | 302 | rclcpp::Time latest_time; 303 | 304 | if (cache_.size() > 0) 305 | latest_time = mt::TimeStamp::value(*cache_.back().getMessage()); 306 | 307 | return latest_time ; 308 | } 309 | 310 | /** 311 | * \brief Returns the timestamp associated with the oldest packet cache 312 | */ 313 | rclcpp::Time getOldestTime() const 314 | { 315 | namespace mt = message_filters::message_traits; 316 | 317 | std::lock_guard lock(cache_lock_); 318 | 319 | rclcpp::Time oldest_time; 320 | 321 | if (cache_.size() > 0) 322 | oldest_time = mt::TimeStamp::value(*cache_.front().getMessage()); 323 | 324 | return oldest_time ; 325 | } 326 | 327 | 328 | private: 329 | void callback(const EventType& evt) 330 | { 331 | add(evt); 332 | } 333 | 334 | mutable std::mutex cache_lock_ ; //!< Lock for cache_ 335 | std::deque cache_ ; //!< Cache for the messages 336 | unsigned int cache_size_ ; //!< Maximum number of elements allowed in the cache. 337 | 338 | Connection incoming_connection_; 339 | }; 340 | 341 | } 342 | 343 | 344 | #endif /* MESSAGE_FILTERS_CACHE_H_ */ 345 | -------------------------------------------------------------------------------- /include/message_filters/chain.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_CHAIN_H 36 | #define MESSAGE_FILTERS_CHAIN_H 37 | 38 | #include "simple_filter.h" 39 | #include "pass_through.h" 40 | 41 | #include 42 | 43 | namespace message_filters 44 | { 45 | using namespace std::placeholders; 46 | /** 47 | * \brief Base class for Chain, allows you to store multiple chains in the same container. Provides filter retrieval 48 | * by index. 49 | */ 50 | class ChainBase 51 | { 52 | public: 53 | virtual ~ChainBase() {} 54 | 55 | /** 56 | * \brief Retrieve a filter from this chain by index. Returns an empty shared_ptr if the index is greater than 57 | * the size of the chain. \b NOT type-safe 58 | * 59 | * \param F [template] The type of the filter 60 | * \param index The index of the filter (returned by addFilter()) 61 | */ 62 | template 63 | std::shared_ptr getFilter(size_t index) const 64 | { 65 | std::shared_ptr filter = getFilterForIndex(index); 66 | if (filter) 67 | { 68 | return std::static_pointer_cast(filter); 69 | } 70 | 71 | return std::shared_ptr(); 72 | } 73 | 74 | protected: 75 | virtual std::shared_ptr getFilterForIndex(size_t index) const = 0; 76 | }; 77 | typedef std::shared_ptr ChainBasePtr; 78 | 79 | /** 80 | * \brief Chains a dynamic number of simple filters together. Allows retrieval of filters by index after they are added. 81 | * 82 | * The Chain filter provides a container for simple filters. It allows you to store an N-long set of filters inside a single 83 | * structure, making it much easier to manage them. 84 | * 85 | * Adding filters to the chain is done by adding shared_ptrs of them to the filter. They are automatically connected to each other 86 | * and the output of the last filter in the chain is forwarded to the callback you've registered with Chain::registerCallback 87 | * 88 | * Example: 89 | \verbatim 90 | void myCallback(const MsgConstPtr& msg) 91 | { 92 | } 93 | 94 | Chain c; 95 | c.addFilter(std::make_shared >()); 96 | c.addFilter(std::make_shared >()); 97 | c.registerCallback(myCallback); 98 | \endverbatim 99 | 100 | * 101 | * It is also possible to pass bare pointers in, which will not be automatically deleted when Chain is destructed: 102 | \verbatim 103 | Chain c; 104 | PassThrough p; 105 | c.addFilter(&p); 106 | c.registerCallback(myCallback); 107 | \endverbatim 108 | * 109 | */ 110 | template 111 | class Chain : public ChainBase, public SimpleFilter 112 | { 113 | public: 114 | typedef std::shared_ptr MConstPtr; 115 | typedef MessageEvent EventType; 116 | 117 | /** 118 | * \brief Default constructor 119 | */ 120 | Chain() 121 | { 122 | } 123 | 124 | /** 125 | * \brief Constructor with filter. Calls connectInput(f) 126 | */ 127 | template 128 | Chain(F& f) 129 | { 130 | connectInput(f); 131 | } 132 | 133 | struct NullDeleter 134 | { 135 | void operator()(void const*) const 136 | { 137 | } 138 | }; 139 | 140 | /** 141 | * \brief Add a filter to this chain, by bare pointer. Returns the index of that filter in the chain. 142 | */ 143 | template 144 | size_t addFilter(F* filter) 145 | { 146 | std::shared_ptr ptr(filter, NullDeleter()); 147 | return addFilter(ptr); 148 | } 149 | 150 | /** 151 | * \brief Add a filter to this chain, by shared_ptr. Returns the index of that filter in the chain 152 | */ 153 | template 154 | size_t addFilter(const std::shared_ptr& filter) 155 | { 156 | FilterInfo info; 157 | info.add_func = std::bind((void(F::*)(const EventType&))&F::add, filter.get(), _1); 158 | info.filter = filter; 159 | info.passthrough = std::make_shared >(); 160 | 161 | last_filter_connection_.disconnect(); 162 | info.passthrough->connectInput(*filter); 163 | last_filter_connection_ = info.passthrough->registerCallback(typename SimpleFilter::EventCallback(std::bind(&Chain::lastFilterCB, this, _1))); 164 | if (!filters_.empty()) 165 | { 166 | filter->connectInput(*filters_.back().passthrough); 167 | } 168 | 169 | uint32_t count = filters_.size(); 170 | filters_.push_back(info); 171 | return count; 172 | } 173 | 174 | /** 175 | * \brief Retrieve a filter from this chain by index. Returns an empty shared_ptr if the index is greater than 176 | * the size of the chain. \b NOT type-safe 177 | * 178 | * \param F [template] The type of the filter 179 | * \param index The index of the filter (returned by addFilter()) 180 | */ 181 | template 182 | std::shared_ptr getFilter(size_t index) const 183 | { 184 | if (index >= filters_.size()) 185 | { 186 | return std::shared_ptr(); 187 | } 188 | 189 | return std::static_pointer_cast(filters_[index].filter); 190 | } 191 | 192 | /** 193 | * \brief Connect this filter's input to another filter's output. 194 | */ 195 | template 196 | void connectInput(F& f) 197 | { 198 | incoming_connection_.disconnect(); 199 | incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback(std::bind(&Chain::incomingCB, this, _1))); 200 | } 201 | 202 | /** 203 | * \brief Add a message to the start of this chain 204 | */ 205 | void add(const MConstPtr& msg) 206 | { 207 | add(EventType(msg)); 208 | } 209 | 210 | void add(const EventType& evt) 211 | { 212 | if (!filters_.empty()) 213 | { 214 | filters_[0].add_func(evt); 215 | } 216 | } 217 | 218 | protected: 219 | virtual std::shared_ptr getFilterForIndex(size_t index) const 220 | { 221 | if (index >= filters_.size()) 222 | { 223 | return std::shared_ptr(); 224 | } 225 | 226 | return filters_[index].filter; 227 | } 228 | 229 | private: 230 | void incomingCB(const EventType& evt) 231 | { 232 | add(evt); 233 | } 234 | 235 | void lastFilterCB(const EventType& evt) 236 | { 237 | this->signalMessage(evt); 238 | } 239 | 240 | struct FilterInfo 241 | { 242 | std::function add_func; 243 | std::shared_ptr filter; 244 | std::shared_ptr > passthrough; 245 | }; 246 | typedef std::vector V_FilterInfo; 247 | 248 | V_FilterInfo filters_; 249 | 250 | Connection incoming_connection_; 251 | Connection last_filter_connection_; 252 | }; 253 | } 254 | 255 | #endif // MESSAGE_FILTERS_CHAIN_H 256 | -------------------------------------------------------------------------------- /include/message_filters/connection.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_CONNECTION_H 36 | #define MESSAGE_FILTERS_CONNECTION_H 37 | 38 | #include 39 | #include 40 | #include "macros.h" 41 | 42 | namespace message_filters 43 | { 44 | 45 | class noncopyable 46 | { 47 | protected: 48 | noncopyable() {} 49 | ~noncopyable() {} 50 | noncopyable( const noncopyable& ) = delete; 51 | noncopyable& operator=( const noncopyable& ) = delete; 52 | }; 53 | 54 | /** 55 | * \brief Encapsulates a connection from one filter to another (or to a user-specified callback) 56 | */ 57 | class MESSAGE_FILTERS_DECL Connection 58 | { 59 | public: 60 | typedef std::function VoidDisconnectFunction; 61 | typedef std::function WithConnectionDisconnectFunction; 62 | Connection() {} 63 | Connection(const VoidDisconnectFunction& func); 64 | 65 | /** 66 | * \brief disconnects this connection 67 | */ 68 | void disconnect(); 69 | 70 | private: 71 | VoidDisconnectFunction void_disconnect_; 72 | WithConnectionDisconnectFunction connection_disconnect_; 73 | }; 74 | 75 | } 76 | 77 | #endif // MESSAGE_FILTERS_CONNECTION_H 78 | -------------------------------------------------------------------------------- /include/message_filters/macros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2008, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef MESSAGE_FILTERS_MACROS_H_ 29 | #define MESSAGE_FILTERS_MACROS_H_ 30 | 31 | #include 32 | // Import/export for windows dll's and visibility for gcc shared libraries. 33 | 34 | #ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries 35 | #ifdef message_filters_EXPORTS // we are building a shared lib/dll 36 | #define MESSAGE_FILTERS_DECL RCLCPP_EXPORT 37 | #else // we are using shared lib/dll 38 | #define MESSAGE_FILTERS_DECL RCLCPP_IMPORT 39 | #endif 40 | #else // ros is being built around static libraries 41 | #define MESSAGE_FILTERS_DECL 42 | #endif 43 | 44 | #endif /* MESSAGE_FILTERS_MACROS_H_ */ 45 | -------------------------------------------------------------------------------- /include/message_filters/message_event.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * Copyright (C) 2010, Willow Garage, Inc. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright notice, 8 | * this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its 13 | * contributors may be used to endorse or promote products derived from 14 | * this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #ifndef RCLCPP_MESSAGE_EVENT_H 30 | #define RCLCPP_MESSAGE_EVENT_H 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace message_filters 37 | { 38 | typedef std::map< std::string, std::string > M_string; 39 | typedef std::shared_ptr< M_string > M_stringPtr; 40 | 41 | template 42 | struct DefaultMessageCreator 43 | { 44 | std::shared_ptr operator()() 45 | { 46 | return std::make_shared(); 47 | } 48 | }; 49 | 50 | /* 51 | template 52 | ROS_DEPRECATED inline std::shared_ptr defaultMessageCreateFunction() 53 | { 54 | return DefaultMessageCreator()(); 55 | } 56 | */ 57 | /** 58 | * \brief Event type for subscriptions, const ros::MessageEvent& can be used in your callback instead of const std::shared_ptr& 59 | * 60 | * Useful if you need to retrieve meta-data about the message, such as the full connection header, or the publisher's node name 61 | */ 62 | template 63 | class MessageEvent 64 | { 65 | public: 66 | typedef typename std::add_const::type ConstMessage; 67 | typedef typename std::remove_const::type Message; 68 | typedef std::shared_ptr MessagePtr; 69 | typedef std::shared_ptr ConstMessagePtr; 70 | typedef std::function CreateFunction; 71 | 72 | MessageEvent() 73 | : nonconst_need_copy_(true) 74 | {} 75 | 76 | MessageEvent(const MessageEvent& rhs) 77 | { 78 | *this = rhs; 79 | } 80 | 81 | MessageEvent(const MessageEvent& rhs) 82 | { 83 | *this = rhs; 84 | } 85 | 86 | MessageEvent(const MessageEvent& rhs, bool nonconst_need_copy) 87 | { 88 | *this = rhs; 89 | nonconst_need_copy_ = nonconst_need_copy; 90 | } 91 | 92 | MessageEvent(const MessageEvent& rhs, bool nonconst_need_copy) 93 | { 94 | *this = rhs; 95 | nonconst_need_copy_ = nonconst_need_copy; 96 | } 97 | 98 | MessageEvent(const MessageEvent& rhs, const CreateFunction& create) 99 | { 100 | init(std::const_pointer_cast(std::static_pointer_cast(rhs.getMessage())), rhs.getConnectionHeaderPtr(), rhs.getReceiptTime(), rhs.nonConstWillCopy(), create); 101 | } 102 | 103 | /** 104 | * \todo Make this explicit in ROS 2.0. Keep as auto-converting for now to maintain backwards compatibility in some places (message_filters) 105 | */ 106 | MessageEvent(const ConstMessagePtr& message) 107 | { 108 | init(message, std::shared_ptr(), rclcpp::Clock().now(), true, message_filters::DefaultMessageCreator()); 109 | } 110 | 111 | MessageEvent(const ConstMessagePtr& message, const std::shared_ptr& connection_header, rclcpp::Time receipt_time) 112 | { 113 | init(message, connection_header, receipt_time, true, message_filters::DefaultMessageCreator()); 114 | } 115 | 116 | MessageEvent(const ConstMessagePtr& message, rclcpp::Time receipt_time) 117 | { 118 | init(message, std::shared_ptr(), receipt_time, true, message_filters::DefaultMessageCreator()); 119 | } 120 | 121 | MessageEvent(const ConstMessagePtr& message, const std::shared_ptr& connection_header, rclcpp::Time receipt_time, bool nonconst_need_copy, const CreateFunction& create) 122 | { 123 | init(message, connection_header, receipt_time, nonconst_need_copy, create); 124 | } 125 | 126 | void init(const ConstMessagePtr& message, const std::shared_ptr& connection_header, rclcpp::Time receipt_time, bool nonconst_need_copy, const CreateFunction& create) 127 | { 128 | message_ = message; 129 | connection_header_ = connection_header; 130 | receipt_time_ = receipt_time; 131 | nonconst_need_copy_ = nonconst_need_copy; 132 | create_ = create; 133 | } 134 | 135 | void operator=(const MessageEvent& rhs) 136 | { 137 | init(std::static_pointer_cast(rhs.getMessage()), rhs.getConnectionHeaderPtr(), rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); 138 | message_copy_.reset(); 139 | } 140 | 141 | void operator=(const MessageEvent& rhs) 142 | { 143 | init(std::const_pointer_cast(std::static_pointer_cast(rhs.getMessage())), rhs.getConnectionHeaderPtr(), rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); 144 | message_copy_.reset(); 145 | } 146 | 147 | /** 148 | * \brief Retrieve the message. If M is const, this returns a reference to it. If M is non const 149 | * and this event requires it, returns a copy. Note that it caches this copy for later use, so it will 150 | * only every make the copy once 151 | */ 152 | std::shared_ptr getMessage() const 153 | { 154 | return copyMessageIfNecessary(); 155 | } 156 | 157 | /** 158 | * \brief Retrieve a const version of the message 159 | */ 160 | const std::shared_ptr& getConstMessage() const { return message_; } 161 | /** 162 | * \brief Retrieve the connection header 163 | */ 164 | M_string& getConnectionHeader() const { return *connection_header_; } 165 | const std::shared_ptr& getConnectionHeaderPtr() const { return connection_header_; } 166 | 167 | /** 168 | * \brief Returns the name of the node which published this message 169 | */ 170 | const std::string& getPublisherName() const { return connection_header_ ? (*connection_header_)["callerid"] : s_unknown_publisher_string_; } 171 | 172 | /** 173 | * \brief Returns the time at which this message was received 174 | */ 175 | rclcpp::Time getReceiptTime() const { return receipt_time_; } 176 | 177 | bool nonConstWillCopy() const { return nonconst_need_copy_; } 178 | bool getMessageWillCopy() const { return !std::is_const::value && nonconst_need_copy_; } 179 | 180 | bool operator<(const MessageEvent& rhs) 181 | { 182 | if (message_ != rhs.message_) 183 | { 184 | return message_ < rhs.message_; 185 | } 186 | 187 | if (receipt_time_ != rhs.receipt_time_) 188 | { 189 | return receipt_time_ < rhs.receipt_time_; 190 | } 191 | 192 | return nonconst_need_copy_ < rhs.nonconst_need_copy_; 193 | } 194 | 195 | bool operator==(const MessageEvent& rhs) 196 | { 197 | return message_ == rhs.message_ && receipt_time_ == rhs.receipt_time_ && nonconst_need_copy_ == rhs.nonconst_need_copy_; 198 | } 199 | 200 | bool operator!=(const MessageEvent& rhs) 201 | { 202 | return !(*this == rhs); 203 | } 204 | 205 | const CreateFunction& getMessageFactory() const { return create_; } 206 | 207 | private: 208 | template 209 | typename std::enable_if::value, std::shared_ptr >::type copyMessageIfNecessary() const 210 | { 211 | if (std::is_const::value || !nonconst_need_copy_) 212 | { 213 | return std::const_pointer_cast(message_); 214 | } 215 | 216 | if (message_copy_) 217 | { 218 | return message_copy_; 219 | } 220 | 221 | assert(create_); 222 | message_copy_ = create_(); 223 | *message_copy_ = *message_; 224 | 225 | return message_copy_; 226 | } 227 | 228 | template 229 | typename std::enable_if::value, std::shared_ptr >::type copyMessageIfNecessary() const 230 | { 231 | return std::const_pointer_cast(message_); 232 | } 233 | 234 | ConstMessagePtr message_; 235 | // Kind of ugly to make this mutable, but it means we can pass a const MessageEvent to a callback and not worry about other things being modified 236 | mutable MessagePtr message_copy_; 237 | std::shared_ptr connection_header_; 238 | rclcpp::Time receipt_time_; 239 | bool nonconst_need_copy_; 240 | CreateFunction create_; 241 | 242 | static const std::string s_unknown_publisher_string_; 243 | }; 244 | 245 | template const std::string MessageEvent::s_unknown_publisher_string_("unknown_publisher"); 246 | 247 | 248 | } 249 | 250 | #endif // RCLCPP_MESSAGE_EVENT_H 251 | -------------------------------------------------------------------------------- /include/message_filters/message_traits.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef ROSLIB_MESSAGE_TRAITS_H 29 | #define ROSLIB_MESSAGE_TRAITS_H 30 | 31 | #include 32 | #include 33 | 34 | namespace message_filters 35 | { 36 | namespace message_traits 37 | { 38 | 39 | /** 40 | * \brief HasHeader informs whether or not there is a header that gets serialized as the first thing in the message 41 | */ 42 | template struct HasHeader : public std::false_type {}; 43 | 44 | template 45 | struct HasHeader : std::true_type {}; 46 | 47 | /** 48 | * \brief TimeStamp trait. In the default implementation pointer() 49 | * returns &m.header.stamp if HasHeader::value is true, otherwise returns NULL. value() 50 | * does not exist, and causes a compile error 51 | */ 52 | template 53 | struct TimeStamp 54 | { 55 | static rclcpp::Time value(const M& m) { 56 | (void)m; 57 | return rclcpp::Time(); 58 | } 59 | }; 60 | 61 | template 62 | struct TimeStamp::value>::type > 63 | { 64 | static rclcpp::Time value(const M& m) { 65 | auto stamp = m.header.stamp; 66 | return rclcpp::Time(stamp.sec, stamp.nanosec); 67 | } 68 | }; 69 | 70 | } // namespace message_traits 71 | } // namespace message_filters 72 | 73 | #endif // ROSLIB_MESSAGE_TRAITS_H 74 | -------------------------------------------------------------------------------- /include/message_filters/null_types.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_NULL_TYPES_H 36 | #define MESSAGE_FILTERS_NULL_TYPES_H 37 | 38 | #include "connection.h" 39 | 40 | #include "message_filters/message_traits.h" 41 | #include 42 | #include 43 | 44 | namespace message_filters 45 | { 46 | struct MsgHeader 47 | { 48 | rclcpp::Time stamp; 49 | }; 50 | 51 | 52 | struct NullType 53 | { 54 | static MsgHeader header; 55 | }; 56 | 57 | typedef std::shared_ptr NullTypeConstPtr; 58 | template 59 | struct NullFilter 60 | { 61 | template 62 | Connection registerCallback(const C&) 63 | { 64 | return Connection(); 65 | } 66 | 67 | template 68 | Connection registerCallback(const std::function&) 69 | { 70 | return Connection(); 71 | } 72 | }; 73 | 74 | namespace message_traits 75 | { 76 | template<> 77 | struct TimeStamp 78 | { 79 | static rclcpp::Time value(const message_filters::NullType&) 80 | { 81 | return rclcpp::Time(); 82 | } 83 | }; 84 | } 85 | } 86 | 87 | #endif // MESSAGE_FILTERS_NULL_TYPES_H 88 | -------------------------------------------------------------------------------- /include/message_filters/parameter_adapter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2010, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef ROSCPP_PARAMETER_ADAPTER_H 29 | #define ROSCPP_PARAMETER_ADAPTER_H 30 | 31 | #include "message_event.h" 32 | #include 33 | #include 34 | 35 | namespace message_filters 36 | { 37 | 38 | /** 39 | * \brief Generally not for outside use. Adapts a function parameter type into the message type, event type and parameter. Allows you to 40 | * retrieve a parameter type from an event type. 41 | * 42 | * ParameterAdapter is generally only useful for outside use when implementing things that require message callbacks 43 | * (such as the message_filters package)and you would like to support all the roscpp message parameter types 44 | * 45 | * The ParameterAdapter is templated on the callback parameter type (\b not the bare message type), and provides 3 things: 46 | * - Message typedef, which provides the bare message type, no const or reference qualifiers 47 | * - Event typedef, which provides the ros::MessageEvent type 48 | * - Parameter typedef, which provides the actual parameter type (may be slightly different from M) 49 | * - static getParameter(event) function, which returns a parameter type given the event 50 | * - static bool is_const informs you whether or not the parameter type is a const message 51 | * 52 | * ParameterAdapter is specialized to allow callbacks of any of the forms: 53 | \verbatim 54 | void callback(const std::shared_ptr&); 55 | void callback(const std::shared_ptr&); 56 | void callback(std::shared_ptr); 57 | void callback(std::shared_ptr); 58 | void callback(const M&); 59 | void callback(M); 60 | void callback(const MessageEvent&); 61 | void callback(const MessageEvent&); 62 | \endverbatim 63 | */ 64 | template 65 | struct ParameterAdapter 66 | { 67 | typedef typename std::remove_reference::type>::type Message; 68 | typedef MessageEvent Event; 69 | typedef M Parameter; 70 | static const bool is_const = true; 71 | 72 | static Parameter getParameter(const Event& event) 73 | { 74 | return *event.getMessage(); 75 | } 76 | }; 77 | //struct message_filters::ParameterAdapter&> 78 | template 79 | struct ParameterAdapter& > 80 | { 81 | typedef typename std::remove_reference::type>::type Message; 82 | typedef MessageEvent Event; 83 | typedef const std::shared_ptr Parameter; 84 | static const bool is_const = true; 85 | 86 | static Parameter getParameter(const Event& event) 87 | { 88 | return event.getMessage(); 89 | } 90 | }; 91 | 92 | template 93 | struct ParameterAdapter& > 94 | { 95 | typedef typename std::remove_reference::type>::type Message; 96 | typedef MessageEvent Event; 97 | typedef std::shared_ptr Parameter; 98 | static const bool is_const = false; 99 | 100 | static Parameter getParameter(const Event& event) 101 | { 102 | return MessageEvent(event).getMessage(); 103 | } 104 | }; 105 | 106 | template 107 | struct ParameterAdapter 108 | { 109 | typedef typename std::remove_reference::type>::type Message; 110 | typedef MessageEvent Event; 111 | typedef const M& Parameter; 112 | static const bool is_const = true; 113 | 114 | static Parameter getParameter(const Event& event) 115 | { 116 | return *event.getMessage(); 117 | } 118 | }; 119 | 120 | template 121 | struct ParameterAdapter > 122 | { 123 | typedef typename std::remove_reference::type>::type Message; 124 | typedef MessageEvent Event; 125 | typedef std::shared_ptr Parameter; 126 | static const bool is_const = true; 127 | 128 | static Parameter getParameter(const Event& event) 129 | { 130 | return event.getMessage(); 131 | } 132 | }; 133 | 134 | template 135 | struct ParameterAdapter > 136 | { 137 | typedef typename std::remove_reference::type>::type Message; 138 | typedef MessageEvent Event; 139 | typedef std::shared_ptr Parameter; 140 | static const bool is_const = false; 141 | 142 | static Parameter getParameter(const Event& event) 143 | { 144 | return MessageEvent(event).getMessage(); 145 | } 146 | }; 147 | 148 | template 149 | struct ParameterAdapter& > 150 | { 151 | typedef typename std::remove_reference::type>::type Message; 152 | typedef MessageEvent Event; 153 | typedef const MessageEvent& Parameter; 154 | static const bool is_const = true; 155 | 156 | static Parameter getParameter(const Event& event) 157 | { 158 | return event; 159 | } 160 | }; 161 | 162 | template 163 | struct ParameterAdapter& > 164 | { 165 | typedef typename std::remove_reference::type>::type Message; 166 | typedef MessageEvent Event; 167 | typedef MessageEvent Parameter; 168 | static const bool is_const = false; 169 | 170 | static Parameter getParameter(const Event& event) 171 | { 172 | return MessageEvent(event); 173 | } 174 | }; 175 | 176 | } 177 | 178 | #endif // ROSCPP_PARAMETER_ADAPTER_H 179 | -------------------------------------------------------------------------------- /include/message_filters/pass_through.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_PASSTHROUGH_H 36 | #define MESSAGE_FILTERS_PASSTHROUGH_H 37 | 38 | #include "simple_filter.h" 39 | 40 | #include 41 | 42 | namespace message_filters 43 | { 44 | /** 45 | * \brief Simple passthrough filter. What comes in goes out immediately. 46 | */ 47 | template 48 | class PassThrough : public SimpleFilter 49 | { 50 | public: 51 | typedef std::shared_ptr MConstPtr; 52 | typedef MessageEvent EventType; 53 | 54 | PassThrough() 55 | { 56 | } 57 | 58 | template 59 | PassThrough(F& f) 60 | { 61 | connectInput(f); 62 | } 63 | 64 | template 65 | void connectInput(F& f) 66 | { 67 | incoming_connection_.disconnect(); 68 | incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback(std::bind(&PassThrough::cb, this, _1))); 69 | } 70 | 71 | void add(const MConstPtr& msg) 72 | { 73 | add(EventType(msg)); 74 | } 75 | 76 | void add(const EventType& evt) 77 | { 78 | this->signalMessage(evt); 79 | } 80 | 81 | private: 82 | void cb(const EventType& evt) 83 | { 84 | add(evt); 85 | } 86 | 87 | Connection incoming_connection_; 88 | }; 89 | 90 | } // namespace message_filters 91 | 92 | #endif // MESSAGE_FILTERS_PASSTHROUGH_H 93 | 94 | -------------------------------------------------------------------------------- /include/message_filters/signal1.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_SIGNAL1_H 36 | #define MESSAGE_FILTERS_SIGNAL1_H 37 | 38 | #include 39 | #include "connection.h" 40 | #include "message_event.h" 41 | #include "parameter_adapter.h" 42 | 43 | #include 44 | 45 | namespace message_filters 46 | { 47 | template 48 | class CallbackHelper1 49 | { 50 | public: 51 | virtual ~CallbackHelper1() {} 52 | 53 | virtual void call(const MessageEvent& event, bool nonconst_need_copy) = 0; 54 | 55 | typedef std::shared_ptr > Ptr; 56 | }; 57 | 58 | template 59 | class CallbackHelper1T : public CallbackHelper1 60 | { 61 | public: 62 | typedef ParameterAdapter

Adapter; 63 | typedef std::function Callback; 64 | typedef typename Adapter::Event Event; 65 | 66 | CallbackHelper1T(const Callback& cb) 67 | : callback_(cb) 68 | { 69 | } 70 | 71 | virtual void call(const MessageEvent& event, bool nonconst_force_copy) 72 | { 73 | Event my_event(event, nonconst_force_copy || event.nonConstWillCopy()); 74 | callback_(Adapter::getParameter(my_event)); 75 | } 76 | 77 | private: 78 | Callback callback_; 79 | }; 80 | 81 | template 82 | class Signal1 83 | { 84 | typedef std::shared_ptr > CallbackHelper1Ptr; 85 | typedef std::vector V_CallbackHelper1; 86 | 87 | public: 88 | template 89 | CallbackHelper1Ptr addCallback(const std::function& callback) 90 | { 91 | CallbackHelper1T* helper = new CallbackHelper1T(callback); 92 | 93 | std::lock_guard lock(mutex_); 94 | callbacks_.push_back(CallbackHelper1Ptr(helper)); 95 | return callbacks_.back(); 96 | } 97 | 98 | void removeCallback(const CallbackHelper1Ptr& helper) 99 | { 100 | std::lock_guard lock(mutex_); 101 | typename V_CallbackHelper1::iterator it = std::find(callbacks_.begin(), callbacks_.end(), helper); 102 | if (it != callbacks_.end()) 103 | { 104 | callbacks_.erase(it); 105 | } 106 | } 107 | 108 | void call(const MessageEvent& event) 109 | { 110 | std::lock_guard lock(mutex_); 111 | bool nonconst_force_copy = callbacks_.size() > 1; 112 | typename V_CallbackHelper1::iterator it = callbacks_.begin(); 113 | typename V_CallbackHelper1::iterator end = callbacks_.end(); 114 | for (; it != end; ++it) 115 | { 116 | const CallbackHelper1Ptr& helper = *it; 117 | helper->call(event, nonconst_force_copy); 118 | } 119 | } 120 | 121 | private: 122 | std::mutex mutex_; 123 | V_CallbackHelper1 callbacks_; 124 | }; 125 | 126 | } // message_filters 127 | 128 | #endif // MESSAGE_FILTERS_SIGNAL1_H 129 | 130 | 131 | -------------------------------------------------------------------------------- /include/message_filters/signal9.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_SIGNAL9_H 36 | #define MESSAGE_FILTERS_SIGNAL9_H 37 | 38 | 39 | #include 40 | #include "connection.h" 41 | #include "null_types.h" 42 | #include "message_event.h" 43 | #include "parameter_adapter.h" 44 | 45 | namespace message_filters 46 | { 47 | using namespace std::placeholders; 48 | template 49 | class CallbackHelper9 50 | { 51 | public: 52 | typedef MessageEvent M0Event; 53 | typedef MessageEvent M1Event; 54 | typedef MessageEvent M2Event; 55 | typedef MessageEvent M3Event; 56 | typedef MessageEvent M4Event; 57 | typedef MessageEvent M5Event; 58 | typedef MessageEvent M6Event; 59 | typedef MessageEvent M7Event; 60 | typedef MessageEvent M8Event; 61 | 62 | virtual ~CallbackHelper9() {} 63 | 64 | virtual void call(bool nonconst_force_copy, const M0Event& e0, const M1Event& e1, const M2Event& e2, const M3Event& e3, 65 | const M4Event& e4, const M5Event& e5, const M6Event& e6, const M7Event& e7, const M8Event& e8) = 0; 66 | 67 | typedef std::shared_ptr Ptr; 68 | }; 69 | 70 | template 71 | class CallbackHelper9T : 72 | public CallbackHelper9::Message, 73 | typename ParameterAdapter::Message, 74 | typename ParameterAdapter::Message, 75 | typename ParameterAdapter::Message, 76 | typename ParameterAdapter::Message, 77 | typename ParameterAdapter::Message, 78 | typename ParameterAdapter::Message, 79 | typename ParameterAdapter::Message, 80 | typename ParameterAdapter::Message> 81 | { 82 | private: 83 | typedef ParameterAdapter A0; 84 | typedef ParameterAdapter A1; 85 | typedef ParameterAdapter A2; 86 | typedef ParameterAdapter A3; 87 | typedef ParameterAdapter A4; 88 | typedef ParameterAdapter A5; 89 | typedef ParameterAdapter A6; 90 | typedef ParameterAdapter A7; 91 | typedef ParameterAdapter A8; 92 | typedef typename A0::Event M0Event; 93 | typedef typename A1::Event M1Event; 94 | typedef typename A2::Event M2Event; 95 | typedef typename A3::Event M3Event; 96 | typedef typename A4::Event M4Event; 97 | typedef typename A5::Event M5Event; 98 | typedef typename A6::Event M6Event; 99 | typedef typename A7::Event M7Event; 100 | typedef typename A8::Event M8Event; 101 | 102 | public: 103 | typedef std::function Callback; 106 | 107 | CallbackHelper9T(const Callback& cb) 108 | : callback_(cb) 109 | { 110 | } 111 | 112 | virtual void call(bool nonconst_force_copy, const M0Event& e0, const M1Event& e1, const M2Event& e2, const M3Event& e3, 113 | const M4Event& e4, const M5Event& e5, const M6Event& e6, const M7Event& e7, const M8Event& e8) 114 | { 115 | M0Event my_e0(e0, nonconst_force_copy || e0.nonConstWillCopy()); 116 | M1Event my_e1(e1, nonconst_force_copy || e0.nonConstWillCopy()); 117 | M2Event my_e2(e2, nonconst_force_copy || e0.nonConstWillCopy()); 118 | M3Event my_e3(e3, nonconst_force_copy || e0.nonConstWillCopy()); 119 | M4Event my_e4(e4, nonconst_force_copy || e0.nonConstWillCopy()); 120 | M5Event my_e5(e5, nonconst_force_copy || e0.nonConstWillCopy()); 121 | M6Event my_e6(e6, nonconst_force_copy || e0.nonConstWillCopy()); 122 | M7Event my_e7(e7, nonconst_force_copy || e0.nonConstWillCopy()); 123 | M8Event my_e8(e8, nonconst_force_copy || e0.nonConstWillCopy()); 124 | callback_(A0::getParameter(e0), 125 | A1::getParameter(e1), 126 | A2::getParameter(e2), 127 | A3::getParameter(e3), 128 | A4::getParameter(e4), 129 | A5::getParameter(e5), 130 | A6::getParameter(e6), 131 | A7::getParameter(e7), 132 | A8::getParameter(e8)); 133 | } 134 | 135 | private: 136 | Callback callback_; 137 | }; 138 | 139 | template 140 | class Signal9 141 | { 142 | typedef std::shared_ptr > CallbackHelper9Ptr; 143 | typedef std::vector V_CallbackHelper9; 144 | 145 | public: 146 | typedef MessageEvent M0Event; 147 | typedef MessageEvent M1Event; 148 | typedef MessageEvent M2Event; 149 | typedef MessageEvent M3Event; 150 | typedef MessageEvent M4Event; 151 | typedef MessageEvent M5Event; 152 | typedef MessageEvent M6Event; 153 | typedef MessageEvent M7Event; 154 | typedef MessageEvent M8Event; 155 | typedef std::shared_ptr M0ConstPtr; 156 | typedef std::shared_ptr M1ConstPtr; 157 | typedef std::shared_ptr M2ConstPtr; 158 | typedef std::shared_ptr M3ConstPtr; 159 | typedef std::shared_ptr M4ConstPtr; 160 | typedef std::shared_ptr M5ConstPtr; 161 | typedef std::shared_ptr M6ConstPtr; 162 | typedef std::shared_ptr M7ConstPtr; 163 | typedef std::shared_ptr M8ConstPtr; 164 | typedef const std::shared_ptr& NullP; 165 | 166 | template 167 | Connection addCallback(const std::function& callback) 168 | { 169 | CallbackHelper9T* helper = new CallbackHelper9T(callback); 170 | 171 | std::lock_guard lock(mutex_); 172 | callbacks_.push_back(CallbackHelper9Ptr(helper)); 173 | return Connection(std::bind(&Signal9::removeCallback, this, callbacks_.back())); 174 | } 175 | 176 | template 177 | Connection addCallback(void(*callback)(P0, P1)) 178 | { 179 | return addCallback(std::function(std::bind(callback, _1, _2))); 180 | } 181 | 182 | template 183 | Connection addCallback(void(*callback)(P0, P1, P2)) 184 | { 185 | return addCallback(std::function(std::bind(callback, _1, _2, _3))); 186 | } 187 | 188 | template 189 | Connection addCallback(void(*callback)(P0, P1, P2, P3)) 190 | { 191 | return addCallback(std::function(std::bind(callback, _1, _2, _3, _4))); 192 | } 193 | 194 | template 195 | Connection addCallback(void(*callback)(P0, P1, P2, P3, P4)) 196 | { 197 | return addCallback(std::function(std::bind(callback, _1, _2, _3, _4, _5))); 198 | } 199 | 200 | template 201 | Connection addCallback(void(*callback)(P0, P1, P2, P3, P4, P5)) 202 | { 203 | return addCallback(std::function(std::bind(callback, _1, _2, _3, _4, _5, _6))); 204 | } 205 | 206 | template 207 | Connection addCallback(void(*callback)(P0, P1, P2, P3, P4, P5, P6)) 208 | { 209 | return addCallback(std::function(std::bind(callback, _1, _2, _3, _4, _5, _6, _7))); 210 | } 211 | 212 | template 213 | Connection addCallback(void(*callback)(P0, P1, P2, P3, P4, P5, P6, P7)) 214 | { 215 | return addCallback(std::function(std::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8))); 216 | } 217 | 218 | template 219 | Connection addCallback(void(*callback)(P0, P1, P2, P3, P4, P5, P6, P7, P8)) 220 | { 221 | return addCallback(std::function(std::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8, _9))); 222 | } 223 | 224 | template 225 | Connection addCallback(void(T::*callback)(P0, P1), T* t) 226 | { 227 | return addCallback(std::function(std::bind(callback, t, _1, _2))); 228 | } 229 | 230 | template 231 | Connection addCallback(void(T::*callback)(P0, P1, P2), T* t) 232 | { 233 | return addCallback(std::function(std::bind(callback, t, _1, _2, _3))); 234 | } 235 | 236 | template 237 | Connection addCallback(void(T::*callback)(P0, P1, P2, P3), T* t) 238 | { 239 | return addCallback(std::function(std::bind(callback, t, _1, _2, _3, _4))); 240 | } 241 | 242 | template 243 | Connection addCallback(void(T::*callback)(P0, P1, P2, P3, P4), T* t) 244 | { 245 | return addCallback(std::function(std::bind(callback, t, _1, _2, _3, _4, _5))); 246 | } 247 | 248 | template 249 | Connection addCallback(void(T::*callback)(P0, P1, P2, P3, P4, P5), T* t) 250 | { 251 | return addCallback(std::function(std::bind(callback, t, _1, _2, _3, _4, _5, _6))); 252 | } 253 | 254 | template 255 | Connection addCallback(void(T::*callback)(P0, P1, P2, P3, P4, P5, P6), T* t) 256 | { 257 | return addCallback(std::function(std::bind(callback, t, _1, _2, _3, _4, _5, _6, _7))); 258 | } 259 | 260 | template 261 | Connection addCallback(void(T::*callback)(P0, P1, P2, P3, P4, P5, P6, P7), T* t) 262 | { 263 | return addCallback(std::function(std::bind(callback, t, _1, _2, _3, _4, _5, _6, _7, _8))); 264 | } 265 | 266 | template 267 | Connection addCallback( C& callback) 268 | { 269 | return addCallback(std::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8, _9)); 278 | } 279 | 280 | void removeCallback(const CallbackHelper9Ptr& helper) 281 | { 282 | std::lock_guard lock(mutex_); 283 | typename V_CallbackHelper9::iterator it = std::find(callbacks_.begin(), callbacks_.end(), helper); 284 | if (it != callbacks_.end()) 285 | { 286 | callbacks_.erase(it); 287 | } 288 | } 289 | 290 | void call(const M0Event& e0, const M1Event& e1, const M2Event& e2, const M3Event& e3, const M4Event& e4, 291 | const M5Event& e5, const M6Event& e6, const M7Event& e7, const M8Event& e8) 292 | { 293 | std::lock_guard lock(mutex_); 294 | bool nonconst_force_copy = callbacks_.size() > 1; 295 | typename V_CallbackHelper9::iterator it = callbacks_.begin(); 296 | typename V_CallbackHelper9::iterator end = callbacks_.end(); 297 | for (; it != end; ++it) 298 | { 299 | const CallbackHelper9Ptr& helper = *it; 300 | helper->call(nonconst_force_copy, e0, e1, e2, e3, e4, e5, e6, e7, e8); 301 | } 302 | } 303 | 304 | private: 305 | std::mutex mutex_; 306 | V_CallbackHelper9 callbacks_; 307 | }; 308 | 309 | } // message_filters 310 | 311 | #endif // MESSAGE_FILTERS_SIGNAL9_H 312 | 313 | 314 | -------------------------------------------------------------------------------- /include/message_filters/simple_filter.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_SIMPLE_FILTER_H 36 | #define MESSAGE_FILTERS_SIMPLE_FILTER_H 37 | #include 38 | #include 39 | #include "connection.h" 40 | #include "signal1.h" 41 | #include "message_event.h" 42 | #include 43 | 44 | namespace message_filters 45 | { 46 | using namespace std::placeholders; 47 | /** 48 | * \brief Convenience base-class for simple filters which output a single message 49 | * 50 | * SimpleFilter provides some of the tricky callback registering functionality, so that 51 | * simple filters do not have to duplicate it. It also provides getName()/setName() for debugging 52 | * purposes. 53 | */ 54 | template 55 | class SimpleFilter : public noncopyable 56 | { 57 | public: 58 | typedef std::shared_ptr MConstPtr; 59 | typedef std::function Callback; 60 | typedef MessageEvent EventType; 61 | typedef std::function EventCallback; 62 | 63 | /** 64 | * \brief Register a callback to be called when this filter has passed 65 | * \param callback The callback to call 66 | */ 67 | template 68 | Connection registerCallback(const C& callback) 69 | { 70 | typename CallbackHelper1::Ptr helper = signal_.addCallback(Callback(callback)); 71 | return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); 72 | } 73 | 74 | /** 75 | * \brief Register a callback to be called when this filter has passed 76 | * \param callback The callback to call 77 | */ 78 | template 79 | Connection registerCallback(const std::function& callback) 80 | { 81 | return Connection(std::bind(&Signal::removeCallback, &signal_, signal_.addCallback(callback))); 82 | } 83 | 84 | 85 | /** 86 | * \brief Register a callback to be called when this filter has passed 87 | * \param callback The callback to call 88 | */ 89 | template 90 | Connection registerCallback(void(*callback)(P)) 91 | { 92 | typename CallbackHelper1::Ptr helper = signal_.template addCallback

(std::bind(callback, _1)); 93 | return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); 94 | } 95 | 96 | /** 97 | * \brief Register a callback to be called when this filter has passed 98 | * \param callback The callback to call 99 | */ 100 | template 101 | Connection registerCallback(void(T::*callback)(P), T* t) 102 | { 103 | typename CallbackHelper1::Ptr helper = signal_.template addCallback

(std::bind(callback, t, _1)); 104 | return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); 105 | } 106 | 107 | /** 108 | * \brief Set the name of this filter. For debugging use. 109 | */ 110 | void setName(const std::string& name) { name_ = name; } 111 | /** 112 | * \brief Get the name of this filter. For debugging use. 113 | */ 114 | const std::string& getName() { return name_; } 115 | 116 | protected: 117 | /** 118 | * \brief Call all registered callbacks, passing them the specified message 119 | */ 120 | void signalMessage(const MConstPtr& msg) 121 | { 122 | MessageEvent event(msg); 123 | 124 | signal_.call(event); 125 | } 126 | 127 | /** 128 | * \brief Call all registered callbacks, passing them the specified message 129 | */ 130 | void signalMessage(const MessageEvent& event) 131 | { 132 | signal_.call(event); 133 | } 134 | 135 | private: 136 | typedef Signal1 Signal; 137 | 138 | Signal signal_; 139 | 140 | std::string name_; 141 | }; 142 | 143 | } 144 | 145 | #endif 146 | 147 | -------------------------------------------------------------------------------- /include/message_filters/subscriber.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_SUBSCRIBER_H 36 | #define MESSAGE_FILTERS_SUBSCRIBER_H 37 | 38 | #include 39 | 40 | #include "connection.h" 41 | #include "simple_filter.h" 42 | 43 | namespace message_filters 44 | { 45 | 46 | class SubscriberBase 47 | { 48 | public: 49 | virtual ~SubscriberBase() {} 50 | /** 51 | * \brief Subscribe to a topic. 52 | * 53 | * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. 54 | * 55 | * \param nh The ros::NodeHandle to use to subscribe. 56 | * \param topic The topic to subscribe to. 57 | * \param queue_size The subscription queue size 58 | * \param transport_hints The transport hints to pass along 59 | * \param callback_queue The callback queue to pass along 60 | */ 61 | // 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; 62 | virtual void subscribe(rclcpp::Node* nh, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; 63 | /** 64 | * \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. 65 | */ 66 | virtual void subscribe() = 0; 67 | /** 68 | * \brief Force immediate unsubscription of this subscriber from its topic 69 | */ 70 | virtual void unsubscribe() = 0; 71 | }; 72 | typedef std::shared_ptr SubscriberBasePtr; 73 | 74 | /** 75 | * \brief ROS subscription filter. 76 | * 77 | * This class acts as a highest-level filter, simply passing messages from a ROS subscription through to the 78 | * filters which have connected to it. 79 | * 80 | * When this object is destroyed it will unsubscribe from the ROS subscription. 81 | * 82 | * The Subscriber object is templated on the type of message being subscribed to. 83 | * 84 | * \section connections CONNECTIONS 85 | * 86 | * Subscriber has no input connection. 87 | * 88 | * The output connection for the Subscriber object is the same signature as for roscpp subscription callbacks, ie. 89 | \verbatim 90 | void callback(const std::shared_ptr&); 91 | \endverbatim 92 | */ 93 | template 94 | class Subscriber : public SubscriberBase, public SimpleFilter 95 | { 96 | public: 97 | // typedef std::shared_ptr MConstPtr; 98 | typedef MessageEvent EventType; 99 | 100 | /** 101 | * \brief Constructor 102 | * 103 | * See the ros::NodeHandle::subscribe() variants for more information on the parameters 104 | * 105 | * \param nh The ros::NodeHandle to use to subscribe. 106 | * \param topic The topic to subscribe to. 107 | * \param queue_size The subscription queue size 108 | * \param transport_hints The transport hints to pass along 109 | * \param callback_queue The callback queue to pass along 110 | */ 111 | Subscriber(rclcpp::Node* nh, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) 112 | { 113 | subscribe(nh, topic, qos); 114 | } 115 | 116 | /** 117 | * \brief Empty constructor, use subscribe() to subscribe to a topic 118 | */ 119 | Subscriber() 120 | { 121 | } 122 | 123 | ~Subscriber() 124 | { 125 | unsubscribe(); 126 | } 127 | 128 | /** 129 | * \brief Subscribe to a topic. 130 | * 131 | * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. 132 | * 133 | * \param nh The ros::NodeHandle to use to subscribe. 134 | * \param topic The topic to subscribe to. 135 | * \param queue_size The subscription queue size 136 | * \param transport_hints The transport hints to pass along 137 | * \param callback_queue The callback queue to pass along 138 | */ 139 | void subscribe(rclcpp::Node* nh, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) 140 | { 141 | unsubscribe(); 142 | 143 | if (!topic.empty()) 144 | { 145 | topic_ = topic; 146 | qos_ = qos; 147 | sub_ = nh->create_subscription(topic, 148 | [this](std::shared_ptr msg) { 149 | this->cb(EventType(msg)); 150 | }, qos); 151 | nh_ = nh; 152 | } 153 | } 154 | 155 | /** 156 | * \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. 157 | */ 158 | void subscribe() 159 | { 160 | unsubscribe(); 161 | 162 | if (!topic_.empty()) 163 | { 164 | subscribe(nh_, topic_, qos_); 165 | } 166 | } 167 | 168 | /** 169 | * \brief Force immediate unsubscription of this subscriber from its topic 170 | */ 171 | void unsubscribe() 172 | { 173 | // sub_.shutdown(); 174 | } 175 | 176 | std::string getTopic() const 177 | { 178 | return this->topic_; 179 | } 180 | 181 | /** 182 | * \brief Returns the internal ros::Subscriber object 183 | */ 184 | const typename rclcpp::Subscription::SharedPtr getSubscriber() const { return sub_; } 185 | 186 | /** 187 | * \brief Does nothing. Provided so that Subscriber may be used in a message_filters::Chain 188 | */ 189 | template 190 | void connectInput(F& f) 191 | { 192 | (void)f; 193 | } 194 | 195 | /** 196 | * \brief Does nothing. Provided so that Subscriber may be used in a message_filters::Chain 197 | */ 198 | void add(const EventType& e) 199 | { 200 | (void)e; 201 | } 202 | 203 | void cb(const EventType& e) 204 | { 205 | this->signalMessage(e); 206 | } 207 | 208 | typename rclcpp::Subscription::SharedPtr sub_; 209 | rclcpp::Node* nh_; 210 | std::string topic_; 211 | rmw_qos_profile_t qos_; 212 | }; 213 | 214 | } 215 | 216 | #endif 217 | -------------------------------------------------------------------------------- /include/message_filters/sync_policies/exact_time.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_SYNC_EXACT_TIME_H 36 | #define MESSAGE_FILTERS_SYNC_EXACT_TIME_H 37 | 38 | #include "message_filters/synchronizer.h" 39 | #include "message_filters/connection.h" 40 | #include "message_filters/null_types.h" 41 | #include "message_filters/signal9.h" 42 | #include "message_filters/message_traits.h" 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | namespace message_filters 51 | { 52 | namespace sync_policies 53 | { 54 | 55 | template 57 | struct ExactTime : public PolicyBase 58 | { 59 | typedef Synchronizer Sync; 60 | typedef PolicyBase Super; 61 | typedef typename Super::Messages Messages; 62 | typedef typename Super::Signal Signal; 63 | typedef typename Super::Events Events; 64 | typedef typename Super::RealTypeCount RealTypeCount; 65 | typedef typename Super::M0Event M0Event; 66 | typedef typename Super::M1Event M1Event; 67 | typedef typename Super::M2Event M2Event; 68 | typedef typename Super::M3Event M3Event; 69 | typedef typename Super::M4Event M4Event; 70 | typedef typename Super::M5Event M5Event; 71 | typedef typename Super::M6Event M6Event; 72 | typedef typename Super::M7Event M7Event; 73 | typedef typename Super::M8Event M8Event; 74 | typedef Events Tuple; 75 | 76 | ExactTime(uint32_t queue_size) 77 | : parent_(0) 78 | , queue_size_(queue_size) 79 | { 80 | } 81 | 82 | ExactTime(const ExactTime& e) 83 | { 84 | *this = e; 85 | } 86 | 87 | ExactTime& operator=(const ExactTime& rhs) 88 | { 89 | parent_ = rhs.parent_; 90 | queue_size_ = rhs.queue_size_; 91 | last_signal_time_ = rhs.last_signal_time_; 92 | tuples_ = rhs.tuples_; 93 | 94 | return *this; 95 | } 96 | 97 | void initParent(Sync* parent) 98 | { 99 | parent_ = parent; 100 | } 101 | 102 | template 103 | void add(const typename std::tuple_element::type& evt) 104 | { 105 | assert(parent_); 106 | 107 | namespace mt = message_filters::message_traits; 108 | 109 | std::lock_guard lock(mutex_); 110 | 111 | Tuple& t = tuples_[mt::TimeStamp::type>::value(*evt.getMessage())]; 112 | std::get(t) = evt; 113 | 114 | checkTuple(t); 115 | } 116 | 117 | template 118 | Connection registerDropCallback(const C& callback) 119 | { 120 | return drop_signal_.template addCallback(callback); 121 | } 122 | 123 | template 124 | Connection registerDropCallback(C& callback) 125 | { 126 | return drop_signal_.template addCallback(callback); 127 | } 128 | 129 | template 130 | Connection registerDropCallback(const C& callback, T* t) 131 | { 132 | return drop_signal_.template addCallback(callback, t); 133 | } 134 | 135 | template 136 | Connection registerDropCallback(C& callback, T* t) 137 | { 138 | return drop_signal_.template addCallback(callback, t); 139 | } 140 | 141 | private: 142 | 143 | // assumes mutex_ is already locked 144 | void checkTuple(Tuple& t) 145 | { 146 | namespace mt = message_filters::message_traits; 147 | 148 | bool full = true; 149 | full = full && (bool)std::get<0>(t).getMessage(); 150 | full = full && (bool)std::get<1>(t).getMessage(); 151 | full = full && (RealTypeCount::value > 2 ? (bool)std::get<2>(t).getMessage() : true); 152 | full = full && (RealTypeCount::value > 3 ? (bool)std::get<3>(t).getMessage() : true); 153 | full = full && (RealTypeCount::value > 4 ? (bool)std::get<4>(t).getMessage() : true); 154 | full = full && (RealTypeCount::value > 5 ? (bool)std::get<5>(t).getMessage() : true); 155 | full = full && (RealTypeCount::value > 6 ? (bool)std::get<6>(t).getMessage() : true); 156 | full = full && (RealTypeCount::value > 7 ? (bool)std::get<7>(t).getMessage() : true); 157 | full = full && (RealTypeCount::value > 8 ? (bool)std::get<8>(t).getMessage() : true); 158 | 159 | if (full) 160 | { 161 | parent_->signal(std::get<0>(t), std::get<1>(t), std::get<2>(t), 162 | std::get<3>(t), std::get<4>(t), std::get<5>(t), 163 | std::get<6>(t), std::get<7>(t), std::get<8>(t)); 164 | 165 | last_signal_time_ = mt::TimeStamp::value(*std::get<0>(t).getMessage()); 166 | 167 | tuples_.erase(last_signal_time_); 168 | 169 | clearOldTuples(); 170 | } 171 | 172 | if (queue_size_ > 0) 173 | { 174 | // printf("********************tuples size=%u*******************\n", tuples_.size()); 175 | while (tuples_.size() > queue_size_) 176 | { 177 | Tuple& t2 = tuples_.begin()->second; 178 | drop_signal_.call(std::get<0>(t2), std::get<1>(t2), std::get<2>(t2), 179 | std::get<3>(t2), std::get<4>(t2), std::get<5>(t2), 180 | std::get<6>(t2), std::get<7>(t2), std::get<8>(t2)); 181 | tuples_.erase(tuples_.begin()); 182 | } 183 | } 184 | } 185 | 186 | // assumes mutex_ is already locked 187 | void clearOldTuples() 188 | { 189 | typename M_TimeToTuple::iterator it = tuples_.begin(); 190 | typename M_TimeToTuple::iterator end = tuples_.end(); 191 | for (; it != end;) 192 | { 193 | if (it->first <= last_signal_time_) 194 | { 195 | typename M_TimeToTuple::iterator old = it; 196 | ++it; 197 | 198 | Tuple& t = old->second; 199 | drop_signal_.call(std::get<0>(t), std::get<1>(t), std::get<2>(t), 200 | std::get<3>(t), std::get<4>(t), std::get<5>(t), 201 | std::get<6>(t), std::get<7>(t), std::get<8>(t)); 202 | tuples_.erase(old); 203 | } 204 | else 205 | { 206 | // the map is sorted by time, so we can ignore anything after this if this one's time is ok 207 | break; 208 | } 209 | } 210 | } 211 | 212 | private: 213 | Sync* parent_; 214 | 215 | uint32_t queue_size_; 216 | typedef std::map M_TimeToTuple; 217 | M_TimeToTuple tuples_; 218 | rclcpp::Time last_signal_time_; 219 | 220 | Signal drop_signal_; 221 | 222 | std::mutex mutex_; 223 | }; 224 | 225 | } // namespace sync 226 | } // namespace message_filters 227 | 228 | #endif // MESSAGE_FILTERS_SYNC_EXACT_TIME_H 229 | 230 | -------------------------------------------------------------------------------- /include/message_filters/time_sequencer.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_TIME_SEQUENCER_H 36 | #define MESSAGE_FILTERS_TIME_SEQUENCER_H 37 | 38 | #include 39 | 40 | #include "connection.h" 41 | #include "simple_filter.h" 42 | #include "message_traits.h" 43 | 44 | namespace message_filters 45 | { 46 | 47 | /** 48 | * \class TimeSequencer 49 | * 50 | * \brief Sequences messages based on the timestamp of their header. 51 | * 52 | * The TimeSequencer object is templated on the type of message being sequenced. 53 | * 54 | * \section behavior BEHAVIOR 55 | 56 | * At construction, the TimeSequencer takes a ros::Duration 57 | * "delay" which specifies how long to queue up messages to 58 | * provide a time sequencing over them. As messages arrive they are 59 | * sorted according to their time stamps. A callback for a message is 60 | * never invoked until the messages' time stamp is out of date by at 61 | * least delay. However, for all messages which are out of date 62 | * by at least delay, their callback are invoked and guaranteed 63 | * to be in temporal order. If a message arrives from a time \b prior 64 | * to a message which has already had its callback invoked, it is 65 | * thrown away. 66 | * 67 | * \section connections CONNECTIONS 68 | * 69 | * TimeSequencer's input and output connections are both of the same signature as roscpp subscription callbacks, ie. 70 | \verbatim 71 | void callback(const std::shared_ptr&); 72 | \endverbatim 73 | * 74 | */ 75 | template 76 | class TimeSequencer : public SimpleFilter 77 | { 78 | public: 79 | typedef std::shared_ptr MConstPtr; 80 | typedef MessageEvent EventType; 81 | 82 | /** 83 | * \brief Constructor 84 | * \param f A filter to connect this sequencer's input to 85 | * \param delay The minimum time to hold a message before passing it through. 86 | * \param update_rate The rate at which to check for messages which have passed "delay" 87 | * \param queue_size The number of messages to store 88 | * \param nh (optional) The NodeHandle to use to create the ros::SteadyTimer that runs at update_rate 89 | */ 90 | template 91 | TimeSequencer(F& f, rclcpp::Duration delay, rclcpp::Duration update_rate, uint32_t queue_size, rclcpp::Node::SharedPtr nh = std::make_shared("test_node")) 92 | : delay_(delay) 93 | , update_rate_(update_rate) 94 | , queue_size_(queue_size) 95 | , nh_(nh) 96 | { 97 | init(); 98 | connectInput(f); 99 | } 100 | 101 | /** 102 | * \brief Constructor 103 | * 104 | * This version of the constructor does not take a filter immediately. You can connect to a filter later with the connectInput() function 105 | * 106 | * \param delay The minimum time to hold a message before passing it through. 107 | * \param update_rate The rate at which to check for messages which have passed "delay" 108 | * \param queue_size The number of messages to store 109 | * \param nh (optional) The NodeHandle to use to create the ros::SteadyTimer that runs at update_rate 110 | */ 111 | TimeSequencer(rclcpp::Duration delay, rclcpp::Duration update_rate, uint32_t queue_size, rclcpp::Node::SharedPtr nh = std::make_shared("test_node")) 112 | : delay_(delay) 113 | , update_rate_(update_rate) 114 | , queue_size_(queue_size) 115 | , nh_(nh) 116 | { 117 | init(); 118 | } 119 | 120 | 121 | /** 122 | * \brief Connect this filter's input to another filter's output. 123 | */ 124 | template 125 | void connectInput(F& f) 126 | { 127 | incoming_connection_.disconnect(); 128 | incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback(std::bind(&TimeSequencer::cb, this, _1))); 129 | } 130 | 131 | ~TimeSequencer() 132 | { 133 | update_timer_->cancel(); 134 | incoming_connection_.disconnect(); 135 | } 136 | 137 | void add(const EventType& evt) 138 | { 139 | namespace mt = message_filters::message_traits; 140 | 141 | std::lock_guard lock(messages_mutex_); 142 | if (mt::TimeStamp::value(*evt.getMessage()) < last_time_) 143 | { 144 | return; 145 | } 146 | 147 | messages_.insert(evt); 148 | 149 | if (queue_size_ != 0 && messages_.size() > queue_size_) 150 | { 151 | messages_.erase(*messages_.begin()); 152 | } 153 | } 154 | 155 | /** 156 | * \brief Manually add a message to the cache. 157 | */ 158 | void add(const MConstPtr& msg) 159 | { 160 | EventType evt(msg); 161 | add(evt); 162 | } 163 | 164 | rclcpp::Node::SharedPtr get_node(void) 165 | { 166 | return nh_; 167 | } 168 | 169 | private: 170 | class MessageSort 171 | { 172 | public: 173 | bool operator()(const EventType& lhs, const EventType& rhs) const 174 | { 175 | namespace mt = message_filters::message_traits; 176 | return mt::TimeStamp::value(*lhs.getMessage()) < mt::TimeStamp::value(*rhs.getMessage()); 177 | } 178 | }; 179 | typedef std::multiset S_Message; 180 | typedef std::vector V_Message; 181 | 182 | void cb(const EventType& evt) 183 | { 184 | add(evt); 185 | } 186 | 187 | void dispatch() 188 | { 189 | namespace mt = message_filters::message_traits; 190 | 191 | V_Message to_call; 192 | 193 | { 194 | std::lock_guard lock(messages_mutex_); 195 | 196 | while (!messages_.empty()) 197 | { 198 | const EventType& e = *messages_.begin(); 199 | rclcpp::Time stamp = mt::TimeStamp::value(*e.getMessage()); 200 | if ((stamp + delay_) <= rclcpp::Clock().now()) 201 | { 202 | last_time_ = stamp; 203 | to_call.push_back(e); 204 | messages_.erase(messages_.begin()); 205 | } 206 | else 207 | { 208 | break; 209 | } 210 | } 211 | } 212 | 213 | { 214 | typename V_Message::iterator it = to_call.begin(); 215 | typename V_Message::iterator end = to_call.end(); 216 | for (; it != end; ++it) 217 | { 218 | this->signalMessage(*it); 219 | } 220 | } 221 | } 222 | /* 223 | void update(const ros::SteadyTimerEvent&) 224 | { 225 | dispatch(); 226 | } 227 | */ 228 | void init() 229 | { 230 | update_timer_ = nh_->create_wall_timer(std::chrono::nanoseconds(update_rate_.nanoseconds()), [this]() { 231 | dispatch(); 232 | }); 233 | } 234 | 235 | rclcpp::Duration delay_; 236 | rclcpp::Duration update_rate_; 237 | uint32_t queue_size_; 238 | rclcpp::Node::SharedPtr nh_; 239 | rclcpp::TimerBase::SharedPtr update_timer_; 240 | Connection incoming_connection_; 241 | 242 | 243 | S_Message messages_; 244 | std::mutex messages_mutex_; 245 | rclcpp::Time last_time_; 246 | }; 247 | 248 | } 249 | 250 | #endif 251 | -------------------------------------------------------------------------------- /include/message_filters/time_synchronizer.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef MESSAGE_FILTERS_TIME_SYNCHRONIZER_H 36 | #define MESSAGE_FILTERS_TIME_SYNCHRONIZER_H 37 | 38 | #include "synchronizer.h" 39 | #include "sync_policies/exact_time.h" 40 | 41 | #include 42 | #include "message_event.h" 43 | 44 | namespace message_filters 45 | { 46 | 47 | /** 48 | * \brief Synchronizes up to 9 messages by their timestamps. 49 | * 50 | * TimeSynchronizer synchronizes up to 9 incoming channels by the timestamps contained in their messages' headers. 51 | * TimeSynchronizer takes anywhere from 2 to 9 message types as template parameters, and passes them through to a 52 | * callback which takes a shared pointer of each. 53 | * 54 | * The required queue size parameter when constructing the TimeSynchronizer tells it how many sets of messages it should 55 | * store (by timestamp) while waiting for messages to arrive and complete their "set" 56 | * 57 | * \section connections CONNECTIONS 58 | * 59 | * The input connections for the TimeSynchronizer object is the same signature as for roscpp subscription callbacks, ie. 60 | \verbatim 61 | void callback(const std::shared_ptr&); 62 | \endverbatim 63 | * The output connection for the TimeSynchronizer object is dependent on the number of messages being synchronized. For 64 | * a 3-message synchronizer for example, it would be: 65 | \verbatim 66 | void callback(const std::shared_ptr&, const std::shared_ptr&, const std::shared_ptr&); 67 | \endverbatim 68 | * \section usage USAGE 69 | * Example usage would be: 70 | \verbatim 71 | TimeSynchronizer sync_policies(caminfo_sub, limage_sub, rimage_sub, 3); 72 | sync_policies.registerCallback(callback); 73 | \endverbatim 74 | 75 | * The callback is then of the form: 76 | \verbatim 77 | void callback(const sensor_msgs::CameraInfo::ConstPtr&, const sensor_msgs::Image::ConstPtr&, const sensor_msgs::Image::ConstPtr&); 78 | \endverbatim 79 | * 80 | */ 81 | template 83 | class TimeSynchronizer : public Synchronizer > 84 | { 85 | public: 86 | typedef sync_policies::ExactTime Policy; 87 | typedef Synchronizer Base; 88 | typedef std::shared_ptr M0ConstPtr; 89 | typedef std::shared_ptr M1ConstPtr; 90 | typedef std::shared_ptr M2ConstPtr; 91 | typedef std::shared_ptr M3ConstPtr; 92 | typedef std::shared_ptr M4ConstPtr; 93 | typedef std::shared_ptr M5ConstPtr; 94 | typedef std::shared_ptr M6ConstPtr; 95 | typedef std::shared_ptr M7ConstPtr; 96 | typedef std::shared_ptr M8ConstPtr; 97 | 98 | using Base::add; 99 | using Base::connectInput; 100 | using Base::registerCallback; 101 | using Base::setName; 102 | using Base::getName; 103 | using Policy::registerDropCallback; 104 | typedef typename Base::M0Event M0Event; 105 | typedef typename Base::M1Event M1Event; 106 | typedef typename Base::M2Event M2Event; 107 | typedef typename Base::M3Event M3Event; 108 | typedef typename Base::M4Event M4Event; 109 | typedef typename Base::M5Event M5Event; 110 | typedef typename Base::M6Event M6Event; 111 | typedef typename Base::M7Event M7Event; 112 | typedef typename Base::M8Event M8Event; 113 | 114 | template 115 | TimeSynchronizer(F0& f0, F1& f1, uint32_t queue_size) 116 | : Base(Policy(queue_size)) 117 | { 118 | connectInput(f0, f1); 119 | } 120 | 121 | template 122 | TimeSynchronizer(F0& f0, F1& f1, F2& f2, uint32_t queue_size) 123 | : Base(Policy(queue_size)) 124 | { 125 | connectInput(f0, f1, f2); 126 | } 127 | 128 | template 129 | TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, uint32_t queue_size) 130 | : Base(Policy(queue_size)) 131 | { 132 | connectInput(f0, f1, f2, f3); 133 | } 134 | 135 | template 136 | TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, uint32_t queue_size) 137 | : Base(Policy(queue_size)) 138 | { 139 | connectInput(f0, f1, f2, f3, f4); 140 | } 141 | 142 | template 143 | TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, uint32_t queue_size) 144 | : Base(Policy(queue_size)) 145 | { 146 | connectInput(f0, f1, f2, f3, f4, f5); 147 | } 148 | 149 | template 150 | TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, uint32_t queue_size) 151 | : Base(Policy(queue_size)) 152 | { 153 | connectInput(f0, f1, f2, f3, f4, f5, f6); 154 | } 155 | 156 | template 157 | TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, uint32_t queue_size) 158 | : Base(Policy(queue_size)) 159 | { 160 | connectInput(f0, f1, f2, f3, f4, f5, f6, f7); 161 | } 162 | 163 | template 164 | TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8, uint32_t queue_size) 165 | : Base(Policy(queue_size)) 166 | { 167 | connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); 168 | } 169 | 170 | TimeSynchronizer(uint32_t queue_size) 171 | : Base(Policy(queue_size)) 172 | { 173 | } 174 | 175 | //////////////////////////////////////////////////////////////// 176 | // For backwards compatibility 177 | //////////////////////////////////////////////////////////////// 178 | void add0(const M0ConstPtr& msg) 179 | { 180 | this->template add<0>(M0Event(msg)); 181 | } 182 | 183 | void add1(const M1ConstPtr& msg) 184 | { 185 | this->template add<1>(M1Event(msg)); 186 | } 187 | 188 | void add2(const M2ConstPtr& msg) 189 | { 190 | this->template add<2>(M2Event(msg)); 191 | } 192 | 193 | void add3(const M3ConstPtr& msg) 194 | { 195 | this->template add<3>(M3Event(msg)); 196 | } 197 | 198 | void add4(const M4ConstPtr& msg) 199 | { 200 | this->template add<4>(M4Event(msg)); 201 | } 202 | 203 | void add5(const M5ConstPtr& msg) 204 | { 205 | this->template add<5>(M5Event(msg)); 206 | } 207 | 208 | void add6(const M6ConstPtr& msg) 209 | { 210 | this->template add<6>(M6Event(msg)); 211 | } 212 | 213 | void add7(const M7ConstPtr& msg) 214 | { 215 | this->template add<7>(M7Event(msg)); 216 | } 217 | 218 | void add8(const M8ConstPtr& msg) 219 | { 220 | this->template add<8>(M8Event(msg)); 221 | } 222 | }; 223 | 224 | } 225 | 226 | #endif // MESSAGE_FILTERS_TIME_SYNCHRONIZER_H 227 | -------------------------------------------------------------------------------- /index.rst: -------------------------------------------------------------------------------- 1 | Message Filters --- chained message processing 2 | ============================================== 3 | 4 | :mod:`message_filters` is a collection of message "filters" which take messages in, 5 | either from a ROS subscription or another filter, 6 | and may or may not output the message 7 | at some time in the future, depending on a policy defined for that filter. 8 | 9 | message_filters also defines a common interface for these filters, allowing you to chain them together. 10 | 11 | The filters currently implemented in this package are: 12 | 13 | * :class:`message_filters.Subscriber` - A source filter, which wraps a ROS subscription. Most filter chains will begin with a Subscriber. 14 | * :class:`message_filters.Cache` - Caches messages which pass through it, allowing later lookup by time stamp. 15 | * :class:`message_filters.TimeSynchronizer` - Synchronizes multiple messages by their timestamps, only passing them through when all have arrived. 16 | * :class:`message_filters.TimeSequencer` - Tries to pass messages through ordered by their timestamps, even if some arrive out of order. 17 | 18 | Here's a simple example of using a Subscriber with a Cache:: 19 | 20 | def myCallback(posemsg): 21 | print posemsg 22 | 23 | sub = message_filters.Subscriber("pose_topic", robot_msgs.msg.Pose) 24 | cache = message_filters.Cache(sub, 10) 25 | cache.registerCallback(myCallback) 26 | 27 | The Subscriber here acts as the source of messages. Each message is passed to the cache, which then passes it through to the 28 | user's callback ``myCallback``. 29 | 30 | 31 | Using the time synchronizer:: 32 | 33 | from message_filters import TimeSynchronizer, Subscriber 34 | 35 | def gotimage(image, camerainfo): 36 | assert image.header.stamp == camerainfo.header.stamp 37 | print "got an Image and CameraInfo" 38 | 39 | tss = TimeSynchronizer(Subscriber("/wide_stereo/left/image_rect_color", sensor_msgs.msg.Image), 40 | Subscriber("/wide_stereo/left/camera_info", sensor_msgs.msg.CameraInfo)) 41 | tss.registerCallback(gotimage) 42 | 43 | 44 | The message filter interface 45 | ---------------------------- 46 | 47 | For an object to be usable as a message filter, it needs to have one method, 48 | ``registerCallback``. To collect messages from a message filter, register a callback with:: 49 | 50 | anyfilter.registerCallback(my_callback) 51 | 52 | The signature of ``my_callback`` varies according to the message filter. For many filters it is simply:: 53 | 54 | def my_callback(msg): 55 | 56 | where ``msg`` is the message. 57 | 58 | Message filters that accept input from an upstream 59 | message filter (e.g. :class:`message_filters.Cache`) register their own 60 | message handler as a callback. 61 | 62 | Output connections are registered through the ``registerCallback()`` function. 63 | 64 | .. automodule:: message_filters 65 | :members: Subscriber, Cache, TimeSynchronizer, TimeSequencer 66 | :inherited-members: 67 | 68 | Indices and tables 69 | ================== 70 | 71 | * :ref:`genindex` 72 | * :ref:`search` 73 | 74 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b message_filters is a set of message "filters" which take messages in, usually through a callback from somewhere else, 6 | 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 7 | filter to do so. 8 | 9 | \b message_filters also sets up a common pattern for these filters, allowing you to chain them together, even though they 10 | have no explicit base class. 11 | 12 | 13 | \section codeapi Code API 14 | 15 | The filters currently implemented in this package are: 16 | - message_filters::Subscriber - A source filter, which wraps a ROS subscription. Most filter chains will begin with a Subscriber. 17 | - message_filters::Cache - Caches messages which pass through it, allowing later lookup by time stamp. 18 | - message_filters::TimeSynchronizer - Synchronizes multiple messages by their timestamps, only passing them through when all have arrived. 19 | - message_filters::TimeSequencer - Tries to pass messages through ordered by their timestamps, even if some arrive out of order. 20 | 21 | There is also a base-class provided for simple filters: message_filters::SimpleFilter. This provides callback management and disconnection 22 | for any filter that derives from it. A simple filter is defined as one that outputs a single message. message_filters::SimpleFilter provides 23 | the registerCallback() method for any of its derived classes. message_filters::Subscriber, message_filters::Cache and message_filters::TimeSequencer 24 | are all derived from message_filters::SimpleFilter. 25 | 26 | Here's a simple example of using a Subscriber with a Cache: 27 | \verbatim 28 | void myCallback(const robot_msgs::Pose::ConstPtr& pose) 29 | {} 30 | 31 | ros::NodeHandle nh; 32 | message_filters::Subscriber sub(nh, "pose_topic", 1); 33 | message_filters::Cache cache(sub, 10); 34 | cache.registerCallback(myCallback); 35 | \endverbatim 36 | 37 | The Subscriber here acts as the source of messages. Each message is passed to the cache, which then passes it through to the 38 | user's callback (myCallback) 39 | 40 | \section connections CONNECTIONS 41 | 42 | Every filter can have up to two types of connections, input and output. Source filters (such as message_filters::Subscriber) only 43 | have output connections, whereas most other filters have both input and output connections. 44 | 45 | The two connection types do not have to be identical. For example, message_filters::TimeSynchronizer's input connection takes one 46 | parameter, but its output connection has somewhere between 2 and 9 parameters, depending on the number of connections being 47 | synchronized. 48 | 49 | Input connections are registered either in the filter's constructor, or by calling connectInput() on the filter. For example: 50 | \verbatim 51 | message_filters::Cache cache(10); 52 | cache.connectInput(sub); 53 | \endverbatim 54 | 55 | This connects cache's input to sub's output. 56 | 57 | Output connections are registered through the registerCallback() function. 58 | 59 | */ -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | message_filters 5 | 2.0.0 6 | 7 | A set of ROS2 message filters which take in messages and may output those messages at a later time, based on the conditions that filter needs met. 8 | 9 | Ethan Gao 10 | BSD 11 | https://github.com/intel/ros2_message_filters 12 | Jing Wang 13 | Josh Faust 14 | Vijay Pradeep 15 | 16 | ament_cmake 17 | 18 | builtin_interfaces 19 | rclcpp 20 | rclpy 21 | std_msgs 22 | sensor_msgs 23 | 24 | ament_lint_auto 25 | ament_cmake_gtest 26 | ament_cmake_pytest 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | name: Python API 3 | output_dir: python 4 | - builder: doxygen 5 | name: C++ API 6 | output_dir: c++ 7 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' 8 | -------------------------------------------------------------------------------- /src/connection.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "message_filters/connection.h" 36 | 37 | namespace message_filters 38 | { 39 | 40 | 41 | Connection::Connection(const VoidDisconnectFunction& func) 42 | : void_disconnect_(func) 43 | { 44 | } 45 | 46 | void Connection::disconnect() 47 | { 48 | if (void_disconnect_) 49 | { 50 | void_disconnect_(); 51 | } 52 | else if (connection_disconnect_) 53 | { 54 | connection_disconnect_(*this); 55 | } 56 | } 57 | 58 | } 59 | -------------------------------------------------------------------------------- /src/message_filters/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | """ 29 | Message Filter Objects 30 | ====================== 31 | """ 32 | 33 | import itertools 34 | import threading 35 | import rclpy 36 | import time 37 | from functools import reduce 38 | from builtin_interfaces.msg import Time 39 | from builtin_interfaces.msg import Duration 40 | from rclpy.constants import S_TO_NS 41 | 42 | def stamp_time(stamp): 43 | return float(str(stamp.sec) + "." + str(stamp.nanosec)) 44 | 45 | class SimpleFilter(object): 46 | 47 | def __init__(self): 48 | self.callbacks = {} 49 | 50 | def registerCallback(self, cb, *args): 51 | """ 52 | Register a callback function `cb` to be called when this filter 53 | has output. 54 | The filter calls the function ``cb`` with a filter-dependent list of arguments, 55 | followed by the call-supplied arguments ``args``. 56 | """ 57 | 58 | conn = len(self.callbacks) 59 | self.callbacks[conn] = (cb, args) 60 | return conn 61 | 62 | def signalMessage(self, *msg): 63 | for (cb, args) in self.callbacks.values(): 64 | cb(*(msg + args)) 65 | 66 | class Subscriber(SimpleFilter): 67 | 68 | """ 69 | ROS subscription filter. Identical arguments as :class:`rospy.Subscriber`. 70 | 71 | This class acts as a highest-level filter, simply passing messages 72 | from a ROS subscription through to the filters which have connected 73 | to it. 74 | """ 75 | def __init__(self, *args, **kwargs): 76 | SimpleFilter.__init__(self) 77 | self.node = args[0] 78 | self.topic = args[2] 79 | kwargs['callback'] = self.callback 80 | self.sub = self.node.create_subscription(*args[1:], **kwargs) 81 | 82 | def callback(self, msg): 83 | self.signalMessage(msg) 84 | 85 | def getTopic(self): 86 | return self.topic 87 | 88 | def __getattr__(self, key): 89 | """Serve same API as rospy.Subscriber""" 90 | return self.sub.__getattribute__(key) 91 | 92 | class Cache(SimpleFilter): 93 | 94 | """ 95 | Stores a time history of messages. 96 | 97 | Given a stream of messages, the most recent ``cache_size`` messages 98 | are cached in a ring buffer, from which time intervals of the cache 99 | can then be retrieved by the client. The ``allow_headerless`` 100 | option specifies whether to allow storing headerless messages with 101 | current ROS time instead of timestamp. You should avoid this as 102 | much as you can, since the delays are unpredictable. 103 | """ 104 | 105 | def __init__(self, f, cache_size=1, allow_headerless=False): 106 | SimpleFilter.__init__(self) 107 | self.connectInput(f) 108 | self.cache_size = cache_size 109 | # Array to store messages 110 | self.cache_msgs = [] 111 | # Array to store msgs times, auxiliary structure to facilitate 112 | # sorted insertion 113 | self.cache_times = [] 114 | # Whether to allow storing headerless messages with current ROS 115 | # time instead of timestamp. 116 | self.allow_headerless = allow_headerless 117 | 118 | def connectInput(self, f): 119 | self.incoming_connection = f.registerCallback(self.add) 120 | 121 | def add(self, msg): 122 | if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'): 123 | if not self.allow_headerless: 124 | rclpy.logging._root_logger.log("Cannot use message filters with non-stamped messages. " 125 | "Use the 'allow_headerless' constructor option to " 126 | "auto-assign ROS time to headerless messages.") 127 | return 128 | stamp = time.monotonic() 129 | else: 130 | stamp = stamp_time(msg.header.stamp) 131 | 132 | # Insert sorted 133 | self.cache_times.append(stamp) 134 | self.cache_msgs.append(msg) 135 | 136 | # Implement a ring buffer, discard older if oversized 137 | if (len(self.cache_msgs) > self.cache_size): 138 | del self.cache_msgs[0] 139 | del self.cache_times[0] 140 | 141 | # Signal new input 142 | self.signalMessage(msg) 143 | 144 | def getInterval(self, from_stamp, to_stamp): 145 | """Query the current cache content between from_stamp to to_stamp.""" 146 | assert from_stamp <= to_stamp 147 | 148 | return [msg for (msg, time) in zip(self.cache_msgs, self.cache_times) 149 | if from_stamp <= time <= to_stamp] 150 | 151 | def getElemAfterTime(self, stamp): 152 | """Return the oldest element after or equal the passed time stamp.""" 153 | newer = [msg for (msg, time) in zip(self.cache_msgs, self.cache_times) 154 | if time >= stamp] 155 | if not newer: 156 | return None 157 | return newer[0] 158 | 159 | def getElemBeforeTime(self, stamp): 160 | """Return the newest element before or equal the passed time stamp.""" 161 | older = [msg for (msg, time) in zip(self.cache_msgs, self.cache_times) 162 | if time <= stamp] 163 | if not older: 164 | return None 165 | return older[-1] 166 | 167 | def getLastestTime(self): 168 | """Return the newest recorded timestamp.""" 169 | if not self.cache_times: 170 | return None 171 | return self.cache_times[-1] 172 | 173 | def getOldestTime(self): 174 | """Return the oldest recorded timestamp.""" 175 | if not self.cache_times: 176 | return None 177 | return self.cache_times[0] 178 | 179 | def getLast(self): 180 | if self.getLastestTime() is None: 181 | return None 182 | return self.getElemAfterTime(self.getLastestTime()) 183 | 184 | 185 | class TimeSynchronizer(SimpleFilter): 186 | 187 | """ 188 | Synchronizes messages by their timestamps. 189 | 190 | :class:`TimeSynchronizer` synchronizes incoming message filters by the 191 | timestamps contained in their messages' headers. TimeSynchronizer 192 | listens on multiple input message filters ``fs``, and invokes the callback 193 | when it has a collection of messages with matching timestamps. 194 | 195 | The signature of the callback function is:: 196 | 197 | def callback(msg1, ... msgN): 198 | 199 | where N is the number of input message filters, and each message is 200 | the output of the corresponding filter in ``fs``. 201 | The required ``queue size`` parameter specifies how many sets of 202 | messages it should store from each input filter (by timestamp) 203 | while waiting for messages to arrive and complete their "set". 204 | """ 205 | 206 | def __init__(self, fs, queue_size): 207 | SimpleFilter.__init__(self) 208 | self.connectInput(fs) 209 | self.queue_size = queue_size 210 | self.lock = threading.Lock() 211 | 212 | def connectInput(self, fs): 213 | self.queues = [{} for f in fs] 214 | self.input_connections = [ 215 | f.registerCallback(self.add, q, i_q) 216 | for i_q, (f, q) in enumerate(zip(fs, self.queues))] 217 | 218 | def add(self, msg, my_queue, my_queue_index=None): 219 | self.lock.acquire() 220 | my_queue[msg.header.stamp] = msg 221 | while len(my_queue) > self.queue_size: 222 | del my_queue[min(my_queue.keys())] 223 | # common is the set of timestamps that occur in all queues 224 | common = reduce(set.intersection, [set(q) for q in self.queues]) 225 | for t in sorted(common): 226 | # msgs is list of msgs (one from each queue) with stamp t 227 | msgs = [q[t] for q in self.queues] 228 | self.signalMessage(*msgs) 229 | for q in self.queues: 230 | del q[t] 231 | self.lock.release() 232 | 233 | class ApproximateTimeSynchronizer(TimeSynchronizer): 234 | 235 | """ 236 | Approximately synchronizes messages by their timestamps. 237 | 238 | :class:`ApproximateTimeSynchronizer` synchronizes incoming message filters by the 239 | timestamps contained in their messages' headers. The API is the same as TimeSynchronizer 240 | except for an extra `slop` parameter in the constructor that defines the delay (in seconds) 241 | with which messages can be synchronized. The ``allow_headerless`` option specifies whether 242 | to allow storing headerless messages with current ROS time instead of timestamp. You should 243 | avoid this as much as you can, since the delays are unpredictable. 244 | """ 245 | 246 | def __init__(self, fs, queue_size, slop, allow_headerless=False): 247 | TimeSynchronizer.__init__(self, fs, queue_size) 248 | self.slop = slop 249 | self.allow_headerless = allow_headerless 250 | 251 | def add(self, msg, my_queue, my_queue_index=None): 252 | if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'): 253 | if not self.allow_headerless: 254 | rclpy.logging._root_logger.log("Cannot use message filters with non-stamped messages. " 255 | "Use the 'allow_headerless' constructor option to " 256 | "auto-assign ROS time to headerless messages.") 257 | return 258 | stamp = time.monotonic() 259 | else: 260 | stamp = stamp_time(msg.header.stamp) 261 | 262 | self.lock.acquire() 263 | my_queue[stamp] = msg 264 | while len(my_queue) > self.queue_size: 265 | del my_queue[min(my_queue)] 266 | # self.queues = [topic_0 {stamp: msg}, topic_1 {stamp: msg}, ...] 267 | if my_queue_index is None: 268 | search_queues = self.queues 269 | else: 270 | search_queues = self.queues[:my_queue_index] + \ 271 | self.queues[my_queue_index+1:] 272 | # sort and leave only reasonable stamps for synchronization 273 | stamps = [] 274 | for queue in search_queues: 275 | topic_stamps = [] 276 | for s in queue: 277 | stamp_delta = abs(s - stamp) 278 | if stamp_delta > self.slop: 279 | continue # far over the slop 280 | topic_stamps.append((s, stamp_delta)) 281 | if not topic_stamps: 282 | self.lock.release() 283 | return 284 | topic_stamps = sorted(topic_stamps, key=lambda x: x[1]) 285 | stamps.append(topic_stamps) 286 | for vv in itertools.product(*[list(zip(*s))[0] for s in stamps]): 287 | vv = list(vv) 288 | # insert the new message 289 | if my_queue_index is not None: 290 | vv.insert(my_queue_index, stamp) 291 | qt = list(zip(self.queues, vv)) 292 | if ( ((max(vv) - min(vv)) < self.slop) and 293 | (len([1 for q,t in qt if t not in q]) == 0) ): 294 | msgs = [q[t] for q,t in qt] 295 | self.signalMessage(*msgs) 296 | for q,t in qt: 297 | del q[t] 298 | break # fast finish after the synchronization 299 | self.lock.release() 300 | -------------------------------------------------------------------------------- /test/directed.py: -------------------------------------------------------------------------------- 1 | # image_transport::SubscriberFilter wide_left; // "/wide_stereo/left/image_raw" 2 | # image_transport::SubscriberFilter wide_right; // "/wide_stereo/right/image_raw" 3 | # message_filters::Subscriber wide_left_info; // "/wide_stereo/left/camera_info" 4 | # message_filters::Subscriber wide_right_info; // "/wide_stereo/right/camera_info" 5 | # message_filters::TimeSynchronizer wide; 6 | # 7 | # PersonDataRecorder() : 8 | # wide_left(nh_, "/wide_stereo/left/image_raw", 10), 9 | # wide_right(nh_, "/wide_stereo/right/image_raw", 10), 10 | # wide_left_info(nh_, "/wide_stereo/left/camera_info", 10), 11 | # wide_right_info(nh_, "/wide_stereo/right/camera_info", 10), 12 | # wide(wide_left, wide_left_info, wide_right, wide_right_info, 4), 13 | # 14 | # wide.registerCallback(boost::bind(&PersonDataRecorder::wideCB, this, _1, _2, _3, _4)); 15 | 16 | import rclpy 17 | import random 18 | import unittest 19 | 20 | from message_filters import SimpleFilter, Subscriber, Cache, TimeSynchronizer 21 | 22 | 23 | class MockHeader: 24 | pass 25 | 26 | class MockMessage: 27 | def __init__(self, stamp, data): 28 | self.header = MockHeader() 29 | self.header.stamp = stamp 30 | self.data = data 31 | 32 | class MockFilter(SimpleFilter): 33 | pass 34 | 35 | class TestDirected(unittest.TestCase): 36 | def cb_collector_2msg(self, msg1, msg2): 37 | self.collector.append((msg1, msg2)) 38 | 39 | def test_synchronizer(self): 40 | m0 = MockFilter() 41 | m1 = MockFilter() 42 | ts = TimeSynchronizer([m0, m1], 1) 43 | ts.registerCallback(self.cb_collector_2msg) 44 | 45 | if 0: 46 | # Simple case, pairs of messages, make sure that they get combined 47 | for t in range(10): 48 | self.collector = [] 49 | msg0 = MockMessage(t, 33) 50 | msg1 = MockMessage(t, 34) 51 | m0.signalMessage(msg0) 52 | self.assertEqual(self.collector, []) 53 | m1.signalMessage(msg1) 54 | self.assertEqual(self.collector, [(msg0, msg1)]) 55 | 56 | # Scramble sequences of length N. Make sure that TimeSequencer recombines them. 57 | random.seed(0) 58 | for N in range(1, 10): 59 | m0 = MockFilter() 60 | m1 = MockFilter() 61 | seq0 = [MockMessage(t, random.random()) for t in range(N)] 62 | seq1 = [MockMessage(t, random.random()) for t in range(N)] 63 | # random.shuffle(seq0) 64 | ts = TimeSynchronizer([m0, m1], N) 65 | ts.registerCallback(self.cb_collector_2msg) 66 | self.collector = [] 67 | for msg in random.sample(seq0, N): 68 | m0.signalMessage(msg) 69 | self.assertEqual(self.collector, []) 70 | for msg in random.sample(seq1, N): 71 | m1.signalMessage(msg) 72 | self.assertEqual(set(self.collector), set(zip(seq0, seq1))) 73 | 74 | if __name__ == '__main__': 75 | suite = unittest.TestSuite() 76 | suite.addTest(TestDirected('test_synchronizer')) 77 | unittest.TextTestRunner(verbosity=2).run(suite) 78 | -------------------------------------------------------------------------------- /test/msg_cache_unittest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include "message_filters/cache.h" 41 | #include "message_filters/message_traits.h" 42 | 43 | using namespace message_filters ; 44 | 45 | struct Header 46 | { 47 | rclcpp::Time stamp ; 48 | } ; 49 | 50 | 51 | struct Msg 52 | { 53 | Header header ; 54 | int data ; 55 | } ; 56 | typedef std::shared_ptr MsgConstPtr; 57 | namespace message_filters 58 | { 59 | namespace message_traits 60 | { 61 | template<> 62 | struct TimeStamp 63 | { 64 | static rclcpp::Time value(const Msg& m) 65 | { 66 | return m.header.stamp; 67 | } 68 | }; 69 | } 70 | } 71 | 72 | using namespace std ; 73 | void fillCacheEasy(Cache& cache, unsigned int start, unsigned int end) 74 | 75 | { 76 | for (unsigned int i=start; i < end; i++) 77 | { 78 | Msg* msg = new Msg ; 79 | msg->data = i ; 80 | msg->header.stamp= rclcpp::Time(i*10, 0) ; 81 | 82 | std::shared_ptr msg_ptr(msg) ; 83 | cache.add(msg_ptr) ; 84 | } 85 | } 86 | 87 | TEST(Cache, easyInterval) 88 | { 89 | Cache cache(10) ; 90 | fillCacheEasy(cache, 0, 5) ; 91 | 92 | vector > interval_data = cache.getInterval(rclcpp::Time(5, 0), rclcpp::Time(35, 0)) ; 93 | 94 | ASSERT_EQ(interval_data.size(), (unsigned int) 3) ; 95 | EXPECT_EQ(interval_data[0]->data, 1) ; 96 | EXPECT_EQ(interval_data[1]->data, 2) ; 97 | EXPECT_EQ(interval_data[2]->data, 3) ; 98 | 99 | // Look for an interval past the end of the cache 100 | interval_data = cache.getInterval(rclcpp::Time(55, 0), rclcpp::Time(65, 0)) ; 101 | EXPECT_EQ(interval_data.size(), (unsigned int) 0) ; 102 | 103 | // Look for an interval that fell off the back of the cache 104 | fillCacheEasy(cache, 5, 20) ; 105 | interval_data = cache.getInterval(rclcpp::Time(5, 0), rclcpp::Time(35, 0)) ; 106 | EXPECT_EQ(interval_data.size(), (unsigned int) 0) ; 107 | } 108 | 109 | TEST(Cache, easySurroundingInterval) 110 | { 111 | Cache cache(10); 112 | fillCacheEasy(cache, 1, 6); 113 | 114 | vector > interval_data; 115 | interval_data = cache.getSurroundingInterval(rclcpp::Time(15,0), rclcpp::Time(35,0)) ; 116 | ASSERT_EQ(interval_data.size(), (unsigned int) 4); 117 | EXPECT_EQ(interval_data[0]->data, 1); 118 | EXPECT_EQ(interval_data[1]->data, 2); 119 | EXPECT_EQ(interval_data[2]->data, 3); 120 | EXPECT_EQ(interval_data[3]->data, 4); 121 | 122 | interval_data = cache.getSurroundingInterval(rclcpp::Time(0,0), rclcpp::Time(35,0)) ; 123 | ASSERT_EQ(interval_data.size(), (unsigned int) 4); 124 | EXPECT_EQ(interval_data[0]->data, 1); 125 | 126 | interval_data = cache.getSurroundingInterval(rclcpp::Time(35,0), rclcpp::Time(35,0)) ; 127 | ASSERT_EQ(interval_data.size(), (unsigned int) 2); 128 | EXPECT_EQ(interval_data[0]->data, 3); 129 | EXPECT_EQ(interval_data[1]->data, 4); 130 | 131 | interval_data = cache.getSurroundingInterval(rclcpp::Time(55,0), rclcpp::Time(55,0)) ; 132 | ASSERT_EQ(interval_data.size(), (unsigned int) 1); 133 | EXPECT_EQ(interval_data[0]->data, 5); 134 | } 135 | 136 | 137 | std::shared_ptr buildMsg(double time, int data) 138 | { 139 | Msg* msg = new Msg ; 140 | msg->data = data ; 141 | msg->header.stamp = rclcpp::Time(time, 0) ; 142 | 143 | std::shared_ptr msg_ptr(msg) ; 144 | return msg_ptr ; 145 | } 146 | 147 | TEST(Cache, easyUnsorted) 148 | { 149 | Cache cache(10) ; 150 | 151 | cache.add(buildMsg(10.0, 1)) ; 152 | cache.add(buildMsg(30.0, 3)) ; 153 | cache.add(buildMsg(70.0, 7)) ; 154 | cache.add(buildMsg( 5.0, 0)) ; 155 | cache.add(buildMsg(20.0, 2)) ; 156 | 157 | vector > interval_data = cache.getInterval(rclcpp::Time(3, 0), rclcpp::Time(15, 0)) ; 158 | 159 | ASSERT_EQ(interval_data.size(), (unsigned int) 2) ; 160 | EXPECT_EQ(interval_data[0]->data, 0) ; 161 | EXPECT_EQ(interval_data[1]->data, 1) ; 162 | 163 | // Grab all the data 164 | interval_data = cache.getInterval(rclcpp::Time(0, 0), rclcpp::Time(80, 0)) ; 165 | ASSERT_EQ(interval_data.size(), (unsigned int) 5) ; 166 | EXPECT_EQ(interval_data[0]->data, 0) ; 167 | EXPECT_EQ(interval_data[1]->data, 1) ; 168 | EXPECT_EQ(interval_data[2]->data, 2) ; 169 | EXPECT_EQ(interval_data[3]->data, 3) ; 170 | EXPECT_EQ(interval_data[4]->data, 7) ; 171 | } 172 | 173 | 174 | TEST(Cache, easyElemBeforeAfter) 175 | { 176 | Cache cache(10) ; 177 | std::shared_ptr elem_ptr ; 178 | 179 | fillCacheEasy(cache, 5, 10) ; 180 | 181 | elem_ptr = cache.getElemAfterTime( rclcpp::Time(85.0, 0)) ; 182 | 183 | ASSERT_FALSE(!elem_ptr) ; 184 | EXPECT_EQ(elem_ptr->data, 9) ; 185 | 186 | elem_ptr = cache.getElemBeforeTime( rclcpp::Time(85.0, 0)) ; 187 | ASSERT_FALSE(!elem_ptr) ; 188 | EXPECT_EQ(elem_ptr->data, 8) ; 189 | 190 | elem_ptr = cache.getElemBeforeTime( rclcpp::Time(45.0, 0)) ; 191 | EXPECT_TRUE(!elem_ptr) ; 192 | } 193 | 194 | struct EventHelper 195 | { 196 | public: 197 | void cb(const MessageEvent& evt) 198 | { 199 | event_ = evt; 200 | } 201 | 202 | MessageEvent event_; 203 | }; 204 | 205 | TEST(Cache, eventInEventOut) 206 | { 207 | Cache c0(10); 208 | Cache c1(c0, 10); 209 | EventHelper h; 210 | c1.registerCallback(&EventHelper::cb, &h); 211 | 212 | MessageEvent evt(std::make_shared(), rclcpp::Time(4, 0)); 213 | c0.add(evt); 214 | 215 | EXPECT_EQ(h.event_.getReceiptTime(), evt.getReceiptTime()); 216 | EXPECT_EQ(h.event_.getMessage(), evt.getMessage()); 217 | } 218 | 219 | int main(int argc, char **argv){ 220 | testing::InitGoogleTest(&argc, argv); 221 | rclcpp::init(argc, argv); 222 | return RUN_ALL_TESTS(); 223 | } 224 | 225 | -------------------------------------------------------------------------------- /test/test_approxsync.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2009, Willow Garage, Inc. 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the Willow Garage nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import rclpy 36 | import unittest 37 | import random 38 | import time 39 | from builtin_interfaces.msg import Time 40 | 41 | import message_filters 42 | from message_filters import ApproximateTimeSynchronizer 43 | 44 | class MockHeader: 45 | pass 46 | 47 | class MockMessage: 48 | def __init__(self, stamp, data): 49 | self.header = MockHeader() 50 | self.header.stamp = stamp 51 | self.data = data 52 | 53 | class MockHeaderlessMessage: 54 | def __init__(self, data): 55 | self.data = data 56 | 57 | class MockFilter(message_filters.SimpleFilter): 58 | pass 59 | 60 | class TestApproxSync(unittest.TestCase): 61 | 62 | def cb_collector_2msg(self, msg1, msg2): 63 | self.collector.append((msg1, msg2)) 64 | 65 | def test_approx(self): 66 | m0 = MockFilter() 67 | m1 = MockFilter() 68 | ts = ApproximateTimeSynchronizer([m0, m1], 1, 0.1) 69 | ts.registerCallback(self.cb_collector_2msg) 70 | 71 | if 0: 72 | # Simple case, pairs of messages, make sure that they get combined 73 | for t in range(10): 74 | self.collector = [] 75 | msg0 = MockMessage(t, 33) 76 | msg1 = MockMessage(t, 34) 77 | m0.signalMessage(msg0) 78 | self.assertEqual(self.collector, []) 79 | m1.signalMessage(msg1) 80 | self.assertEqual(self.collector, [(msg0, msg1)]) 81 | 82 | # Scramble sequences of length N. Make sure that TimeSequencer recombines them. 83 | random.seed(0) 84 | for N in range(1, 10): 85 | m0 = MockFilter() 86 | m1 = MockFilter() 87 | seq0 = [MockMessage(Time(sec=t), random.random()) for t in range(N)] 88 | seq1 = [MockMessage(Time(sec=t), random.random()) for t in range(N)] 89 | # random.shuffle(seq0) 90 | ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1) 91 | ts.registerCallback(self.cb_collector_2msg) 92 | self.collector = [] 93 | for msg in random.sample(seq0, N): 94 | m0.signalMessage(msg) 95 | self.assertEqual(self.collector, []) 96 | for msg in random.sample(seq1, N): 97 | m1.signalMessage(msg) 98 | self.assertEqual(set(self.collector), set(zip(seq0, seq1))) 99 | 100 | # Scramble sequences of length N of headerless and header-having messages. 101 | # Make sure that TimeSequencer recombines them. 102 | # rospy.rostime.set_rostime_initialized(True) 103 | ''' 104 | random.seed(0) 105 | for N in range(1, 10): 106 | m0 = MockFilter() 107 | m1 = MockFilter() 108 | seq0 = [MockMessage(Time(sec=t), random.random()) for t in range(N)] 109 | seq1 = [MockHeaderlessMessage(random.random()) for t in range(N)] 110 | # random.shuffle(seq0) 111 | ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1, allow_headerless=True) 112 | ts.registerCallback(self.cb_collector_2msg) 113 | self.collector = [] 114 | for msg in random.sample(seq0, N): 115 | m0.signalMessage(msg) 116 | self.assertEqual(self.collector, []) 117 | for i in random.sample(range(N), N): 118 | msg = seq1[i] 119 | rospy.rostime._set_rostime(rospy.Time(i+0.05)) 120 | time.sleep(0.05) 121 | m1.signalMessage(msg) 122 | self.assertEqual(set(self.collector), set(zip(seq0, seq1))) 123 | ''' 124 | 125 | if __name__ == '__main__': 126 | suite = unittest.TestSuite() 127 | suite.addTest(TestApproxSync('test_approx')) 128 | unittest.TextTestRunner(verbosity=2).run(suite) 129 | -------------------------------------------------------------------------------- /test/test_chain.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include "message_filters/chain.h" 39 | 40 | using namespace message_filters; 41 | 42 | struct Msg 43 | { 44 | }; 45 | typedef std::shared_ptr MsgPtr; 46 | typedef std::shared_ptr MsgConstPtr; 47 | 48 | class Helper 49 | { 50 | public: 51 | Helper() 52 | : count_(0) 53 | {} 54 | 55 | void cb() 56 | { 57 | ++count_; 58 | } 59 | 60 | int32_t count_; 61 | }; 62 | 63 | typedef std::shared_ptr > PassThroughPtr; 64 | 65 | TEST(Chain, simple) 66 | { 67 | Helper h; 68 | Chain c; 69 | c.addFilter(std::make_shared >()); 70 | c.registerCallback(std::bind(&Helper::cb, &h)); 71 | 72 | c.add(std::make_shared()); 73 | EXPECT_EQ(h.count_, 1); 74 | c.add(std::make_shared()); 75 | EXPECT_EQ(h.count_, 2); 76 | } 77 | 78 | TEST(Chain, multipleFilters) 79 | { 80 | Helper h; 81 | Chain c; 82 | c.addFilter(std::make_shared >()); 83 | c.addFilter(std::make_shared >()); 84 | c.addFilter(std::make_shared >()); 85 | c.addFilter(std::make_shared >()); 86 | c.registerCallback(std::bind(&Helper::cb, &h)); 87 | 88 | c.add(std::make_shared()); 89 | EXPECT_EQ(h.count_, 1); 90 | c.add(std::make_shared()); 91 | EXPECT_EQ(h.count_, 2); 92 | } 93 | 94 | TEST(Chain, addingFilters) 95 | { 96 | Helper h; 97 | Chain c; 98 | c.addFilter(std::make_shared >()); 99 | c.addFilter(std::make_shared >()); 100 | c.registerCallback(std::bind(&Helper::cb, &h)); 101 | 102 | c.add(std::make_shared()); 103 | EXPECT_EQ(h.count_, 1); 104 | 105 | c.addFilter(std::make_shared >()); 106 | c.addFilter(std::make_shared >()); 107 | 108 | c.add(std::make_shared()); 109 | EXPECT_EQ(h.count_, 2); 110 | } 111 | 112 | TEST(Chain, inputFilter) 113 | { 114 | Helper h; 115 | Chain c; 116 | c.addFilter(std::make_shared >()); 117 | c.registerCallback(std::bind(&Helper::cb, &h)); 118 | 119 | PassThrough p; 120 | c.connectInput(p); 121 | p.add(std::make_shared()); 122 | EXPECT_EQ(h.count_, 1); 123 | 124 | p.add(std::make_shared()); 125 | EXPECT_EQ(h.count_, 2); 126 | } 127 | 128 | TEST(Chain, nonSharedPtrFilter) 129 | { 130 | Helper h; 131 | Chain c; 132 | PassThrough p; 133 | c.addFilter(&p); 134 | c.registerCallback(std::bind(&Helper::cb, &h)); 135 | 136 | c.add(std::make_shared()); 137 | EXPECT_EQ(h.count_, 1); 138 | c.add(std::make_shared()); 139 | EXPECT_EQ(h.count_, 2); 140 | } 141 | 142 | TEST(Chain, retrieveFilter) 143 | { 144 | Chain c; 145 | 146 | ASSERT_FALSE(c.getFilter >(0)); 147 | 148 | c.addFilter(std::make_shared >()); 149 | 150 | ASSERT_TRUE(c.getFilter >(0)); 151 | ASSERT_FALSE(c.getFilter >(1)); 152 | } 153 | 154 | TEST(Chain, retrieveFilterThroughBaseClass) 155 | { 156 | Chain c; 157 | ChainBase* cb = &c; 158 | 159 | ASSERT_FALSE(cb->getFilter >(0)); 160 | 161 | c.addFilter(std::make_shared >()); 162 | 163 | ASSERT_TRUE(cb->getFilter >(0)); 164 | ASSERT_FALSE(cb->getFilter >(1)); 165 | } 166 | 167 | struct PTDerived : public PassThrough 168 | { 169 | 170 | }; 171 | 172 | TEST(Chain, retrieveBaseClass) 173 | { 174 | Chain c; 175 | c.addFilter(std::make_shared()); 176 | ASSERT_TRUE(c.getFilter >(0)); 177 | ASSERT_TRUE(c.getFilter(0)); 178 | } 179 | 180 | int main(int argc, char **argv){ 181 | testing::InitGoogleTest(&argc, argv); 182 | rclcpp::init(argc, argv); 183 | return RUN_ALL_TESTS(); 184 | } 185 | 186 | 187 | -------------------------------------------------------------------------------- /test/test_exact_time_policy.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include "message_filters/synchronizer.h" 39 | #include "message_filters/sync_policies/exact_time.h" 40 | 41 | using namespace message_filters; 42 | using namespace message_filters::sync_policies; 43 | 44 | struct Header 45 | { 46 | rclcpp::Time stamp; 47 | }; 48 | 49 | struct Msg 50 | { 51 | Header header; 52 | int data; 53 | }; 54 | typedef std::shared_ptr MsgPtr; 55 | typedef std::shared_ptr MsgConstPtr; 56 | 57 | namespace message_filters 58 | { 59 | namespace message_traits 60 | { 61 | template<> 62 | struct TimeStamp 63 | { 64 | static rclcpp::Time value(const Msg& m) 65 | { 66 | return m.header.stamp; 67 | } 68 | }; 69 | } 70 | } 71 | 72 | class Helper 73 | { 74 | public: 75 | Helper() 76 | : count_(0) 77 | , drop_count_(0) 78 | {} 79 | 80 | void cb() 81 | { 82 | ++count_; 83 | } 84 | 85 | void dropcb() 86 | { 87 | ++drop_count_; 88 | } 89 | 90 | int32_t count_; 91 | int32_t drop_count_; 92 | }; 93 | 94 | typedef ExactTime Policy2; 95 | typedef ExactTime Policy3; 96 | typedef Synchronizer Sync2; 97 | typedef Synchronizer Sync3; 98 | 99 | ////////////////////////////////////////////////////////////////////////////////////////////////// 100 | // From here on we assume that testing the 3-message version is sufficient, so as not to duplicate 101 | // tests for everywhere from 2-9 102 | ////////////////////////////////////////////////////////////////////////////////////////////////// 103 | TEST(ExactTime, multipleTimes) 104 | { 105 | Sync3 sync(2); 106 | Helper h; 107 | sync.registerCallback(std::bind(&Helper::cb, &h)); 108 | MsgPtr m(std::make_shared()); 109 | m->header.stamp = rclcpp::Time(); 110 | 111 | sync.add<0>(m); 112 | ASSERT_EQ(h.count_, 0); 113 | 114 | m = std::make_shared(); 115 | m->header.stamp = rclcpp::Time(100000000); 116 | sync.add<1>(m); 117 | ASSERT_EQ(h.count_, 0); 118 | sync.add<0>(m); 119 | ASSERT_EQ(h.count_, 0); 120 | sync.add<2>(m); 121 | ASSERT_EQ(h.count_, 1); 122 | } 123 | 124 | TEST(ExactTime, queueSize) 125 | { 126 | Sync3 sync(1); 127 | Helper h; 128 | sync.registerCallback(std::bind(&Helper::cb, &h)); 129 | MsgPtr m(std::make_shared()); 130 | m->header.stamp = rclcpp::Time(); 131 | 132 | sync.add<0>(m); 133 | ASSERT_EQ(h.count_, 0); 134 | sync.add<1>(m); 135 | ASSERT_EQ(h.count_, 0); 136 | 137 | m = std::make_shared(); 138 | m->header.stamp = rclcpp::Time(100000000); 139 | sync.add<1>(m); 140 | ASSERT_EQ(h.count_, 0); 141 | 142 | m = std::make_shared(); 143 | m->header.stamp = rclcpp::Time(); 144 | sync.add<1>(m); 145 | ASSERT_EQ(h.count_, 0); 146 | sync.add<2>(m); 147 | ASSERT_EQ(h.count_, 0); 148 | } 149 | 150 | TEST(ExactTime, dropCallback) 151 | { 152 | Sync2 sync(1); 153 | Helper h; 154 | sync.registerCallback(std::bind(&Helper::cb, &h)); 155 | sync.getPolicy()->registerDropCallback(std::bind(&Helper::dropcb, &h)); 156 | MsgPtr m(std::make_shared()); 157 | m->header.stamp = rclcpp::Time(); 158 | 159 | sync.add<0>(m); 160 | ASSERT_EQ(h.drop_count_, 0); 161 | m->header.stamp = rclcpp::Time(100000000); 162 | sync.add<0>(m); 163 | 164 | ASSERT_EQ(h.drop_count_, 1); 165 | } 166 | 167 | struct EventHelper 168 | { 169 | void callback(const MessageEvent& e1, const MessageEvent& e2) 170 | { 171 | e1_ = e1; 172 | e2_ = e2; 173 | } 174 | 175 | MessageEvent e1_; 176 | MessageEvent e2_; 177 | }; 178 | 179 | TEST(ExactTime, eventInEventOut) 180 | { 181 | Sync2 sync(2); 182 | EventHelper h; 183 | sync.registerCallback(&EventHelper::callback, &h); 184 | MessageEvent evt(std::make_shared(), rclcpp::Time(4, 0)); 185 | 186 | sync.add<0>(evt); 187 | sync.add<1>(evt); 188 | 189 | ASSERT_TRUE(h.e1_.getMessage()); 190 | ASSERT_TRUE(h.e2_.getMessage()); 191 | ASSERT_EQ(h.e1_.getReceiptTime(), evt.getReceiptTime()); 192 | ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime()); 193 | } 194 | 195 | int main(int argc, char **argv){ 196 | testing::InitGoogleTest(&argc, argv); 197 | rclcpp::init(argc, argv); 198 | return RUN_ALL_TESTS(); 199 | } 200 | 201 | 202 | -------------------------------------------------------------------------------- /test/test_fuzz.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include "message_filters/subscriber.h" 40 | #include "message_filters/time_sequencer.h" 41 | #include "message_filters/time_synchronizer.h" 42 | #include "message_filters/chain.h" 43 | #include "sensor_msgs/msg/imu.hpp" 44 | 45 | using namespace message_filters; 46 | typedef sensor_msgs::msg::Imu Msg; 47 | typedef std::shared_ptr MsgConstPtr; 48 | typedef std::shared_ptr MsgPtr; 49 | 50 | class Helper 51 | { 52 | public: 53 | Helper() 54 | : count_(0) 55 | {} 56 | 57 | void cb(const MsgConstPtr&) 58 | { 59 | ++count_; 60 | } 61 | 62 | void cb2(const MsgConstPtr&, const MsgConstPtr&) 63 | { 64 | ++count_; 65 | } 66 | int32_t count_; 67 | }; 68 | 69 | static void fuzz_msg(MsgPtr msg) { 70 | static std::random_device seeder; 71 | std::mt19937 gen(seeder()); 72 | std::uniform_real_distribution distr(1.0, 3.0); 73 | msg->linear_acceleration.x = distr(gen); 74 | msg->linear_acceleration.y = distr(gen); 75 | msg->linear_acceleration.z = distr(gen); 76 | } 77 | 78 | TEST(TimeSequencer, fuzz_sequencer) 79 | { 80 | rclcpp::Node::SharedPtr nh = std::make_shared("test_node"); 81 | TimeSequencer seq(rclcpp::Duration(0, 10000000), rclcpp::Duration(0, 1000000), 10, nh); 82 | Helper h; 83 | seq.registerCallback(std::bind(&Helper::cb, &h, _1)); 84 | rclcpp::Clock ros_clock; 85 | auto start = ros_clock.now(); 86 | auto msg = std::make_shared(); 87 | while ((ros_clock.now() - start) < rclcpp::Duration(5.0, 0)) { 88 | h.count_ = 0; 89 | fuzz_msg(msg); 90 | msg->header.stamp = ros_clock.now(); 91 | seq.add(msg); 92 | 93 | rclcpp::Rate(20).sleep(); 94 | ASSERT_EQ(h.count_, 0); 95 | rclcpp::spin_some(seq.get_node()); 96 | rclcpp::Rate(100).sleep(); 97 | rclcpp::spin_some(seq.get_node()); 98 | ASSERT_EQ(h.count_, 1); 99 | } 100 | } 101 | 102 | TEST(TimeSynchronizer, fuzz_synchronizer) 103 | { 104 | TimeSynchronizer sync(1); 105 | Helper h; 106 | sync.registerCallback(std::bind(&Helper::cb2, &h, _1, _2)); 107 | 108 | rclcpp::Clock ros_clock; 109 | auto start = ros_clock.now(); 110 | auto msg1 = std::make_shared(); 111 | auto msg2 = std::make_shared(); 112 | while ((ros_clock.now() - start) < rclcpp::Duration(5.0, 0)) 113 | { 114 | h.count_ = 0; 115 | fuzz_msg(msg1); 116 | msg1->header.stamp = rclcpp::Clock().now(); 117 | fuzz_msg(msg2); 118 | msg2->header.stamp = msg1->header.stamp; 119 | sync.add0(msg1); 120 | ASSERT_EQ(h.count_, 0); 121 | sync.add1(msg2); 122 | ASSERT_EQ(h.count_, 1); 123 | rclcpp::Rate(50).sleep(); 124 | } 125 | } 126 | 127 | TEST(Subscriber, fuzz_subscriber) 128 | { 129 | auto nh = std::make_shared("test_node"); 130 | Helper h; 131 | Subscriber sub(nh.get(), "test_topic"); 132 | sub.registerCallback(std::bind(&Helper::cb, &h, _1)); 133 | auto pub = nh->create_publisher("test_topic"); 134 | rclcpp::Clock ros_clock; 135 | auto start = ros_clock.now(); 136 | auto msg = std::make_shared(); 137 | while ((ros_clock.now() - start) < rclcpp::Duration(5.0, 0)) 138 | { 139 | h.count_ = 0; 140 | fuzz_msg(msg); 141 | msg->header.stamp = ros_clock.now(); 142 | pub->publish(msg); 143 | rclcpp::Rate(50).sleep(); 144 | rclcpp::spin_some(nh); 145 | ASSERT_EQ(h.count_, 1); 146 | } 147 | rclcpp::spin_some(nh); 148 | } 149 | 150 | int main(int argc, char **argv){ 151 | testing::InitGoogleTest(&argc, argv); 152 | 153 | rclcpp::init(argc, argv); 154 | 155 | return RUN_ALL_TESTS(); 156 | } 157 | 158 | 159 | -------------------------------------------------------------------------------- /test/test_message_filters_cache.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright 2015 Martin Llofriu, Open Source Robotics Foundation, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | import rclpy 35 | import unittest 36 | import time 37 | from builtin_interfaces.msg import Time 38 | from std_msgs.msg import String 39 | 40 | from message_filters import Cache, Subscriber, stamp_time 41 | from rclpy.constants import S_TO_NS 42 | 43 | 44 | PKG = 'message_filters' 45 | 46 | 47 | class AnonymMsg: 48 | class AnonymHeader: 49 | stamp = None 50 | 51 | def __init__(self): 52 | stamp = time.monotonic() 53 | 54 | header = None 55 | 56 | def __init__(self): 57 | self.header = AnonymMsg.AnonymHeader() 58 | 59 | 60 | class TestCache(unittest.TestCase): 61 | @classmethod 62 | def setUpClass(cls): 63 | rclpy.init() 64 | cls.node = rclpy.create_node('my_node', namespace='/my_ns') 65 | 66 | @classmethod 67 | def tearDownClass(cls): 68 | cls.node.destroy_node() 69 | rclpy.shutdown() 70 | 71 | def test_all_funcs(self): 72 | sub = Subscriber(self.node, String, "/empty") 73 | cache = Cache(sub, 5) 74 | 75 | msg = AnonymMsg() 76 | msg.header.stamp = Time(sec=0) 77 | cache.add(msg) 78 | 79 | msg = AnonymMsg() 80 | msg.header.stamp = Time(sec=1) 81 | cache.add(msg) 82 | 83 | msg = AnonymMsg() 84 | msg.header.stamp = Time(sec=2) 85 | cache.add(msg) 86 | 87 | msg = AnonymMsg() 88 | msg.header.stamp = Time(sec=3) 89 | cache.add(msg) 90 | 91 | msg = AnonymMsg() 92 | msg.header.stamp = Time(sec=4) 93 | cache.add(msg) 94 | 95 | l = len(cache.getInterval(2.5, 3.5)) 96 | self.assertEqual(l, 1, "invalid number of messages" + 97 | " returned in getInterval 1") 98 | 99 | l = len(cache.getInterval(2, 3)) 100 | self.assertEqual(l, 2, "invalid number of messages" + 101 | " returned in getInterval 2") 102 | 103 | l = len(cache.getInterval(0, 4)) 104 | self.assertEqual(l, 5, "invalid number of messages" + 105 | " returned in getInterval 3") 106 | 107 | s = cache.getElemAfterTime(0).header.stamp 108 | self.assertEqual(stamp_time(s), 0, 109 | "invalid msg return by getElemAfterTime") 110 | 111 | s = cache.getElemBeforeTime(3.5).header.stamp 112 | self.assertEqual(stamp_time(s), 3, 113 | "invalid msg return by getElemBeforeTime") 114 | 115 | s = cache.getLastestTime() 116 | self.assertEqual(s, 4, 117 | "invalid stamp return by getLastestTime") 118 | 119 | s = cache.getOldestTime() 120 | self.assertEqual(s, 0, 121 | "invalid stamp return by getOldestTime") 122 | 123 | # Add another msg to fill the buffer 124 | msg = AnonymMsg() 125 | msg.header.stamp = Time(sec=5) 126 | cache.add(msg) 127 | 128 | # Test that it discarded the right one 129 | s = cache.getOldestTime() 130 | self.assertEqual(s, 1, 131 | "wrong message discarded") 132 | 133 | ''' 134 | def test_headerless(self): 135 | sub = Subscriber("/empty", String) 136 | cache = Cache(sub, 5, allow_headerless=False) 137 | 138 | msg = String() 139 | cache.add(msg) 140 | 141 | self.assertIsNone(cache.getElemAfterTime(Time(sec=0)), 142 | "Headerless message invalidly added.") 143 | 144 | cache = Cache(sub, 5, allow_headerless=True) 145 | 146 | rospy.rostime.set_rostime_initialized(True) 147 | 148 | rospy.rostime._set_rostime(Time(sec=0)) 149 | cache.add(msg) 150 | 151 | s = cache.getElemAfterTime(Time(sec=0)) 152 | self.assertEqual(s, msg, 153 | "invalid msg returned in headerless scenario") 154 | 155 | s = cache.getElemAfterTime(Time(sec=1)) 156 | self.assertIsNone(s, "invalid msg returned in headerless scenario") 157 | 158 | rospy.rostime._set_rostime(Time(sec=2)) 159 | cache.add(msg) 160 | 161 | s = cache.getInterval(Time(sec=0), Time(sec=1)) 162 | self.assertEqual(s, [msg], 163 | "invalid msg returned in headerless scenario") 164 | 165 | s = cache.getInterval(Time(sec=0), Time(sec=2)) 166 | self.assertEqual(s, [msg, msg], 167 | "invalid msg returned in headerless scenario") 168 | ''' 169 | 170 | if __name__ == '__main__': 171 | unittest.main() 172 | -------------------------------------------------------------------------------- /test/test_simple.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include "message_filters/simple_filter.h" 42 | 43 | using namespace message_filters; 44 | using namespace std::placeholders; 45 | 46 | struct Msg 47 | { 48 | }; 49 | typedef std::shared_ptr MsgPtr; 50 | typedef std::shared_ptr MsgConstPtr; 51 | 52 | struct Filter : public SimpleFilter 53 | { 54 | typedef MessageEvent EventType; 55 | 56 | void add(const EventType& evt) 57 | { 58 | signalMessage(evt); 59 | } 60 | }; 61 | 62 | class Helper 63 | { 64 | public: 65 | Helper() 66 | { 67 | counts_.fill(0); 68 | } 69 | 70 | void cb0(const MsgConstPtr&) 71 | { 72 | ++counts_[0]; 73 | } 74 | 75 | void cb1(const Msg&) 76 | { 77 | ++counts_[1]; 78 | } 79 | 80 | void cb2(MsgConstPtr) 81 | { 82 | ++counts_[2]; 83 | } 84 | 85 | void cb3(const MessageEvent&) 86 | { 87 | ++counts_[3]; 88 | } 89 | 90 | void cb4(Msg) 91 | { 92 | ++counts_[4]; 93 | } 94 | 95 | void cb5(const MsgPtr&) 96 | { 97 | ++counts_[5]; 98 | } 99 | 100 | void cb6(MsgPtr) 101 | { 102 | ++counts_[6]; 103 | } 104 | 105 | void cb7(const MessageEvent&) 106 | { 107 | ++counts_[7]; 108 | } 109 | 110 | std::array counts_; 111 | }; 112 | 113 | TEST(SimpleFilter, callbackTypes) 114 | { 115 | Helper h; 116 | Filter f; 117 | f.registerCallback(std::bind(&Helper::cb0, &h, _1)); 118 | f.registerCallback(std::bind(&Helper::cb1, &h, _1)); 119 | f.registerCallback(std::bind(&Helper::cb2, &h, _1)); 120 | f.registerCallback&>(std::bind(&Helper::cb3, &h, _1)); 121 | f.registerCallback(std::bind(&Helper::cb4, &h, _1)); 122 | f.registerCallback(std::bind(&Helper::cb5, &h, _1)); 123 | f.registerCallback(std::bind(&Helper::cb6, &h, _1)); 124 | f.registerCallback&>(std::bind(&Helper::cb7, &h, _1)); 125 | 126 | f.add(Filter::EventType(std::make_shared())); 127 | EXPECT_EQ(h.counts_[0], 1); 128 | EXPECT_EQ(h.counts_[1], 1); 129 | EXPECT_EQ(h.counts_[2], 1); 130 | EXPECT_EQ(h.counts_[3], 1); 131 | EXPECT_EQ(h.counts_[4], 1); 132 | EXPECT_EQ(h.counts_[5], 1); 133 | EXPECT_EQ(h.counts_[6], 1); 134 | EXPECT_EQ(h.counts_[7], 1); 135 | } 136 | 137 | struct OldFilter 138 | { 139 | Connection registerCallback(const std::function&) 140 | { 141 | return Connection(); 142 | } 143 | }; 144 | 145 | TEST(SimpleFilter, oldRegisterWithNewFilter) 146 | { 147 | OldFilter f; 148 | Helper h; 149 | f.registerCallback(std::bind(&Helper::cb3, &h, _1)); 150 | } 151 | 152 | int main(int argc, char **argv){ 153 | testing::InitGoogleTest(&argc, argv); 154 | rclcpp::init(argc, argv); 155 | return RUN_ALL_TESTS(); 156 | } 157 | 158 | 159 | 160 | -------------------------------------------------------------------------------- /test/test_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include "message_filters/subscriber.h" 39 | #include "message_filters/chain.h" 40 | #include "sensor_msgs/msg/imu.hpp" 41 | 42 | using namespace message_filters; 43 | typedef sensor_msgs::msg::Imu Msg; 44 | typedef std::shared_ptr MsgConstPtr; 45 | typedef std::shared_ptr MsgPtr; 46 | 47 | class Helper 48 | { 49 | public: 50 | Helper() 51 | : count_(0) 52 | {} 53 | 54 | void cb(const MsgConstPtr&) 55 | { 56 | ++count_; 57 | } 58 | 59 | int32_t count_; 60 | }; 61 | 62 | TEST(Subscriber, simple) 63 | { 64 | auto nh = std::make_shared("test_node"); 65 | Helper h; 66 | Subscriber sub(nh.get(), "test_topic"); 67 | sub.registerCallback(std::bind(&Helper::cb, &h, _1)); 68 | auto pub = nh->create_publisher("test_topic"); 69 | rclcpp::Clock ros_clock; 70 | auto start = ros_clock.now(); 71 | while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1.0, 0)) 72 | { 73 | pub->publish(std::make_shared()); 74 | rclcpp::Rate(50).sleep(); 75 | rclcpp::spin_some(nh); 76 | } 77 | 78 | ASSERT_GT(h.count_, 0); 79 | } 80 | 81 | TEST(Subscriber, subUnsubSub) 82 | { 83 | auto nh = std::make_shared("test_node"); 84 | Helper h; 85 | Subscriber sub(nh.get(), "test_topic"); 86 | sub.registerCallback(std::bind(&Helper::cb, &h, _1)); 87 | auto pub = nh->create_publisher("test_topic"); 88 | 89 | sub.unsubscribe(); 90 | sub.subscribe(); 91 | 92 | rclcpp::Clock ros_clock; 93 | auto start = ros_clock.now(); 94 | while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1.0, 0)) 95 | { 96 | pub->publish(std::make_shared()); 97 | rclcpp::Rate(50).sleep(); 98 | rclcpp::spin_some(nh); 99 | } 100 | 101 | ASSERT_GT(h.count_, 0); 102 | } 103 | 104 | TEST(Subscriber, subInChain) 105 | { 106 | auto nh = std::make_shared("test_node"); 107 | Helper h; 108 | Chain c; 109 | c.addFilter(std::make_shared >(nh.get(), "test_topic")); 110 | c.registerCallback(std::bind(&Helper::cb, &h, _1)); 111 | auto pub = nh->create_publisher("test_topic"); 112 | 113 | rclcpp::Clock ros_clock; 114 | auto start = ros_clock.now(); 115 | while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1.0, 0)) 116 | { 117 | pub->publish(std::make_shared()); 118 | rclcpp::Rate(50).sleep(); 119 | rclcpp::spin_some(nh); 120 | } 121 | 122 | ASSERT_GT(h.count_, 0); 123 | } 124 | 125 | struct ConstHelper 126 | { 127 | void cb(const MsgConstPtr& msg) 128 | { 129 | msg_ = msg; 130 | } 131 | 132 | MsgConstPtr msg_; 133 | }; 134 | 135 | struct NonConstHelper 136 | { 137 | void cb(const MsgPtr& msg) 138 | { 139 | msg_ = msg; 140 | } 141 | 142 | MsgPtr msg_; 143 | }; 144 | 145 | TEST(Subscriber, singleNonConstCallback) 146 | { 147 | auto nh = std::make_shared("test_node"); 148 | NonConstHelper h; 149 | Subscriber sub(nh.get(), "test_topic"); 150 | sub.registerCallback(&NonConstHelper::cb, &h); 151 | auto pub = nh->create_publisher("test_topic"); 152 | pub->publish(std::make_shared()); 153 | 154 | rclcpp::Rate(50).sleep(); 155 | rclcpp::spin_some(nh); 156 | 157 | ASSERT_TRUE(h.msg_); 158 | // ASSERT_EQ(msg.get(), h.msg_.get()); 159 | } 160 | 161 | TEST(Subscriber, multipleNonConstCallbacksFilterSubscriber) 162 | { 163 | auto nh = std::make_shared("test_node"); 164 | NonConstHelper h, h2; 165 | Subscriber sub(nh.get(), "test_topic"); 166 | sub.registerCallback(&NonConstHelper::cb, &h); 167 | sub.registerCallback(&NonConstHelper::cb, &h2); 168 | auto pub = nh->create_publisher("test_topic"); 169 | pub->publish(std::make_shared()); 170 | 171 | rclcpp::Rate(50).sleep(); 172 | rclcpp::spin_some(nh); 173 | 174 | ASSERT_TRUE(h.msg_); 175 | ASSERT_TRUE(h2.msg_); 176 | // EXPECT_NE(msg.get(), h.msg_.get()); 177 | // EXPECT_NE(msg.get(), h2.msg_.get()); 178 | // EXPECT_NE(h.msg_.get(), h2.msg_.get()); 179 | } 180 | 181 | TEST(Subscriber, multipleCallbacksSomeFilterSomeDirect) 182 | { 183 | auto nh = std::make_shared("test_node"); 184 | NonConstHelper h, h2; 185 | Subscriber sub(nh.get(), "test_topic"); 186 | sub.registerCallback(&NonConstHelper::cb, &h); 187 | // ros::Subscriber sub2 = nh.subscribe("test_topic", 0, &NonConstHelper::cb, &h2); 188 | // ros::Publisher pub = nh.advertise("test_topic", 0); 189 | 190 | auto pub = nh->create_publisher("test_topic"); 191 | pub->publish(std::make_shared()); 192 | 193 | rclcpp::Rate(50).sleep(); 194 | rclcpp::spin_some(nh); 195 | 196 | ASSERT_TRUE(h.msg_); 197 | // ASSERT_TRUE(h2.msg_); 198 | // EXPECT_NE(msg.get(), h.msg_.get()); 199 | // EXPECT_NE(msg.get(), h2.msg_.get()); 200 | // EXPECT_NE(h.msg_.get(), h2.msg_.get()); 201 | } 202 | 203 | int main(int argc, char **argv){ 204 | testing::InitGoogleTest(&argc, argv); 205 | 206 | rclcpp::init(argc, argv); 207 | 208 | return RUN_ALL_TESTS(); 209 | } 210 | 211 | 212 | -------------------------------------------------------------------------------- /test/test_subscriber.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /test/time_sequencer_unittest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include "message_filters/time_sequencer.h" 39 | 40 | using namespace message_filters; 41 | 42 | struct Header 43 | { 44 | rclcpp::Time stamp; 45 | }; 46 | 47 | 48 | struct Msg 49 | { 50 | Header header; 51 | int data; 52 | }; 53 | typedef std::shared_ptr MsgPtr; 54 | typedef std::shared_ptr MsgConstPtr; 55 | 56 | namespace message_filters 57 | { 58 | namespace message_traits 59 | { 60 | template<> 61 | struct TimeStamp 62 | { 63 | static rclcpp::Time value(const Msg& m) 64 | { 65 | return m.header.stamp; 66 | } 67 | }; 68 | } 69 | } 70 | 71 | class Helper 72 | { 73 | public: 74 | Helper() 75 | : count_(0) 76 | {} 77 | 78 | void cb(const MsgConstPtr&) 79 | { 80 | ++count_; 81 | } 82 | 83 | int32_t count_; 84 | }; 85 | 86 | TEST(TimeSequencer, simple) 87 | { 88 | TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10); 89 | Helper h; 90 | seq.registerCallback(std::bind(&Helper::cb, &h, _1)); 91 | MsgPtr msg(std::make_shared()); 92 | msg->header.stamp = rclcpp::Clock().now(); 93 | seq.add(msg); 94 | 95 | rclcpp::Rate(10).sleep(); 96 | rclcpp::spin_some(seq.get_node()); 97 | ASSERT_EQ(h.count_, 0); 98 | 99 | // ros::Time::setNow(rclcpp::Clock(RCL_ROS_TIME)::now() + rclcpp::Duration(2, 0)); 100 | 101 | rclcpp::Rate(1).sleep(); 102 | rclcpp::spin_some(seq.get_node()); 103 | 104 | ASSERT_EQ(h.count_, 1); 105 | } 106 | 107 | TEST(TimeSequencer, compilation) 108 | { 109 | TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10); 110 | TimeSequencer seq2(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10); 111 | seq2.connectInput(seq); 112 | } 113 | 114 | struct EventHelper 115 | { 116 | public: 117 | void cb(const MessageEvent& evt) 118 | { 119 | event_ = evt; 120 | } 121 | 122 | MessageEvent event_; 123 | }; 124 | TEST(TimeSequencer, eventInEventOut) 125 | { 126 | rclcpp::Node::SharedPtr nh = std::make_shared("test_node"); 127 | TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, nh); 128 | TimeSequencer seq2(seq, rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, nh); 129 | EventHelper h; 130 | seq2.registerCallback(&EventHelper::cb, &h); 131 | 132 | MessageEvent evt(std::make_shared(), rclcpp::Clock().now()); 133 | seq.add(evt); 134 | 135 | // ros::Time::setNow(ros::Time::now() + ros::Duration(2)); 136 | rclcpp::Rate(0.5).sleep(); 137 | while (!h.event_.getMessage()) 138 | { 139 | rclcpp::Rate(100).sleep(); 140 | rclcpp::spin_some(nh); 141 | } 142 | 143 | EXPECT_EQ(h.event_.getReceiptTime(), evt.getReceiptTime()); 144 | EXPECT_EQ(h.event_.getMessage(), evt.getMessage()); 145 | } 146 | 147 | int main(int argc, char **argv){ 148 | testing::InitGoogleTest(&argc, argv); 149 | 150 | rclcpp::init(argc, argv); 151 | 152 | return RUN_ALL_TESTS(); 153 | } 154 | 155 | 156 | -------------------------------------------------------------------------------- /test/time_sequencer_unittest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | --------------------------------------------------------------------------------