├── .github └── workflows │ └── ros2_ci.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── default_plugins.xml ├── include └── filters │ ├── filter_base.h │ ├── filter_base.hpp │ ├── filter_chain.h │ ├── filter_chain.hpp │ ├── increment.h │ ├── increment.hpp │ ├── mean.h │ ├── mean.hpp │ ├── median.h │ ├── median.hpp │ ├── param_test.h │ ├── param_test.hpp │ ├── realtime_circular_buffer.h │ ├── realtime_circular_buffer.hpp │ ├── transfer_function.h │ └── transfer_function.hpp ├── package.xml ├── src ├── increment.cpp ├── mean.cpp ├── median.cpp ├── test_params.cpp └── transfer_function.cpp └── test ├── test_chain.cpp ├── test_mean.cpp ├── test_median.cpp ├── test_params.cpp ├── test_realtime_circular_buffer.cpp └── test_transfer_function.cpp /.github/workflows/ros2_ci.yml: -------------------------------------------------------------------------------- 1 | name: Build and run ROS2 tests 2 | 3 | on: 4 | push: 5 | branches: [ ros2 ] 6 | pull_request: 7 | branches: [ ros2 ] 8 | 9 | jobs: 10 | build: 11 | strategy: 12 | matrix: 13 | rosdistro: [humble, jazzy, rolling] 14 | 15 | runs-on: ubuntu-latest 16 | 17 | continue-on-error: true 18 | 19 | container: 20 | image: ros:${{ matrix.rosdistro }}-ros-base 21 | 22 | steps: 23 | - uses: actions/checkout@v2 24 | with: 25 | path: src/filters 26 | 27 | - name: Install dependencies 28 | run: rosdep update --include-eol-distros && apt-get update && rosdep install --from-path . -i -y --rosdistro ${{ matrix.rosdistro }} 29 | 30 | - name: Build tests 31 | run: . /opt/ros/${{ matrix.rosdistro }}/setup.sh && colcon build --event-handlers console_cohesion+ 32 | 33 | - name: Run tests 34 | run: . /opt/ros/${{ matrix.rosdistro }}/setup.sh && colcon test --event-handlers console_cohesion+ && colcon test-result 35 | 36 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package filters 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.1 (2025-05-14) 6 | ------------------ 7 | * Ignore unused parameter in FilterBase::reconfigureCB() 8 | * Contributors: Christophe Bedard 9 | 10 | 2.2.0 (2025-05-01) 11 | ------------------ 12 | * Handle dynamic reconfiguration of parameters 13 | * Enable the use of a default for parameters 14 | * Configure github action that runs tests 15 | * Add Windows support 16 | * Clean up filter chain parameter loading 17 | * Make read only configurable 18 | * Added parameter already declared check and allowed override 19 | * Contributors: Dominik Moss, Jeanine van Bruggen, Jonathan Binney, Silvio Traversaro 20 | 21 | 2.1.2 (2024-04-17) 22 | ------------------ 23 | * Fix the RHEL-9 build. 24 | * Fix CMake 3.18 25 | * Add a virtual definition of configure() in MultiChannelFilterBase. 26 | * Contributors: Chris Lalancette, Jonathan Binney, Ryan Friedman 27 | 28 | 2.1.1 (2024-03-02) 29 | ------------------ 30 | * Provide target based dependencies in cmake for filter_base and libraries. 31 | * Support Boost 1.71 (Ubuntu 20.04 Humble) 32 | * Allow to iterate over RealtimeCircularBuffer. 33 | * Change function signature to use pointer instead of array. 34 | This really has no downstream consequence, but makes it so that 35 | rosdoc2 can successfully generate documentation. 36 | * Contributors: Chris Lalancette, Daisuke Nishimatsu, Jonathan Binney, 37 | Patrick Roncagliolo, Ryan Friedman, wep21 38 | 39 | 2.1.0 (2021-07-14) 40 | ------------------ 41 | * Fix compiler warnings+test failures on CI (`#56 `_) 42 | * Solve statically parameter error (`#48 `_) 43 | * Contributors: Jon Binney, Patrick Lascombz 44 | 45 | 2.0.1 (2021-07-08) 46 | ------------------ 47 | * Add Jon binney as a maintainer (`#54 `_) 48 | * missing export includes (`#50 `_) 49 | * changed free functions to be inline (`#43 `_) 50 | * Update default_plugins.xml (fix plugin names) 51 | * Contributors: Hang, Jonathan Binney, Marwan Taher, Steve Macenski 52 | 53 | 2.0.0 (2020-05-14) 54 | ------------------ 55 | * Fix a bug in the tests. 56 | * Fixes for Foxy uncrustify. 57 | * Address peer review comments. 58 | * Enforce proper code style. 59 | * Cleanup types and literals. 60 | * Lint all sources. 61 | * Address pending review comments. 62 | * Misc boost fixes 63 | * Fix wrong include guard 64 | * FilterBase to avoid name hiding 65 | * Include what is used 66 | * Alphabetize package deps 67 | * Get rid of unnecessary retval 68 | * Include what is used 69 | * remove unnecessary ';' 70 | * Use `empty()` instead of `size() == 0` 71 | * `Instal` -> `Install` 72 | * Add ament_package() 73 | * Port filters to ROS Eloquent 74 | * [noetic] deprecate h for hpp (`#34 `_) 75 | * [noetic] Delete unused code (`#33 `_) 76 | * Bump CMake version to avoid CMP0048 77 | * Contributors: Alejandro Hernández Cordero, Chris Lalancette, Michel Hidalgo, Shane Loretz 78 | 79 | 1.9.0 (2020-03-10) 80 | ------------------ 81 | * Reduce dependency on boost (`#30 `_) 82 | * Contributors: Shane Loretz 83 | 84 | 1.8.1 (2017-04-25) 85 | ------------------ 86 | * Fix warning about string type 87 | * Contributors: Jon Binney 88 | 89 | 1.8.0 (2017-04-07) 90 | ------------------ 91 | 92 | * Remove promiscuous filter finding 93 | When specifying filters, you must now include the package name and exact 94 | filter name. Previously a workaround would search packages for any filter 95 | with the specified string in the filter name. This has been deprecated for 96 | a while, and here we're removing it. `#14 ` 97 | * Contributors: Jon Binney 98 | 99 | 1.7.5 (2017-03-16) 100 | ------------------ 101 | * make rostest in CMakeLists optional (`ros/rosdistro#3010 `_) 102 | * check for CATKIN_ENABLE_TESTING 103 | * Add support for boolean parameters (fix `#6 `_) 104 | * Contributors: Boris Gromov, Lukas Bulwahn, Tully Foote 105 | 106 | 1.7.4 (2013-07-23) 107 | ------------------ 108 | * Remove trailing whitespace and old export 109 | * Removing vestigial files 110 | * Merge pull request `#5 `_ from ahendrix/hydro-devel 111 | Fix pluginlib macros and createInstance usage. 112 | * Fix pluginlib macros and createInstance usage. 113 | * Contributors: Austin Hendrix, William Woodall, jonbinney 114 | 115 | 1.7.3 (2013-06-27) 116 | ------------------ 117 | * use new pluginlib API 118 | * Contributors: Jon Binney 119 | 120 | 1.7.2 (2013-06-26) 121 | ------------------ 122 | * a bunch of CMake fixes to get tests passing. 123 | * fixing linking 124 | * 1.7.2 125 | For bugfix release 126 | * Merge pull request `#3 `_ from dgossow/patch-1 127 | Export pluginlib as dependency 128 | * Export pluginlib as dependency 129 | * Contributors: David Gossow, Tully Foote 130 | 131 | 1.7.1 (2013-05-24) 132 | ------------------ 133 | * bump version for bugfix 134 | * Merge pull request `#2 `_ from jonbinney/install_headers 135 | added install rule for headers in cmakelists 136 | * added install rule for headers in cmakelists 137 | * Contributors: Jon Binney, jonbinney 138 | 139 | 1.7.0 (2013-05-23) 140 | ------------------ 141 | * bump version for hydro release 142 | * reenabled rostests 143 | * builds with catkin 144 | * branched to separate cpp and ROS parts 145 | --HG-- 146 | branch : cpp_separation 147 | * Added tag unstable for changeset 661a74b486de 148 | --HG-- 149 | branch : filters 150 | * Added tag filters-1.6.0 for changeset 925818adeafe 151 | --HG-- 152 | branch : filters 153 | * 1.6.0 154 | --HG-- 155 | branch : filters 156 | * creating unary stack to refactor from common 157 | --HG-- 158 | branch : filters 159 | * url fix 160 | --HG-- 161 | branch : filters 162 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4037239 163 | * patch for `#4144 `_ including backwards compatability. Also added check to give nice error and quit if invalid filter name, instead of throwing exception 164 | --HG-- 165 | branch : filters 166 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4030358 167 | * rest of fix for `#4181 `_ tests 168 | --HG-- 169 | branch : filters 170 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4030356 171 | * basic tests for `#4181 `_ more to come 172 | --HG-- 173 | branch : filters 174 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4030355 175 | * Added Ubuntu platform tags to manifest 176 | --HG-- 177 | branch : filters 178 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4029647 179 | * removing deprecated TransferFunctionFilter it is replaced by SingleChannelTransferFunctionFilter `#3703 `_ 180 | --HG-- 181 | branch : filters 182 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4028320 183 | * fixing segfault in realtime circular buffer if zero length `#3785 `_ `#3762 `_ 184 | --HG-- 185 | branch : filters 186 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4027710 187 | * adding namespace to all debugging/errors for filter chain loader `#3239 `_ 188 | --HG-- 189 | branch : filters 190 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4026552 191 | * updating the tests 192 | --HG-- 193 | branch : filters 194 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4026106 195 | * adding single channel transferfunctionfilter 196 | --HG-- 197 | branch : filters 198 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4026105 199 | * fixing build for karmic 200 | --HG-- 201 | branch : filters 202 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4025262 203 | * doc reviewed 204 | --HG-- 205 | branch : filters 206 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4024954 207 | * all API issues cleared for filters 208 | --HG-- 209 | branch : filters 210 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4024863 211 | * Fixing warning message in filter_chain.h with regard to `#2959 `_ 212 | --HG-- 213 | branch : filters 214 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4024757 215 | * removing hard codeing of filter_chain parameter list name. `#2618 `_ Backwards compatable statement left in with ROS_WARN to change 216 | --HG-- 217 | branch : filters 218 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4024289 219 | * removing unused dependency on tinyxml 220 | --HG-- 221 | branch : filters 222 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4024141 223 | * Copying commit from latest to trunk. 'Added temporary OSX blacklist files' 224 | --HG-- 225 | branch : filters 226 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4023977 227 | * migration part 1 228 | --HG-- 229 | branch : filters 230 | extra : convert_revision : svn%3Aeb33c2ac-9c88-4c90-87e0-44a10359b0c3/stacks/common/trunk/filters%4023884 231 | * Contributors: Jon Binney, Ken Conley, gerkey, jonbinney, kwc, leibs, mwise, sachinc, tfoote, vpradeep 232 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14.4) 2 | project(filters) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 13 | 14 | ############################################################################## 15 | # Find dependencies 16 | ############################################################################## 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(pluginlib REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(Boost QUIET COMPONENTS headers) 22 | if(NOT Boost_headers_FOUND) 23 | find_package(Boost REQUIRED) 24 | endif() 25 | 26 | ############################################################################## 27 | # Build 28 | ############################################################################## 29 | 30 | 31 | if("${CMAKE_VERSION}" VERSION_GREATER_EQUAL 3.19) 32 | add_library(realtime_circular_buffer 33 | INTERFACE 34 | include/filters/realtime_circular_buffer.h 35 | include/filters/realtime_circular_buffer.hpp 36 | ) 37 | else() 38 | add_library(realtime_circular_buffer INTERFACE) 39 | endif() 40 | 41 | target_link_libraries(realtime_circular_buffer 42 | INTERFACE 43 | Boost::boost 44 | ) 45 | 46 | # filter_base is an INTERFACE library because it only has header files. 47 | # It is not compiled, but sets up the following properties for consumers 48 | # to link to a target. 49 | # For more info, see the CMake Wiki on INTERFACE targets. 50 | # https://cmake.org/cmake/help/latest/command/add_library.html#interface-libraries 51 | 52 | 53 | if("${CMAKE_VERSION}" VERSION_GREATER_EQUAL 3.19) 54 | add_library(filter_base 55 | INTERFACE 56 | include/filters/filter_base.h 57 | include/filters/filter_base.hpp 58 | ) 59 | else() 60 | add_library(filter_base INTERFACE) 61 | endif() 62 | 63 | target_link_libraries(filter_base 64 | INTERFACE 65 | rclcpp::rclcpp 66 | ) 67 | 68 | if("${CMAKE_VERSION}" VERSION_GREATER_EQUAL 3.19) 69 | add_library(filter_chain 70 | INTERFACE 71 | include/filters/filter_chain.h 72 | include/filters/filter_chain.hpp 73 | ) 74 | else() 75 | add_library(filter_chain INTERFACE) 76 | endif() 77 | 78 | 79 | target_link_libraries(filter_chain 80 | INTERFACE 81 | filter_base 82 | pluginlib::pluginlib 83 | ) 84 | 85 | set(interface_targets realtime_circular_buffer filter_base filter_chain) 86 | foreach(target ${interface_targets}) 87 | target_include_directories(${target} 88 | INTERFACE 89 | $ 90 | $ 91 | ) 92 | endforeach() 93 | 94 | 95 | 96 | # Plugins 97 | add_library(mean SHARED src/mean.cpp) 98 | target_link_libraries(mean 99 | PUBLIC 100 | realtime_circular_buffer 101 | filter_base 102 | pluginlib::pluginlib 103 | ) 104 | 105 | add_library(params SHARED src/test_params.cpp) 106 | target_link_libraries(params 107 | PUBLIC 108 | filter_base 109 | pluginlib::pluginlib 110 | ) 111 | 112 | add_library(increment SHARED src/increment.cpp) 113 | target_link_libraries(increment 114 | PUBLIC 115 | realtime_circular_buffer 116 | filter_base 117 | pluginlib::pluginlib 118 | ) 119 | 120 | add_library(median SHARED src/median.cpp) 121 | target_link_libraries(median 122 | PUBLIC 123 | realtime_circular_buffer 124 | filter_base 125 | pluginlib::pluginlib 126 | ) 127 | 128 | add_library(transfer_function SHARED src/transfer_function.cpp) 129 | target_link_libraries(transfer_function 130 | PUBLIC 131 | realtime_circular_buffer 132 | filter_base 133 | pluginlib::pluginlib 134 | ) 135 | 136 | set(plugin_targets mean params increment median transfer_function) 137 | foreach(target ${plugin_targets}) 138 | target_include_directories(${target} 139 | PUBLIC 140 | $ 141 | $ 142 | ) 143 | endforeach() 144 | 145 | install(TARGETS ${interface_targets} ${plugin_targets} 146 | EXPORT export_${PROJECT_NAME} 147 | ARCHIVE DESTINATION lib 148 | LIBRARY DESTINATION lib 149 | RUNTIME DESTINATION bin 150 | ) 151 | 152 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 153 | 154 | if(BUILD_TESTING) 155 | # TODO(hidmic): use ament_lint_common once ament_copyright complains are resolved 156 | find_package(ament_cmake_cppcheck REQUIRED) 157 | find_package(ament_cmake_cpplint REQUIRED) 158 | find_package(ament_cmake_uncrustify REQUIRED) 159 | find_package(ament_cmake_xmllint REQUIRED) 160 | 161 | # Enforce ROS2 style 162 | ament_cpplint() 163 | ament_cppcheck( 164 | LANGUAGE c++ # Force C++ for .h headers 165 | INCLUDE_DIRS include 166 | ) 167 | ament_uncrustify() 168 | ament_xmllint() 169 | 170 | find_package(ament_cmake_gtest REQUIRED) 171 | # Test median filter 172 | ament_add_gtest(median_test test/test_median.cpp) 173 | target_link_libraries(median_test median) 174 | 175 | # Test transfer function filter 176 | ament_add_gtest(transfer_function_test test/test_transfer_function.cpp) 177 | target_link_libraries(transfer_function_test transfer_function) 178 | 179 | # Test mean filter 180 | ament_add_gtest(mean_test test/test_mean.cpp) 181 | target_link_libraries(mean_test mean) 182 | 183 | # Test params filter 184 | ament_add_gtest(params_test test/test_params.cpp) 185 | target_link_libraries(params_test params) 186 | 187 | # Test realtime safe buffer class 188 | ament_add_gtest(realtime_buffer_test test/test_realtime_circular_buffer.cpp) 189 | target_link_libraries(realtime_buffer_test realtime_circular_buffer) 190 | 191 | # Pluginlib specific testing for filter chain 192 | set(test_prefix "${CMAKE_CURRENT_BINARY_DIR}/pluginlib_test_prefix") 193 | set(test_package_name "${PROJECT_NAME}") 194 | set(test_plugins_xml "default_plugins.xml") 195 | set(test_package_xml "package.xml") 196 | set(test_plugin_targets mean increment median transfer_function) 197 | 198 | # Write the necessary files to detect a package with pluginlib plugins 199 | file(COPY "${test_plugins_xml}" DESTINATION "${test_prefix}/share/${test_package_name}/") 200 | configure_file("${test_package_xml}" "${test_prefix}/share/${test_package_name}/package.xml" COPYONLY) 201 | file(WRITE "${test_prefix}/share/ament_index/resource_index/${test_package_name}__pluginlib__plugin/pluginlib" 202 | "share/${test_package_name}/${test_plugins_xml}\n") 203 | file(WRITE "${test_prefix}/share/ament_index/resource_index/packages/${test_package_name}" "") 204 | 205 | # Copy library files to test folder so they can be loaded 206 | foreach(test_plugin_target IN LISTS test_plugin_targets) 207 | set(_resource "$") 208 | set(_output "${test_prefix}/lib/$") 209 | add_custom_command(TARGET ${test_plugin_target} 210 | COMMENT "Copying target ${test_plugin_target} for testing" 211 | DEPENDS "${_resource}" 212 | COMMAND ${CMAKE_COMMAND} -E copy_if_different 213 | "${_resource}" 214 | "${_output}" 215 | ) 216 | add_custom_target("copy_${test_plugin_target}_for_testing" DEPENDS "${test_plugin_target}") 217 | endforeach() 218 | 219 | # Test plugin loading into filter chain 220 | ament_add_gtest(chain_test test/test_chain.cpp 221 | ENV "AMENT_PREFIX_PATH=${test_prefix}") 222 | target_include_directories(chain_test PRIVATE include) 223 | foreach(test_plugin_target IN LISTS test_plugin_targets) 224 | add_dependencies(chain_test "copy_${test_plugin_target}_for_testing") 225 | endforeach() 226 | target_link_libraries(chain_test 227 | filter_chain 228 | ) 229 | endif() 230 | 231 | ############################################################################## 232 | # Install 233 | ############################################################################## 234 | 235 | # Export old-style CMake variables for includes 236 | ament_export_include_directories("include/${PROJECT_NAME}") 237 | 238 | # Export modern CMake targets 239 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 240 | 241 | ament_export_dependencies( 242 | Boost 243 | pluginlib 244 | rclcpp 245 | ) 246 | 247 | # Install pluginlib xml 248 | pluginlib_export_plugin_description_file(filters "default_plugins.xml") 249 | ament_package() 250 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Filters 2 | 3 | ## Usage with ament_cmake 4 | 5 | Here is recommended approach on how to link `filters` to your project, using the `filters::realtime_circular_buffer` target. 6 | 7 | ```cmake 8 | find_package(filters CONFIG REQUIRED) 9 | 10 | add_library(my_library) 11 | # Other library stuff here 12 | 13 | target_link_libraries(my_library PUBLIC filters::realtime_circular_buffer) 14 | ``` 15 | 16 | For more information on using ament_cmake, 17 | see the [ament_cmake](https://docs.ros.org/en/rolling/How-To-Guides/Ament-CMake-Documentation.html) 18 | tutorial. 19 | 20 | Filters creates all of the following CMake targets, including: 21 | * filters::realtime_circular_buffer 22 | * filters::filter_chain 23 | * filters::mean 24 | * filters::params 25 | * filters::increment 26 | * filters::median 27 | * filters::transfer_function 28 | 29 | It is recommended to only link to the libraries needed. 30 | Linking to `filters::filter_base` pulls in all necessary libraries and include directories for targets with classes that extend `FilterBase`. This is useful if you are writing your own filter. 31 | -------------------------------------------------------------------------------- /default_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | This is a median filter which works on a stream of std::vector of doubles. 7 | 8 | 9 | 11 | 12 | This is a median filter which works on a stream of doubles. 13 | 14 | 15 | 17 | 18 | This is a median filter which works on a stream of std::vector of floats. 19 | 20 | 21 | 23 | 24 | This is a median filter which works on a stream of floats. 25 | 26 | 27 | 28 | 29 | 31 | 32 | This is a mean filter which works on a stream of doubles. 33 | 34 | 35 | 37 | 38 | This is a mean filter which works on a stream of floats. 39 | 40 | 41 | 43 | 44 | This is a mean filter which works on a stream of vectors of doubles. 45 | 46 | 47 | 49 | 50 | This is a mean filter which works on a stream of vectors of floats. 51 | 52 | 53 | 54 | 55 | 57 | 58 | This is a increment filter which works on a stream of ints. 59 | 60 | 61 | 63 | 64 | This is a increment filter which works on a stream of vectors of ints. 65 | 66 | 67 | 68 | 69 | 71 | 72 | This is a transfer filter which works on a stream of doubles. 73 | 74 | 75 | 77 | 78 | This is a transfer filter which works on a stream of doubles. 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /include/filters/filter_base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, 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 are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holders nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef FILTERS__FILTER_BASE_H_ 33 | #define FILTERS__FILTER_BASE_H_ 34 | 35 | #ifdef _MSC_VER 36 | #pragma message("Including header is deprecated,") 37 | #pragma message("include instead.") 38 | #else 39 | // *INDENT-OFF* (prevent uncrustify from adding indention below) 40 | #warning Including header is deprecated, \ 41 | include instead. 42 | #endif 43 | 44 | #include "./filter_base.hpp" 45 | 46 | #endif // FILTERS__FILTER_BASE_H_ 47 | -------------------------------------------------------------------------------- /include/filters/filter_base.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef FILTERS__FILTER_BASE_HPP_ 31 | #define FILTERS__FILTER_BASE_HPP_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include "rcl_interfaces/msg/parameter_descriptor.hpp" 38 | #include "rcl_interfaces/msg/parameter_type.hpp" 39 | #include "rclcpp/rclcpp.hpp" 40 | 41 | namespace filters 42 | { 43 | 44 | namespace impl 45 | { 46 | 47 | inline std::string normalize_param_prefix(std::string prefix) 48 | { 49 | if (!prefix.empty()) { 50 | if ('.' != prefix.back()) { 51 | prefix += '.'; 52 | } 53 | } 54 | return prefix; 55 | } 56 | 57 | } // namespace impl 58 | 59 | /** 60 | * \brief A Base filter class to provide a standard interface for all filters 61 | */ 62 | template 63 | class FilterBase 64 | { 65 | public: 66 | /** 67 | * \brief Default constructor used by Filter Factories 68 | */ 69 | FilterBase() 70 | : configured_(false) {} 71 | 72 | /** 73 | * \brief Virtual Destructor 74 | */ 75 | virtual ~FilterBase() = default; 76 | 77 | /** 78 | * \brief Configure the filter from the parameter server 79 | * \param The parameter from which to read the configuration 80 | * \param node_handle The optional node handle, useful if operating in a different namespace. 81 | */ 82 | bool configure( 83 | const std::string & param_prefix, 84 | const std::string & filter_name, 85 | const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger, 86 | const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params) 87 | { 88 | if (configured_) { 89 | RCLCPP_WARN( 90 | node_logger->get_logger(), 91 | "Filter %s already being reconfigured", 92 | filter_name_.c_str()); 93 | } 94 | if (!node_params) { 95 | throw std::runtime_error("Need a parameters interface to function."); 96 | } 97 | 98 | configured_ = false; 99 | 100 | filter_name_ = filter_name; 101 | param_prefix_ = impl::normalize_param_prefix(param_prefix); 102 | params_interface_ = node_params; 103 | logging_interface_ = node_logger; 104 | 105 | configured_ = configure(); 106 | return configured_; 107 | } 108 | 109 | /** 110 | * \brief Update the filter and return the data seperately 111 | * This is an inefficient way to do this and can be overridden in the derived class 112 | * \param data_in A reference to the data to be input to the filter 113 | * \param data_out A reference to the data output location 114 | */ 115 | virtual bool update(const T & data_in, T & data_out) = 0; 116 | 117 | /** 118 | * \brief reconfigureCB 119 | * can be overridden in the derived class 120 | * \param parameters A vector parameters to be reconfigured 121 | */ 122 | virtual rcl_interfaces::msg::SetParametersResult reconfigureCB( 123 | std::vector parameters) 124 | { 125 | // Avoid unused parameter warning 126 | (void)parameters; 127 | auto result = rcl_interfaces::msg::SetParametersResult(); 128 | result.successful = true; 129 | return result; 130 | } 131 | 132 | /** 133 | * \brief Get the name of the filter as a string 134 | */ 135 | inline const std::string & getName() {return filter_name_;} 136 | 137 | /** 138 | * \brief Get the parameter_prefix of the filter as a string 139 | */ 140 | inline const std::string & getParamPrefix() {return param_prefix_;} 141 | 142 | private: 143 | template 144 | bool getParamImpl( 145 | const std::string & name, const uint8_t type, PT default_value, PT & value_out, 146 | bool read_only) 147 | { 148 | std::string param_name = param_prefix_ + name; 149 | 150 | if (!params_interface_->has_parameter(param_name)) { 151 | // Declare parameter 152 | rclcpp::ParameterValue default_parameter_value(default_value); 153 | rcl_interfaces::msg::ParameterDescriptor desc; 154 | desc.name = name; 155 | desc.type = type; 156 | desc.read_only = read_only; 157 | 158 | if (name.empty()) { 159 | throw std::runtime_error("Parameter must have a name"); 160 | } 161 | 162 | params_interface_->declare_parameter(param_name, default_parameter_value, desc); 163 | } 164 | 165 | value_out = params_interface_->get_parameter(param_name).get_parameter_value().get(); 166 | // TODO(sloretz) seems to be no way to tell if parameter was initialized or not 167 | return true; 168 | } 169 | 170 | protected: 171 | /** 172 | * \brief Pure virtual function for the sub class to configure the filter 173 | * This function must be implemented in the derived class. 174 | */ 175 | virtual bool configure() = 0; 176 | 177 | /** 178 | * \brief Get a filter parameter as a string 179 | * \param name The name of the parameter 180 | * \param value The string to set with the value 181 | * \param default_value The default value to use if the parameter is not set 182 | * \return Whether or not the parameter of name/type was set */ 183 | bool getParam( 184 | const std::string & name, std::string & value, bool read_only = true, 185 | std::string default_value = std::string()) 186 | { 187 | return getParamImpl( 188 | name, rcl_interfaces::msg::ParameterType::PARAMETER_STRING, default_value, value, read_only); 189 | } 190 | 191 | /** 192 | * \brief Get a filter parameter as a boolean 193 | * \param name The name of the parameter 194 | * \param value The boolean to set with the value 195 | * \param default_value The default value to use if the parameter is not set 196 | * \return Whether or not the parameter of name/type was set */ 197 | bool getParam( 198 | const std::string & name, bool & value, 199 | bool read_only = true, bool default_value = false) 200 | { 201 | return getParamImpl( 202 | name, rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, default_value, 203 | value, read_only); 204 | } 205 | 206 | /** 207 | * \brief Get a filter parameter as a double 208 | * \param name The name of the parameter 209 | * \param value The double to set with the value 210 | * \param default_value The default value to use if the parameter is not set 211 | * \return Whether or not the parameter of name/type was set */ 212 | bool getParam( 213 | const std::string & name, double & value, 214 | bool read_only = true, double default_value = 0.0) 215 | { 216 | return getParamImpl( 217 | name, rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, default_value, 218 | value, read_only); 219 | } 220 | 221 | /** 222 | * \brief Get a filter parameter as a int 223 | * \param name The name of the parameter 224 | * \param value The int to set with the value 225 | * \param default_value The default value to use if the parameter is not set 226 | * \return Whether or not the parameter of name/type was set */ 227 | bool getParam( 228 | const std::string & name, int & value, 229 | bool read_only = true, int default_value = 0) 230 | { 231 | return getParamImpl( 232 | name, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, default_value, 233 | value, read_only); 234 | } 235 | 236 | /** 237 | * \brief Get a filter parameter as an unsigned int 238 | * \param name The name of the parameter 239 | * \param value The int to set with the value 240 | * \return Whether or not the parameter of name/type was set */ 241 | bool getParam(const std::string & name, unsigned int & value) 242 | { 243 | int signed_value; 244 | if (!getParam(name, signed_value)) { 245 | return false; 246 | } 247 | if (signed_value < 0) { 248 | return false; 249 | } 250 | value = signed_value; 251 | return true; 252 | } 253 | 254 | /** 255 | * \brief Get a filter parameter as a size_t 256 | * \param name The name of the parameter 257 | * \param value The int to set with the value 258 | * \return Whether or not the parameter of name/type was set */ 259 | bool getParam(const std::string & name, size_t & value) 260 | { 261 | int signed_value; 262 | if (!getParam(name, signed_value)) { 263 | return false; 264 | } 265 | if (signed_value < 0) { 266 | return false; 267 | } 268 | value = signed_value; 269 | return true; 270 | } 271 | 272 | /** 273 | * \brief Get a filter parameter as a std::vector 274 | * \param name The name of the parameter 275 | * \param value The std::vector to set with the value 276 | * \param default_value The default value to use if the parameter is not set 277 | * \return Whether or not the parameter of name/type was set */ 278 | bool getParam( 279 | const std::string & name, std::vector & value, 280 | bool read_only = true, 281 | std::vector default_value = {}) 282 | { 283 | return getParamImpl( 284 | name, rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, default_value, value, 285 | read_only); 286 | } 287 | 288 | /** 289 | * \brief Get a filter parameter as a std::vector 290 | * \param name The name of the parameter 291 | * \param value The std::vector to set with the value 292 | * \param default_value The default value to use if the parameter is not set 293 | * \return Whether or not the parameter of name/type was set */ 294 | bool getParam( 295 | const std::string & name, std::vector & value, 296 | bool read_only = true, 297 | std::vector default_value = {}) 298 | { 299 | return getParamImpl( 300 | name, rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, default_value, value, 301 | read_only); 302 | } 303 | 304 | /// The name of the filter 305 | std::string filter_name_; 306 | /// Whether the filter has been configured. 307 | bool configured_; 308 | 309 | std::string param_prefix_; 310 | 311 | rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; 312 | rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; 313 | }; 314 | 315 | 316 | template 317 | class MultiChannelFilterBase : public FilterBase 318 | { 319 | public: 320 | MultiChannelFilterBase() 321 | : number_of_channels_(0) 322 | { 323 | } 324 | 325 | /** 326 | * \brief Virtual Destructor 327 | */ 328 | virtual ~MultiChannelFilterBase() = default; 329 | 330 | virtual bool configure() 331 | { 332 | return true; 333 | } 334 | 335 | /** 336 | * \brief Configure the filter from the parameter server 337 | * \param number_of_channels How many parallel channels the filter will process 338 | * \param The parameter from which to read the configuration 339 | * \param node_handle The optional node handle, useful if operating in a different namespace. 340 | */ 341 | bool configure( 342 | size_t number_of_channels, 343 | const std::string & param_prefix, 344 | const std::string & filter_name, 345 | const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger, 346 | const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params) 347 | { 348 | number_of_channels_ = number_of_channels; 349 | 350 | return FilterBase::configure(param_prefix, filter_name, node_logger, node_params); 351 | } 352 | 353 | /** 354 | * \brief Update the filter and return the data seperately 355 | * \param data_in A reference to the data to be input to the filter 356 | * \param data_out A reference to the data output location 357 | * This funciton must be implemented in the derived class. 358 | */ 359 | virtual bool update(const std::vector & data_in, std::vector & data_out) = 0; 360 | 361 | virtual bool update(const T & /*data_in*/, T & /*data_out*/) 362 | { 363 | RCLCPP_ERROR( 364 | this->logging_interface_->get_logger(), 365 | "THIS IS A MULTI FILTER DON'T CALL SINGLE FORM OF UPDATE"); 366 | return false; 367 | } 368 | 369 | protected: 370 | /// How many parallel inputs for which the filter is to be configured 371 | size_t number_of_channels_; 372 | }; 373 | 374 | } // namespace filters 375 | 376 | #endif // FILTERS__FILTER_BASE_HPP_ 377 | -------------------------------------------------------------------------------- /include/filters/filter_chain.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, 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 are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holders nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef FILTERS__FILTER_CHAIN_H_ 33 | #define FILTERS__FILTER_CHAIN_H_ 34 | 35 | #ifdef _MSC_VER 36 | #pragma message("Including header is deprecated,") 37 | #pragma message("include instead.") 38 | #else 39 | // *INDENT-OFF* (prevent uncrustify from adding indention below) 40 | #warning Including header is deprecated, \ 41 | include instead. 42 | #endif 43 | 44 | #include "./filter_chain.hpp" 45 | 46 | #endif // FILTERS__FILTER_CHAIN_H_ 47 | -------------------------------------------------------------------------------- /include/filters/filter_chain.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef FILTERS__FILTER_CHAIN_HPP_ 31 | #define FILTERS__FILTER_CHAIN_HPP_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "pluginlib/class_loader.hpp" 40 | #include "rcl_interfaces/msg/parameter_descriptor.hpp" 41 | #include "rcl_interfaces/msg/parameter_type.hpp" 42 | #include 43 | #include "rclcpp/rclcpp.hpp" 44 | 45 | #include "filters/filter_base.hpp" 46 | 47 | namespace filters 48 | { 49 | 50 | namespace impl 51 | { 52 | 53 | struct FoundFilter 54 | { 55 | std::string name; 56 | std::string type; 57 | std::string param_prefix; 58 | }; 59 | 60 | /** 61 | * \brief Declare a string param of the given name. If it already exists, just get the value. 62 | */ 63 | inline bool auto_declare_string( 64 | const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger, 65 | const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, 66 | const std::string & param_name, 67 | std::string & param_out) 68 | { 69 | rcl_interfaces::msg::ParameterDescriptor param_desc; 70 | param_desc.name = param_name; 71 | param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; 72 | param_desc.read_only = true; 73 | param_desc.dynamic_typing = false; 74 | 75 | rclcpp::ParameterValue param_value; 76 | try { 77 | param_value = node_params->declare_parameter( 78 | param_desc.name, rclcpp::ParameterType::PARAMETER_STRING, param_desc); 79 | } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException &) { 80 | rclcpp::Parameter param = node_params->get_parameter(param_name); 81 | param_value = param.get_parameter_value(); 82 | } catch (rclcpp::exceptions::InvalidParametersException &) { 83 | RCLCPP_ERROR( 84 | node_logger->get_logger(), "Tried to declare param with invalid name: %s", 85 | param_name.c_str()); 86 | return false; 87 | } 88 | 89 | try { 90 | param_out = param_value.get(); 91 | } catch (rclcpp::exceptions::InvalidParameterTypeException &) { 92 | RCLCPP_ERROR( 93 | node_logger->get_logger(), "Parameter %s of invalid type (must be a string)", 94 | param_name.c_str()); 95 | return false; 96 | } 97 | return true; 98 | } 99 | 100 | /** 101 | * \brief Read params and figure out what filters to load 102 | */ 103 | inline bool 104 | load_chain_config( 105 | const std::string & param_prefix, 106 | const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger, 107 | const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, 108 | std::vector & found_filters) 109 | { 110 | // TODO(sloretz) error if someone tries to do filter0 111 | const std::string norm_param_prefix = impl::normalize_param_prefix(param_prefix); 112 | 113 | const auto & param_overrides = node_params->get_parameter_overrides(); 114 | 115 | // Read parameters for filter1..filterN 116 | for (size_t filter_num = 1; true; ++filter_num) { 117 | // Parameters in chain are prefixed with 'filterN.' 118 | const std::string name_of_name_param = norm_param_prefix + "filter" + 119 | std::to_string(filter_num) + ".name"; 120 | const std::string name_of_type_param = norm_param_prefix + "filter" + 121 | std::to_string(filter_num) + ".type"; 122 | const std::string name_of_param_prefix_param = norm_param_prefix + "filter" + 123 | std::to_string(filter_num) + ".params"; 124 | 125 | if (param_overrides.find(name_of_name_param) == param_overrides.end()) { 126 | break; 127 | } 128 | 129 | std::string filter_name; 130 | if (!auto_declare_string(node_logger, node_params, name_of_name_param, filter_name)) { 131 | return false; 132 | } 133 | 134 | std::string filter_type; 135 | if (!auto_declare_string(node_logger, node_params, name_of_type_param, filter_type)) { 136 | return false; 137 | } 138 | 139 | if (std::find_if( 140 | found_filters.begin(), found_filters.end(), 141 | [&](const FoundFilter & f) {return f.name == filter_name;}) != found_filters.end()) 142 | { 143 | RCLCPP_FATAL( 144 | node_logger->get_logger(), 145 | "A filter with the name %s already exists", filter_name.c_str()); 146 | return false; 147 | } 148 | 149 | // Make sure 'type' is formated as 'package_name/filtername' 150 | if (1 != std::count(filter_type.cbegin(), filter_type.cend(), '/')) { 151 | RCLCPP_FATAL( 152 | node_logger->get_logger(), 153 | "%s must be of form /", filter_type.c_str()); 154 | return false; 155 | } 156 | 157 | // Seems ok; store it for now; it will be loaded further down. 158 | found_filters.push_back({filter_name, filter_type, name_of_param_prefix_param}); 159 | } 160 | return true; 161 | } 162 | 163 | } // namespace impl 164 | 165 | 166 | /** 167 | * \brief A class which will construct and sequentially call Filters according to xml 168 | * This is the primary way in which users are expected to interact with Filters 169 | */ 170 | template 171 | class FilterChain 172 | { 173 | public: 174 | /** 175 | * \brief Create the filter chain object 176 | */ 177 | explicit FilterChain(std::string data_type) 178 | : loader_("filters", "filters::FilterBase<" + data_type + ">"), 179 | configured_(false) 180 | { 181 | } 182 | 183 | ~FilterChain() 184 | { 185 | clear(); 186 | } 187 | 188 | /** 189 | * \brief process data through each of the filters added sequentially 190 | */ 191 | bool update(const T & data_in, T & data_out) 192 | { 193 | bool result; 194 | size_t list_size = reference_pointers_.size(); 195 | if (list_size == 0) { 196 | data_out = data_in; 197 | result = true; 198 | } else if (list_size == 1) { 199 | result = reference_pointers_[0]->update(data_in, data_out); 200 | } else if (list_size == 2) { 201 | result = reference_pointers_[0]->update(data_in, buffer0_); 202 | if (result == false) {return false;} // don't keep processing on failure 203 | result = result && reference_pointers_[1]->update(buffer0_, data_out); 204 | } else { 205 | result = reference_pointers_[0]->update(data_in, buffer0_); // first copy in 206 | for (size_t i = 1; i < reference_pointers_.size() - 1 && result; ++i) { 207 | // all but first and last (never called if size=2) 208 | if (i % 2 == 1) { 209 | result = result && reference_pointers_[i]->update(buffer0_, buffer1_); 210 | } else { 211 | result = result && reference_pointers_[i]->update(buffer1_, buffer0_); 212 | } 213 | } 214 | if (list_size % 2 == 1) { // odd number last deposit was in buffer1 215 | result = result && reference_pointers_.back()->update(buffer1_, data_out); 216 | } else { 217 | result = result && reference_pointers_.back()->update(buffer0_, data_out); 218 | } 219 | } 220 | return result; 221 | } 222 | 223 | /** 224 | * \brief Clear all filters from this chain 225 | */ 226 | bool clear() 227 | { 228 | configured_ = false; 229 | reference_pointers_.clear(); 230 | return true; 231 | } 232 | 233 | /** 234 | * \brief Configure the filter chain and all filters which have been addedj 235 | * \param param_prefix The parameter name prefix of the filter chain to load 236 | * \param node_logger node logging interface to use 237 | * \param node_params node parameter interface to use 238 | */ 239 | bool configure( 240 | const std::string & param_prefix, 241 | const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger, 242 | const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params) 243 | { 244 | if (configured_) { 245 | RCLCPP_ERROR(logging_interface_->get_logger(), "Filter chain is already configured"); 246 | return false; 247 | } 248 | logging_interface_ = node_logger; 249 | params_interface_ = node_params; 250 | on_set_parameters_callback_handle_ = params_interface_->add_on_set_parameters_callback( 251 | std::bind(&FilterChain::reconfigureCB, this, std::placeholders::_1)); 252 | 253 | std::vector found_filters; 254 | if (!impl::load_chain_config( 255 | param_prefix, logging_interface_, params_interface_, found_filters)) 256 | { 257 | // Assume load_chain_config() console logged something sensible 258 | return false; 259 | } 260 | 261 | std::vector>> loaded_filters; 262 | for (const auto & filter : found_filters) { 263 | // Try to load the filters that were described by parameters 264 | pluginlib::UniquePtr> loaded_filter; 265 | try { 266 | loaded_filter = loader_.createUniqueInstance(filter.type); 267 | } catch (const pluginlib::LibraryLoadException & e) { 268 | RCLCPP_FATAL( 269 | logging_interface_->get_logger(), 270 | "Could not load library for %s: %s", filter.type.c_str(), e.what()); 271 | return false; 272 | } catch (const pluginlib::CreateClassException & e) { 273 | RCLCPP_FATAL( 274 | logging_interface_->get_logger(), 275 | "Could not construct class %s: %s", filter.type.c_str(), e.what()); 276 | return false; 277 | } 278 | 279 | // Try to configure the filter 280 | if (!loaded_filter || !loaded_filter->configure( 281 | filter.param_prefix, filter.name, logging_interface_, params_interface_)) 282 | { 283 | RCLCPP_FATAL( 284 | logging_interface_->get_logger(), 285 | "Could not configure %s of type %s", filter.name.c_str(), filter.type.c_str()); 286 | return false; 287 | } 288 | loaded_filters.emplace_back(std::move(loaded_filter)); 289 | } 290 | 291 | // Everything went ok! 292 | reference_pointers_ = std::move(loaded_filters); 293 | configured_ = true; 294 | return true; 295 | } 296 | 297 | rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector parameters) 298 | { 299 | auto result = rcl_interfaces::msg::SetParametersResult(); 300 | result.successful = true; 301 | 302 | for (auto & ref_ptr : reference_pointers_) { 303 | std::vector parameters_subset; 304 | for (auto parameter : parameters) { 305 | if (parameter.get_name().find(ref_ptr->getParamPrefix()) != std::string::npos) { 306 | parameters_subset.push_back(parameter); 307 | } 308 | } 309 | if (!parameters_subset.empty() && !ref_ptr->reconfigureCB(parameters_subset).successful) { 310 | result.successful = false; 311 | } 312 | } 313 | return result; 314 | } 315 | 316 | private: 317 | pluginlib::ClassLoader> loader_; 318 | 319 | /// A vector of pointers to currently constructed filters 320 | std::vector>> reference_pointers_; 321 | 322 | T buffer0_; ///< A temporary intermediate buffer 323 | T buffer1_; ///< A temporary intermediate buffer 324 | bool configured_; ///< whether the system is configured 325 | 326 | rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; 327 | rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; 328 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr 329 | on_set_parameters_callback_handle_; 330 | }; 331 | 332 | /** 333 | * \brief A class which will construct and sequentially call Filters according to xml 334 | * 335 | * This is the primary way in which users are expected to interact with Filters 336 | */ 337 | template 338 | class MultiChannelFilterChain 339 | { 340 | public: 341 | /** 342 | * \brief Create the filter chain object 343 | */ 344 | explicit MultiChannelFilterChain(std::string data_type) 345 | : loader_("filters", "filters::MultiChannelFilterBase<" + data_type + ">"), 346 | configured_(false) 347 | { 348 | } 349 | 350 | /** 351 | * \brief process data through each of the filters added sequentially 352 | */ 353 | bool update(const std::vector & data_in, std::vector & data_out) 354 | { 355 | bool result; 356 | size_t list_size = reference_pointers_.size(); 357 | 358 | if (list_size == 0) { 359 | data_out = data_in; 360 | result = true; 361 | } else if (list_size == 1) { 362 | result = reference_pointers_[0]->update(data_in, data_out); 363 | } else if (list_size == 2) { 364 | result = reference_pointers_[0]->update(data_in, buffer0_); 365 | if (result) { // don't keep processing on failure 366 | result = result && reference_pointers_[1]->update(buffer0_, data_out); 367 | } 368 | } else { 369 | result = reference_pointers_[0]->update(data_in, buffer0_); // first copy in 370 | for (size_t i = 1; i < reference_pointers_.size() - 1 && result; ++i) { 371 | // all but first and last (never if size = 2) 372 | if (i % 2 == 1) { 373 | result = result && reference_pointers_[i]->update(buffer0_, buffer1_); 374 | } else { 375 | result = result && reference_pointers_[i]->update(buffer1_, buffer0_); 376 | } 377 | } 378 | if (list_size % 2 == 1) { // odd number last deposit was in buffer1 379 | result = result && reference_pointers_.back()->update(buffer1_, data_out); 380 | } else { 381 | result = result && reference_pointers_.back()->update(buffer0_, data_out); 382 | } 383 | } 384 | return result; 385 | } 386 | 387 | ~MultiChannelFilterChain() 388 | { 389 | clear(); 390 | } 391 | 392 | /** 393 | * \brief Clear all filters from this chain 394 | */ 395 | bool clear() 396 | { 397 | configured_ = false; 398 | reference_pointers_.clear(); 399 | buffer0_.clear(); 400 | buffer1_.clear(); 401 | return true; 402 | } 403 | 404 | /** 405 | * \brief Configure the filter chain 406 | * 407 | * This will call configure on all filters which have been added 408 | * as well as allocate the buffers 409 | */ 410 | bool configure( 411 | size_t number_of_channels, 412 | const std::string & param_prefix, 413 | const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger, 414 | const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params) 415 | { 416 | if (configured_) { 417 | RCLCPP_ERROR(logging_interface_->get_logger(), "Filter chain is already configured"); 418 | return false; 419 | } 420 | logging_interface_ = node_logger; 421 | params_interface_ = node_params; 422 | 423 | std::vector found_filters; 424 | if (!impl::load_chain_config( 425 | param_prefix, logging_interface_, params_interface_, found_filters)) 426 | { 427 | // Assume load_chain_config() console logged something sensible 428 | return false; 429 | } 430 | 431 | std::vector>> loaded_filters; 432 | for (const auto & filter : found_filters) { 433 | // Try to load the filters that were described by parameters 434 | pluginlib::UniquePtr> loaded_filter; 435 | try { 436 | loaded_filter = loader_.createUniqueInstance(filter.type); 437 | } catch (const pluginlib::LibraryLoadException & e) { 438 | RCLCPP_FATAL( 439 | logging_interface_->get_logger(), 440 | "Could not load library for %s: %s", filter.type.c_str(), e.what()); 441 | return false; 442 | } catch (const pluginlib::CreateClassException & e) { 443 | RCLCPP_FATAL( 444 | logging_interface_->get_logger(), 445 | "Could not construct class %s: %s", filter.type.c_str(), e.what()); 446 | return false; 447 | } 448 | 449 | // Try to configure the filter 450 | if (!loaded_filter || !loaded_filter->configure( 451 | number_of_channels, filter.param_prefix, filter.name, 452 | logging_interface_, params_interface_)) 453 | { 454 | RCLCPP_FATAL( 455 | logging_interface_->get_logger(), 456 | "Could not configure %s of type %s", filter.name.c_str(), filter.type.c_str()); 457 | return false; 458 | } 459 | loaded_filters.emplace_back(std::move(loaded_filter)); 460 | } 461 | 462 | // Everything went ok! 463 | reference_pointers_ = std::move(loaded_filters); 464 | on_set_parameters_callback_handle_ = params_interface_->add_on_set_parameters_callback( 465 | std::bind(&MultiChannelFilterChain::reconfigureCB, this, std::placeholders::_1)); 466 | // Allocate ahead of time 467 | buffer0_.resize(number_of_channels); 468 | buffer1_.resize(number_of_channels); 469 | configured_ = true; 470 | return true; 471 | } 472 | 473 | rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector parameters) 474 | { 475 | auto result = rcl_interfaces::msg::SetParametersResult(); 476 | result.successful = true; 477 | 478 | for (auto & ref_ptr : reference_pointers_) { 479 | std::vector parameters_subset; 480 | for (auto parameter : parameters) { 481 | if (parameter.get_name().find(ref_ptr->getParamPrefix()) != std::string::npos) { 482 | parameters_subset.push_back(parameter); 483 | } 484 | } 485 | if (!parameters_subset.empty() && !ref_ptr->reconfigureCB(parameters_subset).successful) { 486 | result.successful = false; 487 | } 488 | } 489 | return result; 490 | } 491 | 492 | private: 493 | pluginlib::ClassLoader> loader_; 494 | 495 | /// A vector of pointers to currently constructed filters 496 | std::vector>> reference_pointers_; 497 | 498 | std::vector buffer0_; ///< A temporary intermediate buffer 499 | std::vector buffer1_; ///< A temporary intermediate buffer 500 | bool configured_; ///< whether the system is configured 501 | 502 | rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; 503 | rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; 504 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr 505 | on_set_parameters_callback_handle_; 506 | }; 507 | 508 | } // namespace filters 509 | 510 | #endif // FILTERS__FILTER_CHAIN_HPP_ 511 | -------------------------------------------------------------------------------- /include/filters/increment.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, 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 are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holders nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef FILTERS__INCREMENT_H_ 33 | #define FILTERS__INCREMENT_H_ 34 | 35 | #ifdef _MSC_VER 36 | #pragma message("Including header is deprecated,") 37 | #pragma message("include instead.") 38 | #else 39 | // *INDENT-OFF* (prevent uncrustify from adding indention below) 40 | #warning Including header is deprecated, \ 41 | include instead. 42 | #endif 43 | 44 | #include "./increment.hpp" 45 | 46 | #endif // FILTERS__INCREMENT_H_ 47 | -------------------------------------------------------------------------------- /include/filters/increment.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef FILTERS__INCREMENT_HPP_ 31 | #define FILTERS__INCREMENT_HPP_ 32 | 33 | #include 34 | 35 | #include "filters/filter_base.hpp" 36 | 37 | namespace filters 38 | { 39 | 40 | /** 41 | * \brief A increment filter which works on doubles. 42 | */ 43 | template 44 | class IncrementFilter : public FilterBase 45 | { 46 | public: 47 | /** 48 | * \brief Construct the filter with the expected width and height 49 | */ 50 | IncrementFilter(); 51 | 52 | /** 53 | * \brief Destructor to clean up 54 | */ 55 | ~IncrementFilter() override; 56 | 57 | bool configure() override; 58 | 59 | /** 60 | * \brief Update the filter and return the data seperately 61 | * \param data_in T array with length width 62 | * \param data_out T array with length width 63 | */ 64 | bool update(const T & data_in, T & data_out) override; 65 | }; 66 | 67 | 68 | template 69 | IncrementFilter::IncrementFilter() 70 | { 71 | } 72 | 73 | template 74 | IncrementFilter::~IncrementFilter() 75 | { 76 | } 77 | 78 | template 79 | bool IncrementFilter::configure() 80 | { 81 | return true; 82 | } 83 | 84 | template 85 | bool IncrementFilter::update(const T & data_in, T & data_out) 86 | { 87 | data_out = data_in + 1; 88 | return true; 89 | } 90 | 91 | /** 92 | * \brief A increment filter which works on arrays. 93 | */ 94 | template 95 | class MultiChannelIncrementFilter : public MultiChannelFilterBase 96 | { 97 | public: 98 | /** 99 | * \brief Construct the filter with the expected width and height 100 | */ 101 | MultiChannelIncrementFilter(); 102 | 103 | /** 104 | * \brief Destructor to clean up 105 | */ 106 | ~MultiChannelIncrementFilter() override; 107 | 108 | bool configure() override; 109 | 110 | /** 111 | * \brief Update the filter and return the data seperately 112 | * \param data_in T array with length width 113 | * \param data_out T array with length width 114 | */ 115 | bool update(const std::vector & data_in, std::vector & data_out) override; 116 | 117 | protected: 118 | using MultiChannelFilterBase::number_of_channels_; ///< Number of elements per observation 119 | }; 120 | 121 | 122 | template 123 | MultiChannelIncrementFilter::MultiChannelIncrementFilter() 124 | { 125 | } 126 | 127 | template 128 | MultiChannelIncrementFilter::~MultiChannelIncrementFilter() 129 | { 130 | } 131 | 132 | template 133 | bool MultiChannelIncrementFilter::configure() 134 | { 135 | return true; 136 | } 137 | 138 | template 139 | bool MultiChannelIncrementFilter::update( 140 | const std::vector & data_in, std::vector & data_out) 141 | { 142 | if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_) { 143 | RCLCPP_ERROR( 144 | this->logging_interface_->get_logger(), 145 | "Configured with wrong size config: %ld, in: %ld out: %ld", 146 | number_of_channels_, data_in.size(), data_out.size()); 147 | return false; 148 | } 149 | 150 | // Return each value 151 | for (size_t i = 0; i < number_of_channels_; ++i) { 152 | data_out[i] = data_in[i] + 1; 153 | } 154 | 155 | return true; 156 | } 157 | 158 | } // namespace filters 159 | 160 | #endif // FILTERS__INCREMENT_HPP_ 161 | -------------------------------------------------------------------------------- /include/filters/mean.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, 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 are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holders nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef FILTERS__MEAN_H_ 33 | #define FILTERS__MEAN_H_ 34 | 35 | #ifdef _MSC_VER 36 | #pragma message("Including header is deprecated,") 37 | #pragma message("include instead.") 38 | #else 39 | // *INDENT-OFF* (prevent uncrustify from adding indention below) 40 | #warning Including header is deprecated, \ 41 | include instead. 42 | #endif 43 | 44 | #include "./mean.hpp" 45 | 46 | #endif // FILTERS__MEAN_H_ 47 | -------------------------------------------------------------------------------- /include/filters/mean.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef FILTERS__MEAN_HPP_ 31 | #define FILTERS__MEAN_HPP_ 32 | 33 | #include 34 | #include 35 | 36 | #include "filters/filter_base.hpp" 37 | #include "filters/realtime_circular_buffer.hpp" 38 | 39 | namespace filters 40 | { 41 | 42 | /** 43 | * \brief A mean filter which works on doubles. 44 | */ 45 | template 46 | class MeanFilter : public FilterBase 47 | { 48 | public: 49 | /** 50 | * \brief Construct the filter with the expected width and height 51 | */ 52 | MeanFilter(); 53 | 54 | /** 55 | * \brief Destructor to clean up 56 | */ 57 | ~MeanFilter() override; 58 | 59 | bool configure() override; 60 | 61 | /** 62 | * \brief Update the filter and return the data seperately 63 | * \param data_in T array with length width 64 | * \param data_out T array with length width 65 | */ 66 | bool update(const T & data_in, T & data_out) override; 67 | 68 | protected: 69 | std::unique_ptr> data_storage_; ///< Storage for data between updates 70 | uint32_t last_updated_row_; ///< The last row to have been updated by the filter 71 | T temp_; // Temporary storage 72 | uint32_t number_of_observations_; ///< Number of observations over which to filter 73 | }; 74 | 75 | 76 | template 77 | MeanFilter::MeanFilter() 78 | : number_of_observations_(0) 79 | { 80 | } 81 | 82 | template 83 | MeanFilter::~MeanFilter() 84 | { 85 | } 86 | 87 | template 88 | bool MeanFilter::configure() 89 | { 90 | if (!FilterBase::getParam("number_of_observations", number_of_observations_)) { 91 | RCLCPP_ERROR( 92 | this->logging_interface_->get_logger(), 93 | "MeanFilter did not find param number_of_observations"); 94 | return false; 95 | } 96 | 97 | data_storage_.reset(new RealtimeCircularBuffer(number_of_observations_, temp_)); 98 | 99 | return true; 100 | } 101 | 102 | template 103 | bool MeanFilter::update(const T & data_in, T & data_out) 104 | { 105 | // update active row 106 | if (last_updated_row_ >= number_of_observations_ - 1) { 107 | last_updated_row_ = 0; 108 | } else { 109 | ++last_updated_row_; 110 | } 111 | 112 | data_storage_->push_back(data_in); 113 | 114 | size_t length = data_storage_->size(); 115 | 116 | data_out = 0; 117 | for (size_t row = 0; row < length; ++row) { 118 | data_out += data_storage_->at(row); 119 | } 120 | data_out /= length; 121 | 122 | return true; 123 | } 124 | 125 | /** 126 | * \brief A mean filter which works on double arrays. 127 | */ 128 | template 129 | class MultiChannelMeanFilter : public MultiChannelFilterBase 130 | { 131 | public: 132 | /** 133 | * \brief Construct the filter with the expected width and height 134 | */ 135 | MultiChannelMeanFilter(); 136 | 137 | /** 138 | * \brief Destructor to clean up 139 | */ 140 | ~MultiChannelMeanFilter() override; 141 | 142 | bool configure() override; 143 | 144 | /** 145 | * \brief Update the filter and return the data separately 146 | * \param data_in T array with length width 147 | * \param data_out T array with length width 148 | */ 149 | bool update(const std::vector & data_in, std::vector & data_out) override; 150 | 151 | protected: 152 | /// Storage for data between updates 153 | std::unique_ptr>> data_storage_; 154 | uint32_t last_updated_row_; ///< The last row to have been updated by the filter 155 | std::vector temp; // used for preallocation and copying from non vector source 156 | 157 | uint32_t number_of_observations_; ///< Number of observations over which to filter 158 | using MultiChannelFilterBase::number_of_channels_; ///< Number of elements per observation 159 | }; 160 | 161 | 162 | template 163 | MultiChannelMeanFilter::MultiChannelMeanFilter() 164 | : number_of_observations_(0) 165 | { 166 | } 167 | 168 | template 169 | MultiChannelMeanFilter::~MultiChannelMeanFilter() 170 | { 171 | } 172 | 173 | template 174 | bool MultiChannelMeanFilter::configure() 175 | { 176 | if (!FilterBase::getParam("number_of_observations", number_of_observations_)) { 177 | RCLCPP_ERROR( 178 | this->logging_interface_->get_logger(), 179 | "MultiChannelMeanFilter did not find param number_of_observations"); 180 | return false; 181 | } 182 | 183 | temp.resize(number_of_channels_); 184 | data_storage_.reset(new RealtimeCircularBuffer>(number_of_observations_, temp)); 185 | 186 | return true; 187 | } 188 | 189 | template 190 | bool MultiChannelMeanFilter::update(const std::vector & data_in, std::vector & data_out) 191 | { 192 | if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_) { 193 | RCLCPP_ERROR( 194 | this->logging_interface_->get_logger(), 195 | "Configured with wrong size config: %ld, in: %ld out: %ld", 196 | number_of_channels_, data_in.size(), data_out.size()); 197 | return false; 198 | } 199 | 200 | // update active row 201 | if (last_updated_row_ >= number_of_observations_ - 1) { 202 | last_updated_row_ = 0; 203 | } else { 204 | ++last_updated_row_; 205 | } 206 | 207 | data_storage_->push_back(data_in); 208 | 209 | size_t length = data_storage_->size(); 210 | 211 | // Return each value 212 | for (size_t i = 0; i < number_of_channels_; ++i) { 213 | data_out[i] = 0; 214 | for (size_t row = 0; row < length; ++row) { 215 | data_out[i] += data_storage_->at(row)[i]; 216 | } 217 | data_out[i] /= length; 218 | } 219 | 220 | return true; 221 | } 222 | 223 | } // namespace filters 224 | 225 | #endif // FILTERS__MEAN_HPP_ 226 | -------------------------------------------------------------------------------- /include/filters/median.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, 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 are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holders nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef FILTERS__MEDIAN_H_ 33 | #define FILTERS__MEDIAN_H_ 34 | 35 | #ifdef _MSC_VER 36 | #pragma message("Including header is deprecated,") 37 | #pragma message("include instead.") 38 | #else 39 | // *INDENT-OFF* (prevent uncrustify from adding indention below) 40 | #warning Including header is deprecated, \ 41 | include instead. 42 | #endif 43 | 44 | #include "./median.hpp" 45 | 46 | #endif // FILTERS__MEDIAN_H_ 47 | -------------------------------------------------------------------------------- /include/filters/median.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef FILTERS__MEDIAN_HPP_ 31 | #define FILTERS__MEDIAN_HPP_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include "filters/filter_base.hpp" 38 | 39 | #include "filters/realtime_circular_buffer.hpp" 40 | 41 | 42 | /*********************************************************************/ 43 | /* 44 | * Algorithm from N. Wirth's book, implementation by N. Devillard. 45 | * This code in public domain. 46 | */ 47 | 48 | namespace filters 49 | { 50 | 51 | /*--------------------------------------------------------------------------- 52 | Function : kth_smallest() 53 | In : array of elements, # of elements in the array, rank k 54 | Out : one element 55 | Job : find the kth smallest element in the array 56 | Notice : use the median() macro defined below to get the median. 57 | Reference: 58 | Author: Wirth, Niklaus 59 | Title: Algorithms + data structures = programs 60 | Publisher: Englewood Cliffs: Prentice-Hall, 1976 61 | Physical description: 366 p. 62 | Series: Prentice-Hall Series in Automatic Computation 63 | ---------------------------------------------------------------------------*/ 64 | template 65 | elem_type kth_smallest(elem_type * a, int n, int k) 66 | { 67 | int i, j, l, m; 68 | elem_type x; 69 | l = 0; m = n - 1; 70 | while (l < m) { 71 | x = a[k]; 72 | i = l; 73 | j = m; 74 | do { 75 | while (a[i] < x) {i++;} 76 | while (x < a[j]) {j--;} 77 | if (i <= j) { 78 | std::swap(a[i], a[j]); 79 | i++; j--; 80 | } 81 | } while (i <= j); 82 | if (j < k) {l = i;} 83 | if (k < i) {m = j;} 84 | } 85 | return a[k]; 86 | } 87 | 88 | template 89 | elem_type median(elem_type * a, int n) 90 | { 91 | return kth_smallest(a, n, (((n) & 1) ? ((n) / 2) : (((n) / 2) - 1))); 92 | } 93 | 94 | /** 95 | * \brief A median filter which works on arrays. 96 | */ 97 | template 98 | class MedianFilter : public filters::FilterBase 99 | { 100 | public: 101 | /** 102 | * \brief Construct the filter with the expected width and height 103 | */ 104 | MedianFilter(); 105 | 106 | /** 107 | * \brief Destructor to clean up 108 | */ 109 | ~MedianFilter() override; 110 | 111 | bool configure() override; 112 | 113 | /** 114 | * \brief Update the filter and return the data seperately 115 | * \param data_in double array with length width 116 | * \param data_out double array with length width 117 | */ 118 | bool update(const T & data_in, T & data_out) override; 119 | 120 | protected: 121 | std::vector temp_storage_; ///< Preallocated storage for the list to sort 122 | std::unique_ptr> data_storage_; ///< Storage for data between updates 123 | 124 | T temp; // used for preallocation and copying from non vector source 125 | 126 | size_t number_of_observations_; ///< Number of observations over which to filter 127 | }; 128 | 129 | template 130 | MedianFilter::MedianFilter() 131 | : number_of_observations_(0) 132 | { 133 | } 134 | 135 | template 136 | MedianFilter::~MedianFilter() 137 | { 138 | } 139 | 140 | 141 | template 142 | bool MedianFilter::configure() 143 | { 144 | if (!FilterBase::getParam("number_of_observations", number_of_observations_)) { 145 | RCLCPP_ERROR(this->logging_interface_->get_logger(), "MedianFilter was not given params.\n"); 146 | return false; 147 | } 148 | 149 | data_storage_.reset(new RealtimeCircularBuffer(number_of_observations_, temp)); 150 | temp_storage_.resize(number_of_observations_); 151 | 152 | return true; 153 | } 154 | 155 | template 156 | bool MedianFilter::update(const T & data_in, T & data_out) 157 | { 158 | if (!FilterBase::configured_) { 159 | return false; 160 | } 161 | 162 | data_storage_->push_back(data_in); 163 | 164 | size_t length = data_storage_->size(); 165 | for (size_t row = 0; row < length; ++row) { 166 | temp_storage_[row] = (*data_storage_)[row]; 167 | } 168 | data_out = median(&temp_storage_[0], length); 169 | 170 | return true; 171 | } 172 | 173 | /** 174 | * \brief A median filter which works on arrays. 175 | */ 176 | template 177 | class MultiChannelMedianFilter : public filters::MultiChannelFilterBase 178 | { 179 | public: 180 | /** 181 | * \brief Construct the filter with the expected width and height 182 | */ 183 | MultiChannelMedianFilter(); 184 | 185 | /** 186 | * \brief Destructor to clean up 187 | */ 188 | ~MultiChannelMedianFilter() override; 189 | 190 | bool configure() override; 191 | 192 | /** 193 | * \brief Update the filter and return the data seperately 194 | * \param data_in T array with length width 195 | * \param data_out T array with length width 196 | */ 197 | bool update(const std::vector & data_in, std::vector & data_out) override; 198 | 199 | protected: 200 | std::vector temp_storage_; ///< Preallocated storage for the list to sort 201 | /// Storage for data between updates 202 | std::unique_ptr>> data_storage_; 203 | 204 | std::vector temp; // used for preallocation and copying from non vector source 205 | 206 | size_t number_of_observations_; ///< Number of observations over which to filter 207 | }; 208 | 209 | template 210 | MultiChannelMedianFilter::MultiChannelMedianFilter() 211 | : number_of_observations_(0) 212 | { 213 | } 214 | 215 | template 216 | MultiChannelMedianFilter::~MultiChannelMedianFilter() 217 | { 218 | } 219 | 220 | template 221 | bool MultiChannelMedianFilter::configure() 222 | { 223 | if (!FilterBase::getParam("number_of_observations", number_of_observations_)) { 224 | RCLCPP_ERROR( 225 | this->logging_interface_->get_logger(), "MultiChannelMedianFilter was not given params.\n"); 226 | return false; 227 | } 228 | 229 | temp.resize(this->number_of_channels_); 230 | data_storage_.reset(new RealtimeCircularBuffer>(number_of_observations_, temp)); 231 | temp_storage_.resize(number_of_observations_); 232 | 233 | return true; 234 | } 235 | 236 | template 237 | bool MultiChannelMedianFilter::update(const std::vector & data_in, std::vector & data_out) 238 | { 239 | if (data_in.size() != this->number_of_channels_ || data_out.size() != this->number_of_channels_) { 240 | return false; 241 | } 242 | if (!FilterBase::configured_) { 243 | return false; 244 | } 245 | 246 | data_storage_->push_back(data_in); 247 | 248 | size_t length = data_storage_->size(); 249 | for (size_t i = 0; i < this->number_of_channels_; ++i) { 250 | for (size_t row = 0; row < length; ++row) { 251 | temp_storage_[row] = (*data_storage_)[row][i]; 252 | } 253 | data_out[i] = median(&temp_storage_[0], length); 254 | } 255 | 256 | return true; 257 | } 258 | 259 | } // namespace filters 260 | 261 | #endif // FILTERS__MEDIAN_HPP_ 262 | -------------------------------------------------------------------------------- /include/filters/param_test.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, 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 are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holders nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef FILTERS__PARAM_TEST_H_ 33 | #define FILTERS__PARAM_TEST_H_ 34 | 35 | #ifdef _MSC_VER 36 | #pragma message("Including header is deprecated,") 37 | #pragma message("include instead.") 38 | #else 39 | // *INDENT-OFF* (prevent uncrustify from adding indention below) 40 | #warning Including header is deprecated, \ 41 | include instead. 42 | #endif 43 | 44 | #include "./param_test.hpp" 45 | 46 | #endif // FILTERS__PARAM_TEST_H_ 47 | -------------------------------------------------------------------------------- /include/filters/param_test.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef FILTERS__PARAM_TEST_HPP_ 31 | #define FILTERS__PARAM_TEST_HPP_ 32 | 33 | #include "filters/filter_base.hpp" 34 | 35 | namespace filters 36 | { 37 | 38 | /** 39 | * \brief A mean filter which works on doubles. 40 | */ 41 | template 42 | class ParamTest : public FilterBase 43 | { 44 | public: 45 | /** 46 | * \brief Construct the filter with the expected width and height 47 | */ 48 | ParamTest(); 49 | 50 | /** 51 | * \brief Destructor to clean up 52 | */ 53 | ~ParamTest() override; 54 | 55 | bool configure() override; 56 | 57 | /** 58 | * \brief Update the filter and return the data seperately 59 | * \param data_in T array with length width 60 | * \param data_out T array with length width 61 | */ 62 | bool update(const T & data_in, T & data_out) override; 63 | }; 64 | 65 | template 66 | ParamTest::ParamTest() 67 | { 68 | } 69 | 70 | template 71 | ParamTest::~ParamTest() 72 | { 73 | } 74 | 75 | template 76 | bool ParamTest::configure() 77 | { 78 | return true; 79 | } 80 | 81 | template 82 | bool ParamTest::update(const T & /*data_in*/, T & data_out) 83 | { 84 | this->getParam("key", data_out); 85 | return true; 86 | } 87 | 88 | } // namespace filters 89 | 90 | #endif // FILTERS__PARAM_TEST_HPP_ 91 | -------------------------------------------------------------------------------- /include/filters/realtime_circular_buffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, 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 are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holders nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef FILTERS__REALTIME_CIRCULAR_BUFFER_H_ 33 | #define FILTERS__REALTIME_CIRCULAR_BUFFER_H_ 34 | 35 | #ifdef _MSC_VER 36 | #pragma message("Including header is deprecated,") 37 | #pragma message("include instead.") 38 | #else 39 | // *INDENT-OFF* (prevent uncrustify from adding indention below) 40 | #warning Including header is deprecated, \ 41 | include instead. 42 | #endif 43 | 44 | #include "./realtime_circular_buffer.hpp" 45 | 46 | #endif // FILTERS__REALTIME_CIRCULAR_BUFFER_H_ 47 | -------------------------------------------------------------------------------- /include/filters/realtime_circular_buffer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /// \author Tully Foote tfoote@willowgarage.com 31 | 32 | #ifndef FILTERS__REALTIME_CIRCULAR_BUFFER_HPP_ 33 | #define FILTERS__REALTIME_CIRCULAR_BUFFER_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | #include "boost/circular_buffer.hpp" 39 | 40 | namespace filters 41 | { 42 | 43 | /** 44 | * \brief A realtime safe circular (ring) buffer. 45 | */ 46 | template 47 | class RealtimeCircularBuffer 48 | { 49 | public: 50 | using const_iterator = typename boost::circular_buffer::const_iterator; 51 | 52 | RealtimeCircularBuffer(size_t size, const T & default_val) 53 | : counter_(0), cb_(size) 54 | { 55 | for (size_t i = 0; i < cb_.capacity(); ++i) { 56 | cb_.push_back(default_val); 57 | } 58 | } 59 | 60 | void push_back(const T & item) 61 | { 62 | if (cb_.capacity() == 0) { 63 | return; 64 | } 65 | 66 | if (counter_ < cb_.size()) { 67 | cb_[counter_] = item; 68 | } else { 69 | cb_.push_back(item); 70 | } 71 | counter_++; 72 | } 73 | 74 | void push_front(const T & item) 75 | { 76 | if (cb_.capacity() == 0) {return;} 77 | cb_.push_front(item); 78 | counter_++; 79 | } 80 | 81 | void clear() {counter_ = 0;} 82 | 83 | void set_capacity(unsigned int order, const T & value); 84 | 85 | // The returned iterator is considered to be invalidated if the pointed 86 | // element had been removed or overwritten by an another element. 87 | // Please consider invalid any iterator obtained 88 | // prior to the last push_front() or push_back() call. 89 | const_iterator begin() const 90 | { 91 | return cb_.begin(); 92 | } 93 | 94 | // The returned iterator is considered to be invalidated if the pointed 95 | // element had been removed or overwritten by an another element. 96 | // Please consider invalid any iterator obtained 97 | // prior to the last push_front() or push_back() call. 98 | const_iterator end() const 99 | { 100 | return cb_.end(); 101 | } 102 | 103 | T & front() 104 | { 105 | return cb_.front(); 106 | } 107 | 108 | T & back() 109 | { 110 | if (counter_ < cb_.size()) { 111 | return cb_[counter_]; 112 | } else { 113 | return cb_.back(); 114 | } 115 | } 116 | 117 | size_t size() {return std::min(counter_, cb_.size());} 118 | bool empty() {return cb_.empty();} 119 | T & at(size_t index) {return cb_.at(index);} 120 | T & operator[](size_t index) {return cb_[index];} 121 | 122 | private: 123 | RealtimeCircularBuffer(); 124 | 125 | size_t counter_; ///< special counter to keep track of first N times through 126 | 127 | boost::circular_buffer cb_; 128 | }; 129 | 130 | } // namespace filters 131 | 132 | #endif // FILTERS__REALTIME_CIRCULAR_BUFFER_HPP_ 133 | -------------------------------------------------------------------------------- /include/filters/transfer_function.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, 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 are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holders nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef FILTERS__TRANSFER_FUNCTION_H_ 33 | #define FILTERS__TRANSFER_FUNCTION_H_ 34 | 35 | #ifdef _MSC_VER 36 | #pragma message("Including header is deprecated,") 37 | #pragma message("include instead.") 38 | #else 39 | // *INDENT-OFF* (prevent uncrustify from adding indention below) 40 | #warning Including header is deprecated, \ 41 | include instead. 42 | #endif 43 | 44 | #include "./transfer_function.hpp" 45 | 46 | #endif // FILTERS__TRANSFER_FUNCTION_H_ 47 | -------------------------------------------------------------------------------- /include/filters/transfer_function.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // Original version: Melonee Wise 31 | 32 | #ifndef FILTERS__TRANSFER_FUNCTION_HPP_ 33 | #define FILTERS__TRANSFER_FUNCTION_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "filters/filter_base.hpp" 42 | #include "filters/realtime_circular_buffer.hpp" 43 | 44 | namespace filters 45 | { 46 | 47 | /** 48 | * \brief One-dimensional digital filter class. 49 | * 50 | * This class calculates the output for \f$N\f$ one-dimensional 51 | * digital filters. 52 | * The filter is described by vectors \f$a\f$ and \f$b\f$ and 53 | * implemented using the standard difference equation:
54 | * 55 | * \f{eqnarray*} 56 | * a[0]*y[n] = b[0]*x[n] &+& b[1]*x[n-1]+ ... + b[n_b]*x[n-n_b]\ \ 57 | * &-& a[1]*y[n-1]- ... - a[n_a]*y[n-n_a] 58 | * \f}
59 | * 60 | * If \f$a[0]\f$ is not equal to 1, the coefficients are normalized by \f$a[0]\f$. 61 | * 62 | * Example xml config:
63 | * 64 | *
65 | *
66 | *
67 | */ 68 | template 69 | class SingleChannelTransferFunctionFilter : public filters::FilterBase 70 | { 71 | public: 72 | /** 73 | * \brief Construct the filter 74 | */ 75 | SingleChannelTransferFunctionFilter(); 76 | 77 | /** 78 | * \brief Destructor to clean up 79 | */ 80 | ~SingleChannelTransferFunctionFilter() override; 81 | 82 | /** 83 | * \brief Configure the filter with the correct number of channels and params. 84 | * \param number_of_channels The number of inputs filtered. 85 | * \param config The xml that is parsed to configure the filter. 86 | */ 87 | bool configure() override; 88 | 89 | /** 90 | * \brief Update the filter and return the data seperately 91 | * \param data_in vector with number_of_channels elements 92 | * \param data_out vector with number_of_channels elements 93 | */ 94 | bool update(const T & data_in, T & data_out) override; 95 | 96 | protected: 97 | std::unique_ptr> input_buffer_; ///< The input sample history. 98 | std::unique_ptr> output_buffer_; ///< The output sample history. 99 | 100 | T temp_; // used for storage and preallocation 101 | 102 | std::vector a_; ///< Transfer functon coefficients (output). 103 | std::vector b_; ///< Transfer functon coefficients (input). 104 | }; 105 | 106 | template 107 | SingleChannelTransferFunctionFilter::SingleChannelTransferFunctionFilter() 108 | { 109 | } 110 | 111 | template 112 | SingleChannelTransferFunctionFilter::~SingleChannelTransferFunctionFilter() 113 | { 114 | } 115 | 116 | template 117 | bool SingleChannelTransferFunctionFilter::configure() 118 | { 119 | // Parse a and b into a std::vector. 120 | if (!FilterBase::getParam("a", a_)) { 121 | RCLCPP_ERROR( 122 | this->logging_interface_->get_logger(), 123 | "TransferFunctionFilter, \"%s\", params has no attribute a.", 124 | FilterBase::getName().c_str()); 125 | return false; 126 | } /// \todo check length 127 | 128 | 129 | if (!FilterBase::getParam("b", b_)) { 130 | RCLCPP_ERROR( 131 | this->logging_interface_->get_logger(), 132 | "TransferFunctionFilter, \"%s\", params has no attribute b.", 133 | FilterBase::getName().c_str()); 134 | return false; 135 | } /// \todo check length 136 | 137 | // Create the input and output buffers of the correct size. 138 | input_buffer_.reset(new RealtimeCircularBuffer(b_.size() - 1, temp_)); 139 | output_buffer_.reset(new RealtimeCircularBuffer(a_.size() - 1, temp_)); 140 | 141 | // Prevent divide by zero while normalizing coeffs. 142 | if (a_[0] == 0.) { 143 | RCLCPP_ERROR(this->logging_interface_->get_logger(), "a[0] can not equal 0."); 144 | return false; 145 | } 146 | 147 | // Normalize the coeffs by a[0]. 148 | if (a_[0] != 1.) { 149 | for (size_t i = 0; i < b_.size(); i++) { 150 | b_[i] = (b_[i] / a_[0]); 151 | } 152 | for (size_t i = 1; i < a_.size(); i++) { 153 | a_[i] = (a_[i] / a_[0]); 154 | } 155 | a_[0] = 1.; 156 | } 157 | 158 | return true; 159 | } 160 | 161 | template 162 | bool SingleChannelTransferFunctionFilter::update(const T & data_in, T & data_out) 163 | { 164 | if (!FilterBase::configured_) { 165 | return false; 166 | } 167 | 168 | // Copy data to prevent mutation if in and out are the same ptr 169 | temp_ = data_in; 170 | 171 | data_out = b_[0] * temp_; 172 | 173 | for (size_t row = 1; row <= input_buffer_->size(); ++row) { 174 | data_out += b_[row] * (*input_buffer_)[row - 1]; 175 | } 176 | for (size_t row = 1; row <= output_buffer_->size(); ++row) { 177 | data_out -= a_[row] * (*output_buffer_)[row - 1]; 178 | } 179 | 180 | input_buffer_->push_front(temp_); 181 | output_buffer_->push_front(data_out); 182 | 183 | return true; 184 | } 185 | 186 | 187 | /** 188 | * \brief One-dimensional digital filter class. 189 | * 190 | * This class calculates the output for \f$N\f$ one-dimensional 191 | * digital filters. Where the input, \f$x\f$, is a (\f$N\f$ x 1) vector 192 | * of inputs and the output, \f$y\f$, is a (\f$N\f$ x 1) vector of outputs. 193 | * The filter is described by vectors \f$a\f$ and \f$b\f$ and 194 | * implemented using the standard difference equation:
195 | * 196 | * \f{eqnarray*} 197 | * a[0]*y[n] = b[0]*x[n] &+& b[1]*x[n-1]+ ... + b[n_b]*x[n-n_b]\ \ 198 | * &-& a[1]*y[n-1]- ... - a[n_a]*y[n-n_a] 199 | * \f}
200 | * 201 | * If \f$a[0]\f$ is not equal to 1, the coefficients are normalized by \f$a[0]\f$. 202 | * 203 | * Example xml config:
204 | * 205 | *
206 | *
207 | *
208 | * 209 | */ 210 | template 211 | class MultiChannelTransferFunctionFilter : public filters::MultiChannelFilterBase 212 | { 213 | public: 214 | /** 215 | * \brief Construct the filter 216 | */ 217 | MultiChannelTransferFunctionFilter(); 218 | 219 | /** 220 | * \brief Destructor to clean up 221 | */ 222 | ~MultiChannelTransferFunctionFilter() override; 223 | 224 | /** 225 | * \brief Configure the filter with the correct number of channels and params. 226 | * \param number_of_channels The number of inputs filtered. 227 | * \param config The xml that is parsed to configure the filter. 228 | */ 229 | bool configure() override; 230 | 231 | /** 232 | * \brief Update the filter and return the data seperately 233 | * \param data_in vector with number_of_channels elements 234 | * \param data_out vector with number_of_channels elements 235 | */ 236 | bool update(const std::vector & data_in, std::vector & data_out) override; 237 | 238 | protected: 239 | /// The input sample history. 240 | std::unique_ptr>> input_buffer_; 241 | /// The output sample history. 242 | std::unique_ptr>> output_buffer_; 243 | 244 | std::vector temp_; // used for storage and preallocation 245 | 246 | std::vector a_; ///< Transfer functon coefficients (output). 247 | std::vector b_; ///< Transfer functon coefficients (input). 248 | }; 249 | 250 | template 251 | MultiChannelTransferFunctionFilter::MultiChannelTransferFunctionFilter() 252 | { 253 | } 254 | 255 | template 256 | MultiChannelTransferFunctionFilter::~MultiChannelTransferFunctionFilter() 257 | { 258 | } 259 | 260 | template 261 | bool MultiChannelTransferFunctionFilter::configure() 262 | { 263 | // Parse a and b into a std::vector. 264 | if (!FilterBase::getParam("a", a_)) { 265 | RCLCPP_ERROR( 266 | this->logging_interface_->get_logger(), 267 | "TransferFunctionFilter, \"%s\", params has no attribute a.", 268 | FilterBase::getName().c_str()); 269 | return false; 270 | } /// \todo check length 271 | 272 | if (!FilterBase::getParam("b", b_)) { 273 | RCLCPP_ERROR( 274 | this->logging_interface_->get_logger(), 275 | "TransferFunctionFilter, \"%s\", params has no attribute b.", 276 | FilterBase::getName().c_str()); 277 | return false; 278 | } /// \todo check length 279 | 280 | // Create the input and output buffers of the correct size. 281 | temp_.resize(this->number_of_channels_); 282 | input_buffer_.reset(new RealtimeCircularBuffer>(b_.size() - 1, temp_)); 283 | output_buffer_.reset(new RealtimeCircularBuffer>(a_.size() - 1, temp_)); 284 | 285 | // Prevent divide by zero while normalizing coeffs. 286 | if (a_[0] == 0.) { 287 | RCLCPP_ERROR(this->logging_interface_->get_logger(), "a[0] can not equal 0."); 288 | return false; 289 | } 290 | 291 | // Normalize the coeffs by a[0]. 292 | if (a_[0] != 1.) { 293 | for (size_t i = 0; i < b_.size(); ++i) { 294 | b_[i] = (b_[i] / a_[0]); 295 | } 296 | for (size_t i = 1; i < a_.size(); ++i) { 297 | a_[i] = (a_[i] / a_[0]); 298 | } 299 | a_[0] = 1.; 300 | } 301 | 302 | return true; 303 | } 304 | 305 | template 306 | bool MultiChannelTransferFunctionFilter::update( 307 | const std::vector & data_in, 308 | std::vector & data_out) 309 | { 310 | // Ensure the correct number of inputs 311 | if (data_in.size() != this->number_of_channels_ || data_out.size() != this->number_of_channels_) { 312 | RCLCPP_ERROR( 313 | this->logging_interface_->get_logger(), 314 | "Number of channels is %zu, but data_in.size() = %zu and data_out.size() = %zu. " 315 | "They must match", this->number_of_channels_, data_in.size(), data_out.size()); 316 | return false; 317 | } 318 | // Copy data to prevent mutation if in and out are the same ptr 319 | temp_ = data_in; 320 | 321 | for (size_t i = 0; i < temp_.size(); ++i) { 322 | data_out[i] = b_[0] * temp_[i]; 323 | 324 | for (size_t row = 1; row <= input_buffer_->size(); ++row) { 325 | data_out[i] += b_[row] * (*input_buffer_)[row - 1][i]; 326 | } 327 | for (size_t row = 1; row <= output_buffer_->size(); ++row) { 328 | data_out[i] -= a_[row] * (*output_buffer_)[row - 1][i]; 329 | } 330 | } 331 | input_buffer_->push_front(temp_); 332 | output_buffer_->push_front(data_out); 333 | return true; 334 | } 335 | 336 | } // namespace filters 337 | 338 | #endif // FILTERS__TRANSFER_FUNCTION_HPP_ 339 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | filters 3 | 2.2.1 4 | 5 | This library provides a standardized interface for processing data as a sequence 6 | of filters. This package contains a base class upon which to build specific implementations 7 | as well as an interface which dynamically loads filters based on runtime parameters. 8 | 9 | 10 | Tully Foote 11 | Jon Binney 12 | BSD 13 | https://wiki.ros.org/filters 14 | 15 | ament_cmake 16 | 17 | libboost-dev 18 | pluginlib 19 | rclcpp 20 | 21 | ament_cmake_gtest 22 | ament_cmake_cppcheck 23 | ament_cmake_cpplint 24 | ament_cmake_uncrustify 25 | ament_cmake_xmllint 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /src/increment.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "filters/increment.hpp" 31 | #include "pluginlib/class_list_macros.hpp" 32 | 33 | 34 | PLUGINLIB_EXPORT_CLASS(filters::IncrementFilter, filters::FilterBase) 35 | PLUGINLIB_EXPORT_CLASS( 36 | filters::MultiChannelIncrementFilter, 37 | filters::MultiChannelFilterBase) 38 | -------------------------------------------------------------------------------- /src/mean.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "filters/mean.hpp" 31 | #include "pluginlib/class_list_macros.hpp" 32 | 33 | 34 | PLUGINLIB_EXPORT_CLASS(filters::MeanFilter, filters::FilterBase) 35 | PLUGINLIB_EXPORT_CLASS(filters::MeanFilter, filters::FilterBase) 36 | PLUGINLIB_EXPORT_CLASS( 37 | filters::MultiChannelMeanFilter, 38 | filters::MultiChannelFilterBase) 39 | PLUGINLIB_EXPORT_CLASS( 40 | filters::MultiChannelMeanFilter, 41 | filters::MultiChannelFilterBase) 42 | -------------------------------------------------------------------------------- /src/median.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "filters/median.hpp" 31 | #include 32 | 33 | // Double precision 34 | PLUGINLIB_EXPORT_CLASS(filters::MedianFilter, filters::FilterBase) 35 | PLUGINLIB_EXPORT_CLASS( 36 | filters::MultiChannelMedianFilter, 37 | filters::MultiChannelFilterBase) 38 | 39 | // Float precision 40 | PLUGINLIB_EXPORT_CLASS(filters::MedianFilter, filters::FilterBase) 41 | PLUGINLIB_EXPORT_CLASS( 42 | filters::MultiChannelMedianFilter, 43 | filters::MultiChannelFilterBase) 44 | -------------------------------------------------------------------------------- /src/test_params.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #include "pluginlib/class_list_macros.hpp" 34 | 35 | #include "filters/param_test.hpp" 36 | 37 | PLUGINLIB_EXPORT_CLASS(filters::ParamTest, filters::FilterBase) 38 | PLUGINLIB_EXPORT_CLASS(filters::ParamTest, filters::FilterBase) 39 | PLUGINLIB_EXPORT_CLASS(filters::ParamTest, filters::FilterBase) 40 | PLUGINLIB_EXPORT_CLASS(filters::ParamTest, filters::FilterBase) 41 | 42 | PLUGINLIB_EXPORT_CLASS( 43 | filters::ParamTest>, 44 | filters::FilterBase>) 45 | PLUGINLIB_EXPORT_CLASS( 46 | filters::ParamTest>, 47 | filters::FilterBase>) 48 | -------------------------------------------------------------------------------- /src/transfer_function.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "filters/transfer_function.hpp" 31 | #include "pluginlib/class_list_macros.hpp" 32 | 33 | PLUGINLIB_EXPORT_CLASS( 34 | filters::SingleChannelTransferFunctionFilter, 35 | filters::FilterBase) 36 | PLUGINLIB_EXPORT_CLASS( 37 | filters::MultiChannelTransferFunctionFilter, 38 | filters::MultiChannelFilterBase) 39 | -------------------------------------------------------------------------------- /test/test_chain.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include "filters/filter_chain.hpp" 37 | 38 | class ChainTest : public ::testing::Test 39 | { 40 | protected: 41 | ChainTest() 42 | { 43 | rclcpp::init(0, nullptr); 44 | } 45 | 46 | ~ChainTest() override 47 | { 48 | rclcpp::shutdown(); 49 | } 50 | 51 | rclcpp::Node::SharedPtr 52 | make_node_with_params(const std::vector & overrides) 53 | { 54 | rclcpp::NodeOptions options; 55 | options.parameter_overrides() = overrides; 56 | return std::make_shared("chain_test", options); 57 | } 58 | }; 59 | 60 | 61 | TEST_F(ChainTest, multi_channel_configuring) { 62 | double epsilon = 1e-9; 63 | filters::MultiChannelFilterChain chain("double"); 64 | 65 | std::vector overrides; 66 | overrides.emplace_back("MultiChannelMeanFilterDouble5.filter1.name", std::string("mean_test")); 67 | overrides.emplace_back( 68 | "MultiChannelMeanFilterDouble5.filter1.type", 69 | std::string("filters/MultiChannelMeanFilterDouble")); 70 | overrides.emplace_back("MultiChannelMeanFilterDouble5.filter1.params.number_of_observations", 5); 71 | auto node = make_node_with_params(overrides); 72 | ASSERT_TRUE( 73 | chain.configure( 74 | 5, "MultiChannelMeanFilterDouble5", 75 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 76 | 77 | double input1[] = {1., 2., 3., 4., 5.}; 78 | double input1a[] = {9., 9., 9., 9., 9.}; // seed w/incorrect values 79 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(double)); 80 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(double)); 81 | 82 | 83 | EXPECT_TRUE(chain.update(v1, v1a)); 84 | 85 | chain.clear(); 86 | 87 | for (size_t i = 1; i < v1.size(); ++i) { 88 | EXPECT_NEAR(input1[i], v1a[i], epsilon); 89 | } 90 | } 91 | 92 | TEST_F(ChainTest, configuring) { 93 | double epsilon = 1e-9; 94 | std::vector overrides; 95 | overrides.emplace_back("MeanFilterFloat5.filter1.name", std::string("mean_test")); 96 | overrides.emplace_back("MeanFilterFloat5.filter1.type", std::string("filters/MeanFilterFloat")); 97 | overrides.emplace_back("MeanFilterFloat5.filter1.params.number_of_observations", 5); 98 | auto node = make_node_with_params(overrides); 99 | 100 | filters::FilterChain chain("float"); 101 | 102 | ASSERT_TRUE( 103 | chain.configure( 104 | "MeanFilterFloat5", node->get_node_logging_interface(), 105 | node->get_node_parameters_interface())); 106 | 107 | float v1 = 1.; 108 | float v1a = 9.; 109 | 110 | EXPECT_TRUE(chain.update(v1, v1a)); 111 | 112 | chain.clear(); 113 | 114 | EXPECT_NEAR(v1, v1a, epsilon); 115 | } 116 | 117 | /** 118 | * In order to parallelize filtering, users may want to instantiate multiple filter chains using 119 | * one set of parameters. In this case we need to not error out if the parameters have already 120 | * beend declared by one of the other filter chain instances. 121 | */ 122 | TEST_F(ChainTest, ParallelChainConfiguring) { 123 | double epsilon = 1e-9; 124 | std::vector overrides; 125 | overrides.emplace_back( 126 | "ParallelChainMeanFilter.filter1.name", 127 | std::string("parallel_chain_test")); 128 | overrides.emplace_back( 129 | "ParallelChainMeanFilter.filter1.type", 130 | std::string("filters/MeanFilterFloat")); 131 | overrides.emplace_back("ParallelChainMeanFilter.filter1.params.number_of_observations", 5); 132 | auto node = make_node_with_params(overrides); 133 | 134 | std::vector>> chains { 135 | std::make_shared>("float"), 136 | std::make_shared>("float")}; 137 | 138 | for (auto & chain : chains) { 139 | ASSERT_TRUE( 140 | chain->configure( 141 | "ParallelChainMeanFilter", node->get_node_logging_interface(), 142 | node->get_node_parameters_interface())); 143 | } 144 | 145 | for (auto & chain : chains) { 146 | float v1 = 1.; 147 | float v1a = 9.; 148 | 149 | EXPECT_TRUE(chain->update(v1, v1a)); 150 | 151 | chain->clear(); 152 | 153 | EXPECT_NEAR(v1, v1a, epsilon); 154 | } 155 | } 156 | 157 | TEST_F(ChainTest, MisconfiguredNumberOfChannels) { 158 | filters::MultiChannelFilterChain chain("double"); 159 | 160 | std::vector overrides; 161 | overrides.emplace_back( 162 | "MultiChannelMedianFilterDouble5.filter1.name", 163 | std::string("median_test")); 164 | overrides.emplace_back( 165 | "MultiChannelMedianFilterDouble5.filter1.type", 166 | std::string("filters/MultiChannelMedianFilterDouble")); 167 | overrides.emplace_back( 168 | "MultiChannelMedianFilterDouble5.filter1.params.number_of_observations", 169 | 5); 170 | auto node = make_node_with_params(overrides); 171 | 172 | ASSERT_TRUE( 173 | chain.configure( 174 | 10, "MultiChannelMedianFilterDouble5", 175 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 176 | 177 | double input1[] = {1., 2., 3., 4., 5.}; 178 | double input1a[] = {1., 2., 3., 4., 5.}; 179 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(double)); 180 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(double)); 181 | 182 | EXPECT_FALSE(chain.update(v1, v1a)); 183 | } 184 | 185 | TEST_F(ChainTest, MultiChannelTwoFilters) { 186 | double epsilon = 1e-9; 187 | filters::MultiChannelFilterChain chain("double"); 188 | 189 | std::vector overrides; 190 | overrides.emplace_back("TwoFilters.filter1.name", std::string("median_test_unique")); 191 | overrides.emplace_back( 192 | "TwoFilters.filter1.type", 193 | std::string("filters/MultiChannelMedianFilterDouble")); 194 | overrides.emplace_back("TwoFilters.filter1.params.number_of_observations", 5); 195 | overrides.emplace_back("TwoFilters.filter2.name", std::string("median_test2")); 196 | overrides.emplace_back( 197 | "TwoFilters.filter2.type", 198 | std::string("filters/MultiChannelMedianFilterDouble")); 199 | overrides.emplace_back("TwoFilters.filter2.params.number_of_observations", 5); 200 | auto node = make_node_with_params(overrides); 201 | 202 | ASSERT_TRUE( 203 | chain.configure( 204 | 5, "TwoFilters", 205 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 206 | 207 | double input1[] = {1., 2., 3., 4., 5.}; 208 | double input1a[] = {9., 9., 9., 9., 9.}; // seed w/incorrect values 209 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(double)); 210 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(double)); 211 | 212 | EXPECT_TRUE(chain.update(v1, v1a)); 213 | 214 | chain.clear(); 215 | 216 | for (size_t i = 1; i < v1.size(); i++) { 217 | EXPECT_NEAR(input1[i], v1a[i], epsilon); 218 | } 219 | } 220 | 221 | 222 | TEST_F(ChainTest, TransferFunction) { 223 | double epsilon = 1e-4; 224 | 225 | filters::MultiChannelFilterChain chain("double"); 226 | 227 | std::vector overrides; 228 | overrides.emplace_back("TransferFunction.filter1.name", std::string("transfer_function")); 229 | overrides.emplace_back( 230 | "TransferFunction.filter1.type", 231 | std::string("filters/MultiChannelTransferFunctionFilterDouble")); 232 | overrides.emplace_back( 233 | "TransferFunction.filter1.params.a", 234 | std::vector({1.0, -1.760041880343169, 1.182893262037831})); 235 | overrides.emplace_back( 236 | "TransferFunction.filter1.params.b", 237 | std::vector( 238 | {0.018098933007514, 0.054296799022543, 0.054296799022543, 239 | 0.018098933007514})); 240 | auto node = make_node_with_params(overrides); 241 | ASSERT_TRUE( 242 | chain.configure( 243 | 3, "TransferFunction", 244 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 245 | 246 | std::vector in1, in2, in3, in4, in5, in6, in7; 247 | std::vector out1; 248 | 249 | in1.push_back(10.0); 250 | in1.push_back(10.0); 251 | in1.push_back(10.0); 252 | // 253 | in2.push_back(70.0); 254 | in2.push_back(30.0); 255 | in2.push_back(8.0); 256 | // 257 | in3.push_back(-1.0); 258 | in3.push_back(5.0); 259 | in3.push_back(22.0); 260 | // 261 | in4.push_back(44.0); 262 | in4.push_back(23.0); 263 | in4.push_back(8.0); 264 | // 265 | in5.push_back(10.0); 266 | in5.push_back(10.0); 267 | in5.push_back(10.0); 268 | // 269 | in6.push_back(5.0); 270 | in6.push_back(-1.0); 271 | in6.push_back(5.0); 272 | // 273 | in7.push_back(6.0); 274 | in7.push_back(-30.0); 275 | in7.push_back(2.0); 276 | // 277 | out1.push_back(17.1112); 278 | out1.push_back(9.0285); 279 | out1.push_back(8.3102); 280 | EXPECT_TRUE(chain.update(in1, in1)); 281 | EXPECT_TRUE(chain.update(in2, in2)); 282 | EXPECT_TRUE(chain.update(in3, in3)); 283 | EXPECT_TRUE(chain.update(in4, in4)); 284 | EXPECT_TRUE(chain.update(in5, in5)); 285 | EXPECT_TRUE(chain.update(in6, in6)); 286 | EXPECT_TRUE(chain.update(in7, in7)); 287 | 288 | chain.clear(); 289 | 290 | for (size_t i = 0; i < out1.size(); i++) { 291 | EXPECT_NEAR(out1[i], in7[i], epsilon); 292 | } 293 | } 294 | 295 | TEST_F(ChainTest, ReconfiguringChain) { 296 | filters::FilterChain chain("int"); 297 | 298 | int v1 = 1; 299 | int v1a = 9; 300 | 301 | std::vector overrides; 302 | overrides.emplace_back("OneIncrements.filter1.name", std::string("increment1")); 303 | overrides.emplace_back("OneIncrements.filter1.type", std::string("filters/IncrementFilterInt")); 304 | overrides.emplace_back("TwoIncrements.filter1.name", std::string("increment1")); 305 | overrides.emplace_back("TwoIncrements.filter1.type", std::string("filters/IncrementFilterInt")); 306 | overrides.emplace_back("TwoIncrements.filter2.name", std::string("increment2")); 307 | overrides.emplace_back("TwoIncrements.filter2.type", std::string("filters/IncrementFilterInt")); 308 | auto node = make_node_with_params(overrides); 309 | 310 | ASSERT_TRUE( 311 | chain.configure( 312 | "OneIncrements", node->get_node_logging_interface(), node->get_node_parameters_interface())); 313 | EXPECT_TRUE(chain.update(v1, v1a)); 314 | EXPECT_EQ(2, v1a); 315 | chain.clear(); 316 | 317 | ASSERT_TRUE( 318 | chain.configure( 319 | "TwoIncrements", node->get_node_logging_interface(), node->get_node_parameters_interface())); 320 | EXPECT_TRUE(chain.update(v1, v1a)); 321 | EXPECT_EQ(3, v1a); 322 | } 323 | 324 | TEST_F(ChainTest, ThreeIncrementChains) { 325 | filters::FilterChain chain("int"); 326 | int v1 = 1; 327 | int v1a = 9; 328 | 329 | std::vector overrides; 330 | overrides.emplace_back("ThreeIncrements.filter1.name", std::string("increment1")); 331 | overrides.emplace_back("ThreeIncrements.filter1.type", std::string("filters/IncrementFilterInt")); 332 | overrides.emplace_back("ThreeIncrements.filter2.name", std::string("increment2")); 333 | overrides.emplace_back("ThreeIncrements.filter2.type", std::string("filters/IncrementFilterInt")); 334 | overrides.emplace_back("ThreeIncrements.filter3.name", std::string("increment3")); 335 | overrides.emplace_back("ThreeIncrements.filter3.type", std::string("filters/IncrementFilterInt")); 336 | auto node = make_node_with_params(overrides); 337 | 338 | ASSERT_TRUE( 339 | chain.configure( 340 | "ThreeIncrements", node->get_node_logging_interface(), 341 | node->get_node_parameters_interface())); 342 | EXPECT_TRUE(chain.update(v1, v1a)); 343 | EXPECT_EQ(4, v1a); 344 | } 345 | 346 | TEST_F(ChainTest, TenIncrementChains) { 347 | filters::FilterChain chain("int"); 348 | int v1 = 1; 349 | int v1a = 9; 350 | 351 | std::vector overrides; 352 | overrides.emplace_back("TenIncrements.filter1.name", std::string("increment1")); 353 | overrides.emplace_back("TenIncrements.filter1.type", std::string("filters/IncrementFilterInt")); 354 | overrides.emplace_back("TenIncrements.filter2.name", std::string("increment2")); 355 | overrides.emplace_back("TenIncrements.filter2.type", std::string("filters/IncrementFilterInt")); 356 | overrides.emplace_back("TenIncrements.filter3.name", std::string("increment3")); 357 | overrides.emplace_back("TenIncrements.filter3.type", std::string("filters/IncrementFilterInt")); 358 | overrides.emplace_back("TenIncrements.filter4.name", std::string("increment4")); 359 | overrides.emplace_back("TenIncrements.filter4.type", std::string("filters/IncrementFilterInt")); 360 | overrides.emplace_back("TenIncrements.filter5.name", std::string("increment5")); 361 | overrides.emplace_back("TenIncrements.filter5.type", std::string("filters/IncrementFilterInt")); 362 | overrides.emplace_back("TenIncrements.filter6.name", std::string("increment6")); 363 | overrides.emplace_back("TenIncrements.filter6.type", std::string("filters/IncrementFilterInt")); 364 | overrides.emplace_back("TenIncrements.filter7.name", std::string("increment7")); 365 | overrides.emplace_back("TenIncrements.filter7.type", std::string("filters/IncrementFilterInt")); 366 | overrides.emplace_back("TenIncrements.filter8.name", std::string("increment8")); 367 | overrides.emplace_back("TenIncrements.filter8.type", std::string("filters/IncrementFilterInt")); 368 | overrides.emplace_back("TenIncrements.filter9.name", std::string("increment9")); 369 | overrides.emplace_back("TenIncrements.filter9.type", std::string("filters/IncrementFilterInt")); 370 | overrides.emplace_back("TenIncrements.filter10.name", std::string("increment10")); 371 | overrides.emplace_back("TenIncrements.filter10.type", std::string("filters/IncrementFilterInt")); 372 | auto node = make_node_with_params(overrides); 373 | 374 | ASSERT_TRUE( 375 | chain.configure( 376 | "TenIncrements", node->get_node_logging_interface(), node->get_node_parameters_interface())); 377 | EXPECT_TRUE(chain.update(v1, v1a)); 378 | EXPECT_EQ(11, v1a); 379 | } 380 | 381 | TEST_F(ChainTest, TenMultiChannelIncrementChains) { 382 | filters::MultiChannelFilterChain chain("int"); 383 | std::vector v1; 384 | v1.push_back(1); 385 | v1.push_back(1); 386 | v1.push_back(1); 387 | std::vector v1a = v1; 388 | 389 | std::vector overrides; 390 | overrides.emplace_back("TenMultiChannelIncrements.filter1.name", std::string("increment1")); 391 | overrides.emplace_back( 392 | "TenMultiChannelIncrements.filter1.type", 393 | std::string("filters/MultiChannelIncrementFilterInt")); 394 | overrides.emplace_back("TenMultiChannelIncrements.filter2.name", std::string("increment2")); 395 | overrides.emplace_back( 396 | "TenMultiChannelIncrements.filter2.type", 397 | std::string("filters/MultiChannelIncrementFilterInt")); 398 | overrides.emplace_back("TenMultiChannelIncrements.filter3.name", std::string("increment3")); 399 | overrides.emplace_back( 400 | "TenMultiChannelIncrements.filter3.type", 401 | std::string("filters/MultiChannelIncrementFilterInt")); 402 | overrides.emplace_back("TenMultiChannelIncrements.filter4.name", std::string("increment4")); 403 | overrides.emplace_back( 404 | "TenMultiChannelIncrements.filter4.type", 405 | std::string("filters/MultiChannelIncrementFilterInt")); 406 | overrides.emplace_back("TenMultiChannelIncrements.filter5.name", std::string("increment5")); 407 | overrides.emplace_back( 408 | "TenMultiChannelIncrements.filter5.type", 409 | std::string("filters/MultiChannelIncrementFilterInt")); 410 | overrides.emplace_back("TenMultiChannelIncrements.filter6.name", std::string("increment6")); 411 | overrides.emplace_back( 412 | "TenMultiChannelIncrements.filter6.type", 413 | std::string("filters/MultiChannelIncrementFilterInt")); 414 | overrides.emplace_back("TenMultiChannelIncrements.filter7.name", std::string("increment7")); 415 | overrides.emplace_back( 416 | "TenMultiChannelIncrements.filter7.type", 417 | std::string("filters/MultiChannelIncrementFilterInt")); 418 | overrides.emplace_back("TenMultiChannelIncrements.filter8.name", std::string("increment8")); 419 | overrides.emplace_back( 420 | "TenMultiChannelIncrements.filter8.type", 421 | std::string("filters/MultiChannelIncrementFilterInt")); 422 | overrides.emplace_back("TenMultiChannelIncrements.filter9.name", std::string("increment9")); 423 | overrides.emplace_back( 424 | "TenMultiChannelIncrements.filter9.type", 425 | std::string("filters/MultiChannelIncrementFilterInt")); 426 | overrides.emplace_back("TenMultiChannelIncrements.filter10.name", std::string("increment10")); 427 | overrides.emplace_back( 428 | "TenMultiChannelIncrements.filter10.type", 429 | std::string("filters/MultiChannelIncrementFilterInt")); 430 | auto node = make_node_with_params(overrides); 431 | 432 | ASSERT_TRUE( 433 | chain.configure( 434 | 3, "TenMultiChannelIncrements", node->get_node_logging_interface(), 435 | node->get_node_parameters_interface())); 436 | EXPECT_TRUE(chain.update(v1, v1a)); 437 | for (size_t i = 0; i < 3; ++i) { 438 | EXPECT_EQ(11, v1a[i]); 439 | } 440 | } 441 | -------------------------------------------------------------------------------- /test/test_mean.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include "rclcpp/rclcpp.hpp" 36 | 37 | #include "filters/mean.hpp" 38 | 39 | class MeanFilterTest : public ::testing::Test 40 | { 41 | protected: 42 | MeanFilterTest() 43 | { 44 | rclcpp::init(0, nullptr); 45 | rclcpp::NodeOptions options; 46 | options.parameter_overrides().emplace_back("dummy.prefix.number_of_observations", 5); 47 | node_ = std::make_shared("mean_filter_test", options); 48 | } 49 | 50 | ~MeanFilterTest() override 51 | { 52 | node_.reset(); 53 | rclcpp::shutdown(); 54 | } 55 | 56 | rclcpp::Node::SharedPtr node_; 57 | }; 58 | 59 | TEST_F(MeanFilterTest, MultiChannelConfirmIdentityNRows) 60 | { 61 | double epsilon = 1e-6; 62 | size_t length = 5; 63 | size_t rows = 5; 64 | 65 | std::shared_ptr> filter = 66 | std::make_shared>(); 67 | ASSERT_TRUE( 68 | filter->configure( 69 | rows, "dummy.prefix", "MultiChannelMeanFilterDouble5", 70 | node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 71 | 72 | double input1[] = {1., 2., 3., 4., 5.}; 73 | double input1a[] = {1., 2., 3., 4., 5.}; 74 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(double)); 75 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(double)); 76 | 77 | for (size_t i = 0; i < rows * 10; ++i) { 78 | EXPECT_TRUE(filter->update(v1, v1a)); 79 | 80 | for (size_t j = 1; j < length; ++j) { 81 | EXPECT_NEAR(v1[j], v1a[j], epsilon); 82 | } 83 | } 84 | } 85 | 86 | TEST_F(MeanFilterTest, MultiChannelThreeRows) 87 | { 88 | double epsilon = 1e-6; 89 | size_t length = 5; 90 | size_t rows = 5; 91 | 92 | std::shared_ptr> filter = 93 | std::make_shared>(); 94 | ASSERT_TRUE( 95 | filter->configure( 96 | rows, "dummy.prefix", "MultiChannelMeanFilterDouble5", 97 | node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 98 | 99 | double input1[] = {0., 1., 2., 3., 4.}; 100 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(double)); 101 | double input2[] = {1., 2., 3., 4., 5.}; 102 | std::vector v2(input2, input2 + sizeof(input2) / sizeof(double)); 103 | double input3[] = {2., 3., 4., 5., 6.}; 104 | std::vector v3(input3, input3 + sizeof(input3) / sizeof(double)); 105 | double input1a[] = {1., 2., 3., 4., 5.}; 106 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(double)); 107 | 108 | EXPECT_TRUE(filter->update(v1, v1a)); 109 | EXPECT_TRUE(filter->update(v2, v1a)); 110 | EXPECT_TRUE(filter->update(v3, v1a)); 111 | 112 | for (size_t i = 1; i < length; ++i) { 113 | EXPECT_NEAR(v2[i], v1a[i], epsilon); 114 | } 115 | } 116 | 117 | TEST_F(MeanFilterTest, ConfirmIdentityNRows) 118 | { 119 | double epsilon = 1e-6; 120 | 121 | std::shared_ptr> filter = 122 | std::make_shared>(); 123 | ASSERT_TRUE( 124 | filter->configure( 125 | "dummy.prefix", "MeanFilterDouble5", 126 | node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 127 | 128 | double input = 1.; 129 | double output = 0.; 130 | 131 | EXPECT_TRUE(filter->update(input, output)); 132 | EXPECT_NEAR(input, output, epsilon); 133 | } 134 | 135 | TEST_F(MeanFilterTest, ThreeRows) 136 | { 137 | double epsilon = 1e-6; 138 | 139 | std::shared_ptr> filter = 140 | std::make_shared>(); 141 | ASSERT_TRUE( 142 | filter->configure( 143 | "dummy.prefix", "MeanFilterDouble5", 144 | node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 145 | 146 | double input1 = 0.; 147 | double input2 = 1.; 148 | double input3 = 2.; 149 | double output = 3.; 150 | 151 | EXPECT_TRUE(filter->update(input1, output)); 152 | EXPECT_NEAR(input1, output, epsilon); 153 | EXPECT_TRUE(filter->update(input2, output)); 154 | EXPECT_NEAR((input1 + input2) / 2.0, output, epsilon); 155 | EXPECT_TRUE(filter->update(input3, output)); 156 | EXPECT_NEAR((input1 + input2 + input3) / 3.0, output, epsilon); 157 | } 158 | -------------------------------------------------------------------------------- /test/test_median.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include "filters/median.hpp" 36 | 37 | class MedianFilterTest : public ::testing::Test 38 | { 39 | protected: 40 | MedianFilterTest() 41 | { 42 | rclcpp::init(0, nullptr); 43 | rclcpp::NodeOptions options; 44 | options.parameter_overrides().emplace_back("dummy.prefix.number_of_observations", 5); 45 | node_ = std::make_shared("median_filter_test", options); 46 | } 47 | 48 | ~MedianFilterTest() override 49 | { 50 | node_.reset(); 51 | rclcpp::shutdown(); 52 | } 53 | 54 | rclcpp::Node::SharedPtr node_; 55 | }; 56 | 57 | TEST_F(MedianFilterTest, MultiChannelDoubleConfirmIdentityNRows) 58 | { 59 | double epsilon = 1e-6; 60 | size_t length = 5; 61 | size_t rows = 5; 62 | 63 | std::shared_ptr> filter = 64 | std::make_shared>(); 65 | ASSERT_TRUE( 66 | filter->configure( 67 | rows, "dummy.prefix", "MultiChannelMedianFilterDouble5", 68 | node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 69 | 70 | double input1[] = {1., 2., 3., 4., 5.}; 71 | double input1a[] = {11., 12., 13., 14., 15.}; 72 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(double)); 73 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(double)); 74 | 75 | for (size_t i = 0; i < rows * 10; ++i) { 76 | EXPECT_TRUE(filter->update(v1, v1a)); 77 | 78 | for (size_t j = 1; j < length; ++j) { 79 | EXPECT_NEAR(input1[j], v1a[j], epsilon); 80 | } 81 | } 82 | } 83 | 84 | TEST_F(MedianFilterTest, MultiChannelDoubleThreeRows) 85 | { 86 | double epsilon = 1e-6; 87 | size_t length = 5; 88 | size_t rows = 5; 89 | 90 | std::shared_ptr> filter = 91 | std::make_shared>(); 92 | ASSERT_TRUE( 93 | filter->configure( 94 | rows, "dummy.prefix", "MultiChannelMedianFilterDouble5", 95 | node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 96 | 97 | double input1[] = {0., 1., 2., 3., 4.}; 98 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(double)); 99 | double input2[] = {1., 2., 3., 4., 5.}; 100 | std::vector v2(input2, input2 + sizeof(input2) / sizeof(double)); 101 | double input3[] = {2., 3., 4., 5., 6.}; 102 | std::vector v3(input3, input3 + sizeof(input3) / sizeof(double)); 103 | double input1a[] = {1., 2., 3., 4., 5.}; 104 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(double)); 105 | 106 | EXPECT_TRUE(filter->update(v1, v1a)); 107 | EXPECT_TRUE(filter->update(v2, v1a)); 108 | EXPECT_TRUE(filter->update(v3, v1a)); 109 | 110 | for (size_t i = 1; i < length; i++) { 111 | EXPECT_NEAR(v2[i], v1a[i], epsilon); 112 | } 113 | } 114 | 115 | TEST_F(MedianFilterTest, MultiChannelFloatConfirmIdentityNRows) 116 | { 117 | float epsilon = 1e-6; 118 | size_t length = 5; 119 | size_t rows = 5; 120 | 121 | std::shared_ptr> filter = 122 | std::make_shared>(); 123 | ASSERT_TRUE( 124 | filter->configure( 125 | rows, "dummy.prefix", "MultiChannelMedianFilterFloat5", 126 | node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 127 | 128 | float input1[] = {1., 2., 3., 4., 5.}; 129 | float input1a[] = {1., 2., 3., 4., 5.}; 130 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(float)); 131 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(float)); 132 | 133 | for (size_t i = 0; i < rows * 10; ++i) { 134 | EXPECT_TRUE(filter->update(v1, v1a)); 135 | 136 | for (size_t j = 1; j < length; ++j) { 137 | EXPECT_NEAR(input1[j], v1a[j], epsilon); 138 | } 139 | } 140 | } 141 | 142 | TEST_F(MedianFilterTest, MultiChannelFloatThreeRows) 143 | { 144 | float epsilon = 1e-6; 145 | size_t length = 5; 146 | size_t rows = 5; 147 | 148 | std::shared_ptr> filter = 149 | std::make_shared>(); 150 | ASSERT_TRUE( 151 | filter->configure( 152 | rows, "dummy.prefix", "MultiChannelMedianFilterFloat5", 153 | node_->get_node_logging_interface(), node_->get_node_parameters_interface())); 154 | 155 | float input1[] = {0., 1., 2., 3., 4.}; 156 | std::vector v1(input1, input1 + sizeof(input1) / sizeof(float)); 157 | float input2[] = {1., 2., 3., 4., 5.}; 158 | std::vector v2(input2, input2 + sizeof(input2) / sizeof(float)); 159 | float input3[] = {2., 3., 4., 5., 6.}; 160 | std::vector v3(input3, input3 + sizeof(input3) / sizeof(float)); 161 | float input1a[] = {1., 2., 3., 4., 5.}; 162 | std::vector v1a(input1a, input1a + sizeof(input1a) / sizeof(float)); 163 | 164 | EXPECT_TRUE(filter->update(v1, v1a)); 165 | EXPECT_TRUE(filter->update(v2, v1a)); 166 | EXPECT_TRUE(filter->update(v3, v1a)); 167 | 168 | for (size_t i = 1; i < length; ++i) { 169 | EXPECT_NEAR(v2[i], v1a[i], epsilon); 170 | } 171 | } 172 | -------------------------------------------------------------------------------- /test/test_params.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include "filters/param_test.hpp" 37 | 38 | class ParametersTest : public ::testing::Test 39 | { 40 | protected: 41 | ParametersTest() 42 | { 43 | rclcpp::init(0, nullptr); 44 | } 45 | 46 | ~ParametersTest() override 47 | { 48 | rclcpp::shutdown(); 49 | } 50 | 51 | template 52 | rclcpp::Node::SharedPtr 53 | make_node_with_one_param(const T & value) 54 | { 55 | rclcpp::NodeOptions options; 56 | options.parameter_overrides().emplace_back("dummy.prefix.key", value); 57 | return std::make_shared("param_test", options); 58 | } 59 | }; 60 | 61 | TEST_F(ParametersTest, Double) 62 | { 63 | double epsilon = 1e-6; 64 | 65 | auto node = make_node_with_one_param(4.0); 66 | std::shared_ptr> filter = 67 | std::make_shared>(); 68 | ASSERT_TRUE( 69 | filter->configure( 70 | "dummy.prefix", "TestDouble", 71 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 72 | double out; 73 | filter->update(out, out); 74 | EXPECT_NEAR(4, out, epsilon); 75 | } 76 | 77 | TEST_F(ParametersTest, Int) 78 | { 79 | auto node = make_node_with_one_param(static_cast(4)); 80 | std::shared_ptr> filter = 81 | std::make_shared>(); 82 | ASSERT_TRUE( 83 | filter->configure( 84 | "dummy.prefix", "TestInt", 85 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 86 | int out; 87 | filter->update(out, out); 88 | EXPECT_EQ(4, out); 89 | } 90 | 91 | TEST_F(ParametersTest, UInt) 92 | { 93 | auto node = make_node_with_one_param(static_cast(4)); // int because no unsigned param type 94 | std::shared_ptr> filter = 95 | std::make_shared>(); 96 | ASSERT_TRUE( 97 | filter->configure( 98 | "dummy.prefix", "TestUInt", 99 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 100 | unsigned int out; 101 | filter->update(out, out); 102 | EXPECT_EQ(4u, out); 103 | } 104 | 105 | TEST_F(ParametersTest, String) 106 | { 107 | auto node = make_node_with_one_param(std::string("four")); 108 | std::shared_ptr> filter = 109 | std::make_shared>(); 110 | ASSERT_TRUE( 111 | filter->configure( 112 | "dummy.prefix", "TestString", 113 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 114 | std::string out; 115 | filter->update(out, out); 116 | EXPECT_STREQ("four", out.c_str()); 117 | } 118 | 119 | TEST_F(ParametersTest, DoubleVector) 120 | { 121 | double epsilon = 1e-6; 122 | 123 | auto node = make_node_with_one_param(std::vector({4.0, 4.0, 4.0, 4.0})); 124 | std::shared_ptr>> filter = 125 | std::make_shared>>(); 126 | ASSERT_TRUE( 127 | filter->configure( 128 | "dummy.prefix", "TestDoubleVector", 129 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 130 | std::vector out; 131 | filter->update(out, out); 132 | for (std::vector::iterator it = out.begin(); it != out.end(); ++it) { 133 | EXPECT_NEAR(4, *it, epsilon); 134 | } 135 | } 136 | 137 | TEST_F(ParametersTest, StringVector) 138 | { 139 | auto node = make_node_with_one_param(std::vector({"four", "four", "four", "four"})); 140 | std::shared_ptr>> filter = 141 | std::make_shared>>(); 142 | ASSERT_TRUE( 143 | filter->configure( 144 | "dummy.prefix", "TestStringVector", 145 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 146 | std::vector out; 147 | filter->update(out, out); 148 | for (std::vector::iterator it = out.begin(); it != out.end(); ++it) { 149 | EXPECT_STREQ("four", it->c_str()); 150 | } 151 | } 152 | -------------------------------------------------------------------------------- /test/test_realtime_circular_buffer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include "filters/realtime_circular_buffer.hpp" 36 | 37 | TEST(RealtimeCircularBuffer, InitializationScalar) 38 | { 39 | filters::RealtimeCircularBuffer buf(3, 0); 40 | for (size_t i = 0; i < buf.size(); ++i) { 41 | EXPECT_EQ(buf[i], 0); 42 | } 43 | } 44 | 45 | TEST(RealtimeCircularBuffer, InitializationVector) 46 | { 47 | std::vector init_vector; 48 | for (size_t i = 0; i < 100; ++i) { 49 | init_vector.push_back(i); 50 | } 51 | 52 | filters::RealtimeCircularBuffer> vec_buf(3, init_vector); 53 | for (size_t i = 0; i < vec_buf.size(); ++i) { 54 | for (size_t j = 0; j < 100; ++j) { 55 | EXPECT_EQ(vec_buf[i][j], j); 56 | } 57 | } 58 | } 59 | 60 | TEST(RealtimeCircularBuffer, RangeBasedLoop) 61 | { 62 | filters::RealtimeCircularBuffer vec_buf(100, 1); 63 | int sum = 0; 64 | for (const auto & i : vec_buf) { 65 | sum += i; 66 | } 67 | EXPECT_EQ(sum, 100); 68 | } 69 | -------------------------------------------------------------------------------- /test/test_transfer_function.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 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 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include "filters/transfer_function.hpp" 36 | 37 | class TransferFunctionTest : public ::testing::Test 38 | { 39 | protected: 40 | TransferFunctionTest() 41 | { 42 | rclcpp::init(0, nullptr); 43 | } 44 | 45 | ~TransferFunctionTest() override 46 | { 47 | rclcpp::shutdown(); 48 | } 49 | 50 | rclcpp::Node::SharedPtr 51 | make_node_with_params(const std::vector & a, const std::vector & b) 52 | { 53 | rclcpp::NodeOptions options; 54 | options.parameter_overrides().emplace_back("dummy.prefix.a", a); 55 | options.parameter_overrides().emplace_back("dummy.prefix.b", b); 56 | return std::make_shared("transfer_function_test", options); 57 | } 58 | }; 59 | 60 | TEST_F(TransferFunctionTest, LowPass) 61 | { 62 | const size_t channels = 1; 63 | double epsilon = 1e-4; 64 | 65 | auto node = make_node_with_params( 66 | {1.0, -0.509525449494429}, 67 | {0.245237275252786, 0.245237275252786}); 68 | std::shared_ptr> filter = 69 | std::make_shared>(); 70 | ASSERT_TRUE( 71 | filter->configure( 72 | channels, "dummy.prefix", "LowPass", 73 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 74 | 75 | 76 | std::vector in1, in2, in3, in4, in5, in6, in7; 77 | std::vector out1; 78 | 79 | in1.push_back(10.0); 80 | in2.push_back(70.0); 81 | in3.push_back(10.0); 82 | in4.push_back(44.0); 83 | in5.push_back(10.0); 84 | in6.push_back(5.0); 85 | in7.push_back(6.0); 86 | out1.push_back(11.8008); 87 | EXPECT_TRUE(filter->update(in1, in1)); 88 | EXPECT_TRUE(filter->update(in2, in2)); 89 | EXPECT_TRUE(filter->update(in3, in3)); 90 | EXPECT_TRUE(filter->update(in4, in4)); 91 | EXPECT_TRUE(filter->update(in5, in5)); 92 | EXPECT_TRUE(filter->update(in6, in6)); 93 | EXPECT_TRUE(filter->update(in7, in7)); 94 | 95 | EXPECT_NEAR(out1[0], in7[0], epsilon); 96 | } 97 | 98 | TEST_F(TransferFunctionTest, SingleLowPass) 99 | { 100 | double epsilon = 1e-4; 101 | 102 | auto node = make_node_with_params( 103 | {1.0, -0.509525449494429}, 104 | {0.245237275252786, 0.245237275252786}); 105 | std::shared_ptr> filter = 106 | std::make_shared>(); 107 | ASSERT_TRUE( 108 | filter->configure( 109 | "dummy.prefix", "LowPassSingle", 110 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 111 | 112 | 113 | double in1, in2, in3, in4, in5, in6, in7; 114 | double out1; 115 | 116 | in1 = 10.0; 117 | in2 = 70.0; 118 | in3 = 10.0; 119 | in4 = 44.0; 120 | in5 = 10.0; 121 | in6 = 5.0; 122 | in7 = 6.0; 123 | out1 = 11.8008; 124 | EXPECT_TRUE(filter->update(in1, in1)); 125 | EXPECT_TRUE(filter->update(in2, in2)); 126 | EXPECT_TRUE(filter->update(in3, in3)); 127 | EXPECT_TRUE(filter->update(in4, in4)); 128 | EXPECT_TRUE(filter->update(in5, in5)); 129 | EXPECT_TRUE(filter->update(in6, in6)); 130 | EXPECT_TRUE(filter->update(in7, in7)); 131 | 132 | EXPECT_NEAR(out1, in7, epsilon); 133 | } 134 | 135 | 136 | TEST_F(TransferFunctionTest, LowPassNonUnity) 137 | { 138 | const size_t channels = 1; 139 | double epsilon = 1e-4; 140 | 141 | auto node = make_node_with_params( 142 | {2.0, -0.509525449494429}, 143 | {0.245237275252786, 0.245237275252786}); 144 | std::shared_ptr> filter = 145 | std::make_shared>(); 146 | ASSERT_TRUE( 147 | filter->configure( 148 | channels, "dummy.prefix", "LowPassNonUnity", 149 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 150 | 151 | std::vector in1, in2, in3, in4, in5, in6, in7; 152 | std::vector out1; 153 | 154 | in1.push_back(10.0); 155 | in2.push_back(70.0); 156 | in3.push_back(10.0); 157 | in4.push_back(44.0); 158 | in5.push_back(10.0); 159 | in6.push_back(5.0); 160 | in7.push_back(6.0); 161 | out1.push_back(2.4088); 162 | EXPECT_TRUE(filter->update(in1, in1)); 163 | EXPECT_TRUE(filter->update(in2, in2)); 164 | EXPECT_TRUE(filter->update(in3, in3)); 165 | EXPECT_TRUE(filter->update(in4, in4)); 166 | EXPECT_TRUE(filter->update(in5, in5)); 167 | EXPECT_TRUE(filter->update(in6, in6)); 168 | EXPECT_TRUE(filter->update(in7, in7)); 169 | 170 | EXPECT_NEAR(out1[0], in7[0], epsilon); 171 | } 172 | 173 | TEST_F(TransferFunctionTest, LowPassMulti) 174 | { 175 | const size_t channels = 3; 176 | double epsilon = 1e-4; 177 | 178 | auto node = make_node_with_params( 179 | {1.0, -1.760041880343169, 1.182893262037831, -0.278059917634546}, 180 | {0.018098933007514, 0.245237275252786, 0.054296799022543, 0.018098933007514}); 181 | std::shared_ptr> filter = 182 | std::make_shared>(); 183 | ASSERT_TRUE( 184 | filter->configure( 185 | channels, "dummy.prefix", "LowPassMulti", 186 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 187 | 188 | std::vector in1, in2, in3, in4, in5, in6, in7; 189 | std::vector out1; 190 | 191 | in1.push_back(10.0); 192 | in1.push_back(10.0); 193 | in1.push_back(10.0); 194 | // 195 | in2.push_back(70.0); 196 | in2.push_back(30.0); 197 | in2.push_back(8.0); 198 | // 199 | in3.push_back(-1.0); 200 | in3.push_back(5.0); 201 | in3.push_back(22.0); 202 | // 203 | in4.push_back(44.0); 204 | in4.push_back(23.0); 205 | in4.push_back(8.0); 206 | // 207 | in5.push_back(10.0); 208 | in5.push_back(10.0); 209 | in5.push_back(10.0); 210 | // 211 | in6.push_back(5.0); 212 | in6.push_back(-1.0); 213 | in6.push_back(5.0); 214 | // 215 | in7.push_back(6.0); 216 | in7.push_back(-30.0); 217 | in7.push_back(2.0); 218 | // 219 | out1.push_back(60.6216); 220 | out1.push_back(33.9829); 221 | out1.push_back(28.1027); 222 | EXPECT_TRUE(filter->update(in1, in1)); 223 | EXPECT_TRUE(filter->update(in2, in2)); 224 | EXPECT_TRUE(filter->update(in3, in3)); 225 | EXPECT_TRUE(filter->update(in4, in4)); 226 | EXPECT_TRUE(filter->update(in5, in5)); 227 | EXPECT_TRUE(filter->update(in6, in6)); 228 | EXPECT_TRUE(filter->update(in7, in7)); 229 | 230 | for (size_t i = 0; i < out1.size(); i++) { 231 | EXPECT_NEAR(out1[i], in7[i], epsilon); 232 | } 233 | } 234 | 235 | TEST_F(TransferFunctionTest, LowPassIrrational) 236 | { 237 | const size_t channels = 3; 238 | double epsilon = 1e-4; 239 | 240 | auto node = make_node_with_params( 241 | {1.0, -1.760041880343169, 1.182893262037831}, 242 | {0.018098933007514, 0.054296799022543, 0.054296799022543, 0.018098933007514}); 243 | std::shared_ptr> filter = 244 | std::make_shared>(); 245 | ASSERT_TRUE( 246 | filter->configure( 247 | channels, "dummy.prefix", "LowPassIrrational", 248 | node->get_node_logging_interface(), node->get_node_parameters_interface())); 249 | 250 | std::vector in1, in2, in3, in4, in5, in6, in7; 251 | std::vector out1; 252 | 253 | in1.push_back(10.0); 254 | in1.push_back(10.0); 255 | in1.push_back(10.0); 256 | // 257 | in2.push_back(70.0); 258 | in2.push_back(30.0); 259 | in2.push_back(8.0); 260 | // 261 | in3.push_back(-1.0); 262 | in3.push_back(5.0); 263 | in3.push_back(22.0); 264 | // 265 | in4.push_back(44.0); 266 | in4.push_back(23.0); 267 | in4.push_back(8.0); 268 | // 269 | in5.push_back(10.0); 270 | in5.push_back(10.0); 271 | in5.push_back(10.0); 272 | // 273 | in6.push_back(5.0); 274 | in6.push_back(-1.0); 275 | in6.push_back(5.0); 276 | // 277 | in7.push_back(6.0); 278 | in7.push_back(-30.0); 279 | in7.push_back(2.0); 280 | // 281 | out1.push_back(17.1112); 282 | out1.push_back(9.0285); 283 | out1.push_back(8.3102); 284 | EXPECT_TRUE(filter->update(in1, in1)); 285 | EXPECT_TRUE(filter->update(in2, in2)); 286 | EXPECT_TRUE(filter->update(in3, in3)); 287 | EXPECT_TRUE(filter->update(in4, in4)); 288 | EXPECT_TRUE(filter->update(in5, in5)); 289 | EXPECT_TRUE(filter->update(in6, in6)); 290 | EXPECT_TRUE(filter->update(in7, in7)); 291 | 292 | for (size_t i = 0; i < out1.size(); i++) { 293 | EXPECT_NEAR(out1[i], in7[i], epsilon); 294 | } 295 | } 296 | --------------------------------------------------------------------------------