├── CHANGELOG.rst ├── CMakeLists.txt ├── ChangeHeaderFilter.xml ├── LICENSE ├── README.md ├── examples ├── example_custom_chain_node.cpp ├── example_custom_chain_nodelet.cpp ├── filter.launch ├── filter.yaml ├── image_filter.launch ├── image_filter.yaml ├── pointcloud2_filter.launch └── pointcloud2_filter.yaml ├── include └── sensor_filters │ ├── FilterChainBase.h │ ├── FilterChainNode.h │ ├── FilterChainNodelet.h │ ├── ImageFilterChainBase.h │ └── PointCloud2FilterChainBase.h ├── nodelets.xml ├── package.xml └── src ├── CompressedImageFilterChainNode.cc ├── CompressedImageFilterChainNodelet.cc ├── ImageFilterChainBase.cc ├── ImageFilterChainNode.cc ├── ImageFilterChainNodelet.cc ├── ImuFilterChainNode.cc ├── ImuFilterChainNodelet.cc ├── JoyFilterChainNode.cc ├── JoyFilterChainNodelet.cc ├── LaserScanFilterChainNode.cc ├── LaserScanFilterChainNodelet.cc ├── MagneticFieldFilterChainNode.cc ├── MagneticFieldFilterChainNodelet.cc ├── MultiEchoLaserScanFilterChainNode.cc ├── MultiEchoLaserScanFilterChainNodelet.cc ├── NavSatFixFilterChainNode.cc ├── NavSatFixFilterChainNodelet.cc ├── PointCloud2FilterChainBase.cc ├── PointCloud2FilterChainNode.cc ├── PointCloud2FilterChainNodelet.cc ├── PointCloudFilterChainNode.cc ├── PointCloudFilterChainNodelet.cc ├── RangeFilterChainNode.cc ├── RangeFilterChainNodelet.cc ├── RelativeHumidityFilterChainNode.cc ├── RelativeHumidityFilterChainNodelet.cc ├── TemperatureFilterChainNode.cc ├── TemperatureFilterChainNodelet.cc └── filters ├── ChangeHeaderFilter.cc └── include_all_msgs.hh /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package sensor_filters 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.1 (2023-06-07) 6 | ------------------ 7 | * Reformatted, added catkin lint. 8 | * Simplified the license statements. 9 | * Contributors: Martin Pecka 10 | 11 | 1.1.0 (2023-05-29) 12 | ------------------ 13 | * Using image_transport and point_cloud_transport where applicable. 14 | * Noetic compatibility. 15 | * Improved README and added example files. 16 | Closes `#1 `_. 17 | * Contributors: Martin Pecka 18 | 19 | 1.0.5 (2021-07-30) 20 | ------------------ 21 | * Fixed typo leading to segfaults when using ChangeHeader filter. 22 | * Contributors: Martin Pecka 23 | 24 | 1.0.4 (2021-06-24) 25 | ------------------ 26 | * Added ChangeHeader filter 27 | * Contributors: Martin Pecka 28 | 29 | 1.0.3 (2021-05-20) 30 | ------------------ 31 | * Fixed data types of nodes. 32 | * Contributors: Martin Pecka 33 | 34 | 1.0.2 (2021-05-20) 35 | ------------------ 36 | * Fix for Noetic. 37 | * Contributors: Martin Pecka 38 | 39 | 1.0.1 (2021-05-19) 40 | ------------------ 41 | * Initial version. 42 | * Contributors: Martin Pecka 43 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: BSD-3-Clause 2 | # SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | cmake_minimum_required(VERSION 3.10.2) 5 | project(sensor_filters) 6 | 7 | set(CMAKE_CXX_STANDARD 17) 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | filters 11 | image_transport 12 | nodelet 13 | point_cloud_transport 14 | roscpp 15 | sensor_msgs 16 | ) 17 | 18 | #catkin_lint: ignore missing_export_lib 19 | catkin_package( 20 | INCLUDE_DIRS include 21 | CATKIN_DEPENDS filters nodelet roscpp sensor_msgs 22 | ) 23 | 24 | include_directories(include ${catkin_INCLUDE_DIRS}) 25 | 26 | set(types 27 | CompressedImage 28 | Image 29 | Imu 30 | Joy 31 | LaserScan 32 | MultiEchoLaserScan 33 | MagneticField 34 | NavSatFix 35 | PointCloud 36 | PointCloud2 37 | Range 38 | RelativeHumidity 39 | Temperature 40 | ) 41 | 42 | foreach(type IN LISTS types) 43 | string(TOLOWER "${type}" type_lower) 44 | 45 | set(nodelet_name "${type_lower}_filter_chain_nodelet") 46 | set(node_name "${type_lower}_filter_chain") 47 | 48 | add_library(${nodelet_name} src/${type}FilterChainNodelet.cc) 49 | add_dependencies(${nodelet_name} ${catkin_EXPORTED_TARGETS}) 50 | target_link_libraries(${nodelet_name} ${catkin_LIBRARIES}) 51 | 52 | add_executable(${node_name} src/${type}FilterChainNode.cc) 53 | add_dependencies(${node_name} ${catkin_EXPORTED_TARGETS}) 54 | target_link_libraries(${node_name} ${catkin_LIBRARIES}) 55 | 56 | install(TARGETS ${node_name} 57 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 58 | ) 59 | 60 | install(TARGETS ${nodelet_name} 61 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 64 | ) 65 | endforeach() 66 | 67 | # Additional sources for the targets that use custom transport 68 | target_sources(image_filter_chain_nodelet PRIVATE src/ImageFilterChainBase.cc) 69 | target_sources(image_filter_chain PRIVATE src/ImageFilterChainBase.cc) 70 | target_sources(pointcloud2_filter_chain_nodelet PRIVATE src/PointCloud2FilterChainBase.cc) 71 | target_sources(pointcloud2_filter_chain PRIVATE src/PointCloud2FilterChainBase.cc) 72 | 73 | set(filters 74 | ChangeHeaderFilter 75 | ) 76 | 77 | foreach(filter IN LISTS filters) 78 | add_library(${filter} src/filters/${filter}.cc) 79 | add_dependencies(${filter} ${catkin_EXPORTED_TARGETS}) 80 | target_link_libraries(${filter} ${catkin_LIBRARIES}) 81 | install(TARGETS ${filter} 82 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 84 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 85 | ) 86 | install(FILES 87 | ${filter}.xml 88 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 89 | ) 90 | endforeach() 91 | 92 | install(DIRECTORY include/${PROJECT_NAME}/ 93 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 94 | FILES_MATCHING PATTERN "*.h" 95 | ) 96 | 97 | install(FILES 98 | nodelets.xml 99 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 100 | ) 101 | 102 | if (CATKIN_ENABLE_TESTING) 103 | find_package(roslint REQUIRED) 104 | 105 | # catkin_lint - checks validity of package.xml and CMakeLists.txt 106 | # ROS buildfarm calls this without any environment and with empty rosdep cache, 107 | # so we have problems reading the list of packages from env 108 | # see https://github.com/ros-infrastructure/ros_buildfarm/issues/923 109 | if(DEFINED ENV{ROS_HOME}) 110 | #catkin_lint: ignore_once env_var 111 | set(ROS_HOME "$ENV{ROS_HOME}") 112 | else() 113 | #catkin_lint: ignore_once env_var 114 | set(ROS_HOME "$ENV{HOME}/.ros") 115 | endif() 116 | #catkin_lint: ignore_once env_var 117 | if(DEFINED ENV{ROS_ROOT} AND EXISTS "${ROS_HOME}/rosdep/sources.cache") 118 | roslint_custom(catkin_lint "-W2" .) 119 | endif() 120 | 121 | # Roslint C++ - checks formatting and some other rules for C++ files 122 | 123 | file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp) 124 | file(GLOB_RECURSE ROSLINT_SRC src/*.cc src/filters/*.cc src/filters/*.hh examples/*.cpp) 125 | #file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) 126 | 127 | set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ 128 | -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\ 129 | -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces") 130 | roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC}) 131 | 132 | roslint_add_test() 133 | endif() 134 | -------------------------------------------------------------------------------- /ChangeHeaderFilter.xml: -------------------------------------------------------------------------------- 1 | 5 | 6 | 7 | 10 | 11 | Change the header of a message. 12 | 13 | 14 | 17 | 18 | Change the header of a message. 19 | 20 | 21 | 24 | 25 | Change the header of a message. 26 | 27 | 28 | 31 | 32 | Change the header of a message. 33 | 34 | 35 | 38 | 39 | Change the header of a message. 40 | 41 | 42 | 45 | 46 | Change the header of a message. 47 | 48 | 49 | 52 | 53 | Change the header of a message. 54 | 55 | 56 | 59 | 60 | Change the header of a message. 61 | 62 | 63 | 66 | 67 | Change the header of a message. 68 | 69 | 70 | 73 | 74 | Change the header of a message. 75 | 76 | 77 | 80 | 81 | Change the header of a message. 82 | 83 | 84 | 87 | 88 | Change the header of a message. 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Czech Technical University 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 5 | 6 | # sensor\_filters 7 | 8 | This package is a collection of nodes and nodelets that service a `filters::FilterChain` for message types from `sensor_msgs` package. 9 | See https://wiki.ros.org/filters to read more about the `filters` package. 10 | 11 | Attention: The *PCL filters* provided by package [pcl_ros](http://wiki.ros.org/pcl_ros/Tutorials/filters) are not "compatible" with this package. They are instances of *PCL filters*, but written as ROS nodelets, not as *ROS filters* implementing the `filters::FilterBase<>` interface required by this package. Running pcl_ros filter nodelets does not require any additional setup except a running nodelet manager. Package [point_cloud2_filters](https://github.com/torydebra/point_cloud2_filters) contains reimplementation of some of these pcl_ros *PCL filters* as *ROS filters*. 12 | 13 | The task of each node(let) in sensor_filters is very simple: load the filter chain, subscribe `~input` topic, and publish the filtered messages to `~output` topic. 14 | 15 | The size of message queues is configured via `~input_queue_size` and `~output_queue_size`, both defaulting to 10 messages. 16 | 17 | The namespaces from which the filters load are the following: 18 | 19 | | Message Type | Namespace | Nodelet Name | 20 | |--------------|-----------|--------------| 21 | | CompressedImage | `~image_filter_chain` | `sensor_filters/compressedimage_filter_chain` | 22 | | Image | `~image_filter_chain` | `sensor_filters/image_filter_chain` | 23 | | Imu | `~imu_filter_chain` | `sensor_filters/imu_filter_chain` | 24 | | Joy | `~joy_filter_chain` | `sensor_filters/joy_filter_chain` | 25 | | LaserScan | `~scan_filter_chain` | `sensor_filters/laserscan_filter_chain` | 26 | | MagneticField | `~magnetic_field_filter_chain` | `sensor_filters/magneticfield_filter_chain` | 27 | | MultiEchoLaserScan | `~scan_filter_chain` | `sensor_filters/multiecholaserscan_filter_chain` | 28 | | NavSatFix | `~nav_sat_fix_filter_chain` | `sensor_filters/navsatfix_filter_chain` | 29 | | PointCloud | `~cloud_filter_chain` | `sensor_filters/pointcloud_filter_chain` | 30 | | PointCloud2 | `~cloud_filter_chain` | `sensor_filters/pointcloud2_filter_chain` | 31 | | Range | `~range_filter_chain` | `sensor_filters/range_filter_chain` | 32 | | RelativeHumidity | `~humidity_filter_chain` | `sensor_filters/relativehumidity_filter_chain` | 33 | | Temperature | `~temperature_filter_chain` | `sensor_filters/temperature_filter_chain` | 34 | 35 | ## Provided filters 36 | 37 | This package also provides a few high-level filters. 38 | 39 | ### sensor_filters/ChangeHeader/$MESSAGE_TYPE 40 | 41 | This filter allows changing the contents of the header of sensor messages. You can use it for example to correct the `frame_id` of a sensor, or adjust the timestamp of its messages. 42 | 43 | #### Parameters 44 | 45 | - `frame_id_prefix` (string): Add this prefix to `header.frame_id` 46 | - `frame_id_suffix` (string): Add this suffix to `header.frame_id` 47 | - `frame_id` (string): Replace `header.frame_id`with this value (prefix and suffix are then ignored) 48 | - `seq` (uint): Replace `header.seq` with this value 49 | - `seq_relative` (uint): Add this value to `header.seq` (watch out for unsigned int underflow) 50 | - `stamp` (double): Set `header.stamp` to this value (double value is converted to `ros::Time`) 51 | - `stamp_relative` (double): Add this value to `header.stamp` (double value is converted to `ros::Duration`) 52 | 53 | ## Where to get other filters 54 | 55 | If you are looking for other implementations of sensor filters, you will probably be disappointed, as they are really scarce. I don't know why it is the case, because filter chains are the most efficient data processing path ROS offers, yet most people chose to write nodelets instead. 56 | 57 | There is, however, one noteworthy exception: 58 | 59 | ### `robot_body_filter` 60 | 61 | [robot_body_filter](http://wiki.ros.org/robot_body_filter) is a versatile tool for removing the body parts of a robot from laser scans and point clouds exactly according to the robot's URDF model. No more box approximations of your robots! Represent them exactly as they are! 62 | 63 | ### `point_cloud2_filters` 64 | 65 | [point_cloud2_filters](https://github.com/torydebra/point_cloud2_filters) provides an implementation similar to `pcl::PassThrough` or `pcl::CropBox` to cut `PointCloud2` messages. 66 | 67 | ### `laser_filters` compatibility 68 | 69 | The `LaserScan` filter chain node is compatible with the `scan_to_scan_node` from `laser_filters` package (it can load the same filters using the same config). 70 | It does not, however, use TF message filter, so each filter has to wait for the required TFs itself. 71 | 72 | ## Transport 73 | 74 | Filters of type `Image` and `PointCloud2` will automatically use the image_transport/point_cloud_transport to publish and subscribe to topics. 75 | 76 | ## Why ROS filters? 77 | 78 | The simple answer is - performance. ROS filters are the most efficient way to run a chain of processors on sensor data. Passing the message from one filter to another is as simple as doing a C++ object copy. There is even a proposal for an [in-place (zero-copy) implementation](https://github.com/ros/filters/pull/38). 79 | 80 | You can think about nodelets and their said performance. If used right and you run only non-modifying filters, you might actually get to true zero-copy filtering. But you have a dynamic allocation of at least one shared_ptr in your path, which might be costly and with unpredictable impacts on run time. 81 | 82 | If you use nodelets and publish/subscribe references to messages instead of shared_ptrs, passing the message from one nodelet to another in the same manager not only does a copy of the message, but it also undergoes a serialization and deserialization step, which is most probably costly. 83 | 84 | If you do not even use nodelets, but plain nodes, passing a message requires serializing it and sending via a local (or, even worse, remote TCP) socket to the other node. So you have copying, (de)serializing and sending via the network stack. 85 | 86 | In the future, I plan to add some measurements to give some solid ground to these propositions. 87 | 88 | ## Extensibility 89 | 90 | Both the nodes and nodelets offer common code that can be shared so that you can build extensions of the simple filter chain handlers provided by this package. 91 | 92 | See the [examples folder](https://github.com/ctu-vras/sensor_filters/blob/master/examples) for more verbose examples. 93 | 94 | ### Nodes 95 | 96 | ```C++ 97 | #include 98 | #include 99 | 100 | class MyClass : public sensor_filters::FilterChainNode 101 | { 102 | // custom class code 103 | }; 104 | ``` 105 | 106 | ### Nodelets 107 | 108 | ```C++ 109 | #include 110 | #include 111 | #include 112 | 113 | class MyNodelet : public sensor_filters::FilterChainNodelet 114 | { 115 | public: MyNodelet() : sensor_filters::FilterChainNodelet("my_filter_chain") {} 116 | 117 | protected: void onInit() override 118 | { 119 | sensor_filters::FilterChainNodelet::onInit(); 120 | // custom nodelet code 121 | } 122 | }; 123 | 124 | PLUGINLIB_EXPORT_CLASS(MyNodelet, nodelet::Nodelet) 125 | ``` 126 | 127 | ## Example 128 | 129 | `filter.yaml` 130 | 131 | ```YAML 132 | output_queue_size: 100 133 | scan_filter_chain: 134 | # Warning: using laser_filters like this might fail if they need to wait for some TFs; intensity filter does not. 135 | - name: intensity 136 | type: laser_filters/LaserScanIntensityFilter 137 | params: 138 | lower_threshold: 8000 139 | upper_threshold: 100000 140 | invert: false 141 | filter_override_range: true 142 | filter_override_intensity: false 143 | # This filter will subtract 25 ms from each scan timestamp, e.g. to account for transport delay. 144 | - name: delay 145 | type: sensor_filters/ChangeHeader/LaserScan 146 | params: 147 | stamp_relative: -0.025 148 | ``` 149 | 150 | `filter.launch` 151 | 152 | ```XML 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | ``` 161 | -------------------------------------------------------------------------------- /examples/example_custom_chain_node.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: Unlicense 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | /** 5 | * \file 6 | * \brief An example how you can write a custom node that runs the sensor filter chain and does some other tasks. 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | class MyClass : public sensor_filters::FilterChainNode 13 | { 14 | public: 15 | explicit MyClass(ros::NodeHandle nh) : 16 | sensor_filters::FilterChainNode("my_filter_chain", nh, nh) 17 | { 18 | // Constructor of your class 19 | } 20 | 21 | protected: 22 | bool filter(const sensor_msgs::PointCloud2& msgIn, sensor_msgs::PointCloud2& msgOut) override 23 | { 24 | // your custom logic that is run before filtering each message 25 | return sensor_filters::FilterChainNode::filter(msgIn, msgOut); 26 | } 27 | 28 | // custom class code 29 | }; 30 | 31 | int main(int argc, char** argv) 32 | { 33 | ros::init(argc, argv, "my_filter_chain"); 34 | ros::NodeHandle nh("~"); 35 | MyClass node(nh); 36 | ros::spin(); 37 | } 38 | -------------------------------------------------------------------------------- /examples/example_custom_chain_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: Unlicense 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | /** 5 | * \file 6 | * \brief An example how you can write a custom nodelet that runs the sensor filter chain and does some other tasks. 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | class MyNodelet : public sensor_filters::FilterChainNodelet 14 | { 15 | public: 16 | MyNodelet() : sensor_filters::FilterChainNodelet("my_filter_chain") {} 17 | 18 | protected: 19 | void onInit() override 20 | { 21 | sensor_filters::FilterChainNodelet::onInit(); 22 | // custom nodelet code 23 | } 24 | 25 | bool filter(const sensor_msgs::PointCloud2& msgIn, sensor_msgs::PointCloud2& msgOut) override 26 | { 27 | // your custom logic that is run before filtering each message 28 | return sensor_filters::FilterChainNodelet::filter(msgIn, msgOut); 29 | } 30 | }; 31 | 32 | PLUGINLIB_EXPORT_CLASS(MyNodelet, nodelet::Nodelet) 33 | -------------------------------------------------------------------------------- /examples/filter.launch: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /examples/filter.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Unlicense 2 | # SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | # An example config file for running a filter on LaserScan messages that does intensity filtering and adjusts scan 5 | # timestamps. 6 | 7 | output_queue_size: 100 8 | scan_filter_chain: 9 | # Warning: using laser_filters like this might fail if they need to wait for some TFs; intensity filter does not. 10 | - name: intensity 11 | type: laser_filters/LaserScanIntensityFilter 12 | params: 13 | lower_threshold: 8000 14 | upper_threshold: 100000 15 | invert: false 16 | filter_override_range: true 17 | filter_override_intensity: false 18 | # This filter will subtract 25 ms from each scan timestamp, e.g. to account for transport delay. 19 | - name: delay 20 | type: sensor_filters/ChangeHeader/LaserScan 21 | params: 22 | stamp_relative: -0.025 23 | -------------------------------------------------------------------------------- /examples/image_filter.launch: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /examples/image_filter.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Unlicense 2 | # SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | # An example launch file for running a filter on Image messages. This example showcases that the filter chain uses 5 | # image_transport to subscribe and publish. 6 | # Author: Martin Pecka 7 | 8 | image_transport: compressed 9 | output_queue_size: 100 10 | image_filter_chain: 11 | # This filter will subtract 25 ms from each scan timestamp, e.g. to account for transport delay. 12 | - name: delay 13 | type: sensor_filters/ChangeHeader/Image 14 | params: 15 | stamp_relative: -0.025 16 | -------------------------------------------------------------------------------- /examples/pointcloud2_filter.launch: -------------------------------------------------------------------------------- 1 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /examples/pointcloud2_filter.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Unlicense 2 | # SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | # An example launch file for running a filter on PointCloud2 messages. This example showcases that the filter chain uses 5 | # point_cloud_transport to subscribe and publish. 6 | 7 | point_cloud_transport: draco 8 | output_queue_size: 10 9 | cloud_filter_chain: 10 | # This filter will subtract 25 ms from each cloud timestamp, e.g. to account for transport delay. 11 | - name: delay 12 | type: sensor_filters/ChangeHeader/PointCloud2 13 | params: 14 | stamp_relative: -0.025 15 | -------------------------------------------------------------------------------- /include/sensor_filters/FilterChainBase.h: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #pragma once 5 | 6 | /** 7 | * \file 8 | * \brief Base for all sensor filter chains. 9 | */ 10 | 11 | #include 12 | 13 | #include 14 | 15 | #if ROS_VERSION_MINIMUM(1, 15, 0) 16 | #include 17 | #else 18 | 19 | #include 20 | 21 | #endif 22 | 23 | 24 | namespace sensor_filters 25 | { 26 | 27 | template 28 | class FilterChainBase 29 | { 30 | protected: 31 | ros::Subscriber inputSubscriber; 32 | ros::Publisher outputPublisher; 33 | ros::NodeHandle topicNodeHandle; 34 | size_t inputQueueSize{10u}; 35 | size_t outputQueueSize{10u}; 36 | bool useSharedPtrMessages{true}; 37 | 38 | filters::FilterChain filterChain; 39 | T msg; 40 | 41 | typedef ros::message_traits::DataType DataType; 42 | 43 | public: 44 | FilterChainBase() : 45 | filterChain(std::string(DataType::value()).replace(std::string(DataType::value()).find('/'), 1, "::")) 46 | { 47 | } 48 | 49 | virtual ~FilterChainBase() = default; 50 | 51 | protected: 52 | virtual void initFilters( 53 | const std::string& filterChainNamespace, ros::NodeHandle filterNodeHandle, ros::NodeHandle topicNodeHandle, 54 | const bool useSharedPtrMessages, const size_t inputQueueSize, const size_t outputQueueSize) 55 | { 56 | if (!this->filterChain.configure(filterChainNamespace, filterNodeHandle)) 57 | { 58 | ROS_ERROR_STREAM("Configuration of filter chain for " 59 | << DataType::value() << " is invalid, the chain will not be run."); 60 | throw std::runtime_error("Filter configuration error"); 61 | } 62 | 63 | ROS_INFO_STREAM("Configured filter chain of type " << DataType::value() << " from namespace " 64 | << filterNodeHandle.getNamespace() << "/" 65 | << filterChainNamespace); 66 | 67 | this->topicNodeHandle = topicNodeHandle; 68 | this->outputQueueSize = outputQueueSize; 69 | this->inputQueueSize = inputQueueSize; 70 | this->useSharedPtrMessages = useSharedPtrMessages; 71 | 72 | this->advertise(); 73 | this->subscribe(); 74 | } 75 | 76 | virtual void advertise() 77 | { 78 | this->outputPublisher = this->topicNodeHandle.template advertise("output", this->outputQueueSize); 79 | } 80 | 81 | virtual void subscribe() 82 | { 83 | if (this->useSharedPtrMessages) 84 | this->inputSubscriber = this->topicNodeHandle.subscribe( 85 | "input", this->inputQueueSize, &FilterChainBase::callbackShared, this); 86 | else 87 | this->inputSubscriber = this->topicNodeHandle.subscribe( 88 | "input", this->inputQueueSize, &FilterChainBase::callbackReference, this); 89 | } 90 | 91 | virtual void publishShared(const typename T::ConstPtr& msg) 92 | { 93 | this->outputPublisher.publish(msg); 94 | } 95 | 96 | virtual void publishReference(const T& msg) 97 | { 98 | this->outputPublisher.publish(msg); 99 | } 100 | 101 | virtual void callbackShared(const typename T::ConstPtr& msgIn) 102 | { 103 | typename T::Ptr msgOut(new T); 104 | if (this->filter(*msgIn, *msgOut)) 105 | this->publishShared(msgOut); 106 | } 107 | 108 | virtual void callbackReference(const T& msgIn) 109 | { 110 | if (this->filter(msgIn, this->msg)) 111 | this->publishReference(this->msg); 112 | } 113 | 114 | virtual bool filter(const T& msgIn, T& msgOut) 115 | { 116 | ros::WallTime start = ros::WallTime::now(); 117 | if (!this->filterChain.update(msgIn, msgOut)) 118 | { 119 | ROS_ERROR_THROTTLE(1, "Filtering data from time %i.%i failed.", 120 | msgIn.header.stamp.sec, msgIn.header.stamp.nsec); 121 | return false; 122 | } 123 | ROS_DEBUG_STREAM("Filtering took " << (ros::WallTime::now() - start).toSec() << " s."); 124 | return true; 125 | } 126 | }; 127 | 128 | } 129 | -------------------------------------------------------------------------------- /include/sensor_filters/FilterChainNode.h: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #pragma once 5 | 6 | /** 7 | * \file 8 | * \brief Base for a sensor filter chain node. 9 | */ 10 | 11 | #include 12 | 13 | #include 14 | 15 | namespace sensor_filters 16 | { 17 | 18 | template > 19 | class FilterChainNode : public Base 20 | { 21 | public: 22 | explicit FilterChainNode(const std::string& filterChainNamespace, ros::NodeHandle filterNodeHandle, 23 | ros::NodeHandle topicNodeHandle) : Base() 24 | { 25 | this->initFilters( 26 | filterChainNamespace, filterNodeHandle, topicNodeHandle, false, 27 | filterNodeHandle.param("input_queue_size", 10), 28 | filterNodeHandle.param("output_queue_size", 10)); 29 | } 30 | }; 31 | 32 | 33 | template > 34 | void spinFilterChain(const std::string& filterChainNamespace, int argc, char** argv) 35 | { 36 | ros::init(argc, argv, "filter_chain"); 37 | ros::NodeHandle nh("~"); 38 | const FilterChainNode node(filterChainNamespace, nh, nh); 39 | ros::spin(); 40 | } 41 | 42 | } 43 | -------------------------------------------------------------------------------- /include/sensor_filters/FilterChainNodelet.h: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #pragma once 5 | 6 | /** 7 | * \file 8 | * \brief Base for all sensor filter chain nodelets. 9 | */ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace sensor_filters 19 | { 20 | 21 | template> 22 | class FilterChainNodelet : public nodelet::Nodelet, public Base 23 | { 24 | protected: 25 | std::string filterChainNamespace; 26 | 27 | public: 28 | explicit FilterChainNodelet(std::string filterChainNamespace) : 29 | nodelet::Nodelet(), Base(), filterChainNamespace(std::move(filterChainNamespace)) 30 | { 31 | } 32 | 33 | ~FilterChainNodelet() override = default; 34 | 35 | protected: 36 | void onInit() override 37 | { 38 | this->initFilters( 39 | this->filterChainNamespace, this->getPrivateNodeHandle(), this->getPrivateNodeHandle(), true, 40 | this->getPrivateNodeHandle().param("input_queue_size", 10), 41 | this->getPrivateNodeHandle().param("output_queue_size", 10)); 42 | } 43 | }; 44 | 45 | } 46 | 47 | #define DECLARE_SENSOR_FILTER_BASE(TYPE, BASE, CONFIG) \ 48 | namespace sensor_filters \ 49 | { \ 50 | class TYPE ## FilterChainNodelet : public FilterChainNodelet \ 51 | { \ 52 | public: TYPE ## FilterChainNodelet() : FilterChainNodelet(CONFIG "_filter_chain") {} \ 53 | }; \ 54 | }\ 55 | PLUGINLIB_EXPORT_CLASS(sensor_filters::TYPE ## FilterChainNodelet, nodelet::Nodelet) 56 | 57 | #define DECLARE_SENSOR_FILTER(TYPE, CONFIG) \ 58 | DECLARE_SENSOR_FILTER_BASE(TYPE, sensor_filters::FilterChainBase, CONFIG) 59 | -------------------------------------------------------------------------------- /include/sensor_filters/ImageFilterChainBase.h: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #pragma once 5 | 6 | /** 7 | * \file 8 | * \brief Specialized base for Image filter chains that uses image_transport. 9 | */ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | namespace sensor_filters 22 | { 23 | class ImageFilterChainBase : public FilterChainBase 24 | { 25 | public: 26 | ImageFilterChainBase() : FilterChainBase() 27 | { 28 | } 29 | 30 | protected: 31 | std::unique_ptr it; 32 | image_transport::Publisher itPublisher; 33 | image_transport::Subscriber itSubscriber; 34 | 35 | void initFilters(const std::string& filterChainNamespace, ros::NodeHandle filterNodeHandle, 36 | ros::NodeHandle topicNodeHandle, bool useSharedPtrMessages, size_t inputQueueSize, 37 | size_t outputQueueSize) override; 38 | 39 | void advertise() override; 40 | 41 | void subscribe() override; 42 | 43 | void publishShared(const sensor_msgs::ImageConstPtr& msg) override; 44 | }; 45 | 46 | } 47 | -------------------------------------------------------------------------------- /include/sensor_filters/PointCloud2FilterChainBase.h: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #pragma once 5 | 6 | /** 7 | * \file 8 | * \brief Specialized base for PointCloud2 filter chains that uses point_cloud_transport. 9 | */ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | namespace sensor_filters 22 | { 23 | class PointCloud2FilterChainBase : public FilterChainBase 24 | { 25 | public: 26 | PointCloud2FilterChainBase() : FilterChainBase() 27 | { 28 | } 29 | 30 | protected: 31 | std::unique_ptr pct; 32 | point_cloud_transport::Publisher pctPublisher; 33 | point_cloud_transport::Subscriber pctSubscriber; 34 | 35 | void initFilters(const std::string& filterChainNamespace, ros::NodeHandle filterNodeHandle, 36 | ros::NodeHandle topicNodeHandle, bool useSharedPtrMessages, size_t inputQueueSize, 37 | size_t outputQueueSize) override; 38 | 39 | void advertise() override; 40 | 41 | void subscribe() override; 42 | 43 | void publishShared(const sensor_msgs::PointCloud2ConstPtr& msg) override; 44 | }; 45 | 46 | } 47 | -------------------------------------------------------------------------------- /nodelets.xml: -------------------------------------------------------------------------------- 1 | 5 | 6 | 7 | 8 | Nodelet running a CompressedImage filter chain. 9 | 10 | 11 | 12 | 13 | 14 | 15 | Nodelet running a Image filter chain. 16 | 17 | 18 | 19 | 20 | 21 | 22 | Nodelet running a Imu filter chain. 23 | 24 | 25 | 26 | 27 | 28 | 29 | Nodelet running a Joy filter chain. 30 | 31 | 32 | 33 | 34 | 35 | 36 | Nodelet running a LaserScan filter chain. 37 | 38 | 39 | 40 | 41 | 42 | 43 | Nodelet running a MagneticField filter chain. 44 | 45 | 46 | 47 | 48 | 49 | 50 | Nodelet running a MultiEchoLaserScan filter chain. 51 | 52 | 53 | 54 | 55 | 56 | 57 | Nodelet running a NavSatFix filter chain. 58 | 59 | 60 | 61 | 62 | 63 | 64 | Nodelet running a PointCloud filter chain. 65 | 66 | 67 | 68 | 69 | 70 | 71 | Nodelet running a PointCloud2 filter chain. 72 | 73 | 74 | 75 | 76 | 77 | 78 | Nodelet running a Range filter chain. 79 | 80 | 81 | 82 | 83 | 84 | 85 | Nodelet running a RelativeHumidity filter chain. 86 | 87 | 88 | 89 | 90 | 91 | 92 | Nodelet running a Temperature filter chain. 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | sensor_filters 8 | 1.1.1 9 | Simple sensor filter chain nodes and nodelets 10 | 11 | Martin Pecka 12 | Martin Pecka 13 | 14 | BSD 15 | 16 | https://github.com/ctu-vras/sensor_filters 17 | 18 | catkin 19 | 20 | filters 21 | nodelet 22 | roscpp 23 | sensor_msgs 24 | 25 | image_transport 26 | point_cloud_transport 27 | 28 | image_transport 29 | point_cloud_transport 30 | 31 | python-catkin-lint 32 | python3-catkin-lint 33 | roslint 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/CompressedImageFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("image_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/CompressedImageFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(CompressedImage, "image") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/ImageFilterChainBase.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace sensor_filters 15 | { 16 | 17 | void ImageFilterChainBase::initFilters(const std::string& filterChainNamespace, ros::NodeHandle filterNodeHandle, 18 | ros::NodeHandle topicNodeHandle, const bool useSharedPtrMessages, 19 | const size_t inputQueueSize, const size_t outputQueueSize) 20 | { 21 | this->it = std::make_unique(topicNodeHandle); 22 | FilterChainBase::initFilters(filterChainNamespace, filterNodeHandle, topicNodeHandle, useSharedPtrMessages, 23 | inputQueueSize, outputQueueSize); 24 | } 25 | 26 | void ImageFilterChainBase::advertise() 27 | { 28 | const auto resolvedOutput = this->topicNodeHandle.resolveName("output"); 29 | this->itPublisher = this->it->advertise(resolvedOutput, this->outputQueueSize); 30 | } 31 | 32 | void ImageFilterChainBase::subscribe() 33 | { 34 | const auto resolvedInput = this->topicNodeHandle.resolveName("input"); 35 | this->itSubscriber = this->it->subscribe( 36 | resolvedInput, this->inputQueueSize, &ImageFilterChainBase::callbackShared, this); 37 | } 38 | 39 | void ImageFilterChainBase::publishShared(const sensor_msgs::ImageConstPtr& msg) 40 | { 41 | this->itPublisher.publish(msg); 42 | } 43 | 44 | } 45 | -------------------------------------------------------------------------------- /src/ImageFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) 10 | { 11 | sensor_filters::spinFilterChain( 12 | "image_filter_chain", argc, argv); 13 | } 14 | -------------------------------------------------------------------------------- /src/ImageFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | DECLARE_SENSOR_FILTER_BASE(Image, ImageFilterChainBase, "image") // NOLINT(cert-err58-cpp) 11 | -------------------------------------------------------------------------------- /src/ImuFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("imu_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/ImuFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(Imu, "imu") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/JoyFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("joy_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/JoyFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(Joy, "joy") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/LaserScanFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("scan_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/LaserScanFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(LaserScan, "scan") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/MagneticFieldFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("magnetic_field_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/MagneticFieldFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(MagneticField, "magnetic_field") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/MultiEchoLaserScanFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("scan_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/MultiEchoLaserScanFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(MultiEchoLaserScan, "scan") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/NavSatFixFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("nav_sat_fix_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/NavSatFixFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(NavSatFix, "nav_sat_fix") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/PointCloud2FilterChainBase.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace sensor_filters 15 | { 16 | 17 | void PointCloud2FilterChainBase::initFilters(const std::string& filterChainNamespace, ros::NodeHandle filterNodeHandle, 18 | ros::NodeHandle topicNodeHandle, const bool useSharedPtrMessages, 19 | const size_t inputQueueSize, const size_t outputQueueSize) 20 | { 21 | this->pct = std::make_unique(topicNodeHandle); 22 | FilterChainBase::initFilters(filterChainNamespace, filterNodeHandle, topicNodeHandle, useSharedPtrMessages, 23 | inputQueueSize, outputQueueSize); 24 | } 25 | 26 | void PointCloud2FilterChainBase::advertise() 27 | { 28 | const auto resolvedOutput = this->topicNodeHandle.resolveName("output"); 29 | this->pctPublisher = this->pct->advertise(resolvedOutput, this->outputQueueSize); 30 | } 31 | 32 | void PointCloud2FilterChainBase::subscribe() 33 | { 34 | const auto resolvedInput = this->topicNodeHandle.resolveName("input"); 35 | this->pctSubscriber = this->pct->subscribe( 36 | resolvedInput, this->inputQueueSize, &PointCloud2FilterChainBase::callbackShared, this); 37 | } 38 | 39 | void PointCloud2FilterChainBase::publishShared(const sensor_msgs::PointCloud2ConstPtr& msg) 40 | { 41 | this->pctPublisher.publish(msg); 42 | } 43 | 44 | } 45 | -------------------------------------------------------------------------------- /src/PointCloud2FilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) 10 | { 11 | sensor_filters::spinFilterChain( 12 | "cloud_filter_chain", argc, argv); 13 | } 14 | -------------------------------------------------------------------------------- /src/PointCloud2FilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | DECLARE_SENSOR_FILTER_BASE(PointCloud2, PointCloud2FilterChainBase, "cloud") // NOLINT(cert-err58-cpp) 11 | -------------------------------------------------------------------------------- /src/PointCloudFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("cloud_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/PointCloudFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(PointCloud, "cloud") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/RangeFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("range_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/RangeFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(Range, "range") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/RelativeHumidityFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("humidity_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/RelativeHumidityFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(RelativeHumidity, "humidity") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/TemperatureFilterChainNode.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | sensor_filters::spinFilterChain("temperature_filter_chain", argc, argv); 11 | } 12 | -------------------------------------------------------------------------------- /src/TemperatureFilterChainNodelet.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | DECLARE_SENSOR_FILTER(Temperature, "temperature") // NOLINT(cert-err58-cpp) 10 | -------------------------------------------------------------------------------- /src/filters/ChangeHeaderFilter.cc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #if ROS_VERSION_MINIMUM(1, 15, 0) 11 | #include 12 | #else 13 | #include 14 | #endif 15 | 16 | #include "include_all_msgs.hh" 17 | 18 | namespace sensor_filters 19 | { 20 | 21 | template 22 | class ChangeHeader : public filters::FilterBase 23 | { 24 | protected: 25 | bool configure() override 26 | { 27 | { 28 | std::string frameIdParam; 29 | if (this->getParam("frame_id_prefix", frameIdParam)) 30 | this->newFrameIdPrefix = frameIdParam; 31 | 32 | if (this->getParam("frame_id_suffix", frameIdParam)) 33 | this->newFrameIdSuffix = frameIdParam; 34 | 35 | if (this->getParam("frame_id", frameIdParam)) 36 | this->newFrameId = frameIdParam; 37 | } 38 | 39 | { 40 | uint32_t seqParam; 41 | if (this->getParam("seq_relative", seqParam)) 42 | this->newSeqRel = seqParam; 43 | 44 | if (this->getParam("seq", seqParam)) 45 | this->newSeqAbs = seqParam; 46 | } 47 | 48 | { 49 | double stampParam; 50 | if (this->getParam("stamp_relative", stampParam)) 51 | this->newStampRel = ros::Duration(stampParam); 52 | 53 | if (this->getParam("stamp", stampParam)) 54 | this->newStampAbs = ros::Time(stampParam); 55 | } 56 | 57 | return true; 58 | } 59 | 60 | public: 61 | bool update(const T& data_in, T& data_out) override 62 | { 63 | data_out = data_in; 64 | 65 | if (this->newFrameIdPrefix.has_value()) 66 | data_out.header.frame_id = this->newFrameIdPrefix.value() + data_out.header.frame_id; 67 | 68 | if (this->newFrameIdSuffix.has_value()) 69 | data_out.header.frame_id += this->newFrameIdSuffix.value(); 70 | 71 | if (this->newFrameId.has_value()) 72 | data_out.header.frame_id = this->newFrameId.value(); 73 | 74 | if (this->newSeqRel.has_value()) 75 | data_out.header.seq += this->newSeqRel.value(); 76 | 77 | if (this->newSeqAbs.has_value()) 78 | data_out.header.seq = this->newSeqAbs.value(); 79 | 80 | if (this->newStampRel.has_value()) 81 | data_out.header.stamp += this->newStampRel.value(); 82 | 83 | if (this->newStampAbs.has_value()) 84 | data_out.header.stamp = this->newStampAbs.value(); 85 | 86 | return true; 87 | } 88 | 89 | private: 90 | std::optional newFrameId; 91 | std::optional newFrameIdPrefix; 92 | std::optional newFrameIdSuffix; 93 | 94 | std::optional newSeqAbs; 95 | std::optional newSeqRel; 96 | 97 | std::optional newStampAbs; 98 | std::optional newStampRel; 99 | }; 100 | 101 | } 102 | 103 | REGISTER_ALL_MSG_FILTER(sensor_filters::ChangeHeader) 104 | -------------------------------------------------------------------------------- /src/filters/include_all_msgs.hh: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: BSD-3-Clause 2 | // SPDX-FileCopyrightText: Czech Technical University in Prague 3 | 4 | /** 5 | * \file 6 | * \brief A macro that registers a generic filter with all sensor message types. 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #define REGISTER_ALL_MSG_FILTER(filter) \ 27 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 28 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 29 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 30 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 31 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 32 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 33 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 34 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 35 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 36 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 37 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 38 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) \ 39 | PLUGINLIB_EXPORT_CLASS(filter, filters::FilterBase) 40 | --------------------------------------------------------------------------------