添加手写数字识别;颜色识别;键盘控制多机;雷达跟踪;二维码跟踪

.gitignore

@@ -2,3 +2,4 @@ build/
 devel/
 *.pyc
 *.so
+.vscode/

src/aruco_ros

@@ -0,0 +1 @@
+Subproject commit 959fe5fcdc0148e358a180e0366b79bde08c74a8

src/laser_filters/.travis.yml

@@ -0,0 +1,21 @@
+# This config file for Travis CI utilizes ros-industrial/industrial_ci package.
+# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst
+sudo: required 
+dist: xenial
+language: generic 
+compiler:
+  - gcc
+notifications:
+  email:
+    on_success: always
+    on_failure: always
+env:
+  matrix:
+    - USE_DEB=true  ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
+    - USE_DEB=true  ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu
+    - USE_DEB=true  ROS_DISTRO="lunar"   ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
+    - USE_DEB=true  ROS_DISTRO="lunar"   ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu
+install:
+  - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
+script: 
+  - source .ci_config/travis.sh

src/laser_filters/CHANGELOG.rst

@@ -0,0 +1,325 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package laser_filters
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.8.11 (2020-06-03)
+-------------------
+* Merge pull request `#97 <https://github.com/ros-perception/laser_filters/issues/97>`_ from eurogroep/feat/speckle-filter-for-noise-removal
+* Merge pull request `#96 <https://github.com/ros-perception/laser_filters/issues/96>`_ from eurogroep/feat/intensity-filter-dynamic-reconfigure-and-optionally-override-intensity-values
+  feat(IntensityFilter): Dynamic reconfigure and optionally override intensity
+* Merge pull request `#3 <https://github.com/ros-perception/laser_filters/issues/3>`_ from nlimpert/nlimpert/speckle-filter-radius-outlier-merge
+  Merge distance based speckle filter with RadiusOutlier removal
+* Contributors: Jonathan Binney, Nicolas Limpert, Rein Appeldoorn
+
+1.8.10 (2020-04-07)
+-------------------
+* radius_outlier_filter: new filter for radius based outlier removal
+  Add a new filter to remove measurements that do not have a number of
+  neighbors within a certain range.
+* Contributors: Jonathan Binney, Nicolas Limpert
+
+1.8.9 (2020-04-05)
+------------------
+* Bump CMake version to avoid CMP0048 warning
+* Polygon filter
+* Add dynamic reconfigure for scan shadows filter
+* Parameter to remove shadow start point in scan shadows filter
+* Contributors: Jonathan Binney, Rein Appeldoorn, Yannick_de_Hoop, ahcorde
+
+1.8.8 (2019-11-07)
+------------------
+* Merge pull request `#83 <https://github.com/ros-perception/laser_filters/issues/83>`_ from remco-r/indigo-devel
+  [fix] when high fidelity true added laser_max_range\_ to projection
+* [fix] when high fidelity true added laser_max_range\_ to projection
+* Merge pull request `#79 <https://github.com/ros-perception/laser_filters/issues/79>`_ from Jailander/indigo-devel
+  Adding invert filter parameter to BOX filter
+* Merge pull request `#80 <https://github.com/ros-perception/laser_filters/issues/80>`_ from k-okada/indigo-devel
+  Add scan blob filters
+* add scan blob filters
+* Merge pull request `#72 <https://github.com/ros-perception/laser_filters/issues/72>`_ from ms-iot/windows_port_tests_fix
+  [Windows][indigo] Use ${GTEST_LIBRARIES} for more portable gtest library linkage.
+* Adding invert filter parameter to BOX filter
+* Remove extra changes.
+* windows bring up
+* Contributors: Jonathan Binney, Kei Okada, Remco, Sean Yen, jailander
+
+1.8.7 (2019-06-13)
+------------------
+* Merge pull request `#77 <https://github.com/ros-perception/laser_filters/issues/77>`_ from bionade24/indigo-devel
+  Removed boost signals from CMakeLists.txt
+* Removed boost signals from CMakeLists.txt
+  With boost=>1.69 there `signals` isn't available anymore. As it's not necessary, it should be removed to be compatible to all boost versions.
+* Merge pull request `#76 <https://github.com/ros-perception/laser_filters/issues/76>`_ from peci1/fix_travis
+  Fix Travis and move on to Kinetic and Lunar.
+* Fix Travis and move on to Kinetic and Lunar.
+* Merge pull request `#73 <https://github.com/ros-perception/laser_filters/issues/73>`_ from peci1/patch-1
+  Added error message when the filter chain failed.
+* Added error message when the filter chain failed.
+* Merge pull request `#62 <https://github.com/ros-perception/laser_filters/issues/62>`_ from at-wat/optimize-shadows-filter
+  Reduce computation cost of ScanShadowsFilter
+* Merge pull request `#63 <https://github.com/ros-perception/laser_filters/issues/63>`_ from procopiostein/indigo-devel
+  set values for variables that could be used uninitialized
+* Add some comments to ScanShadowDetector
+* set values for variables that could be used uninitialized
+* Reduce computation cost of ScanShadowsFilter
+  ScanShadowsFilter required a lot of CPU power mainly due to atan2.
+  This commit reduces computation cost of the filter.
+  * Remove atan2 and directly compare tangent values
+  * Add a test to check geometric calculation
+* Apply ROS recommended indent style to ScanShadowsFilter
+* Contributors: Atsushi Watanabe, Jonathan Binney, Martin Pecka, Oskar Roesler, Procópio Stein
+
+1.8.6 (2018-04-11)
+------------------
+* Updated deprecated pluginlib macros to avoid warning messages
+* Contributors: Jonathan Binney, Nick Lamprianidis
+
+1.8.5 (2017-09-06)
+------------------
+* rename parameter to be more descriptive
+* change range_filter to infinity for it to work with obstacle_layer
+  if you use the ´inf_is_valid´ parameter raytracing is still possible for
+  scans out of the window.
+  Usefull for laserscanners that may deliver ranges > range_max ... or
+* Fix a small typo in one of the test cases.
+* Add LaserScanMaskFilter.
+  This commit adds LaserScanMaskFilter that removes points on directions defined in a mask, defined as a parameter, from a laser scan.
+  It can be used to remove unreliable points caused by hardware related problems for example scratches on an optical window of the sensor.
+* Contributors: Atsushi Watanabe, Hunter L. Allen, Jannik Abbenseth, Jonathan Binney
+
+1.8.4 (2017-04-07)
+------------------
+* Specify packages names for filters in tests
+* Use std:: namespace for c++11 compat.
+* Contributors: Jon Binney, Jonathan Binney, Mike Purvis
+
+1.8.3 (2016-05-20)
+------------------
+* Replaced the invalid value of scans for the footprint_filter by NaN
+* Contributors: Alain Minier
+
+1.8.2 (2016-04-06)
+------------------
+* Remove unneeded eigen and cmake_modules
+  Nothing was actually compiling against eigen.
+* Contributors: Jonathan Binney
+
+1.8.1 (2016-03-26)
+-----------
+* Remove deprecated warning from footprint filter
+* catkin_make requires cmake_modules in run_depends
+* Restore cmake_modules build dependency
+* Update package.xml
+* Update maintainer email address
+* Add Travis CI config
+* Update scan_to_scan_filter_chain.cpp
+* only publish result if filter succeeded
+* Contributors: Isaac I.Y. Saito, Jon Binney, Jonathan Binney, Kei Okada, Naveed Usmani, asimay
+
+1.7.4 (2015-12-17)
+------------------
+* [intensity_filter.h] fix: check if cur_bucket value is out of index of histogram array
+* [intensity_filter.h] refactor codes; clearify by using boolean to enable/disable displaying histogram
+* scan_to_scan_filter_chain: make tf filter tolerance customizable
+  0.03 is completely arbitrary and too small in my case.
+* scan2scan filter: only publish result if filter succeeded
+* added cartesian box filter
+* add check inf or nan of input laser_scan intensities
+* scan_to_scan_filter_chain: Only subscribe to /tf if requested by parameter
+* Contributors: Furushchev, Jonathan Binney, Kevin Hallenbeck, Sebastian Pütz, Vincent Rabaud, Yuto Inagaki, v4hn
+
+1.7.3 (2014-09-06)
+------------------
+* Added new filter LaserScanAngularRemovalFilterInPlace to remove sections of a LaserScan
+* Contributors: Kevin Hallenbeck, Vincent Rabaud
+
+1.7.2 (2014-06-24)
+------------------
+* Merge pull request `#19 <https://github.com/ros-perception/laser_filters/issues/19>`_ from v4hn/no-DEPENDS-dependency
+  remove superfluous DEPENDS
+* remove superfluous DEPENDS
+  There never was a DEPENDS flag in add_dependencies...
+* Contributors: Vincent Rabaud, v4hn
+
+1.7.1 (2014-06-06)
+------------------
+* Tests expect NaN for invalid ranges
+* Modify intensity, scan shadow, and range filters to set invalid values to NaN
+* Contributors: Allison Tse, Jonathan Binney
+
+1.6.14 (2014-03-04)
+-------------------
+* fix compilation on some platforms
+* Contributors: Vincent Rabaud
+
+1.6.13 (2014-03-02)
+-------------------
+* separate tests
+* remove PCL dependency
+* Don't check the intensities
+  The intensities are not used in the range filter.
+  Furthermore, some laser don't have intensities ---e.g hokuyo URG-04LX-UG01---, so this fails for them.
+* Contributors: Enrique Fernández Perdomo, Vincent Rabaud
+
+1.6.12 (2013-12-24)
+-------------------
+* "1.6.12"
+* Merge pull request `#13 <https://github.com/ros-perception/laser_filters/issues/13>`_ from v4hn/less_startup_noise
+  footprint_filter: print less tf warnings
+* footprint_filter: print less tf warnings
+  On startup this filter produces about two pages of console output
+  (ROS_ERRORs) on ExtrapolationExceptions because the listener is
+  not setup yet. This commit reduces this to throttled info messages
+  until the transform works for the first time.
+* compile rostests with add_executable, not catkin_add_gtest
+* Contributors: Jon Binney, Vincent Rabaud, v4hn
+
+1.6.11 (2013-07-19)
+-------------------
+* Merge pull request `#12 <https://github.com/ros-perception/laser_filters/issues/12>`_ from ros-perception/fix_angles_dep
+  missing dependency break isolated build without --install
+* add missing dependency on the angles package
+* Contributors: William Woodall
+
+1.6.10 (2013-06-27 16:11)
+-------------------------
+* install scan_to_scan filter
+* Contributors: Jon Binney
+
+1.6.9 (2013-06-27 09:36)
+------------------------
+* Merge pull request `#11 <https://github.com/ros-perception/laser_filters/issues/11>`_ from piyushk/patch-1
+  Fixed typo in exported library names
+* Fixed typo in exported library names
+  It's a pretty minor error, but unfortunately breaks the system release due to nonexistent lib_point_cloud_filters.so
+* Contributors: Piyush Khandelwal, Vincent Rabaud
+
+1.6.8 (2013-05-30)
+------------------
+* Merge pull request `#7 <https://github.com/ros-perception/laser_filters/issues/7>`_ from ros-perception/scan-scan-filter-chain
+  Restored scan_to_scan_filter_chain executable lost in the catkinization.
+* Restored scan_to_scan_filter_chain executable lost in the catkinization.
+* Contributors: Dave Hershberger, jonbinney
+
+1.6.7 (2013-05-24)
+------------------
+* bump version for bugfix
+* Merge pull request `#6 <https://github.com/ros-perception/laser_filters/issues/6>`_ from jonbinney/install_include
+  added install rule for headers in cmakelists
+* added install rule for headers in cmakelists
+* Contributors: Jon Binney, jonbinney
+
+1.6.6 (2013-05-23)
+------------------
+* bumped version for hydro release
+* Merge pull request `#5 <https://github.com/ros-perception/laser_filters/issues/5>`_ from jonbinney/build_fixes
+  fixed rostests
+* fixed rostests
+* Merge pull request `#4 <https://github.com/ros-perception/laser_filters/issues/4>`_ from jonbinney/catkinized
+  Catkinized
+* fixes to cmakelists
+* deleted unneeded cmake file
+* catkinized laser_filters
+* Contributors: Jon Binney, jonbinney
+
+1.5.7 (2013-07-11 15:22)
+------------------------
+* restore dependecy on laser_geometry
+* Contributors: Jon Binney
+
+1.5.6 (2013-07-11 15:06)
+------------------------
+* fix crash with negative values
+* Merge pull request `#3 <https://github.com/ros-perception/laser_filters/issues/3>`_ from YoheiKakiuchi/groovy-devel
+  add range_filter to laser_scan_filters.cpp
+* comment out laser_geometry (it was needed to use this package in fuerte)
+* add range_filter to laser_scan_filters.cpp
+* Contributors: Vincent Rabaud, YoheiKakiuchi
+
+1.5.5 (2012-10-12 11:16)
+------------------------
+* releasing 1.5.5
+* added missing dependency on laser_geometry
+* Contributors: Dave Hershberger
+
+1.5.4 (2012-10-12 10:38)
+------------------------
+* added .gitignore
+* created stack.xml and added stuff for unary-stack-ification
+* revert to the angles package
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@40134 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* fix the non-inclusion of PCL
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@40128 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* more angles fixing
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@40123 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Changing the name of the incident angle correction parameter to make some amount of sense
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@38975 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Fixing the scan to cloud filter chain to actually work properly with PointCloud2 messages
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@38974 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* added param for hack
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@38655 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* add missing boost links, needed for catkin, but backward compatible
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@38615 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* - first try at converting the PointCloud to PointCloud2
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@38479 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* use the new bullet and eigen conventions
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@38342 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Removing deprecation warnings
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35256 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Killing deprecated preservative param
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35241 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Added Ubuntu platform tags to manifest
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@29657 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Removing deprecated usage of ~ for `#3771 <https://github.com/ros-perception/laser_filters/issues/3771>`_
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@27729 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* adding test for array filter
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26944 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* adding shadow filter test
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26874 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* adding test for interp filter
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26872 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* adding simple tests
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26866 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* adding tests but checking in with CMake comeented out for now
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26803 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Updating stack/manifest.xml files
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26801 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Adding a angular bounds filter that allows scans to be truncated to be within a user-specified range.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26736 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Added link against boost::system, to fix build on OS X
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@25628 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Removing old/unused/broken code from scan_to_cloud_filter_chain.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24700 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Checking in the node diagrams.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24687 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Making scan_to_cloud_filter_chain robust to a likely user migration error.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24660 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Making the scan_to_scan_filter_chain use scan_filter_chain.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24659 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Making scan_to_cloud_filter_chain adhere to new API from http://www.ros.org/wiki/laser_filters/Reviews/2009-9-28_API_Review
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24629 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Making laser_filters adhere to new API from http://www.ros.org/wiki/laser_filters/Reviews/2009-9-28_API_Review
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24627 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* A little more laser_filter code cleanup.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24485 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Cleaning up generic_laser_filter_node code since it is used as part of the laser_filters tutorial.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24482 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Leaving point_cloud_footprint_filter_example in laser_pipeline as well for now.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24415 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Adding back int the footprint_filter_examples despite deprecation to avoid breaking people using deprecated plugins.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24389 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Fixing laser_filter to use tf::MessageFilter instead of tf::MessageNotifier and deprecating the footprint filters.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24388 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Removing invalid linking from laser_filters.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24353 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Deprecating preservative parameter.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24324 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* capitalization in filter description
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24312 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Convert to NodeHandle
+  git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24160 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Adding a filter to interpolate between laser readings to generate range readings for scans that return errors
+  git-svn-id: https://code.ros.org/svn/ros-pkg/pkg/trunk/stacks/laser_pipeline@23875 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Merging in remaining missing contents for laser_piple that svn ignored on the first merge.
+  git-svn-id: https://code.ros.org/svn/ros-pkg/pkg/trunk/stacks/laser_pipeline@23510 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
+* Contributors: Brian Gerkey, Dave Hershberger, Eitan Marder-Eppstein, Eric Berger, Jeremy Leibs, Josh Faust, Kaijen Hsaio, Melonee Wise, Vincent Rabaud

src/laser_filters/CMakeLists.txt

@@ -0,0 +1,98 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(laser_filters)
+
+set(CMAKE_CXX_STANDARD 11)
+
+##############################################################################
+# Find dependencies
+##############################################################################
+
+set(THIS_PACKAGE_ROS_DEPS sensor_msgs roscpp tf filters message_filters
+  laser_geometry pluginlib angles dynamic_reconfigure)
+
+find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS})
+find_package(Boost REQUIRED COMPONENTS system)
+include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+
+##############################################################################
+# Define package
+##############################################################################
+
+# dynamic reconfigure
+generate_dynamic_reconfigure_options(
+  cfg/IntensityFilter.cfg
+  cfg/PolygonFilter.cfg
+  cfg/ScanShadowsFilter.cfg
+  cfg/SpeckleFilter.cfg
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES pointcloud_filters laser_scan_filters
+  CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS}
+  DEPENDS
+  )
+
+##############################################################################
+# Build
+##############################################################################
+
+add_library(pointcloud_filters src/pointcloud_filters.cpp)
+target_link_libraries(pointcloud_filters ${catkin_LIBRARIES})
+
+add_library(laser_scan_filters
+  src/laser_scan_filters.cpp
+  src/median_filter.cpp
+  src/array_filter.cpp
+  src/box_filter.cpp
+  src/polygon_filter.cpp
+  src/speckle_filter.cpp
+  src/intensity_filter.cpp
+)
+target_link_libraries(laser_scan_filters ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+add_executable(scan_to_cloud_filter_chain src/scan_to_cloud_filter_chain.cpp)
+target_link_libraries(scan_to_cloud_filter_chain ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+add_executable(scan_to_scan_filter_chain src/scan_to_scan_filter_chain.cpp)
+target_link_libraries(scan_to_scan_filter_chain ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+add_executable(generic_laser_filter_node src/generic_laser_filter_node.cpp)
+target_link_libraries(generic_laser_filter_node ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+add_dependencies(laser_scan_filters ${PROJECT_NAME}_gencfg)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest)
+  add_executable(test_scan_filter_chain test/test_scan_filter_chain.cpp)
+  target_link_libraries(test_scan_filter_chain laser_scan_filters ${rostest_LIBRARIES} ${GTEST_LIBRARIES})
+  add_dependencies(test_scan_filter_chain gtest)
+
+  add_rostest(test/test_scan_filter_chain.launch)
+  add_rostest(test/test_polygon_filter.launch)
+  add_rostest(test/test_speckle_filter.launch)
+
+  catkin_add_gtest(test_shadow_detector test/test_shadow_detector.cpp)
+  target_link_libraries(test_shadow_detector ${catkin_LIBRARIES} ${rostest_LIBRARIES})
+endif()
+
+##############################################################################
+# Install
+##############################################################################
+
+install(TARGETS pointcloud_filters laser_scan_filters 
+  scan_to_cloud_filter_chain
+  scan_to_scan_filter_chain
+  generic_laser_filter_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Install headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(FILES laser_filters_plugins.xml
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

src/laser_filters/cfg/IntensityFilter.cfg

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, Eurotec, Netherlands
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the TNO IVS nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# \author Rein Appeldoorn
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+PACKAGE = "laser_filters"
+
+gen = ParameterGenerator()
+
+gen.add("lower_threshold", double_t, 0,
+        "Intensity values lower than this value will be filtered", 8000.0, 0, 100000.0)
+gen.add("upper_threshold", double_t, 0,
+        "Intensity values greater than this value will be filtered", 100000.0, 0, 100000.0)
+gen.add("invert", bool_t, 0, "A Boolean to invert the filter", False)
+
+gen.add("filter_override_range", bool_t, 0,
+        "Whether to set the filtered laser beam ranges to NaN", True)
+gen.add("filter_override_intensity", bool_t, 0,
+        "Whether to set the filtered and non-filtered laser beam intensities to 0.0 and 1.0 respectively", False)
+
+exit(gen.generate(PACKAGE, "laser_filters", "IntensityFilter"))

src/laser_filters/cfg/PolygonFilter.cfg

@@ -0,0 +1,48 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, Eurotec, Netherlands
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the TNO IVS nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# \author Yannick de Hoop, Rein Appeldoorn
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+PACKAGE = "laser_filters"
+
+gen = ParameterGenerator()
+
+gen.add("polygon", str_t, 0,
+        "A list of coordinates that represents a polygon in the format [ [x1, y1], [x2, y2], ..., [xn, yn] ]", "[]")
+gen.add("polygon_padding", double_t, 0, "Polygon padding in meters", 0, 0, 100.)
+gen.add("invert", bool_t, 0, "A Boolean to invert the filter", False)
+
+exit(gen.generate(PACKAGE, "laser_filters", "PolygonFilter"))

src/laser_filters/cfg/ScanShadowsFilter.cfg

@@ -0,0 +1,49 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, Eurotec, Netherlands
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the TNO IVS nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# \author Yannick de Hoop
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+PACKAGE = "laser_filters"
+
+gen = ParameterGenerator()
+
+gen.add("min_angle", double_t, 0, "Minimum perpendicular angle", 10, 0, 360)
+gen.add("max_angle", double_t, 0, "Maximum perpendicular angle", 170, 0, 360)
+gen.add("window", int_t, 0, "Number of consecutive measurements to consider angles inside of", 1, 0, 100)
+gen.add("neighbors", int_t, 0, "Number of further-away neighbors to remove", 20, 0, 100)
+gen.add("remove_shadow_start_point", bool_t, 0, "Whether to remove the shadow start point", False)
+
+exit(gen.generate(PACKAGE, "laser_filters", "ScanShadowsFilter"))

src/laser_filters/cfg/SpeckleFilter.cfg

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, Eurotec, Netherlands
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the TNO IVS nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# \author Rein Appeldoorn
+#         Nicolas Limpert
+
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+PACKAGE = "laser_filters"
+
+gen = ParameterGenerator()
+
+filter_type_enum = gen.enum([ gen.const("Distance",      int_t, 0, "Range based filtering (distance between consecutive points)"),
+                              gen.const("RadiusOutlier", int_t, 1, "Euclidean filtering based on radius outlier search")],
+                              "Enum to select the filtering method")
+
+gen.add("filter_type", int_t, 0, "Filtering method selection", 0, 0, 1, edit_method=filter_type_enum)
+gen.add("max_range", double_t, 0, "Only ranges smaller than this range are taken into account", 2.0, 0, 100)
+gen.add("max_range_difference", double_t, 0, "Distance: max distance between consecutive points - RadiusOutlier: max distance between points", 0.4, 0, 100)
+gen.add("filter_window", int_t, 0, "Minimum number of neighbors", 4, 0, 100)
+
+exit(gen.generate(PACKAGE, "laser_filters", "SpeckleFilter"))

src/laser_filters/doc/node_organization.svg

@@ -0,0 +1,1138 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   width="744.09448819"
+   height="1052.3622047"
+   id="svg2"
+   sodipodi:version="0.32"
+   inkscape:version="0.46"
+   sodipodi:docname="node_organization.svg"
+   inkscape:output_extension="org.inkscape.output.svg.inkscape">
+  <sodipodi:namedview
+     id="base"
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     gridtolerance="10000"
+     guidetolerance="10"
+     objecttolerance="10"
+     inkscape:pageopacity="1"
+     inkscape:pageshadow="2"
+     inkscape:zoom="1"
+     inkscape:cx="326.08497"
+     inkscape:cy="372.90058"
+     inkscape:document-units="px"
+     inkscape:current-layer="layer1"
+     showgrid="false"
+     inkscape:window-width="1448"
+     inkscape:window-height="1028"
+     inkscape:window-x="1220"
+     inkscape:window-y="329" />
+  <defs
+     id="defs4">
+    <marker
+       inkscape:stockid="Arrow1Lstart"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1Lstart"
+       style="overflow:visible">
+      <path
+         id="path4172"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+         transform="scale(0.8) translate(12.5,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1Lend"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1Lend"
+       style="overflow:visible;">
+      <path
+         id="path4175"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+         transform="scale(0.8) rotate(180) translate(12.5,0)" />
+    </marker>
+    <inkscape:perspective
+       sodipodi:type="inkscape:persp3d"
+       inkscape:vp_x="0 : 526.18109 : 1"
+       inkscape:vp_y="0 : 1000 : 0"
+       inkscape:vp_z="744.09448 : 526.18109 : 1"
+       inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
+       id="perspective10" />
+    <marker
+       viewBox="0 0 10 10"
+       refY="5"
+       refX="10"
+       orient="auto"
+       markerWidth="4"
+       markerUnits="strokeWidth"
+       markerHeight="3"
+       id="ArrowStart">
+      <path
+         id="path3586"
+         d="M 10 0 L 0 5 L 10 10 z" />
+    </marker>
+    <marker
+       viewBox="0 0 10 10"
+       refY="5"
+       refX="0"
+       orient="auto"
+       markerWidth="4"
+       markerUnits="strokeWidth"
+       markerHeight="3"
+       id="ArrowEnd">
+      <path
+         id="path3583"
+         d="M 0 0 L 10 5 L 0 10 z" />
+    </marker>
+    <inkscape:perspective
+       id="perspective7140"
+       inkscape:persp3d-origin="215.661 : 120.203 : 1"
+       inkscape:vp_z="431.32199 : 180.3045 : 1"
+       inkscape:vp_y="0 : 1000 : 0"
+       inkscape:vp_x="0 : 180.3045 : 1"
+       sodipodi:type="inkscape:persp3d" />
+    <marker
+       inkscape:stockid="Arrow1Lendi"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1Lendi"
+       style="overflow:visible;">
+      <path
+         id="path2611"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="marker-start:none;stroke:#a2a2a2;stroke-width:1.0pt;fill:#a2a2a2;fill-rule:evenodd"
+         transform="scale(0.8) rotate(180) translate(12.5,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1LendiJ"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1LendiJ"
+       style="overflow:visible;">
+      <path
+         id="path3867"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="fill-rule:evenodd;marker-start:none;stroke:#808080;stroke-width:1.0pt;fill:#808080"
+         transform="scale(0.8) rotate(180) translate(12.5,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1LendiJ2"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1LendiJ2"
+       style="overflow:visible;">
+      <path
+         id="path4871"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="marker-start:none;stroke:#c0c0c0;stroke-width:1.0pt;fill:#c0c0c0;fill-rule:evenodd"
+         transform="scale(0.8) rotate(180) translate(12.5,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1LendiJ2C"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1LendiJ2C"
+       style="overflow:visible;">
+      <path
+         id="path5334"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="fill-rule:evenodd;marker-start:none;stroke:#8c8c8c;stroke-width:1.0pt;fill:#8c8c8c"
+         transform="scale(0.8) rotate(180) translate(12.5,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1LendH"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1LendH"
+       style="overflow:visible;">
+      <path
+         id="path5801"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="marker-start:none;stroke:#8c8c8c;stroke-width:1.0pt;fill:#8c8c8c;fill-rule:evenodd"
+         transform="scale(0.8) rotate(180) translate(12.5,0)" />
+    </marker>
+    <marker
+       inkscape:stockid="Arrow1LendV"
+       orient="auto"
+       refY="0.0"
+       refX="0.0"
+       id="Arrow1LendV"
+       style="overflow:visible;">
+      <path
+         id="path6365"
+         d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+         style="marker-start:none;stroke:#8c8c8c;stroke-width:1.0pt;fill:#8c8c8c;fill-rule:evenodd"
+         transform="scale(0.8) rotate(180) translate(12.5,0)" />
+    </marker>
+  </defs>
+  <metadata
+     id="metadata7">
+    <rdf:RDF>
+      <cc:Work
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
+        <dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <g
+     inkscape:label="Layer 1"
+     inkscape:groupmode="layer"
+     id="layer1">
+    <rect
+       inkscape:export-ydpi="90"
+       inkscape:export-xdpi="90"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       ry="0.094667137"
+       rx="0.41487131"
+       y="62.615711"
+       x="102.0151"
+       height="160.46841"
+       width="467.30087"
+       id="rect2606"
+       style="opacity:1;fill:#d5e9fa;fill-opacity:1;fill-rule:nonzero;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate" />
+    <flowRoot
+       xml:space="preserve"
+       id="flowRoot5112"
+       style="font-size:40px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"><flowRegion
+         id="flowRegion5114"><rect
+           id="rect5116"
+           width="168"
+           height="51"
+           x="147"
+           y="472.36218" /></flowRegion><flowPara
+         id="flowPara5118">LaserSca</flowPara></flowRoot>    <flowRoot
+       xml:space="preserve"
+       id="flowRoot5120"
+       style="fill:black;stroke:none;stroke-opacity:1;stroke-width:1px;stroke-linejoin:miter;stroke-linecap:butt;fill-opacity:1;font-family:Bitstream Vera Sans;font-style:normal;font-weight:normal;font-size:16"><flowRegion
+         id="flowRegion5122"><rect
+           id="rect5124"
+           width="282"
+           height="82"
+           x="76"
+           y="440.36218"
+           style="font-size:16" /></flowRegion><flowPara
+         id="flowPara5126" /></flowRoot>    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="32.975616"
+       y="141.28397"
+       id="text5128"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan5130"
+         x="32.975616"
+         y="141.28397">scan</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="101.25381"
+       y="52.829468"
+       id="text6689"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan6691"
+         x="101.25381"
+         y="52.829468"
+         style="font-weight:normal;-inkscape-font-specification:Bitstream Vera Sans Bold">scan_to_scan_filter_chain</tspan></text>
+    <path
+       inkscape:export-ydpi="90"
+       inkscape:export-xdpi="90"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.87862456;stroke-linecap:butt;stroke-linejoin:miter;marker-start:none;marker-end:url(#Arrow1Lend);stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       d="M 69.011602,137.28397 L 128.93962,137.28397"
+       id="path5030" />
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#8c8c8c;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="25.215851"
+       y="201.1611"
+       id="text7164"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan7166"
+         x="25.215851"
+         y="201.1611">tf data</tspan></text>
+    <rect
+       ry="0.15627673"
+       rx="0.34393859"
+       y="84.358505"
+       x="280.99634"
+       height="86.007355"
+       width="264.00735"
+       id="rect2610"
+       style="opacity:1;fill:#b3b3b3;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.99264622;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="289.36502"
+       y="103.75778"
+       id="text2629"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan2631"
+         x="289.36502"
+         y="103.75778">filter_chain&lt;sensor_msgs::LaserScan&gt;</tspan></text>
+    <rect
+       style="fill:#e6e6e6;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.83422947;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect2627"
+       width="66.218414"
+       height="40.416122"
+       x="290.76755"
+       y="117.07609"
+       rx="0.16101648"
+       ry="0.074691474"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       id="text3440"
+       y="142.36218"
+       x="303"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       xml:space="preserve"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         y="142.36218"
+         x="303"
+         id="tspan3442"
+         sodipodi:role="line">filter 1</tspan></text>
+    <rect
+       style="fill:#e6e6e6;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.83422947;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect3461"
+       width="66.218414"
+       height="40.416122"
+       x="466.89081"
+       y="117.15412"
+       rx="0.16101648"
+       ry="0.074691474"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       id="text3463"
+       y="142.44022"
+       x="479.12323"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       xml:space="preserve"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         y="142.44022"
+         x="479.12323"
+         id="tspan3465"
+         sodipodi:role="line">filter N</tspan></text>
+    <path
+       inkscape:connector-type="polyline"
+       id="path3467"
+       d="M 357.99591,137.3144 L 386.88086,137.33176"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       inkscape:connector-type="polyline"
+       id="path4040"
+       d="M 431.17316,137.36218 L 465.82687,137.36218"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="625.37061"
+       y="141.36218"
+       id="text4605"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         id="tspan4611"
+         sodipodi:role="line"
+         x="625.37061"
+         y="141.36218">scan_filtered</tspan></text>
+    <path
+       inkscape:export-ydpi="90"
+       inkscape:export-xdpi="90"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;marker-start:none;marker-end:url(#Arrow1Lend);stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+       d="M 532.48444,137.36218 L 616.25676,137.36218"
+       id="path4609" />
+    <g
+       transform="translate(-18,29)"
+       id="g4643"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90">
+      <rect
+         inkscape:export-ydpi="90"
+         inkscape:export-xdpi="90"
+         inkscape:export-filename="/u/vpradeep/ros/pkgs/laser_pipeline/laser_assembler/doc/point_cloud_assembler.png"
+         ry="0.10180182"
+         rx="0.27528837"
+         y="79.981789"
+         x="147.89349"
+         height="55.085735"
+         width="113.21298"
+         id="rect4615"
+         style="fill:#bababa;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.91426229;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate" />
+      <text
+         inkscape:export-ydpi="90"
+         inkscape:export-xdpi="90"
+         inkscape:export-filename="/u/vpradeep/ros/pkgs/laser_pipeline/laser_assembler/doc/laser_assembler.png"
+         id="text4617"
+         y="110.50574"
+         x="155.43945"
+         style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+         xml:space="preserve"><tspan
+           y="110.50574"
+           x="155.43945"
+           sodipodi:role="line"
+           id="tspan4621">tf::MessageFilter</tspan></text>
+    </g>
+    <path
+       inkscape:connection-end="#rect2627"
+       inkscape:connection-start="#g4643"
+       inkscape:connector-type="polyline"
+       id="path4648"
+       d="M 244.06361,136.8429 L 289.85043,137.09603"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Lend)"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       sodipodi:nodetypes="cc"
+       id="path5213"
+       d="M 77,197.36218 C 191.11518,196.36921 180.50824,203.38073 181,164.36218"
+       style="fill:none;fill-rule:evenodd;stroke:#8c8c8c;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-mid:none;marker-end:url(#Arrow1LendV);stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="402"
+       y="139.36218"
+       id="text6794"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan6796"
+         x="402"
+         y="139.36218"
+         style="font-size:14px">...</tspan></text>
+    <rect
+       style="fill:#e6e6e6;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect8669"
+       width="151.09151"
+       height="144.09151"
+       x="100.95425"
+       y="261.31641"
+       rx="1.1631178"
+       ry="0.34872583"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <flowRoot
+       xml:space="preserve"
+       id="flowRoot8671"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       transform="translate(9,12)"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><flowRegion
+         id="flowRegion8673"><rect
+           id="rect8675"
+           width="219"
+           height="175"
+           x="110"
+           y="270.36218"
+           style="stroke-width:2;stroke-miterlimit:4;stroke-dasharray:none" /></flowRegion><flowPara
+         id="flowPara8691"
+         style="font-size:10px">- name: filter1</flowPara><flowPara
+         id="flowPara8727"
+         style="font-size:10px">  type: ScanFilterType1</flowPara><flowPara
+         id="flowPara8699"
+         style="font-size:10px">  params:</flowPara><flowPara
+         id="flowPara8739"
+         style="font-size:10px">    foo: 123</flowPara><flowPara
+         id="flowPara8741"
+         style="font-size:10px">...</flowPara><flowPara
+         id="flowPara8703"
+         style="font-size:10px">- name: filterN</flowPara><flowPara
+         id="flowPara8749"
+         style="font-size:10px">  type: ScanFilterTypeN</flowPara><flowPara
+         id="flowPara8751"
+         style="font-size:10px">  params:</flowPara><flowPara
+         id="flowPara8753"
+         style="font-size:10px">    bar: abc</flowPara></flowRoot>    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="104"
+       y="275.36218"
+       id="text8757"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         x="104"
+         y="275.36218"
+         id="tspan8761"
+         style="font-weight:normal;-inkscape-font-specification:Bitstream Vera Sans">~scan_filter_chain</tspan></text>
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;marker-end:none;stroke-miterlimit:4;stroke-dasharray:4, 4;stroke-dashoffset:0;stroke-opacity:1"
+       d="M 237.58137,364.14869 C 408.67996,364.14869 499.26157,342.5833 499.26157,247.07948 C 499.26157,151.57567 499.26157,151.57567 499.26157,151.57567"
+       id="path8763"
+       sodipodi:nodetypes="csc"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;marker-end:none;stroke-miterlimit:4;stroke-dasharray:4, 4;stroke-dashoffset:0;stroke-opacity:1"
+       d="M 240.77649,300.95686 C 293.99875,300.95686 322.17522,285.77097 322.17522,218.51923 C 322.17522,151.2675 322.17522,151.2675 322.17522,151.2675"
+       id="path7384"
+       sodipodi:nodetypes="csc"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="100.45068"
+       y="255.02966"
+       id="text8835"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan8837"
+         x="100.45068"
+         y="255.02966"
+         style="font-weight:normal;-inkscape-font-specification:Bitstream Vera Sans Bold">Parameter Server</tspan></text>
+    <rect
+       inkscape:export-ydpi="90"
+       inkscape:export-xdpi="90"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       ry="0"
+       rx="0.41935045"
+       y="500.15494"
+       x="89.022011"
+       height="261.51361"
+       width="470.34607"
+       id="rect8839"
+       style="opacity:1;fill:#d5e9fa;fill-opacity:1;fill-rule:nonzero;stroke:#000000;stroke-width:2.95479417;stroke-linecap:butt;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate" />
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="19.505142"
+       y="578.34583"
+       id="text8841"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan8843"
+         x="19.505142"
+         y="578.34583">scan</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="87.783333"
+       y="489.89136"
+       id="text8845"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan8847"
+         x="87.783333"
+         y="489.89136"
+         style="font-weight:normal;-inkscape-font-specification:Bitstream Vera Sans Bold">scan_to_cloud_filter_chain</tspan></text>
+    <path
+       inkscape:export-ydpi="90"
+       inkscape:export-xdpi="90"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.87862456;stroke-linecap:butt;stroke-linejoin:miter;marker-start:none;marker-end:url(#Arrow1Lend);stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       d="M 55.54113,574.34582 L 115.46915,574.34582"
+       id="path8849" />
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#8c8c8c;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="12.745377"
+       y="641.22296"
+       id="text8851"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan8853"
+         x="12.745377"
+         y="641.22296">tf data</tspan></text>
+    <rect
+       ry="0.15627673"
+       rx="0.34393859"
+       y="521.42035"
+       x="267.52588"
+       height="86.007355"
+       width="264.00735"
+       id="rect8855"
+       style="opacity:1;fill:#b3b3b3;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.99264622;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="275.89453"
+       y="540.81964"
+       id="text8857"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan8859"
+         x="275.89453"
+         y="540.81964">filter_chain&lt;sensor_msgs::LaserScan&gt;</tspan></text>
+    <rect
+       style="fill:#e6e6e6;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.83422947;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect8861"
+       width="66.218414"
+       height="40.416122"
+       x="277.29706"
+       y="554.13794"
+       rx="0.16101648"
+       ry="0.074691474"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       id="text8863"
+       y="570.42401"
+       x="288.52954"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       xml:space="preserve"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         y="570.42401"
+         x="288.52954"
+         id="tspan8865"
+         sodipodi:role="line">scan</tspan><tspan
+         y="585.42401"
+         x="288.52954"
+         sodipodi:role="line"
+         id="tspan9712">filter 1</tspan></text>
+    <rect
+       style="fill:#e6e6e6;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.83422947;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect8867"
+       width="66.218414"
+       height="40.416122"
+       x="453.42035"
+       y="554.21594"
+       rx="0.16101648"
+       ry="0.074691474"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       id="text8869"
+       y="568.50208"
+       x="467.65277"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       xml:space="preserve"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         y="568.50208"
+         x="467.65277"
+         id="tspan8871"
+         sodipodi:role="line">scan</tspan><tspan
+         y="583.50208"
+         x="467.65277"
+         sodipodi:role="line"
+         id="tspan9714">filter N</tspan></text>
+    <path
+       inkscape:connector-type="polyline"
+       id="path8873"
+       d="M 344.52544,574.37625 L 373.41039,574.39361"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       inkscape:connector-type="polyline"
+       id="path8875"
+       d="M 417.70269,574.42403 L 452.3564,574.42403"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="611.90015"
+       y="713.42401"
+       id="text8877"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         id="tspan8879"
+         sodipodi:role="line"
+         x="611.90015"
+         y="713.42401">cloud_filtered</tspan></text>
+    <g
+       transform="translate(-31.470474,466.06185)"
+       id="g8883"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90">
+      <rect
+         inkscape:export-ydpi="90"
+         inkscape:export-xdpi="90"
+         inkscape:export-filename="/u/vpradeep/ros/pkgs/laser_pipeline/laser_assembler/doc/point_cloud_assembler.png"
+         ry="0.10180182"
+         rx="0.27528837"
+         y="79.981789"
+         x="147.89349"
+         height="55.085735"
+         width="113.21298"
+         id="rect8885"
+         style="fill:#bababa;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.91426229;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate" />
+      <text
+         inkscape:export-ydpi="90"
+         inkscape:export-xdpi="90"
+         inkscape:export-filename="/u/vpradeep/ros/pkgs/laser_pipeline/laser_assembler/doc/laser_assembler.png"
+         id="text8887"
+         y="110.50574"
+         x="155.43945"
+         style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+         xml:space="preserve"><tspan
+           y="110.50574"
+           x="155.43945"
+           sodipodi:role="line"
+           id="tspan8889">tf::MessageFilter</tspan></text>
+    </g>
+    <path
+       inkscape:connector-type="polyline"
+       id="path8891"
+       d="M 230.59314,573.90475 L 276.37996,574.15788"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       sodipodi:nodetypes="cc"
+       id="path8893"
+       d="M 63.52953,634.42403 C 177.64471,633.43106 167.03777,640.44258 167.52953,601.42403"
+       style="fill:none;fill-rule:evenodd;stroke:#8c8c8c;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-mid:none;marker-end:url(#Arrow1LendV);stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="388.52954"
+       y="576.42401"
+       id="text8895"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan8897"
+         x="388.52954"
+         y="576.42401"
+         style="font-size:14px">...</tspan></text>
+    <rect
+       style="fill:#e6e6e6;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect8899"
+       width="299.09149"
+       height="149.09151"
+       x="89.483765"
+       y="804.37817"
+       rx="1.1631178"
+       ry="0.34872583"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <flowRoot
+       xml:space="preserve"
+       id="flowRoot8901"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       transform="translate(-2.47047,555.06185)"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><flowRegion
+         id="flowRegion8903"><rect
+           id="rect8905"
+           width="219"
+           height="175"
+           x="110"
+           y="270.36218"
+           style="stroke-width:2;stroke-miterlimit:4;stroke-dasharray:none" /></flowRegion><flowPara
+         id="flowPara8907"
+         style="font-size:10px">- name: scan filter 1</flowPara><flowPara
+         id="flowPara8909"
+         style="font-size:10px">  type: ScanFilterType1</flowPara><flowPara
+         id="flowPara8911"
+         style="font-size:10px">  params:</flowPara><flowPara
+         id="flowPara8913"
+         style="font-size:10px">    foo: 123</flowPara><flowPara
+         id="flowPara8915"
+         style="font-size:10px">...</flowPara><flowPara
+         id="flowPara8917"
+         style="font-size:10px">- name: scan filter N</flowPara><flowPara
+         id="flowPara8919"
+         style="font-size:10px">  type: ScanFilterTypeN</flowPara><flowPara
+         id="flowPara8921"
+         style="font-size:10px">  params:</flowPara><flowPara
+         id="flowPara8923"
+         style="font-size:10px">    bar: abc</flowPara></flowRoot>    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="92.529541"
+       y="818.42407"
+       id="text8925"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         x="92.529541"
+         y="818.42407"
+         id="tspan8927"
+         style="font-weight:normal;-inkscape-font-specification:Bitstream Vera Sans">~scan_filter_chain</tspan></text>
+    <text
+       xml:space="preserve"
+       style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="88.980225"
+       y="798.09149"
+       id="text8933"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan8935"
+         x="88.980225"
+         y="798.09149"
+         style="font-weight:normal;-inkscape-font-specification:Bitstream Vera Sans Bold">Parameter Server</tspan></text>
+    <rect
+       ry="0.15627673"
+       rx="0.34393859"
+       y="655.35852"
+       x="265.61798"
+       height="86.007355"
+       width="264.00735"
+       id="rect9096"
+       style="opacity:1;fill:#b3b3b3;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.99264622;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="273.98663"
+       y="674.75781"
+       id="text9098"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan9100"
+         x="273.98663"
+         y="674.75781">filter_chain&lt;sensor_msgs::PointCloud&gt;</tspan></text>
+    <rect
+       style="fill:#e6e6e6;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.83422947;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect9102"
+       width="66.218414"
+       height="40.416122"
+       x="275.38916"
+       y="688.07611"
+       rx="0.16101648"
+       ry="0.074691474"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       id="text9104"
+       y="704.36218"
+       x="288.12164"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       xml:space="preserve"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         y="704.36218"
+         x="288.12164"
+         id="tspan9106"
+         sodipodi:role="line">cloud</tspan><tspan
+         y="719.36218"
+         x="288.12164"
+         sodipodi:role="line"
+         id="tspan9710">filter 1</tspan></text>
+    <rect
+       style="fill:#e6e6e6;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.83422947;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect9108"
+       width="66.218414"
+       height="40.416122"
+       x="451.51245"
+       y="688.15411"
+       rx="0.16101648"
+       ry="0.074691474"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       id="text9110"
+       y="704.44025"
+       x="463.74487"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       xml:space="preserve"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         y="704.44025"
+         x="463.74487"
+         id="tspan9112"
+         sodipodi:role="line">cloud</tspan><tspan
+         y="719.44025"
+         x="463.74487"
+         sodipodi:role="line"
+         id="tspan9708">filter N</tspan></text>
+    <path
+       inkscape:connector-type="polyline"
+       id="path9114"
+       d="M 342.61752,708.31441 L 371.50247,708.33177"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       inkscape:connector-type="polyline"
+       id="path9116"
+       d="M 415.79477,708.36219 L 450.44848,708.36219"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       inkscape:export-ydpi="90"
+       inkscape:export-xdpi="90"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;marker-start:none;marker-end:url(#Arrow1Lend);stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       d="M 517.10605,709.36219 L 600.87837,709.36219"
+       id="path9118" />
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="386.62164"
+       y="710.36218"
+       id="text9120"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         id="tspan9122"
+         x="386.62164"
+         y="710.36218"
+         style="font-size:14px">...</tspan></text>
+    <rect
+       style="fill:#bababa;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.91426229;stroke-linecap:round;stroke-linejoin:miter;marker:none;marker-start:none;marker-mid:none;marker-end:none;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;visibility:visible;display:inline;overflow:visible;enable-background:accumulate"
+       id="rect9126"
+       width="113.21298"
+       height="55.085735"
+       x="113.39351"
+       y="681.81934"
+       rx="0.27528837"
+       ry="0.10180182"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="123.93947"
+       y="713.34326"
+       id="text9128"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         id="tspan9130"
+         sodipodi:role="line"
+         x="123.93947"
+         y="713.34326">LaserProjection</tspan></text>
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Lend)"
+       d="M 485,594.36218 C 481,638.36218 449,632.36218 326,634.36218 C 185.01652,638.4602 185.98655,630.5487 185,679.36218"
+       id="path9135"
+       sodipodi:nodetypes="ccc"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       sodipodi:nodetypes="cc"
+       id="path9706"
+       d="M 63.170734,633.94454 C 177.11184,635.29364 166.52109,625.76747 167.0121,678.77982"
+       style="fill:none;fill-rule:evenodd;stroke:#8c8c8c;stroke-width:1.16471994px;stroke-linecap:butt;stroke-linejoin:miter;marker-mid:none;marker-end:url(#Arrow1LendV);stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       inkscape:connector-type="polyline"
+       id="path9716"
+       d="M 228.10659,711.18645 L 273.89341,711.43958"
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <flowRoot
+       xml:space="preserve"
+       id="flowRoot9720"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       transform="translate(149.50001,555.00011)"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><flowRegion
+         id="flowRegion9722"><rect
+           id="rect9724"
+           width="219"
+           height="175"
+           x="110"
+           y="270.36218"
+           style="stroke-width:2;stroke-miterlimit:4;stroke-dasharray:none" /></flowRegion><flowPara
+         id="flowPara9726"
+         style="font-size:10px">- name: cloud filter 1</flowPara><flowPara
+         id="flowPara9728"
+         style="font-size:10px">  type: CloudFilterType1</flowPara><flowPara
+         id="flowPara9730"
+         style="font-size:10px">  params:</flowPara><flowPara
+         id="flowPara9732"
+         style="font-size:10px">    foo: 123</flowPara><flowPara
+         id="flowPara9734"
+         style="font-size:10px">...</flowPara><flowPara
+         id="flowPara9736"
+         style="font-size:10px">- name: cloud filter N</flowPara><flowPara
+         id="flowPara9738"
+         style="font-size:10px">  type: CloudFilterTypeN</flowPara><flowPara
+         id="flowPara9740"
+         style="font-size:10px">  params:</flowPara><flowPara
+         id="flowPara9742"
+         style="font-size:10px">    bar: abc</flowPara></flowRoot>    <text
+       xml:space="preserve"
+       style="font-size:12px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+       x="244.50003"
+       y="818.3623"
+       id="text9744"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90"><tspan
+         sodipodi:role="line"
+         x="244.50003"
+         y="818.3623"
+         id="tspan9746"
+         style="font-weight:normal;-inkscape-font-specification:Bitstream Vera Sans">~cloud_filter_chain</tspan></text>
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:none;stroke-miterlimit:4;stroke-dasharray:4, 4;stroke-dashoffset:0"
+       d="M 221,843.36218 C 236.16702,840.57893 222.11265,670.89164 255.6066,636.99822 C 288.64925,606.22765 298.04003,620.40128 307.23223,589.83705"
+       id="path11463"
+       sodipodi:nodetypes="ccc"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:none;stroke-miterlimit:4;stroke-dasharray:4,4;stroke-dashoffset:0"
+       d="M 224.52513,911.07969 C 248.6261,823.78065 230.15092,676.10375 264.58579,646.4939 C 300.96852,618.54361 377.65935,619.40607 418.89901,618.86925 C 446.34998,618.01958 450.77017,615.04637 464.53553,593.25127"
+       id="path11465"
+       sodipodi:nodetypes="cccc"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1"
+       d="M 464.95423,593.18724 C 467.37822,586.9323 467.37822,586.9323 467.37822,586.9323"
+       id="path15452"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1"
+       d="M 306.73332,593.16221 C 309.15731,586.90727 309.15731,586.90727 309.15731,586.90727"
+       id="path16592"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1"
+       d="M 499.58556,156.20774 C 499.63391,149.49971 499.63391,149.49971 499.63391,149.49971"
+       id="path17732"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1"
+       d="M 322.55256,156.55598 C 322.60091,149.84795 322.60091,149.84795 322.60091,149.84795"
+       id="path17734"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_scan_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:4,4;stroke-dashoffset:0"
+       d="M 383,843.36218 C 429.00179,828.32562 418.41005,774.89848 339,776.36218 C 302.96327,776.3965 305,728.36218 305,728.36218"
+       id="path17736"
+       sodipodi:nodetypes="ccc"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:4,4;stroke-dashoffset:0"
+       d="M 384,905.36218 C 464.83461,881.89214 483,797.85477 483,726.36218"
+       id="path17738"
+       sodipodi:nodetypes="cc"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1"
+       d="M 483.05256,731.05598 C 483.10091,724.34795 483.10091,724.34795 483.10091,724.34795"
+       id="path17740"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Lend);stroke-opacity:1"
+       d="M 306.05256,729.55598 C 306.10091,722.84795 306.10091,722.84795 306.10091,722.84795"
+       id="path17742"
+       inkscape:export-filename="/wg/avl/leibs/ros/ros-pkg/laser_pipeline/laser_filters/doc/scan_to_cloud_filter_chain.png"
+       inkscape:export-xdpi="90"
+       inkscape:export-ydpi="90" />
+  </g>
+</svg>

src/laser_filters/include/laser_filters/angular_bounds_filter.h

@@ -0,0 +1,124 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef LASER_SCAN_ANGULAR_BOUNDS_FILTER_H
+#define LASER_SCAN_ANGULAR_BOUNDS_FILTER_H
+
+#include <filters/filter_base.h>
+#include <sensor_msgs/LaserScan.h>
+
+namespace laser_filters
+{
+  class LaserScanAngularBoundsFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+  {
+    public:
+      double lower_angle_;
+      double upper_angle_;
+
+      bool configure()
+      {
+        lower_angle_ = 0;
+        upper_angle_ = 0;
+
+        if(!getParam("lower_angle", lower_angle_) || !getParam("upper_angle", upper_angle_)){
+          ROS_ERROR("Both the lower_angle and upper_angle parameters must be set to use this filter.");
+          return false;
+        }
+
+        return true;
+      }
+
+      virtual ~LaserScanAngularBoundsFilter(){}
+
+      bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan){
+        filtered_scan.ranges.resize(input_scan.ranges.size());
+        filtered_scan.intensities.resize(input_scan.intensities.size());
+
+        double start_angle = input_scan.angle_min;
+        double current_angle = input_scan.angle_min;
+        ros::Time start_time = input_scan.header.stamp;
+        unsigned int count = 0;
+        //loop through the scan and truncate the beginning and the end of the scan as necessary
+        for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){
+          //wait until we get to our desired starting angle
+          if(start_angle < lower_angle_){
+            start_angle += input_scan.angle_increment;
+            current_angle += input_scan.angle_increment;
+            start_time += ros::Duration(input_scan.time_increment);
+          }
+          else{
+            filtered_scan.ranges[count] = input_scan.ranges[i];
+
+            //make sure  that we don't update intensity data if its not available
+            if(input_scan.intensities.size() > i)
+              filtered_scan.intensities[count] = input_scan.intensities[i];
+
+            count++;
+
+            //check if we need to break out of the loop, basically if the next increment will put us over the threshold
+            if(current_angle + input_scan.angle_increment > upper_angle_){
+              break;
+            }
+
+            current_angle += input_scan.angle_increment;
+
+          }
+        }
+
+        //make sure to set all the needed fields on the filtered scan
+        filtered_scan.header.frame_id = input_scan.header.frame_id;
+        filtered_scan.header.stamp = start_time;
+        filtered_scan.angle_min = start_angle;
+        filtered_scan.angle_max = current_angle;
+        filtered_scan.angle_increment = input_scan.angle_increment;
+        filtered_scan.time_increment = input_scan.time_increment;
+        filtered_scan.scan_time = input_scan.scan_time;
+        filtered_scan.range_min = input_scan.range_min;
+        filtered_scan.range_max = input_scan.range_max;
+
+        filtered_scan.ranges.resize(count);
+
+        if(input_scan.intensities.size() >= count)
+          filtered_scan.intensities.resize(count);
+
+        ROS_DEBUG("Filtered out %d points from the laser scan.", (int)input_scan.ranges.size() - (int)count);
+
+        return true;
+
+      }
+  };
+};
+#endif

src/laser_filters/include/laser_filters/angular_bounds_filter_in_place.h

@@ -0,0 +1,90 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Kevin Hallenbeck
+*********************************************************************/
+#ifndef LASER_SCAN_ANGULAR_BOUNDS_FILTER_IN_PLACE_H
+#define LASER_SCAN_ANGULAR_BOUNDS_FILTER_IN_PLACE_H
+
+#include <filters/filter_base.h>
+#include <sensor_msgs/LaserScan.h>
+
+namespace laser_filters
+{
+  class LaserScanAngularBoundsFilterInPlace : public filters::FilterBase<sensor_msgs::LaserScan>
+  {
+    public:
+      double lower_angle_;
+      double upper_angle_;
+
+      bool configure()
+      {
+        lower_angle_ = 0;
+        upper_angle_ = 0;
+
+        if(!getParam("lower_angle", lower_angle_) || !getParam("upper_angle", upper_angle_)){
+          ROS_ERROR("Both the lower_angle and upper_angle parameters must be set to use this filter.");
+          return false;
+        }
+
+        return true;
+      }
+
+      virtual ~LaserScanAngularBoundsFilterInPlace(){}
+
+      bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan){
+        filtered_scan = input_scan; //copy entire message
+
+        double current_angle = input_scan.angle_min;
+        unsigned int count = 0;
+        //loop through the scan and remove ranges at angles between lower_angle_ and upper_angle_
+        for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){
+          if((current_angle > lower_angle_) && (current_angle < upper_angle_)){
+            filtered_scan.ranges[i] = input_scan.range_max + 1.0;
+            if(i < filtered_scan.intensities.size()){
+              filtered_scan.intensities[i] = 0.0;
+            }
+            count++;
+          }
+          current_angle += input_scan.angle_increment;
+        }
+
+        ROS_DEBUG("Filtered out %u points from the laser scan.", count);
+
+        return true;
+
+      }
+  };
+};
+#endif

src/laser_filters/include/laser_filters/array_filter.h

@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef LASER_SCAN_ARRAY_FILTER_H
+#define LASER_SCAN_ARRAY_FILTER_H
+
+#include <map>
+#include <iostream>
+#include <sstream>
+
+#include "boost/thread/mutex.hpp"
+#include "boost/scoped_ptr.hpp"
+#include "sensor_msgs/LaserScan.h"
+
+#include "filters/median.h"
+#include "filters/mean.h"
+#include "filters/filter_chain.h"
+#include "boost/thread/mutex.hpp"
+
+namespace laser_filters{
+
+/** \brief A class to provide median filtering of laser scans in time*/
+class LaserArrayFilter : public filters::FilterBase<sensor_msgs::LaserScan> 
+{
+public:
+  /** \brief Constructor
+   * \param averaging_length How many scans to average over.
+   */
+  LaserArrayFilter();
+  ~LaserArrayFilter();
+
+  bool configure();
+
+  /** \brief Update the filter and get the response
+   * \param scan_in The new scan to filter
+   * \param scan_out The filtered scan
+   */
+  bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out);
+
+
+private:
+  unsigned int filter_length_; ///How many scans to average over
+  unsigned int num_ranges_; /// How many data point are in each row
+
+  XmlRpc::XmlRpcValue range_config_;
+  XmlRpc::XmlRpcValue intensity_config_;
+
+  boost::mutex data_lock; /// Protection from multi threaded programs
+  sensor_msgs::LaserScan temp_scan_; /** \todo cache only shallow info not full scan */
+  
+  filters::MultiChannelFilterChain<float> * range_filter_;
+  filters::MultiChannelFilterChain<float> * intensity_filter_;
+  
+};
+
+
+
+
+
+}
+
+
+#endif //LASER_SCAN_UTILS_LASERSCAN_H

src/laser_filters/include/laser_filters/box_filter.h

@@ -0,0 +1,92 @@
+/*
+ *  Software License Agreement (BSD License)
+ *
+ *  Robot Operating System code by the University of Osnabrück
+ *  Copyright (c) 2015, University of Osnabrück
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1. Redistributions of source code must retain the above 
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer.
+ *
+ *   2. Redistributions in binary form must reproduce the above 
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer in the documentation and/or other materials provided
+ *      with the distribution.
+ *
+ *   3. Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived
+ *      from this software without specific prior written permission.
+ *
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ *  TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ *  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 
+ *  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ *
+ *  box_filter.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ */
+
+
+
+#ifndef BOXFILTER_H
+#define BOXFILTER_H
+
+#include <filters/filter_base.h>
+
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <laser_geometry/laser_geometry.h>
+
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+
+namespace laser_filters
+{
+/**
+ * @brief This is a filter that removes points in a laser scan inside of a cartesian box.
+ */
+class LaserScanBoxFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+  public:
+    LaserScanBoxFilter();
+    bool configure();
+
+    bool update(
+      const sensor_msgs::LaserScan& input_scan,
+      sensor_msgs::LaserScan& filtered_scan);
+
+  private:
+    bool inBox(tf::Point &point);
+    std::string box_frame_;
+    laser_geometry::LaserProjection projector_;
+    
+    // tf listener to transform scans into the box_frame
+    tf::TransformListener tf_; 
+    
+    // defines two opposite corners of the box
+    tf::Point min_, max_; 
+    bool invert_filter;
+    bool up_and_running_;
+};
+
+}
+
+
+#endif /* box_filter.h */

src/laser_filters/include/laser_filters/footprint_filter.h

@@ -0,0 +1,140 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef LASER_SCAN_FOOTPRINT_FILTER_H
+#define LASER_SCAN_FOOTPRINT_FILTER_H
+/**
+\author Tully Foote
+@b ScanFootprintFilter takes input scans and corrects for footprint angle assuming a flat target.
+This is useful for ground plane extraction
+
+**/
+
+
+#include "filters/filter_base.h"
+#include "sensor_msgs/LaserScan.h"
+#include "tf/transform_listener.h"
+#include "sensor_msgs/PointCloud.h"
+#include "ros/ros.h"
+#include "laser_geometry/laser_geometry.h"
+
+namespace laser_filters
+{
+
+class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+  LaserScanFootprintFilter(): up_and_running_(false) {}
+
+  bool configure()
+  {
+    if(!getParam("inscribed_radius", inscribed_radius_))
+    {
+      ROS_ERROR("LaserScanFootprintFilter needs inscribed_radius to be set");
+      return false;
+    }
+    return true;
+  }
+
+  virtual ~LaserScanFootprintFilter()
+  {
+
+  }
+
+  bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
+  {
+    filtered_scan = input_scan ;
+    sensor_msgs::PointCloud laser_cloud;
+
+    try{
+      projector_.transformLaserScanToPointCloud("base_link", input_scan, laser_cloud, tf_);
+    }
+    catch(tf::TransformException& ex){
+      if(up_and_running_){
+        ROS_WARN_THROTTLE(1, "Dropping Scan: Transform unavailable %s", ex.what());
+      }
+      else {
+        ROS_INFO_THROTTLE(.3, "Ignoring Scan: Waiting for TF");
+      }
+      return false;
+    }
+
+    int c_idx = indexChannel(laser_cloud);
+
+    if (c_idx == -1 || laser_cloud.channels[c_idx].values.size () == 0){
+      ROS_ERROR("We need an index channel to be able to filter out the footprint");
+      return false;
+    }
+
+    for (unsigned int i=0; i < laser_cloud.points.size(); i++)
+    {
+      if (inFootprint(laser_cloud.points[i])){
+        int index = laser_cloud.channels[c_idx].values[i];
+        filtered_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
+      }
+    }
+
+    up_and_running_ = true;
+    return true;
+  }
+
+  int indexChannel(const sensor_msgs::PointCloud& scan_cloud){
+      int c_idx = -1;
+      for (unsigned int d = 0; d < scan_cloud.channels.size (); d++)
+      {
+        if (scan_cloud.channels[d].name == "index")
+        {
+          c_idx = d;
+          break;
+        }
+      }
+      return c_idx;
+  }
+
+  bool inFootprint(const geometry_msgs::Point32& scan_pt){
+    if(scan_pt.x < -1.0 * inscribed_radius_ || scan_pt.x > inscribed_radius_ || scan_pt.y < -1.0 * inscribed_radius_ || scan_pt.y > inscribed_radius_)
+      return false;
+    return true;
+  }
+
+private:
+  tf::TransformListener tf_;
+  laser_geometry::LaserProjection projector_;
+  double inscribed_radius_;
+  bool up_and_running_;
+} ;
+
+}
+
+#endif // LASER_SCAN_FOOTPRINT_FILTER_H

src/laser_filters/include/laser_filters/intensity_filter.h

@@ -0,0 +1,62 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2020, Eurotec B.V.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+*  \author Vijay Pradeep, Rein Appeldoorn
+*
+*********************************************************************/
+
+#pragma once
+
+#include <dynamic_reconfigure/server.h>
+#include <filters/filter_base.h>
+#include <laser_filters/IntensityFilterConfig.h>
+#include <sensor_msgs/LaserScan.h>
+
+namespace laser_filters
+{
+class LaserScanIntensityFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+  LaserScanIntensityFilter();
+  bool configure();
+  bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan);
+
+private:
+  std::shared_ptr<dynamic_reconfigure::Server<IntensityFilterConfig>> dyn_server_;
+  void reconfigureCB(IntensityFilterConfig& config, uint32_t level);
+  boost::recursive_mutex own_mutex_;
+
+  IntensityFilterConfig config_ = IntensityFilterConfig::__getDefault__();
+};
+}

src/laser_filters/include/laser_filters/interpolation_filter.h

@@ -0,0 +1,115 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef INTERPOLATION_FILTER_H
+#define INTERPOLATION_FILTER_H
+/**
+\author Eitan Marder-Eppstein
+@b InterpolationFilter takes input scans and for readings that come back as errors, interpolates between valid readings to generate range values for them.
+
+**/
+
+
+#include "filters/filter_base.h"
+#include "sensor_msgs/LaserScan.h"
+
+namespace laser_filters
+{
+
+class InterpolationFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+
+  bool configure()
+  {
+    return true;
+  }
+
+  virtual ~InterpolationFilter()
+  { 
+  }
+
+  bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
+  {
+    double previous_valid_range = input_scan.range_max - .01;
+    double next_valid_range = input_scan.range_max - .01;
+    filtered_scan= input_scan;
+
+    unsigned int i = 0;
+    while(i < input_scan.ranges.size()) // Need to check every reading in the current scan
+    {
+      //check if the reading is out of range for some reason
+      if (filtered_scan.ranges[i] <= input_scan.range_min ||
+          filtered_scan.ranges[i] >= input_scan.range_max){
+
+        //we need to find the next valid range reading
+        unsigned int j = i + 1;
+        unsigned int start_index = i;
+        unsigned int end_index = i;
+        while(j < input_scan.ranges.size()){
+          if (filtered_scan.ranges[j] <= input_scan.range_min || 
+              filtered_scan.ranges[j] >= input_scan.range_max){                                                                                 
+            end_index = j;
+          }
+          else{
+            next_valid_range = filtered_scan.ranges[j];
+            break;
+          }
+          ++j;
+        }
+
+        //for now, we'll just take the average between the two valid range readings
+        double average_range = (previous_valid_range + next_valid_range) / 2.0;
+
+        for(unsigned int k = start_index; k <= end_index; k++){
+          filtered_scan.ranges[k] = average_range;
+        }
+
+        //make sure to update our previous valid range reading
+        previous_valid_range = next_valid_range;
+        i = j + 1;
+      }
+      else{
+        previous_valid_range = filtered_scan.ranges[i];
+        ++i;
+      }
+
+    }
+    return true;
+  }
+};
+
+}
+
+#endif // LASER_SCAN_INTENSITY_FILTER_H

src/laser_filters/include/laser_filters/median_filter.h

@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef LASER_SCAN_MEDIAN_FILTER_H
+#define LASER_SCAN_MEDIAN_FILTER_H
+
+#include <map>
+#include <iostream>
+#include <sstream>
+
+#include "boost/thread/mutex.hpp"
+#include "boost/scoped_ptr.hpp"
+#include "sensor_msgs/LaserScan.h"
+
+#include "filters/median.h"
+#include "filters/mean.h"
+#include "filters/filter_chain.h"
+#include "boost/thread/mutex.hpp"
+
+namespace laser_filters{
+
+/** \brief A class to provide median filtering of laser scans in time*/
+class LaserMedianFilter : public filters::FilterBase<sensor_msgs::LaserScan> 
+{
+public:
+  /** \brief Constructor
+   * \param averaging_length How many scans to average over.
+   */
+  LaserMedianFilter();
+  ~LaserMedianFilter();
+
+  bool configure();
+
+  /** \brief Update the filter and get the response
+   * \param scan_in The new scan to filter
+   * \param scan_out The filtered scan
+   */
+  bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out);
+
+
+private:
+  unsigned int filter_length_; ///How many scans to average over
+  unsigned int num_ranges_; /// How many data point are in each row
+
+  boost::mutex data_lock; /// Protection from multi threaded programs
+  sensor_msgs::LaserScan temp_scan_; /** \todo cache only shallow info not full scan */
+
+  XmlRpc::XmlRpcValue xmlrpc_value_;
+  
+  filters::MultiChannelFilterChain<float> * range_filter_;
+  filters::MultiChannelFilterChain<float> * intensity_filter_;
+  
+};
+
+
+
+
+
+}
+
+
+#endif //LASER_SCAN_UTILS_LASERSCAN_H

src/laser_filters/include/laser_filters/point_cloud_footprint_filter.h

@@ -0,0 +1,133 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef LASER_SCAN_FOOTPRINT_FILTER_H
+#define LASER_SCAN_FOOTPRINT_FILTER_H
+/**
+\author Tully Foote
+@b ScanFootprintFilter takes input scans and corrects for footprint angle assuming a flat target.  
+This is useful for ground plane extraction
+
+**/
+
+#include "laser_geometry/laser_geometry.h"
+#include "filters/filter_base.h"
+#include "tf/transform_listener.h"
+#include "sensor_msgs/PointCloud.h"
+#include "ros/ros.h"
+
+namespace laser_filters
+{
+
+class PointCloudFootprintFilter : public filters::FilterBase<sensor_msgs::PointCloud>
+{
+public:
+  PointCloudFootprintFilter() {
+    ROS_WARN("PointCloudFootprintFilter has been deprecated.  Please use PR2PointCloudFootprintFilter instead.\n");
+  }
+
+  bool configure()
+  {
+    if(!getParam("inscribed_radius", inscribed_radius_))
+    {
+      ROS_ERROR("PointCloudFootprintFilter needs inscribed_radius to be set");
+      return false;
+    }
+    return true;
+  }
+
+  virtual ~PointCloudFootprintFilter()
+  { 
+
+  }
+
+  bool update(const sensor_msgs::PointCloud& input_scan, sensor_msgs::PointCloud& filtered_scan)
+  {
+    if(&input_scan == &filtered_scan){
+      ROS_ERROR("This filter does not currently support in place copying");
+      return false;
+    }
+    sensor_msgs::PointCloud laser_cloud;
+
+    try{
+      tf_.transformPointCloud("base_link", input_scan, laser_cloud);
+    }
+    catch(tf::TransformException& ex){
+      ROS_ERROR("Transform unavailable %s", ex.what());
+      return false;
+    }
+
+    filtered_scan.header = input_scan.header;
+    filtered_scan.points.resize (input_scan.points.size());
+    filtered_scan.channels.resize (input_scan.channels.size());
+    for (unsigned int d = 0; d < input_scan.channels.size (); d++){
+      filtered_scan.channels[d].values.resize  (input_scan.points.size());
+      filtered_scan.channels[d].name = input_scan.channels[d].name;
+    }
+
+    int num_pts = 0;
+    for (unsigned int i=0; i < laser_cloud.points.size(); i++)  
+    {
+      if (!inFootprint(laser_cloud.points[i])){
+        filtered_scan.points[num_pts] = input_scan.points[i];
+        for (unsigned int d = 0; d < filtered_scan.channels.size (); d++)
+          filtered_scan.channels[d].values[num_pts] = input_scan.channels[d].values[i];
+        num_pts++;
+      }
+    }
+
+    // Resize output vectors
+    filtered_scan.points.resize (num_pts);
+    for (unsigned int d = 0; d < filtered_scan.channels.size (); d++)
+      filtered_scan.channels[d].values.resize (num_pts);
+
+    return true;
+  }
+
+
+  bool inFootprint(const geometry_msgs::Point32& scan_pt){
+    if(scan_pt.x < -1.0 * inscribed_radius_ || scan_pt.x > inscribed_radius_ || scan_pt.y < -1.0 * inscribed_radius_ || scan_pt.y > inscribed_radius_)
+      return false;
+    return true;
+  }
+
+private:
+  tf::TransformListener tf_;
+  laser_geometry::LaserProjection projector_;
+  double inscribed_radius_;
+} ;
+
+}
+
+#endif // LASER_SCAN_FOOTPRINT_FILTER_H

src/laser_filters/include/laser_filters/polygon_filter.h

@@ -0,0 +1,92 @@
+/*
+ *  Software License Agreement (BSD License)
+ *
+ *  Robot Operating System code by Eurotec B.V.
+ *  Copyright (c) 2020, Eurotec B.V.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1. Redistributions of source code must retain the above
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer.
+ *
+ *   2. Redistributions in binary form must reproduce the above
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer in the documentation and/or other materials provided
+ *      with the distribution.
+ *
+ *   3. Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived
+ *      from this software without specific prior written permission.
+ *
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ *  TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ *  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ *  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ *
+ *  polygon_filter.h
+ */
+
+#ifndef POLYGON_FILTER_H
+#define POLYGON_FILTER_H
+
+#include <filters/filter_base.h>
+
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <laser_geometry/laser_geometry.h>
+#include <geometry_msgs/Polygon.h>
+#include <geometry_msgs/PolygonStamped.h>
+#include <laser_filters/PolygonFilterConfig.h>
+#include <dynamic_reconfigure/server.h>
+
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+namespace laser_filters
+{
+/**
+ * @brief This is a filter that removes points in a laser scan inside of a polygon.
+ */
+class LaserScanPolygonFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+  LaserScanPolygonFilter();
+  bool configure();
+
+  bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan);
+
+private:
+  // configuration
+  ros::Publisher polygon_pub_;
+  std::shared_ptr<dynamic_reconfigure::Server<laser_filters::PolygonFilterConfig>> dyn_server_;
+  void reconfigureCB(laser_filters::PolygonFilterConfig& config, uint32_t level);
+  boost::recursive_mutex own_mutex_;
+  std::string polygon_frame_;
+  geometry_msgs::Polygon polygon_;
+  double polygon_padding_;
+  bool invert_filter_;
+
+  // checks if points in polygon
+  bool inPolygon(tf::Point& point) const;
+
+  laser_geometry::LaserProjection projector_;
+
+  // tf listener to transform scans into the polygon_frame
+  tf::TransformListener tf_;
+};
+}
+#endif /* polygon_filter.h */

src/laser_filters/include/laser_filters/range_filter.h

@@ -0,0 +1,117 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013, JSK (The University of Tokyo).
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef LASER_SCAN_RANGE_FILTER_H
+#define LASER_SCAN_RANGE_FILTER_H
+/**
+\author Yohei Kakiuchi
+@b ScanRangeFilter takes input scans and filters within indicated range.
+**/
+
+
+#include "filters/filter_base.h"
+#include "sensor_msgs/LaserScan.h"
+
+namespace laser_filters
+{
+
+class LaserScanRangeFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+
+  double lower_threshold_ ;
+  double upper_threshold_ ;
+  bool use_message_range_limits_ ;
+  float lower_replacement_value_ ;
+  float upper_replacement_value_ ;
+
+  bool configure()
+  {
+    use_message_range_limits_ = false;
+    getParam("use_message_range_limits", use_message_range_limits_);
+
+    // work around the not implemented getParam(std::string name, float& value) method
+    double temp_replacement_value = std::numeric_limits<double>::quiet_NaN();
+    getParam("lower_replacement_value", temp_replacement_value);
+    lower_replacement_value_ = static_cast<float>(temp_replacement_value);
+
+    // work around the not implemented getParam(std::string name, float& value) method
+    temp_replacement_value = std::numeric_limits<double>::quiet_NaN();
+    getParam("upper_replacement_value", temp_replacement_value);
+    upper_replacement_value_ = static_cast<float>(temp_replacement_value);
+
+
+    lower_threshold_ = 0.0;
+    upper_threshold_ = 100000.0;
+    getParam("lower_threshold", lower_threshold_);
+    getParam("upper_threshold", upper_threshold_) ;
+    return true;
+  }
+
+  virtual ~LaserScanRangeFilter()
+  {
+
+  }
+
+  bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
+  {
+    if (use_message_range_limits_)
+    {
+      lower_threshold_ = input_scan.range_min;
+      upper_threshold_ = input_scan.range_max;
+    }
+    filtered_scan = input_scan;
+    for (unsigned int i=0;
+         i < input_scan.ranges.size();
+         i++) // Need to check ever reading in the current scan
+    {
+
+      if (filtered_scan.ranges[i] <= lower_threshold_)
+      {
+        filtered_scan.ranges[i] = lower_replacement_value_;
+
+      }
+      else if (filtered_scan.ranges[i] >= upper_threshold_)
+      {
+        filtered_scan.ranges[i] = upper_replacement_value_;
+      }
+    }
+
+    return true;
+  }
+} ;
+
+}
+
+#endif // LASER_SCAN_RANGE_FILTER_H

src/laser_filters/include/laser_filters/scan_blob_filter.h

@@ -0,0 +1,158 @@
+/*
+ * Copyright (c) 2013 Kei Okada
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+/*
+  \author Kei OKada
+
+
+*/
+
+#ifndef LASER_SCAN_BLOB_FILTER_H
+#define LASER_SCAN_BLOB_FILTER_H
+
+#include <set>
+
+#include "filters/filter_base.h"
+#include <sensor_msgs/LaserScan.h>
+#include "angles/angles.h"
+
+namespace laser_filters{
+
+/** @b ScanBlobFilter is a simple filter that filters shadow points in a laser scan line 
+ */
+
+class ScanBlobFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+
+  double max_radius_;          // Filter angle threshold
+  int min_points_;
+    
+
+  ////////////////////////////////////////////////////////////////////////////////
+  ScanBlobFilter () 
+  {
+
+
+  }
+
+  /**@b Configure the filter from XML */
+  bool configure()
+  {
+    max_radius_ = 0.1;//default value
+    if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("max_radius"), max_radius_))
+    {
+      ROS_ERROR("Error: BlobFilter was not given min_radius.\n");
+      return false;
+    }
+
+    min_points_ = 5;//default value
+    if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("min_points"), min_points_))
+    {
+      ROS_INFO("Error: BlobFilter was not given min_points.\n");
+      return false;
+    }
+    return true;
+  }
+
+  ////////////////////////////////////////////////////////////////////////////////
+  virtual ~ScanBlobFilter () { }
+
+  ////////////////////////////////////////////////////////////////////////////////
+  /** \brief 
+   * \param scan_in the input LaserScan message
+   * \param scan_out the output LaserScan message
+   */
+  bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
+  {
+    //copy across all data first
+    scan_out = scan_in;
+
+    std::set<int> indices_to_publish;
+    // assume that all points is pass thorugh shadow filter, so each blob is separeted by invalide scan data
+    std::vector<std::vector<int> > range_blobs;
+    std::vector<int> range_blob;
+    for (unsigned int i = 0; i < scan_in.ranges.size (); i++)
+    {
+      scan_out.ranges[i] = -1.0 * fabs(scan_in.ranges[i]); // set all ranges to invalid (*)
+      if ( scan_in.ranges[i] < 0 || std::isnan(scan_in.ranges[i])) {
+          if ( range_blob.size() > min_points_ ) {
+              range_blobs.push_back(range_blob);
+          }
+          range_blob.clear();
+      }else{
+          range_blob.push_back(i);
+      }
+    }
+    if ( range_blob.size() > min_points_ ) {
+        range_blobs.push_back(range_blob);
+    }
+    // for each blob calculate center and radius
+    for (unsigned int i = 0; i < range_blobs.size(); i++) {
+        int size = range_blobs[i].size();
+        // check center of blob
+        double center_x = 0, center_y = 0;
+        for (unsigned int j = 0; j < size; j++) {
+            double x = scan_in.ranges[range_blobs[i][j]];
+            double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment;
+            center_x += x;
+            center_y += y;
+        }
+        center_x /= size;
+        center_y /= size;
+
+        // check range of blob
+        double radius = 0;
+        for (unsigned int j = 0; j < size; j++) {
+            double x = scan_in.ranges[range_blobs[i][j]];
+            double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment;
+            if ( radius < fabs(center_x - x) ) radius = fabs(center_x - x) ;
+            if ( radius < fabs(center_y - y) ) radius = fabs(center_y - y) ;
+        }
+
+        ROS_DEBUG_STREAM("blob center " << center_x << " " << center_y << ", radius " << radius << ", num of ponits " << size);
+        if ( radius < max_radius_ ) {
+            indices_to_publish.insert(range_blobs[i][0] + size/2);
+        }
+    }
+    ROS_DEBUG("ScanBlobFilter  %d Points from scan with min radius: %.2f, num of pints: %d", (int)indices_to_publish.size(), max_radius_, min_points_);
+    for ( std::set<int>::iterator it = indices_to_publish.begin(); it != indices_to_publish.end(); ++it)
+      {
+	scan_out.ranges[*it] = fabs(scan_in.ranges[*it]); // valid only the ranges that passwd the test (*)
+      }
+    return true;
+  }
+
+  ////////////////////////////////////////////////////////////////////////////////
+
+} ;
+}
+
+#endif //LASER_SCAN_BLOB_FILTER_H

src/laser_filters/include/laser_filters/scan_mask_filter.h

@@ -0,0 +1,130 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2017, laser_filters authors
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef LASER_FILTERS_SCAN_MASK_FILTER_H
+#define LASER_FILTERS_SCAN_MASK_FILTER_H
+/**
+\author Atsushi Watanabe (SEQSENSE, Inc.)
+\brief LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
+**/
+
+
+#include "filters/filter_base.h"
+#include "sensor_msgs/LaserScan.h"
+
+#include <XmlRpcException.h>
+
+#include <limits>
+#include <map>
+#include <string>
+#include <vector>
+
+namespace laser_filters
+{
+
+class LaserScanMaskFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+  std::map<std::string, std::vector<size_t> > masks_;
+
+  bool configure()
+  {
+    XmlRpc::XmlRpcValue config;
+    if (!getParam("masks", config))
+    {
+      ROS_ERROR("LaserScanMaskFilter: masks is not defined in the config.");
+      return false;
+    }
+    if (config.getType() == XmlRpc::XmlRpcValue::TypeArray)
+    {
+      ROS_ERROR("LaserScanMaskFilter: masks must be an array of frame_ids with direction list.");
+      return false;
+    }
+    for (XmlRpc::XmlRpcValue::iterator it = config.begin();
+        it != config.end(); ++it)
+    {
+      if (it->second.getType() == XmlRpc::XmlRpcValue::TypeArray)
+      {
+        std::string frame_id = (std::string)(it->first);
+        masks_[frame_id] = std::vector<size_t>();
+        try
+        {
+          for (size_t i = 0; i < it->second.size(); ++i)
+          {
+            size_t id = static_cast<int>(it->second[i]);
+            masks_[frame_id].push_back(id);
+          }
+          ROS_INFO("LaserScanMaskFilter: %s: %d directions will be masked.",
+              frame_id.c_str(), (int)masks_[frame_id].size());
+        }
+        catch(XmlRpc::XmlRpcException &e)
+        {
+          ROS_ERROR("LaserScanMaskFilter: %s", e.getMessage().c_str());
+          return false;
+        }
+      }
+    }
+    return true;
+  }
+
+  virtual ~LaserScanMaskFilter()
+  {
+  }
+
+  bool update(const sensor_msgs::LaserScan& data_in, sensor_msgs::LaserScan& data_out)
+  {
+    data_out = data_in;
+    if (masks_.find(data_out.header.frame_id) == masks_.end())
+    {
+      ROS_WARN("LaserScanMaskFilter: frame_id %s is not registered.",
+          data_out.header.frame_id.c_str());
+      return true;
+    }
+
+    const std::vector<size_t> &mask = masks_[data_out.header.frame_id];
+    const size_t len = data_out.ranges.size();
+    for (std::vector<size_t>::const_iterator it = mask.begin();
+        it != mask.end(); ++it)
+    {
+      if (*it > len) continue;
+      data_out.ranges[*it] = std::numeric_limits<float>::quiet_NaN();
+    }
+
+    return true;
+  }
+};
+
+}  // namespace laser_filters
+
+#endif  // LASER_FILTERS_SCAN_MASK_FILTER_H

src/laser_filters/include/laser_filters/scan_shadow_detector.h

@@ -0,0 +1,81 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2017, laser_filters authors
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/*
+\author Atsushi Watanabe (SEQSENSE, Inc.)
+*/
+
+#ifndef SCAN_SHADOW_DETECTOR_H
+#define SCAN_SHADOW_DETECTOR_H
+
+namespace laser_filters
+{
+class ScanShadowDetector
+{
+public:
+  float min_angle_tan_, max_angle_tan_;  // Filter angle thresholds
+
+  void configure(const float min_angle, const float max_angle)
+  {
+    min_angle_tan_ = tanf(min_angle);
+    max_angle_tan_ = tanf(max_angle);
+
+    // Correct sign of tan around singularity points
+    if (min_angle_tan_ < 0.0)
+      min_angle_tan_ = -min_angle_tan_;
+    if (max_angle_tan_ > 0.0)
+      max_angle_tan_ = -max_angle_tan_;
+  }
+  bool isShadow(const float r1, const float r2, const float included_angle)
+  {
+    const float perpendicular_y_ = r2 * sinf(included_angle);
+    const float perpendicular_x_ = r1 - r2 * cosf(included_angle);
+    const float perpendicular_tan_ = fabs(perpendicular_y_) / perpendicular_x_;
+
+    if (perpendicular_tan_ > 0)
+    {
+      if (perpendicular_tan_ < min_angle_tan_)
+        return true;
+    }
+    else
+    {
+      if (perpendicular_tan_ > max_angle_tan_)
+        return true;
+    }
+    return false;
+  }
+};
+}
+
+#endif  //SCAN_SHADOW_DETECTOR_H

src/laser_filters/include/laser_filters/scan_shadows_filter.h

@@ -0,0 +1,217 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu@cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/*
+  \author Radu Bogdan Rusu <rusu@cs.tum.edu> Tully Foote <tfoote@willowgarage.com>
+
+
+*/
+
+#ifndef LASER_SCAN_SHADOWS_FILTER_H
+#define LASER_SCAN_SHADOWS_FILTER_H
+
+#include <set>
+
+#include "filters/filter_base.h"
+#include "laser_filters/scan_shadow_detector.h"
+#include <sensor_msgs/LaserScan.h>
+#include <angles/angles.h>
+#include <laser_filters/ScanShadowsFilterConfig.h>
+#include <dynamic_reconfigure/server.h>
+#include <ros/ros.h>
+
+namespace laser_filters
+{
+/** @b ScanShadowsFilter is a simple filter that filters shadow points in a laser scan line 
+ */
+
+class ScanShadowsFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+  double laser_max_range_;        // Used in laser scan projection
+  double min_angle_, max_angle_;  // Filter angle threshold
+  int window_, neighbors_;
+  bool remove_shadow_start_point_;  // Whether to also remove the start point of the shadow
+
+  ScanShadowDetector shadow_detector_;
+
+  std::shared_ptr<dynamic_reconfigure::Server<laser_filters::ScanShadowsFilterConfig>> dyn_server_;
+  boost::recursive_mutex own_mutex_;
+  ScanShadowsFilterConfig param_config;
+
+  ////////////////////////////////////////////////////////////////////////////////
+  ScanShadowsFilter()
+  {
+  }
+
+  /**@b Configure the filter from XML */
+  bool configure()
+  {
+    ros::NodeHandle private_nh("~" + getName());
+    dyn_server_.reset(new dynamic_reconfigure::Server<laser_filters::ScanShadowsFilterConfig>(own_mutex_, private_nh));
+    dynamic_reconfigure::Server<laser_filters::ScanShadowsFilterConfig>::CallbackType f;
+    f = boost::bind(&laser_filters::ScanShadowsFilter::reconfigureCB, this, _1, _2);
+    dyn_server_->setCallback(f);
+
+    if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("min_angle"), min_angle_))
+    {
+      ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n");
+      return false;
+    }
+    if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("max_angle"), max_angle_))
+    {
+      ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n");
+      return false;
+    }
+    if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("window"), window_))
+    {
+      ROS_ERROR("Error: ShadowsFilter was not given window.\n");
+      return false;
+    }
+    neighbors_ = 0;  // default value
+    if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("neighbors"), neighbors_))
+    {
+      ROS_INFO("Error: ShadowsFilter was not given neighbors.\n");
+    }
+    remove_shadow_start_point_ = false;  // default value
+    filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("remove_shadow_start_point"), remove_shadow_start_point_);
+    ROS_INFO("Remove shadow start point: %s", remove_shadow_start_point_ ? "true" : "false");
+
+    if (min_angle_ < 0)
+    {
+      ROS_ERROR("min_angle must be 0 <= min_angle. Forcing min_angle = 0.\n");
+      min_angle_ = 0.0;
+    }
+    if (90 < min_angle_)
+    {
+      ROS_ERROR("min_angle must be min_angle <= 90. Forcing min_angle = 90.\n");
+      min_angle_ = 90.0;
+    }
+    if (max_angle_ < 90)
+    {
+      ROS_ERROR("max_angle must be 90 <= max_angle. Forcing max_angle = 90.\n");
+      max_angle_ = 90.0;
+    }
+    if (180 < min_angle_)
+    {
+      ROS_ERROR("max_angle must be max_angle <= 180. Forcing max_angle = 180.\n");
+      max_angle_ = 180.0;
+    }
+
+    shadow_detector_.configure(
+        angles::from_degrees(min_angle_),
+        angles::from_degrees(max_angle_));
+
+    param_config.min_angle = min_angle_;
+    param_config.max_angle = max_angle_;
+    param_config.window = window_;
+    param_config.neighbors = neighbors_;
+    param_config.remove_shadow_start_point = remove_shadow_start_point_;
+    dyn_server_->updateConfig(param_config);
+
+    return true;
+  }
+
+  void reconfigureCB(ScanShadowsFilterConfig& config, uint32_t level)
+  {
+    min_angle_ = config.min_angle;
+    max_angle_ = config.max_angle;
+    shadow_detector_.configure(
+        angles::from_degrees(min_angle_),
+        angles::from_degrees(max_angle_));
+    neighbors_ = config.neighbors;
+    window_ = config.window;
+    remove_shadow_start_point_ = config.remove_shadow_start_point;
+  }
+
+  ////////////////////////////////////////////////////////////////////////////////
+  virtual ~ScanShadowsFilter()
+  {
+  }
+
+  ////////////////////////////////////////////////////////////////////////////////
+  /** \brief Filter shadow points based on 3 global parameters: min_angle, max_angle
+   * and window. {min,max}_angle specify the allowed angle interval (in degrees)
+   * between the created lines (see getAngleWithViewPoint). Window specifies how many
+   * consecutive measurements to take into account for one point.
+   * \param scan_in the input LaserScan message
+   * \param scan_out the output LaserScan message
+   */
+  bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
+  {
+    boost::recursive_mutex::scoped_lock lock(own_mutex_);
+
+    // copy across all data first
+    scan_out = scan_in;
+
+    std::set<int> indices_to_delete;
+    // For each point in the current line scan
+    for (unsigned int i = 0; i < scan_in.ranges.size(); i++)
+    {
+      for (int y = -window_; y < window_ + 1; y++)
+      {
+        int j = i + y;
+        if (j < 0 || j >= (int)scan_in.ranges.size() || (int)i == j)
+        {  // Out of scan bounds or itself
+          continue;
+        }
+
+        if (shadow_detector_.isShadow(
+                scan_in.ranges[i], scan_in.ranges[j], y * scan_in.angle_increment))
+        {
+          for (int index = std::max<int>(i - neighbors_, 0); index <= std::min<int>(i + neighbors_, (int)scan_in.ranges.size() - 1); index++)
+          {
+            if (scan_in.ranges[i] < scan_in.ranges[index])
+            {  // delete neighbor if they are farther away (note not self)
+              indices_to_delete.insert(index);
+            }
+          }
+          if (remove_shadow_start_point_)
+          {
+            indices_to_delete.insert(i);
+          }
+        }
+      }
+    }
+
+    ROS_DEBUG("ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, and window: %d",
+              (int)indices_to_delete.size(), min_angle_, max_angle_, neighbors_, window_);
+    for (std::set<int>::iterator it = indices_to_delete.begin(); it != indices_to_delete.end(); ++it)
+    {
+      scan_out.ranges[*it] = std::numeric_limits<float>::quiet_NaN();  // Failed test to set the ranges to invalid value
+    }
+    return true;
+  }
+
+  ////////////////////////////////////////////////////////////////////////////////
+};
+}
+
+#endif  // LASER_SCAN_SHADOWS_FILTER_H

src/laser_filters/include/laser_filters/speckle_filter.h

@@ -0,0 +1,172 @@
+/*
+ *  Software License Agreement (BSD License)
+ *
+ *  Robot Operating System code by Eurotec B.V.
+ *  Copyright (c) 2020, Eurotec B.V.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1. Redistributions of source code must retain the above
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer.
+ *
+ *   2. Redistributions in binary form must reproduce the above
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer in the documentation and/or other materials provided
+ *      with the distribution.
+ *
+ *   3. Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived
+ *      from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ *  TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ *  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ *  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  speckle_filter.h
+ */
+
+#ifndef SPECKLE_FILTER_H
+#define SPECKLE_FILTER_H
+
+#include <dynamic_reconfigure/server.h>
+#include <filters/filter_base.h>
+#include <laser_filters/SpeckleFilterConfig.h>
+#include <sensor_msgs/LaserScan.h>
+
+namespace laser_filters
+{
+
+class WindowValidator
+{
+public:
+  virtual bool checkWindowValid(const sensor_msgs::LaserScan& scan, size_t idx, size_t window, double max_range_difference) = 0;
+};
+
+class DistanceWindowValidator : public WindowValidator
+{
+  virtual bool checkWindowValid(const sensor_msgs::LaserScan& scan, size_t idx, size_t window, double max_range_difference)
+  {
+    const float& range = scan.ranges[idx];
+    if (range != range) {
+      return false;
+    }
+
+    for (size_t neighbor_idx_nr = 1; neighbor_idx_nr < window; ++neighbor_idx_nr)
+    {
+      size_t neighbor_idx = idx + neighbor_idx_nr;
+      if (neighbor_idx < scan.ranges.size())  // Out of bound check
+      {
+        const float& neighbor_range = scan.ranges[neighbor_idx];
+        if (neighbor_range != neighbor_range || fabs(neighbor_range - range) > max_range_difference)
+        {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
+};
+
+class RadiusOutlierWindowValidator : public WindowValidator
+{
+  virtual bool checkWindowValid(const sensor_msgs::LaserScan& scan, size_t idx, size_t window, double max_distance)
+  {
+    int num_neighbors = 0;
+    const float& r1 = scan.ranges[idx];
+    float r2 = 0.;
+
+    // Look around the current point until either the window is exceeded
+    // or the number of neighbors was found.
+    for (int y = -(int)window; y < (int)window + 1 && num_neighbors < (int)window; y++)
+    {
+      int j = idx + y;
+      r2 = scan.ranges[j];
+
+      if (j < 0 || j >= static_cast<int>(scan.ranges.size()) || idx == j || std::isnan(r2))
+      {  // Out of scan bounds or itself or infinity
+        continue;
+      }
+
+      // Explanation:
+      //
+      // Distance between two points:
+      // d² = (x2 - x1)² + (y2 - y1)²
+      //
+      // Substitute x with r * cos(phi) and y with r * sin(phi):
+      // d² = (r2 * cos(phi2) - r1 * cos(phi1))² + (r2 * sin(phi2) - r1 * sin(phi1))²
+      //
+      // Apply binomial theorem:
+      // d² = ((r2² * cos(phi2)² + r1² * cos(phi1)² - 2 * r1 * r2 * cos(phi1) * cos(phi2)) +
+      //      ((r2² * sin(phi2)² + r1² * sin(phi1)² - 2 * r1 * r2 * sin(phi1) * sin(phi2))
+      //
+      // Merge sums:
+      // d² = r2² * (cos(phi2)² + sin(phi2)²) + r1² * (cos(phi1)² + sin(phi1)² -
+      //      2 * r1 * r2 * (cos(phi1) * cos(phi2) + sin(phi1) * sin(phi2))
+      //
+      // Apply cos² + sin² = 1:
+      // d² = r2² + r1² - 2 * r1 * r2 * (cos(phi1) * cos(phi2) + sin(phi1) * sin(phi2))
+      //
+      // Note the following:
+      // cos(phi1) * cos(phi2) = 1/2 * (cos(phi1 - phi2) + cos(phi1 + phi2))
+      // sin(phi1) * sin(phi2) = 1/2 * (cos(phi1 - phi2) - cos(phi1 + phi2))
+      //
+      // cos(phi1) * cos(phi2) + sin(phi1) * sin(phi2) = cos(phi1 - phi2)
+      //
+      // Finally, phi1 - phi2 is our included_angle.
+
+      const float d = sqrt(
+            pow(r1,2) + pow(r2,2) -
+            (2 * r1 * r2 * cosf(y * scan.angle_increment)));
+
+
+      if (d <= max_distance)
+      {
+        num_neighbors++;
+      }
+    }
+
+    // consider the window to be the number of neighbors we need
+    if (num_neighbors < window)
+    {
+      return false;
+    }
+    else
+    {
+      return true;
+    }
+  }
+};
+
+/**
+ * @brief This is a filter that removes speckle points in a laser scan based on consecutive ranges
+ */
+class LaserScanSpeckleFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+public:
+  LaserScanSpeckleFilter();
+  ~LaserScanSpeckleFilter();
+  bool configure();
+  bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan);
+
+private:
+  std::shared_ptr<dynamic_reconfigure::Server<laser_filters::SpeckleFilterConfig>> dyn_server_;
+  void reconfigureCB(laser_filters::SpeckleFilterConfig& config, uint32_t level);
+  boost::recursive_mutex own_mutex_;
+
+  SpeckleFilterConfig config_ = SpeckleFilterConfig::__getDefault__();
+  WindowValidator* validator_;
+};
+}
+#endif /* speckle_filter.h */

src/laser_filters/laser_filters_plugins.xml

@@ -0,0 +1,97 @@
+<class_libraries>
+  <library path="lib/liblaser_scan_filters">
+    <class name="laser_filters/LaserArrayFilter" type="laser_filters::LaserArrayFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	This is a filter which runs two internal MultiChannelFilterChain filters on the range and intensity measurements.  
+      </description>
+    </class>
+    <class name="laser_filters/LaserScanIntensityFilter" type="laser_filters::LaserScanIntensityFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	This is a filter which filters sensor_msgs::LaserScan messages based on intensity
+      </description>
+    </class>
+    <class name="laser_filters/LaserScanRangeFilter" type="laser_filters::LaserScanRangeFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	This is a filter which filters sensor_msgs::LaserScan messages based on range
+      </description>
+    </class>
+    <class name="laser_filters/ScanShadowsFilter" type="laser_filters::ScanShadowsFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	This is a filter which filters points from a laser scan that look like the veiling effect.
+      </description>
+    </class>
+    <class name="laser_filters/InterpolationFilter" type="laser_filters::InterpolationFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+      This is a filter that will generate range readings for error readings in a scan by interpolating between valid readings on either side of the error
+      </description>
+    </class>
+    <class name="laser_filters/LaserScanAngularBoundsFilter" type="laser_filters::LaserScanAngularBoundsFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	 This is a filter that removes points in a laser scan outside of certain angular bounds.
+      </description>
+    </class>
+    <class name="laser_filters/LaserScanAngularBoundsFilterInPlace" type="laser_filters::LaserScanAngularBoundsFilterInPlace" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	 This is a filter that removes points in a laser scan inside of certain angular bounds.
+      </description>
+    </class>
+     <class name="laser_filters/LaserScanBoxFilter" type="laser_filters::LaserScanBoxFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	 This is a filter that removes points in a laser scan inside of a cartesian box.
+      </description>
+    </class>
+      <class name="laser_filters/LaserScanPolygonFilter" type="laser_filters::LaserScanPolygonFilter"
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	This is a filter that removes points in a laser scan inside of a polygon.
+      </description>
+    </class>
+    <class name="laser_filters/LaserScanSpeckleFilter" type="laser_filters::LaserScanSpeckleFilter"
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+ 	This is a filter that removes speckle points in a laser scan by looking at neighbor points.
+      </description>
+    </class>
+    <class name="laser_filters/LaserScanMaskFilter" type="laser_filters::LaserScanMaskFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	 This is a filter that removes points on directions defined in a mask from a laser scan.
+      </description>
+    </class>
+    <class name="laser_filters/LaserMedianFilter" type="laser_filters::LaserMedianFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	DEPRECATED: This is a median filter which filters sensor_msgs::LaserScan messages.  
+      </description>
+    </class>
+    <class name="laser_filters/LaserScanFootprintFilter" type="laser_filters::LaserScanFootprintFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+	DEPRECATED: This is a filter which filters points out of a laser scan which are inside the inscribed radius.
+      </description>
+    </class>
+    <class name="laser_filters/ScanBlobFilter"
+           type="laser_filters::ScanBlobFilter"
+               base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+        This is a filter which extract blob object (human's foot, chair's foot) from a laser.
+      </description>
+    </class>
+  </library>
+  <library path="lib/libpointcloud_filters">
+    <class name="laser_filters/PointCloudFootprintFilter" type="laser_filters::PointCloudFootprintFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::PointCloud>">
+      <description>
+	DEPRECATED: Remove points from the pointcloud inside the robot base. 
+      </description>
+    </class>
+  </library>
+</class_libraries>

src/laser_filters/launch/laser_scan_angle_filter.launch

@@ -0,0 +1,5 @@
+<launch>
+<node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter">
+      <rosparam command="load" file="$(find laser_filters)/launch/laser_scan_angle_filter.yaml" />
+</node>
+</launch>

src/laser_filters/launch/laser_scan_angle_filter.yaml

@@ -0,0 +1,6 @@
+scan_filter_chain:
+  - name: angle
+    type: laser_filters/LaserScanAngularBoundsFilter
+    params:
+      lower_angle: -1.57
+      upper_angle: 1.57

src/laser_filters/mainpage.dox

@@ -0,0 +1,25 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+This package is designed to provide filters for working with laser scans.
+
+\section filters Laser Scan Filters 
+
+There are a number of filters that this class provides.  There is also
+a factory class to provide easy creation and chaining of laser scan
+filters as a input to any process.
+
+\subsection median Median Filter
+laser_scan::LaserMedianFilter applies a median filter to scans both in range and intensity.  
+
+\subsection mean Mean Filter
+\todo Document
+
+\subsection shadows Scan Shadows Filter
+\todo document
+
+\subsection intensity Intensity Filter
+\todo document
+
+*/

src/laser_filters/package.xml

@@ -0,0 +1,41 @@
+<package>
+  <name>laser_filters</name>
+  <description>
+    Assorted filters designed to operate on 2D planar laser scanners,
+    which use the sensor_msgs/LaserScan type.
+  </description>
+  <version>1.8.11</version>
+  <maintainer email="jon.binney@gmail.com">Jon Binney</maintainer>
+  <author>Tully Foote</author>
+  <license>BSD</license>
+
+  <url>http://ros.org/wiki/laser_filters</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>filters</build_depend>
+  <build_depend>message_filters</build_depend>
+  <build_depend>laser_geometry</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>rostest</build_depend>
+  <build_depend>angles</build_depend>
+
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>filters</run_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>laser_geometry</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>angles</run_depend>
+
+  <export>
+    <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags=""/>
+    <filters plugin="${prefix}/laser_filters_plugins.xml"/>
+  </export>
+</package>

src/laser_filters/src/array_filter.cpp

@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu@cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ *
+ */
+
+#include "laser_filters/array_filter.h"
+
+namespace laser_filters
+{
+LaserArrayFilter::LaserArrayFilter() :
+  num_ranges_(1), range_filter_(NULL), intensity_filter_(NULL)
+{
+  
+};
+
+bool LaserArrayFilter::configure()
+{
+ 
+  bool found_range_config = getParam("range_filter_chain", range_config_);
+  bool found_intensity_config = getParam("intensity_filter_chain", intensity_config_);
+ 
+  if (!found_range_config && !found_intensity_config)
+  {
+    ROS_ERROR("Cannot Configure LaserArrayFilter: Didn't find \"range_filter\" or \"intensity _filter\" tag within LaserArrayFilter params. Filter definitions needed inside for processing range and intensity");
+    return false;
+  }
+  
+  if (range_filter_)
+    delete range_filter_;
+
+  if (intensity_filter_)
+    delete intensity_filter_;
+  
+  if (found_range_config)
+  {
+    range_filter_ = new filters::MultiChannelFilterChain<float>("float");
+    if (!range_filter_->configure(num_ranges_, range_config_))
+      return false;
+  }
+
+  if (found_intensity_config)
+  {
+    intensity_filter_ = new filters::MultiChannelFilterChain<float>("float");
+    if (!intensity_filter_->configure(num_ranges_, intensity_config_))
+      return false;
+  }
+  
+  return true;
+};
+
+LaserArrayFilter::~LaserArrayFilter()
+{
+  if (range_filter_)
+    delete range_filter_;
+
+  if (intensity_filter_)
+    delete intensity_filter_;
+};
+
+bool LaserArrayFilter::update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
+{
+  if (!this->configured_) 
+  {
+    ROS_ERROR("LaserArrayFilter not configured");
+    return false;
+  }
+
+  boost::mutex::scoped_lock lock(data_lock);
+  scan_out = scan_in; ///Quickly pass through all data \todo don't copy data too
+
+  if (scan_in.ranges.size() != num_ranges_) //Reallocating
+  {
+    num_ranges_ = scan_in.ranges.size();
+
+    ROS_INFO("LaserArrayFilter cleaning and reallocating due to larger scan size");
+    
+    configure();
+  }
+
+  /** \todo check for length of intensities too */
+  range_filter_->update(scan_in.ranges, scan_out.ranges);
+  intensity_filter_->update(scan_in.intensities, scan_out.intensities);
+
+
+  return true;
+}
+}

src/laser_filters/src/box_filter.cpp

@@ -0,0 +1,207 @@
+/*
+ *  Software License Agreement (BSD License)
+ *
+ *  Robot Operating System code by the University of Osnabrück
+ *  Copyright (c) 2015, University of Osnabrück
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1. Redistributions of source code must retain the above 
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer.
+ *
+ *   2. Redistributions in binary form must reproduce the above 
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer in the documentation and/or other materials provided
+ *      with the distribution.
+ *
+ *   3. Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived
+ *      from this software without specific prior written permission.
+ *
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ *  TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ *  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 
+ *  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ *
+ *  box_filter.cpp
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ */
+
+#include "laser_filters/box_filter.h"
+#include <ros/ros.h>
+
+laser_filters::LaserScanBoxFilter::LaserScanBoxFilter(){
+
+}
+
+bool laser_filters::LaserScanBoxFilter::configure(){
+  up_and_running_ = true;
+  double min_x = 0, min_y = 0, min_z = 0, max_x = 0, max_y = 0, max_z = 0;
+  bool box_frame_set = getParam("box_frame", box_frame_);
+  bool x_max_set = getParam("max_x", max_x);
+  bool y_max_set = getParam("max_y", max_y);
+  bool z_max_set = getParam("max_z", max_z);
+  bool x_min_set = getParam("min_x", min_x);
+  bool y_min_set = getParam("min_y", min_y);
+  bool z_min_set = getParam("min_z", min_z);
+  bool invert_set = getParam("invert", invert_filter);
+  
+  ROS_INFO("BOX filter started");
+
+  max_.setX(max_x);
+  max_.setY(max_y);
+  max_.setZ(max_z);
+  min_.setX(min_x);
+  min_.setY(min_y);
+  min_.setZ(min_z);
+  
+  if(!box_frame_set){
+    ROS_ERROR("box_frame is not set!");
+  }
+  if(!x_max_set){
+    ROS_ERROR("max_x is not set!");
+  }
+  if(!y_max_set){
+    ROS_ERROR("max_y is not set!");
+  }
+  if(!z_max_set){
+    ROS_ERROR("max_z is not set!");
+  }
+  if(!x_min_set){
+    ROS_ERROR("min_x is not set!");
+  }
+  if(!y_min_set){
+    ROS_ERROR("min_y is not set!");
+  }
+  if(!z_min_set){
+    ROS_ERROR("min_z is not set!");
+  }
+  if(!invert_set){
+    ROS_INFO("invert filter not set, assuming false");
+    invert_filter=false;
+  }
+
+
+  return box_frame_set && x_max_set && y_max_set && z_max_set &&
+    x_min_set && y_min_set && z_min_set;
+
+}
+
+bool laser_filters::LaserScanBoxFilter::update(
+    const sensor_msgs::LaserScan& input_scan,
+    sensor_msgs::LaserScan &output_scan)
+{
+  output_scan = input_scan;
+  sensor_msgs::PointCloud2 laser_cloud;
+  
+  std::string error_msg;
+
+  bool success = tf_.waitForTransform(
+    box_frame_,
+    input_scan.header.frame_id,
+    input_scan.header.stamp + ros::Duration().fromSec(input_scan.ranges.size()*input_scan.time_increment),
+    ros::Duration(1.0),
+    ros::Duration(0.01),
+    &error_msg
+  );
+  if(!success){
+    ROS_WARN("Could not get transform, irgnoring laser scan! %s", error_msg.c_str());
+    return false;
+  }
+
+  try{
+    projector_.transformLaserScanToPointCloud(box_frame_, input_scan, laser_cloud, tf_);
+  }
+  catch(tf::TransformException& ex){
+    if(up_and_running_){
+      ROS_WARN_THROTTLE(1, "Dropping Scan: Tansform unavailable %s", ex.what());
+      return true;
+    }
+    else
+    {
+      ROS_INFO_THROTTLE(.3, "Ignoring Scan: Waiting for TF");
+    }
+    return false;
+  }
+  const int i_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "index");
+  const int x_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "x");
+  const int y_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "y");
+  const int z_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "z");
+
+  if(i_idx_c == -1 || x_idx_c == -1 || y_idx_c == -1 || z_idx_c == -1){
+      ROS_INFO_THROTTLE(.3, "x, y, z and index fields are required, skipping scan");
+
+  }
+
+
+  const int i_idx_offset = laser_cloud.fields[i_idx_c].offset;
+  const int x_idx_offset = laser_cloud.fields[x_idx_c].offset;
+  const int y_idx_offset = laser_cloud.fields[y_idx_c].offset;
+  const int z_idx_offset = laser_cloud.fields[z_idx_c].offset;
+
+  const int pstep = laser_cloud.point_step;
+  const long int pcount = laser_cloud.width * laser_cloud.height;
+  const long int limit = pstep * pcount;
+
+  int i_idx, x_idx, y_idx, z_idx;  
+  for(
+    i_idx = i_idx_offset,
+    x_idx = x_idx_offset,
+    y_idx = y_idx_offset,
+    z_idx = z_idx_offset;
+
+    x_idx < limit;
+
+    i_idx += pstep,
+    x_idx += pstep,
+    y_idx += pstep,
+    z_idx += pstep)
+  {
+
+    // TODO works only for float data types and with an index field
+    // I'm working on it, see https://github.com/ros/common_msgs/pull/78 
+    float x = *((float*)(&laser_cloud.data[x_idx]));
+    float y = *((float*)(&laser_cloud.data[y_idx]));
+    float z = *((float*)(&laser_cloud.data[z_idx]));
+    int index = *((int*)(&laser_cloud.data[i_idx]));
+
+    tf::Point point(x, y, z);
+
+    if(!invert_filter){
+      if(inBox(point)){
+        output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
+      }
+    }
+    else{
+      if(!inBox(point)){
+        output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
+      }
+    }
+
+  }
+  up_and_running_ = true;
+  return true;
+}
+
+bool laser_filters::LaserScanBoxFilter::inBox(tf::Point &point){
+  return
+    point.x() < max_.x() && point.x() > min_.x() && 
+    point.y() < max_.y() && point.y() > min_.y() &&
+    point.z() < max_.z() && point.z() > min_.z();
+}
+

src/laser_filters/src/generic_laser_filter_node.cpp

@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#include "ros/ros.h"
+#include "sensor_msgs/LaserScan.h"
+#include "message_filters/subscriber.h"
+#include "tf/message_filter.h"
+#include "tf/transform_listener.h"
+#include "filters/filter_chain.h"
+
+class GenericLaserScanFilterNode
+{
+protected:
+  // Our NodeHandle
+  ros::NodeHandle nh_;
+
+  // Components for tf::MessageFilter
+  tf::TransformListener tf_;
+  message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_;
+  tf::MessageFilter<sensor_msgs::LaserScan> tf_filter_;
+
+  // Filter Chain
+  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
+
+  // Components for publishing
+  sensor_msgs::LaserScan msg_;
+  ros::Publisher output_pub_;
+
+  ros::Timer deprecation_timer_;
+
+public:
+  // Constructor
+  GenericLaserScanFilterNode() :
+    scan_sub_(nh_, "scan_in", 50),
+    tf_filter_(scan_sub_, tf_, "base_link", 50),
+    filter_chain_("sensor_msgs::LaserScan")
+  {
+    // Configure filter chain
+    filter_chain_.configure("");
+    
+    // Setup tf::MessageFilter for input
+    tf_filter_.registerCallback(boost::bind(&GenericLaserScanFilterNode::callback, this, _1));
+    tf_filter_.setTolerance(ros::Duration(0.03));
+    
+    // Advertise output
+    output_pub_ = nh_.advertise<sensor_msgs::LaserScan>("output", 1000);
+
+    deprecation_timer_ = nh_.createTimer(ros::Duration(5.0), boost::bind(&GenericLaserScanFilterNode::deprecation_warn, this, _1));
+  }
+  
+  void deprecation_warn(const ros::TimerEvent& e)
+  {
+    ROS_WARN("'generic_laser_filter_node' has been deprecated.  Please switch to 'scan_to_scan_filter_chain'.");
+  }
+
+  // Callback
+  void callback(const sensor_msgs::LaserScan::ConstPtr& msg_in)
+  {
+    // Run the filter chain
+    filter_chain_.update (*msg_in, msg_);
+    
+    // Publish the output
+    output_pub_.publish(msg_);
+  }
+};
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "scan_filter_node");
+  
+  GenericLaserScanFilterNode t;
+  ros::spin();
+  
+  return 0;
+}

src/laser_filters/src/intensity_filter.cpp

@@ -0,0 +1,112 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2020, Eurotec B.V.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+*  \author Vijay Pradeep, Rein Appeldoorn
+*
+*********************************************************************/
+
+#include <laser_filters/intensity_filter.h>
+#include <ros/node_handle.h>
+
+namespace laser_filters
+{
+LaserScanIntensityFilter::LaserScanIntensityFilter()
+{
+}
+
+bool LaserScanIntensityFilter::configure()
+{
+  ros::NodeHandle private_nh("~" + getName());
+  dyn_server_.reset(new dynamic_reconfigure::Server<IntensityFilterConfig>(own_mutex_, private_nh));
+  dynamic_reconfigure::Server<IntensityFilterConfig>::CallbackType f;
+  f = boost::bind(&LaserScanIntensityFilter::reconfigureCB, this, _1, _2);
+  dyn_server_->setCallback(f);
+
+  getParam("lower_threshold", config_.lower_threshold);
+  getParam("upper_threshold", config_.upper_threshold);
+  getParam("invert", config_.invert);
+
+  getParam("filter_override_range", config_.filter_override_range);
+  getParam("filter_override_intensity", config_.filter_override_intensity);
+  dyn_server_->updateConfig(config_);
+  return true;
+}
+
+bool LaserScanIntensityFilter::update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
+{
+  filtered_scan = input_scan;
+
+  // Need to check ever reading in the current scan
+  for (unsigned int i=0; i < input_scan.ranges.size() && i < input_scan.intensities.size(); i++)
+  {
+    float& range = filtered_scan.ranges[i];
+    float& intensity = filtered_scan.intensities[i];
+
+    // Is this reading below our lower threshold?
+    // Is this reading above our upper threshold?
+    bool filter = intensity <= config_.lower_threshold || intensity >= config_.upper_threshold;
+    if (config_.invert)
+    {
+      filter = !filter;
+    }
+
+    if (filter)
+    {
+      if (config_.filter_override_range)
+      {
+        // If so, then make it an invalid value (NaN)
+        range = std::numeric_limits<float>::quiet_NaN();
+      }
+      if (config_.filter_override_intensity)
+      {
+        intensity = 0.0;  // Not intense
+      }
+    }
+    else
+    {
+      if (config_.filter_override_intensity)
+      {
+        intensity = 1.0;  // Intense
+      }
+    }
+  }
+
+  return true;
+}
+
+void LaserScanIntensityFilter::reconfigureCB(IntensityFilterConfig& config, uint32_t level)
+{
+  config_ = config;
+}
+}

src/laser_filters/src/laser_scan_filters.cpp

@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "laser_filters/median_filter.h"
+#include "laser_filters/array_filter.h"
+#include "laser_filters/intensity_filter.h"
+#include "laser_filters/range_filter.h"
+#include "laser_filters/scan_mask_filter.h"
+#include "laser_filters/scan_shadows_filter.h"
+#include "laser_filters/footprint_filter.h"
+#include "laser_filters/interpolation_filter.h"
+#include "laser_filters/angular_bounds_filter.h"
+#include "laser_filters/angular_bounds_filter_in_place.h"
+#include "laser_filters/box_filter.h"
+#include "laser_filters/polygon_filter.h"
+#include "laser_filters/speckle_filter.h"
+#include "laser_filters/scan_blob_filter.h"
+#include "sensor_msgs/LaserScan.h"
+#include "filters/filter_base.h"
+
+#include "pluginlib/class_list_macros.h"
+
+
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserMedianFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserArrayFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanIntensityFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanRangeFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanAngularBoundsFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanAngularBoundsFilterInPlace, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanFootprintFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::ScanShadowsFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::InterpolationFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanBoxFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanPolygonFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanSpeckleFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanMaskFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+PLUGINLIB_EXPORT_CLASS(laser_filters::ScanBlobFilter, filters::FilterBase<sensor_msgs::LaserScan>)
+

src/laser_filters/src/median_filter.cpp

@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu@cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ *
+ */
+
+#include "laser_filters/median_filter.h"
+
+namespace laser_filters
+{
+LaserMedianFilter::LaserMedianFilter() :
+  num_ranges_(1), xmlrpc_value_(), range_filter_(NULL), intensity_filter_(NULL)
+{
+    ROS_WARN("LaserMedianFilter has been deprecated.  Please use LaserArrayFilter instead.\n");  
+};
+
+bool LaserMedianFilter::configure()
+{
+  
+  if (!getParam("internal_filter", xmlrpc_value_))
+  {
+    ROS_ERROR("Cannot Configure LaserMedianFilter: Didn't find \"internal_filter\" tag within LaserMedianFilter params. Filter definitions needed inside for processing range and intensity");
+    return false;
+  }
+  
+  if (range_filter_) delete range_filter_;
+  range_filter_ = new filters::MultiChannelFilterChain<float>("float");
+  if (!range_filter_->configure(num_ranges_, xmlrpc_value_)) return false;
+  
+  if (intensity_filter_) delete intensity_filter_;
+  intensity_filter_ = new filters::MultiChannelFilterChain<float>("float");
+  if (!intensity_filter_->configure(num_ranges_, xmlrpc_value_)) return false;
+  return true;
+};
+
+LaserMedianFilter::~LaserMedianFilter()
+{
+  delete range_filter_;
+  delete intensity_filter_;
+};
+
+bool LaserMedianFilter::update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
+{
+  if (!this->configured_) 
+  {
+    ROS_ERROR("LaserMedianFilter not configured");
+    return false;
+  }
+  boost::mutex::scoped_lock lock(data_lock);
+  scan_out = scan_in; ///Quickly pass through all data \todo don't copy data too
+
+
+  if (scan_in.ranges.size() != num_ranges_) //Reallocating
+  {
+    ROS_INFO("Laser filter clearning and reallocating due to larger scan size");
+    delete range_filter_;
+    delete intensity_filter_;
+
+
+    num_ranges_ = scan_in.ranges.size();
+    
+    range_filter_ = new filters::MultiChannelFilterChain<float>("float");
+    if (!range_filter_->configure(num_ranges_, xmlrpc_value_)) return false;
+    
+    intensity_filter_ = new filters::MultiChannelFilterChain<float>("float");
+    if (!intensity_filter_->configure(num_ranges_, xmlrpc_value_)) return false;
+    
+  }
+
+  /** \todo check for length of intensities too */
+  range_filter_->update(scan_in.ranges, scan_out.ranges);
+  intensity_filter_->update(scan_in.intensities, scan_out.intensities);
+
+
+  return true;
+}
+}

src/laser_filters/src/pointcloud_filters.cpp

@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "sensor_msgs/PointCloud.h"
+#include "laser_filters/point_cloud_footprint_filter.h"
+#include "pluginlib/class_list_macros.h"
+
+PLUGINLIB_EXPORT_CLASS(laser_filters::PointCloudFootprintFilter, filters::FilterBase<sensor_msgs::PointCloud>)

src/laser_filters/src/polygon_filter.cpp

@@ -0,0 +1,413 @@
+/*
+ *  Software License Agreement (BSD License)
+ *
+ *  Robot Operating System code by Eurotec B.V.
+ *  Copyright (c) 2020, Eurotec B.V.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1. Redistributions of source code must retain the above
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer.
+ *
+ *   2. Redistributions in binary form must reproduce the above
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer in the documentation and/or other materials provided
+ *      with the distribution.
+ *
+ *   3. Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived
+ *      from this software without specific prior written permission.
+ *
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ *  TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ *  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ *  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ *
+ *  polygon_filter.cpp
+ */
+
+#include "laser_filters/polygon_filter.h"
+#include <ros/ros.h>
+#include <dynamic_reconfigure/server.h>
+#include <laser_filters/PolygonFilterConfig.h>
+#include <geometry_msgs/PolygonStamped.h>
+#include <cstdio>  // for EOF
+#include <string>
+#include <sstream>
+#include <vector>
+
+/** @brief Same as sign(x) but returns 0 if x is 0. */
+inline double sign0(double x)
+{
+  return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0);
+}
+
+void padPolygon(geometry_msgs::Polygon& polygon, double padding)
+{
+  // pad polygon in place
+  for (unsigned int i = 0; i < polygon.points.size(); i++)
+  {
+    geometry_msgs::Point32& pt = polygon.points[ i ];
+    pt.x += sign0(pt.x) * padding;
+    pt.y += sign0(pt.y) * padding;
+  }
+}
+
+double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
+{
+  // Make sure that the value we're looking at is either a double or an int.
+  if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
+  {
+    std::string& value_string = value;
+    ROS_FATAL("Values in the polygon specification (param %s) must be numbers. Found value %s.",
+              full_param_name.c_str(), value_string.c_str());
+    throw std::runtime_error("Values in the polygon specification must be numbers");
+  }
+  return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
+}
+
+geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_xmlrpc,
+                                             const std::string& full_param_name)
+{
+  // Make sure we have an array of at least 3 elements.
+  if (polygon_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
+      polygon_xmlrpc.size() > 0 && polygon_xmlrpc.size() < 3)
+  {
+    ROS_FATAL("The polygon (parameter %s) must be specified as nested list on the parameter server with at least "
+              "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]",
+              full_param_name.c_str());
+
+    throw std::runtime_error("The polygon must be specified as nested list on the parameter server with at least "
+                             "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
+  }
+  geometry_msgs::Polygon polygon;
+  geometry_msgs::Point32 pt;
+
+  for (int i = 0; i < polygon_xmlrpc.size(); ++i)
+  {
+    // Make sure each element of the list is an array of size 2. (x and y coordinates)
+    XmlRpc::XmlRpcValue point = polygon_xmlrpc[i];
+    if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || point.size() != 2)
+    {
+      ROS_FATAL("The polygon (parameter %s) must be specified as list of lists on the parameter server eg: "
+                "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.",
+                full_param_name.c_str());
+      throw std::runtime_error("The polygon must be specified as list of lists on the parameter server eg: "
+                               "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
+    }
+
+    pt.x = getNumberFromXMLRPC(point[0], full_param_name);
+    pt.y = getNumberFromXMLRPC(point[1], full_param_name);
+
+    polygon.points.push_back(pt);
+  }
+  return polygon;
+}
+
+std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return)
+{  // Source: https://github.com/ros-planning/navigation/blob/melodic-devel/costmap_2d/src/array_parser.cpp
+  std::vector<std::vector<float> > result;
+
+  std::stringstream input_ss(input);
+  int depth = 0;
+  std::vector<float> current_vector;
+  while (!!input_ss && !input_ss.eof())
+  {
+    switch (input_ss.peek())
+    {
+    case EOF:
+      break;
+    case '[':
+      depth++;
+      if (depth > 2)
+      {
+        error_return = "Array depth greater than 2";
+        return result;
+      }
+      input_ss.get();
+      current_vector.clear();
+      break;
+    case ']':
+      depth--;
+      if (depth < 0)
+      {
+        error_return = "More close ] than open [";
+        return result;
+      }
+      input_ss.get();
+      if (depth == 1)
+      {
+        result.push_back(current_vector);
+      }
+      break;
+    case ',':
+    case ' ':
+    case '\t':
+      input_ss.get();
+      break;
+    default:  // All other characters should be part of the numbers.
+      if (depth != 2)
+      {
+        std::stringstream err_ss;
+        err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
+        error_return = err_ss.str();
+        return result;
+      }
+      float value;
+      input_ss >> value;
+      if (!!input_ss)
+      {
+        current_vector.push_back(value);
+      }
+      break;
+    }
+  }
+
+  if (depth != 0)
+  {
+    error_return = "Unterminated vector string.";
+  }
+  else
+  {
+    error_return = "";
+  }
+
+  return result;
+}
+
+geometry_msgs::Polygon makePolygonFromString(const std::string& polygon_string, const geometry_msgs::Polygon& last_polygon)
+{
+  std::string error;
+  std::vector<std::vector<float> > vvf = parseVVF(polygon_string, error);
+
+    if (error != "")
+    {
+      ROS_ERROR("Error parsing polygon parameter: '%s'", error.c_str());
+      ROS_ERROR(" Polygon string was '%s'.", polygon_string.c_str());
+      return last_polygon;
+    }
+
+    geometry_msgs::Polygon polygon;
+    geometry_msgs::Point32 point;
+
+    // convert vvf into points.
+    if (vvf.size() < 3 && vvf.size() > 0)
+    {
+      ROS_WARN("You must specify at least three points for the robot polygon");
+      return last_polygon;
+    }
+
+    for (unsigned int i = 0; i < vvf.size(); i++)
+    {
+      if (vvf[ i ].size() == 2)
+      {
+        point.x = vvf[ i ][ 0 ];
+        point.y = vvf[ i ][ 1 ];
+        point.z = 0;
+        polygon.points.push_back(point);
+      }
+      else
+      {
+        ROS_ERROR("Points in the polygon specification must be pairs of numbers. Found a point with %d numbers.",
+                   int(vvf[ i ].size()));
+        return last_polygon;
+      }
+    }
+
+    return polygon;
+}
+
+std::string polygonToString(geometry_msgs::Polygon polygon)
+{
+  std::string polygon_string = "[";
+  bool first = true;
+  for (auto point : polygon.points) {
+    if (!first) {
+      polygon_string += ", ";
+    }
+    first = false;
+    polygon_string += "[" + std::to_string(point.x) + ", " + std::to_string(point.y) + "]";
+  }
+  polygon_string += "]";
+  return polygon_string;
+}
+
+namespace laser_filters{
+LaserScanPolygonFilter::LaserScanPolygonFilter()
+{
+}
+
+bool LaserScanPolygonFilter::configure()
+{
+  XmlRpc::XmlRpcValue polygon_xmlrpc;
+  std::string polygon_string;
+  PolygonFilterConfig param_config;
+
+  ros::NodeHandle private_nh("~" + getName());
+  dyn_server_.reset(new dynamic_reconfigure::Server<laser_filters::PolygonFilterConfig>(own_mutex_, private_nh));
+  dynamic_reconfigure::Server<laser_filters::PolygonFilterConfig>::CallbackType f;
+  f = boost::bind(&laser_filters::LaserScanPolygonFilter::reconfigureCB, this, _1, _2);
+  dyn_server_->setCallback(f);
+
+  bool polygon_set = getParam("polygon", polygon_xmlrpc);
+  bool polygon_frame_set = getParam("polygon_frame", polygon_frame_);
+  bool invert_set = getParam("invert", invert_filter_);
+  polygon_ = makePolygonFromXMLRPC(polygon_xmlrpc, "polygon");
+
+  double polygon_padding = 0;
+  getParam("polygon_padding", polygon_padding);
+
+  polygon_string = polygonToString(polygon_);
+  param_config.polygon = polygon_string;
+  param_config.polygon_padding = polygon_padding;
+  param_config.invert = invert_filter_;
+  dyn_server_->updateConfig(param_config);
+
+  polygon_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("polygon", 1);
+
+  if (!polygon_frame_set)
+  {
+    ROS_ERROR("polygon_frame is not set!");
+  }
+  if (!polygon_set)
+  {
+    ROS_ERROR("polygon is not set!");
+  }
+  if (!invert_set)
+  {
+    ROS_INFO("invert filter not set, assuming false");
+    invert_filter_ = false;
+  }
+
+  return polygon_frame_set && polygon_set;
+}
+
+bool LaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_scan,
+                                                   sensor_msgs::LaserScan& output_scan)
+{
+  boost::recursive_mutex::scoped_lock lock(own_mutex_);
+
+  geometry_msgs::PolygonStamped polygon_stamped;
+  polygon_stamped.header.frame_id = polygon_frame_;
+  polygon_stamped.header.stamp = ros::Time::now();
+  polygon_stamped.polygon = polygon_;
+  polygon_pub_.publish(polygon_stamped);
+
+  output_scan = input_scan;
+
+  sensor_msgs::PointCloud2 laser_cloud;
+
+  std::string error_msg;
+
+  bool success = tf_.waitForTransform(
+      polygon_frame_, input_scan.header.frame_id,
+      input_scan.header.stamp + ros::Duration().fromSec(input_scan.ranges.size() * input_scan.time_increment),
+      ros::Duration(1.0), ros::Duration(0.01), &error_msg);
+  if (!success)
+  {
+    ROS_WARN("Could not get transform, ignoring laser scan! %s", error_msg.c_str());
+    return false;
+  }
+
+  try
+  {
+    projector_.transformLaserScanToPointCloud(polygon_frame_, input_scan, laser_cloud, tf_);
+  }
+  catch (tf::TransformException& ex)
+  {
+    ROS_INFO_THROTTLE(.3, "Ignoring Scan: Waiting for TF");
+    return false;
+  }
+  const int i_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "index");
+  const int x_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "x");
+  const int y_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "y");
+  const int z_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "z");
+
+  if (i_idx_c == -1 || x_idx_c == -1 || y_idx_c == -1 || z_idx_c == -1)
+  {
+    ROS_INFO_THROTTLE(.3, "x, y, z and index fields are required, skipping scan");
+  }
+
+  const int i_idx_offset = laser_cloud.fields[i_idx_c].offset;
+  const int x_idx_offset = laser_cloud.fields[x_idx_c].offset;
+  const int y_idx_offset = laser_cloud.fields[y_idx_c].offset;
+  const int z_idx_offset = laser_cloud.fields[z_idx_c].offset;
+
+  const int pstep = laser_cloud.point_step;
+  const long int pcount = laser_cloud.width * laser_cloud.height;
+  const long int limit = pstep * pcount;
+
+  int i_idx, x_idx, y_idx, z_idx;
+  for (i_idx = i_idx_offset, x_idx = x_idx_offset, y_idx = y_idx_offset, z_idx = z_idx_offset;
+
+       x_idx < limit;
+
+       i_idx += pstep, x_idx += pstep, y_idx += pstep, z_idx += pstep)
+  {
+    // TODO works only for float data types and with an index field
+    // I'm working on it, see https://github.com/ros/common_msgs/pull/78
+    float x = *((float*)(&laser_cloud.data[x_idx]));
+    float y = *((float*)(&laser_cloud.data[y_idx]));
+    float z = *((float*)(&laser_cloud.data[z_idx]));
+    int index = *((int*)(&laser_cloud.data[i_idx]));
+
+    tf::Point point(x, y, z);
+
+    if (!invert_filter_)
+    {
+      if (inPolygon(point))
+      {
+        output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
+      }
+    }
+    else
+    {
+      if (!inPolygon(point))
+      {
+        output_scan.ranges[index] = std::numeric_limits<float>::quiet_NaN();
+      }
+    }
+  }
+
+  return true;
+}
+
+
+bool LaserScanPolygonFilter::inPolygon(tf::Point& point) const
+{
+  int i, j;
+  bool c = false;
+
+  for (i = 0, j = polygon_.points.size() - 1; i < polygon_.points.size(); j = i++)
+  {
+    if ((polygon_.points.at(i).y > point.y() != (polygon_.points.at(j).y > point.y()) &&
+         (point.x() < (polygon_.points[j].x - polygon_.points[i].x) * (point.y() - polygon_.points[i].y) /
+                              (polygon_.points[j].y - polygon_.points[i].y) +
+                          polygon_.points[i].x)))
+      c = !c;
+  }
+  return c;
+}
+
+
+void LaserScanPolygonFilter::reconfigureCB(laser_filters::PolygonFilterConfig& config, uint32_t level)
+{
+  invert_filter_ = config.invert;
+  polygon_ = makePolygonFromString(config.polygon, polygon_);
+  padPolygon(polygon_, config.polygon_padding);
+}
+}

src/laser_filters/src/scan_to_cloud_filter_chain.cpp

@@ -0,0 +1,262 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu@cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/*
+\author Radu Bogdan Rusu <rusu@cs.tum.edu>
+
+
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <float.h>
+
+// Laser projection
+#include <laser_geometry/laser_geometry.h>
+
+// TF
+#include <tf/transform_listener.h>
+#include "tf/message_filter.h"
+#include "message_filters/subscriber.h"
+
+//Filters
+#include "filters/filter_chain.h"
+
+/** @b ScanShadowsFilter is a simple node that filters shadow points in a laser scan line and publishes the results in a cloud.
+ */
+class ScanToCloudFilterChain
+{
+public:
+
+  // ROS related
+  laser_geometry::LaserProjection projector_; // Used to project laser scans
+
+  double laser_max_range_;           // Used in laser scan projection
+  int window_;
+    
+  bool high_fidelity_;                    // High fidelity (interpolating time across scan)
+  std::string target_frame_;                   // Target frame for high fidelity result
+  std::string scan_topic_, cloud_topic_;
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh;
+    
+  // TF
+  tf::TransformListener tf_;
+
+  message_filters::Subscriber<sensor_msgs::LaserScan> sub_;
+  tf::MessageFilter<sensor_msgs::LaserScan> filter_;
+
+  double tf_tolerance_;
+  filters::FilterChain<sensor_msgs::PointCloud2> cloud_filter_chain_;
+  filters::FilterChain<sensor_msgs::LaserScan> scan_filter_chain_;
+  ros::Publisher cloud_pub_;
+
+  // Timer for displaying deprecation warnings
+  ros::Timer deprecation_timer_;
+  bool  using_scan_topic_deprecated_;
+  bool  using_cloud_topic_deprecated_;
+  bool  using_default_target_frame_deprecated_;
+  bool  using_laser_max_range_deprecated_;
+  bool  using_filter_window_deprecated_;
+  bool  using_scan_filters_deprecated_;
+  bool  using_cloud_filters_deprecated_;
+  bool  using_scan_filters_wrong_deprecated_;
+  bool  using_cloud_filters_wrong_deprecated_;
+  bool  incident_angle_correction_;
+
+  ////////////////////////////////////////////////////////////////////////////////
+  ScanToCloudFilterChain () : laser_max_range_ (DBL_MAX), private_nh("~"), filter_(tf_, "", 50),
+                   cloud_filter_chain_("sensor_msgs::PointCloud2"), scan_filter_chain_("sensor_msgs::LaserScan")
+  {
+    private_nh.param("high_fidelity", high_fidelity_, false);
+    private_nh.param("notifier_tolerance", tf_tolerance_, 0.03);
+    private_nh.param("target_frame", target_frame_, std::string ("base_link"));
+
+    // DEPRECATED with default value
+    using_default_target_frame_deprecated_ = !private_nh.hasParam("target_frame");
+
+    // DEPRECATED
+    using_scan_topic_deprecated_ = private_nh.hasParam("scan_topic");
+    using_cloud_topic_deprecated_ = private_nh.hasParam("cloud_topic");
+    using_laser_max_range_deprecated_ = private_nh.hasParam("laser_max_range");
+    using_filter_window_deprecated_ = private_nh.hasParam("filter_window");
+    using_cloud_filters_deprecated_ = private_nh.hasParam("cloud_filters/filter_chain");
+    using_scan_filters_deprecated_ = private_nh.hasParam("scan_filters/filter_chain");
+    using_cloud_filters_wrong_deprecated_ = private_nh.hasParam("cloud_filters/cloud_filter_chain");
+    using_scan_filters_wrong_deprecated_ = private_nh.hasParam("scan_filters/scan_filter_chain");
+
+
+    private_nh.param("filter_window", window_, 2);
+    private_nh.param("laser_max_range", laser_max_range_, DBL_MAX);
+    private_nh.param("scan_topic", scan_topic_, std::string("tilt_scan"));
+    private_nh.param("cloud_topic", cloud_topic_, std::string("tilt_laser_cloud_filtered"));
+    private_nh.param("incident_angle_correction", incident_angle_correction_, true);
+
+    filter_.setTargetFrame(target_frame_);
+    filter_.registerCallback(boost::bind(&ScanToCloudFilterChain::scanCallback, this, _1));
+    filter_.setTolerance(ros::Duration(tf_tolerance_));
+
+    if (using_scan_topic_deprecated_)
+      sub_.subscribe(nh, scan_topic_, 50);
+    else
+      sub_.subscribe(nh, "scan", 50);
+
+    filter_.connectInput(sub_);
+
+    if (using_cloud_topic_deprecated_)
+      cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2> (cloud_topic_, 10);
+    else
+      cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2> ("cloud_filtered", 10);
+
+    std::string cloud_filter_xml;
+
+    if (using_cloud_filters_deprecated_)
+      cloud_filter_chain_.configure("cloud_filters/filter_chain", private_nh);
+    else if (using_cloud_filters_wrong_deprecated_)
+      cloud_filter_chain_.configure("cloud_filters/cloud_filter_chain", private_nh);
+    else
+      cloud_filter_chain_.configure("cloud_filter_chain", private_nh);
+
+    if (using_scan_filters_deprecated_)
+      scan_filter_chain_.configure("scan_filter/filter_chain", private_nh);
+    else if (using_scan_filters_wrong_deprecated_)
+      scan_filter_chain_.configure("scan_filters/scan_filter_chain", private_nh);
+    else
+      scan_filter_chain_.configure("scan_filter_chain", private_nh);
+
+    deprecation_timer_ = nh.createTimer(ros::Duration(5.0), boost::bind(&ScanToCloudFilterChain::deprecation_warn, this, _1));
+  }
+
+  // We use a deprecation warning on a timer to avoid warnings getting lost in the noise
+  void deprecation_warn(const ros::TimerEvent& e)
+  {
+    if (using_scan_topic_deprecated_)
+      ROS_WARN("Use of '~scan_topic' parameter in scan_to_cloud_filter_chain has been deprecated.");
+
+    if (using_cloud_topic_deprecated_)
+      ROS_WARN("Use of '~cloud_topic' parameter in scan_to_cloud_filter_chain has been deprecated.");
+
+    if (using_laser_max_range_deprecated_)
+      ROS_WARN("Use of '~laser_max_range' parameter in scan_to_cloud_filter_chain has been deprecated.");
+
+    if (using_filter_window_deprecated_)
+      ROS_WARN("Use of '~filter_window' parameter in scan_to_cloud_filter_chain has been deprecated.");
+
+    if (using_default_target_frame_deprecated_)
+      ROS_WARN("Use of default '~target_frame' parameter in scan_to_cloud_filter_chain has been deprecated.  Default currently set to 'base_link' please set explicitly as appropriate.");
+
+    if (using_cloud_filters_deprecated_)
+      ROS_WARN("Use of '~cloud_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated.  Replace with '~cloud_filter_chain'");
+
+    if (using_scan_filters_deprecated_)
+      ROS_WARN("Use of '~scan_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated.  Replace with '~scan_filter_chain'");
+
+    if (using_cloud_filters_wrong_deprecated_)
+      ROS_WARN("Use of '~cloud_filters/cloud_filter_chain' parameter in scan_to_cloud_filter_chain is incorrect.  Please Replace with '~cloud_filter_chain'");
+
+    if (using_scan_filters_wrong_deprecated_)
+      ROS_WARN("Use of '~scan_filters/scan_filter_chain' parameter in scan_to_scan_filter_chain is incorrect.  Please Replace with '~scan_filter_chain'");
+
+  }
+
+
+  ////////////////////////////////////////////////////////////////////////////////
+  void
+  scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
+  {
+    //    sensor_msgs::LaserScan scan_msg = *scan_in;
+
+    sensor_msgs::LaserScan filtered_scan;
+    scan_filter_chain_.update (*scan_msg, filtered_scan);
+
+    // Project laser into point cloud
+    sensor_msgs::PointCloud2 scan_cloud;
+
+    //\TODO CLEAN UP HACK 
+    // This is a trial at correcting for incident angles.  It makes many assumptions that do not generalise
+    if(incident_angle_correction_)
+    {
+      for (unsigned int i = 0; i < filtered_scan.ranges.size(); i++)
+      {
+        double angle = filtered_scan.angle_min + i * filtered_scan.angle_increment;
+        filtered_scan.ranges[i] = filtered_scan.ranges[i] + 0.03 * exp(-fabs(sin(angle)));
+      }
+    }
+
+    // Transform into a PointCloud message
+    int mask = laser_geometry::channel_option::Intensity |
+      laser_geometry::channel_option::Distance |
+      laser_geometry::channel_option::Index |
+      laser_geometry::channel_option::Timestamp;
+      
+    if (high_fidelity_)
+    {
+      try
+      {
+        projector_.transformLaserScanToPointCloud (target_frame_, filtered_scan, scan_cloud, tf_, laser_max_range_, mask);
+      }
+      catch (tf::TransformException &ex)
+      {
+        ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", target_frame_.c_str (), ex.what ());
+        return;
+        //projector_.projectLaser (filtered_scan, scan_cloud, laser_max_range_, preservative_, mask);
+      }
+    }
+    else
+    {
+      projector_.transformLaserScanToPointCloud(target_frame_, filtered_scan, scan_cloud, tf_, laser_max_range_, mask);
+    }
+      
+    sensor_msgs::PointCloud2 filtered_cloud;
+    cloud_filter_chain_.update (scan_cloud, filtered_cloud);
+
+    cloud_pub_.publish(filtered_cloud);
+  }
+
+} ;
+
+
+
+int
+main (int argc, char** argv)
+{
+  ros::init (argc, argv, "scan_to_cloud_filter_chain");
+  ros::NodeHandle nh;
+  ScanToCloudFilterChain f;
+
+  ros::spin();
+
+  return (0);
+}
+
+

src/laser_filters/src/scan_to_scan_filter_chain.cpp

@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#include "ros/ros.h"
+#include "sensor_msgs/LaserScan.h"
+#include "message_filters/subscriber.h"
+#include "tf/message_filter.h"
+#include "tf/transform_listener.h"
+#include "filters/filter_chain.h"
+
+class ScanToScanFilterChain
+{
+protected:
+  // Our NodeHandle
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+
+  // Components for tf::MessageFilter
+  tf::TransformListener *tf_;
+  message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_;
+  tf::MessageFilter<sensor_msgs::LaserScan> *tf_filter_;
+  double tf_filter_tolerance_;
+
+  // Filter Chain
+  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
+
+  // Components for publishing
+  sensor_msgs::LaserScan msg_;
+  ros::Publisher output_pub_;
+
+  // Deprecation helpers
+  ros::Timer deprecation_timer_;
+  bool  using_filter_chain_deprecated_;
+
+public:
+  // Constructor
+  ScanToScanFilterChain() :
+    private_nh_("~"),
+    scan_sub_(nh_, "scan", 50),
+    tf_(NULL),
+    tf_filter_(NULL),
+    filter_chain_("sensor_msgs::LaserScan")
+  {
+    // Configure filter chain
+    
+    using_filter_chain_deprecated_ = private_nh_.hasParam("filter_chain");
+
+    if (using_filter_chain_deprecated_)
+      filter_chain_.configure("filter_chain", private_nh_);
+    else
+      filter_chain_.configure("scan_filter_chain", private_nh_);
+    
+    std::string tf_message_filter_target_frame;
+
+    if (private_nh_.hasParam("tf_message_filter_target_frame"))
+    {
+      private_nh_.getParam("tf_message_filter_target_frame", tf_message_filter_target_frame);
+
+      private_nh_.param("tf_message_filter_tolerance", tf_filter_tolerance_, 0.03);
+
+      tf_ = new tf::TransformListener();
+      tf_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(scan_sub_, *tf_, "", 50);
+      tf_filter_->setTargetFrame(tf_message_filter_target_frame);
+      tf_filter_->setTolerance(ros::Duration(tf_filter_tolerance_));
+
+      // Setup tf::MessageFilter generates callback
+      tf_filter_->registerCallback(boost::bind(&ScanToScanFilterChain::callback, this, _1));
+    }
+    else 
+    {
+      // Pass through if no tf_message_filter_target_frame
+      scan_sub_.registerCallback(boost::bind(&ScanToScanFilterChain::callback, this, _1));
+    }
+    
+    // Advertise output
+    output_pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan_filtered", 1000);
+
+    // Set up deprecation printout
+    deprecation_timer_ = nh_.createTimer(ros::Duration(5.0), boost::bind(&ScanToScanFilterChain::deprecation_warn, this, _1));
+  }
+
+  // Destructor
+  ~ScanToScanFilterChain()
+  {
+    if (tf_filter_)
+      delete tf_filter_;
+    if (tf_)
+      delete tf_;
+  }
+  
+  // Deprecation warning callback
+  void deprecation_warn(const ros::TimerEvent& e)
+  {
+    if (using_filter_chain_deprecated_)
+      ROS_WARN("Use of '~filter_chain' parameter in scan_to_scan_filter_chain has been deprecated. Please replace with '~scan_filter_chain'.");
+  }
+
+  // Callback
+  void callback(const sensor_msgs::LaserScan::ConstPtr& msg_in)
+  {
+    // Run the filter chain
+    if (filter_chain_.update(*msg_in, msg_))
+    {
+      //only publish result if filter succeeded
+      output_pub_.publish(msg_);
+    } else {
+      ROS_ERROR_THROTTLE(1, "Filtering the scan from time %i.%i failed.", msg_in->header.stamp.sec, msg_in->header.stamp.nsec);
+    }
+  }
+};
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "scan_to_scan_filter_chain");
+  
+  ScanToScanFilterChain t;
+  ros::spin();
+  
+  return 0;
+}

src/laser_filters/src/speckle_filter.cpp

@@ -0,0 +1,132 @@
+/*
+ *  Software License Agreement (BSD License)
+ *
+ *  Robot Operating System code by Eurotec B.V.
+ *  Copyright (c) 2020, Eurotec B.V.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   1. Redistributions of source code must retain the above
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer.
+ *
+ *   2. Redistributions in binary form must reproduce the above
+ *      copyright notice, this list of conditions and the following
+ *      disclaimer in the documentation and/or other materials provided
+ *      with the distribution.
+ *
+ *   3. Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived
+ *      from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+ *  TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ *  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ *  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  speckle_filter.cpp
+ */
+
+#include <laser_filters/speckle_filter.h>
+#include <ros/node_handle.h>
+
+namespace laser_filters
+{
+LaserScanSpeckleFilter::LaserScanSpeckleFilter()
+{
+  validator_ = 0;
+}
+
+LaserScanSpeckleFilter::~LaserScanSpeckleFilter()
+{
+  if (!validator_)
+  {
+    delete validator_;
+  }
+}
+
+bool LaserScanSpeckleFilter::configure()
+{
+  ros::NodeHandle private_nh("~" + getName());
+  dyn_server_.reset(new dynamic_reconfigure::Server<laser_filters::SpeckleFilterConfig>(own_mutex_, private_nh));
+  dynamic_reconfigure::Server<laser_filters::SpeckleFilterConfig>::CallbackType f;
+  f = boost::bind(&laser_filters::LaserScanSpeckleFilter::reconfigureCB, this, _1, _2);
+  dyn_server_->setCallback(f);
+
+  getParam("filter_type", config_.filter_type);
+  getParam("max_range", config_.max_range);
+  getParam("max_range_difference", config_.max_range_difference);
+  getParam("filter_window", config_.filter_window);
+  dyn_server_->updateConfig(config_);
+  return true;
+}
+
+bool LaserScanSpeckleFilter::update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan)
+{
+  output_scan = input_scan;
+  std::vector<bool> valid_ranges(output_scan.ranges.size(), false);
+  for (size_t idx = 0; idx < output_scan.ranges.size() - config_.filter_window + 1; ++idx)
+  {
+    bool window_valid = validator_->checkWindowValid(
+          output_scan, idx, config_.filter_window, config_.max_range_difference);
+
+    // Actually set the valid ranges (do not set to false if it was already valid or out of range)
+    for (size_t neighbor_idx_or_self_nr = 0; neighbor_idx_or_self_nr < config_.filter_window; ++neighbor_idx_or_self_nr)
+    {
+      size_t neighbor_idx_or_self = idx + neighbor_idx_or_self_nr;
+      if (neighbor_idx_or_self < output_scan.ranges.size())  // Out of bound check
+      {
+        bool out_of_range = output_scan.ranges[neighbor_idx_or_self] > config_.max_range;
+        valid_ranges[neighbor_idx_or_self] = valid_ranges[neighbor_idx_or_self] || window_valid || out_of_range;
+      }
+    }
+  }
+
+  for (size_t idx = 0; idx < valid_ranges.size(); ++idx)
+  {
+    if (!valid_ranges[idx])
+    {
+      output_scan.ranges[idx] = std::numeric_limits<float>::quiet_NaN();
+    }
+  }
+
+  return true;
+}
+
+void LaserScanSpeckleFilter::reconfigureCB(laser_filters::SpeckleFilterConfig& config, uint32_t level)
+{
+  config_ = config;
+
+  switch (config_.filter_type) {
+    case laser_filters::SpeckleFilter_RadiusOutlier:
+      if (validator_)
+      {
+        delete validator_;
+      }
+      validator_ = new laser_filters::RadiusOutlierWindowValidator();
+      break;
+
+    case laser_filters::SpeckleFilter_Distance:
+      if (validator_)
+      {
+        delete validator_;
+      }
+      validator_ = new laser_filters::DistanceWindowValidator();
+      break;
+
+    default:
+      break;
+  }
+
+}
+}

src/laser_filters/test/fake_laser.py

@@ -0,0 +1,39 @@
+#!/usr/bin/python
+
+PKG = 'laser_filters' # this package name
+import roslib; roslib.load_manifest(PKG)
+
+import rospy
+from sensor_msgs.msg import LaserScan
+from Numeric import ones
+
+def laser_test():
+    pub = rospy.Publisher('laser_scan', LaserScan)
+    rospy.init_node('laser_test')
+    laser_msg = LaserScan()
+
+    laser_msg.header.frame_id = 'laser'
+    laser_msg.angle_min = -1.5
+    laser_msg.angle_max = 1.5
+    laser_msg.angle_increment = 0.1
+    laser_msg.time_increment = 0.1
+    laser_msg.scan_time = 0.1
+    laser_msg.range_min = 0.5
+    laser_msg.range_max = 1.5    
+    laser_msg.ranges = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 1.0]
+    laser_msg.intensities = laser_msg.ranges 
+
+    r = rospy.Rate(10) # 10hz
+    while not rospy.is_shutdown():
+        laser_msg.header.stamp = rospy.get_rostime()
+        pub.publish(laser_msg)
+        r.sleep()
+
+
+if __name__ == '__main__':
+  try:
+        laser_test()
+  except rospy.ROSInterruptException: pass
+
+
+

src/laser_filters/test/test_polygon_filter

@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+#
+# Copyright (c) 2020, Eurotec, Netherlands
+# All rights reserved.
+#
+# \author Rein Appeldoorn
+
+import math
+import unittest
+
+import rospy
+import rostest
+from sensor_msgs.msg import LaserScan
+from std_msgs.msg import Header
+
+
+class TestPolygonFilter(unittest.TestCase):
+    rospy.init_node('test_polygon_filter')
+    num_beams = 11
+    rospy.Publisher("scan", LaserScan, queue_size=1, latch=True).publish(LaserScan(
+        header=Header(
+            frame_id="laser",
+            stamp=rospy.Time.now()
+        ),
+        angle_min=-math.pi / 2,
+        angle_max=math.pi / 2,
+        angle_increment=math.pi / (num_beams - 1),
+        ranges=[1] * num_beams,
+        range_max=100
+    ))
+
+    def test_polygon_filter(self):
+        msg = rospy.wait_for_message("scan_filtered", LaserScan, 10.)  # type: LaserScan
+        expected_scan_ranges = [1.0, 1.0, 1.0, 1.0, float('nan'), float('nan'), float('nan'), 1, 1, 1, 1]
+        for scan_range, expected_scan_range in zip(msg.ranges, expected_scan_ranges):
+            if math.isnan(expected_scan_range) or math.isnan(scan_range):
+                self.assertEqual(math.isnan(expected_scan_range), math.isnan(scan_range))
+            else:
+                self.assertEqual(scan_range, expected_scan_range)
+
+
+PKG = 'laser_filters'
+NAME = 'test_polygon_filter'
+if __name__ == '__main__':
+    rostest.unitrun(PKG, NAME, TestPolygonFilter)

src/laser_filters/test/test_polygon_filter.launch

@@ -0,0 +1,6 @@
+<launch>
+<test test-name="test_polygon_filter" pkg="laser_filters" type="test_polygon_filter" />
+<node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter">
+      <rosparam command="load" file="$(find laser_filters)/test/test_polygon_filter.yaml" />
+</node>
+</launch>

src/laser_filters/test/test_polygon_filter.yaml

@@ -0,0 +1,7 @@
+scan_filter_chain:
+- name: polygon_filter
+  type: laser_filters/LaserScanPolygonFilter
+  params:
+    polygon_frame: laser
+    polygon: [[0, 0.5], [2, 0.5], [2, -0.5], [0, -0.5]]
+    invert: false

src/laser_filters/test/test_scan_filter_chain.cpp

@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <gtest/gtest.h>
+#include <filters/filter_chain.h>
+#include <ros/ros.h>
+#include "sensor_msgs/LaserScan.h"
+#include <pluginlib/class_loader.h>
+
+
+sensor_msgs::LaserScan gen_msg(){
+  sensor_msgs::LaserScan msg;
+
+  float temp[] = {1.0, 0.1, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 2.3};
+  std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
+
+  msg.header.stamp = ros::Time::now();
+  msg.header.frame_id = "laser";
+  msg.angle_min = -.5;
+  msg.angle_max = .5;
+  msg.angle_increment = 0.1;
+  msg.time_increment = 0.1;
+  msg.scan_time = 0.1;
+  msg.range_min = 0.5;
+  msg.range_max = 1.5;
+  msg.ranges = v1;
+  msg.intensities = v1;
+
+  return msg;
+}
+
+/** Verifies that two vectors of range values are the same. Allows the case
+ * where corresponding values are both NaN.
+ */
+void expect_ranges_eq(const std::vector<float> &a, const std::vector<float> &b) {
+  for( int i=0; i<10; i++) {
+    if(std::isnan(a[i])) {
+      EXPECT_TRUE(std::isnan(b[i]));
+    }
+    else {
+      EXPECT_NEAR(a[i], b[i], 1e-6);
+    }
+  }
+}
+
+TEST(ScanToScanFilterChain, BadConfiguration)
+{
+  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
+
+  try
+  {
+    filter_chain_.configure("bad_filter_chain");
+  }
+  catch(pluginlib::LibraryLoadException)
+  {
+    EXPECT_FALSE(false);
+  }
+  
+  filter_chain_.clear();
+}
+
+TEST(ScanToScanFilterChain, IntensityFilter)
+{
+  sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
+  float nanval = std::numeric_limits<float>::quiet_NaN();
+  float temp[] = {1.0, nanval, 1.0, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, 2.3};
+  std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
+  expected_msg.ranges = v1;
+  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
+
+  EXPECT_TRUE(filter_chain_.configure("intensity_filter_chain"));
+
+  msg_in = gen_msg();
+
+  EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
+  expect_ranges_eq(msg_out.ranges, expected_msg.ranges);
+
+  filter_chain_.clear();
+}
+
+TEST(ScanToScanFilterChain, InterpFilter)
+{
+  sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
+  float temp[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+  std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
+  expected_msg.ranges = v1;
+  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
+
+  EXPECT_TRUE(filter_chain_.configure("interp_filter_chain"));
+
+  msg_in = gen_msg();
+
+  EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
+  
+  for( int i=0; i<10; i++){
+    EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-6);
+  }
+
+  filter_chain_.clear();
+}
+
+TEST(ScanToScanFilterChain, ShadowFilter)
+{
+  sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
+  float nanval = std::numeric_limits<float>::quiet_NaN();
+  float temp[] = {nanval, 0.1, nanval, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, nanval};
+  std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
+  expected_msg.ranges = v1; 
+  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
+
+  EXPECT_TRUE(filter_chain_.configure("shadow_filter_chain"));
+
+  msg_in = gen_msg();
+
+  EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
+
+  expect_ranges_eq(msg_out.ranges, expected_msg.ranges);
+
+  filter_chain_.clear();
+}
+
+TEST(ScanToScanFilterChain, ArrayFilter)
+{
+  sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
+  float temp[] = {1.0, 0.4, 1.0, 1.0, 1.0, 6.3333, 1.0, 1.0, 1.0, 1.8667};
+  std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
+  expected_msg.ranges = v1; 
+  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
+
+  EXPECT_TRUE(filter_chain_.configure("array_filter_chain"));
+
+  msg_in = gen_msg();
+  
+  EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
+  float temp2[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+  std::vector<float> v2 (temp2, temp2 + sizeof(temp2) / sizeof(float));
+  msg_in.ranges = v2;
+  EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
+  msg_in = gen_msg();
+  EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
+  
+  for( int i=0; i<10; i++){
+    EXPECT_NEAR(msg_out.ranges[i],expected_msg.ranges[i],1e-3);
+    EXPECT_NEAR(msg_out.intensities[i],msg_in.intensities[i],1e-3);
+  }
+
+  filter_chain_.clear();
+}
+
+TEST(ScanToScanFilterChain, MaskFilter)
+{
+  sensor_msgs::LaserScan msg_in, msg_out, expected_msg;
+  const float nanval = std::numeric_limits<float>::quiet_NaN();
+  const float temp[] = {1.0, nanval, 1.0, 1.0, 1.0, nanval, 1.0, 1.0, 1.0, 2.3};
+  const std::vector<float> v1 (temp, temp + sizeof(temp) / sizeof(float));
+  expected_msg.ranges = v1;
+  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_("sensor_msgs::LaserScan");
+
+  EXPECT_TRUE(filter_chain_.configure("mask_filter_chain"));
+
+  msg_in = gen_msg();
+
+  EXPECT_TRUE(filter_chain_.update(msg_in, msg_out));
+
+  expect_ranges_eq(msg_out.ranges, expected_msg.ranges);
+
+  filter_chain_.clear();
+}
+
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_scan_to_scan_filter_chain");
+  return RUN_ALL_TESTS();
+}

src/laser_filters/test/test_scan_filter_chain.launch

@@ -0,0 +1,4 @@
+<launch>
+        <rosparam command="load" file="$(find laser_filters)/test/test_scan_filter_chain.yaml"/>
+        <test test-name="test_scan_filter_chain" pkg="laser_filters" type="test_scan_filter_chain"/>
+</launch>

src/laser_filters/test/test_scan_filter_chain.yaml

@@ -0,0 +1,52 @@
+intensity_filter_chain:
+- name: intensity_threshold
+  type: laser_filters/LaserScanIntensityFilter
+  params: 
+    lower_threshold: 0.5
+    upper_threshold: 3.0
+    disp_histogram: 1
+
+bad_filter_chain:
+- name: dark_shadows
+  type: laser_filters/LaserScanIntensityFilter
+  params:
+    lower_threshold: 0.5
+    upper_threshold: 3.0
+    disp_histogram: 0
+
+interp_filter_chain:
+- name: interpolation
+  type: laser_filters/InterpolationFilter
+
+shadow_filter_chain:
+- name: shadows
+  type: laser_filters/ScanShadowsFilter
+  params:
+    min_angle: 80
+    max_angle: 100
+    neighbors: 1
+    window: 1
+
+array_filter_chain:
+  - type: laser_filters/LaserArrayFilter
+    name: laser_median_filter
+    params: 
+      range_filter_chain:
+        - name: median_2
+          type: filters/MultiChannelMeanFilterFloat 
+          params:
+            number_of_observations: 3
+      intensity_filter_chain:
+        - name: median_2
+          type: filters/MultiChannelMeanFilterFloat
+          params:
+            number_of_observations: 3
+
+mask_filter_chain:
+- name: mask
+  type: laser_filters/LaserScanMaskFilter
+  params:
+    masks:
+      laser:
+      - 1
+      - 5

src/laser_filters/test/test_shadow_detector.cpp

@@ -0,0 +1,88 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2017, laser_filters authors
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/*
+\author Atsushi Watanabe (SEQSENSE, Inc.)
+*/
+
+#include <gtest/gtest.h>
+#include <angles/angles.h>
+
+#include "laser_filters/scan_shadow_detector.h"
+
+double getAngleWithViewpoint(const float r1, const float r2, const float included_angle)
+{
+  return atan2(r2 * sin(included_angle), r1 - r2 * cos(included_angle));
+}
+
+bool isShadowPureImpl(const float r1, const float r2, const float included_angle, const double min_angle, const double max_angle)
+{
+  const double angle = fabs(angles::to_degrees(
+      getAngleWithViewpoint(r1, r2, included_angle)));
+  if (angle < min_angle || angle > max_angle)
+    return true;
+  return false;
+}
+
+TEST(ScanShadowDetector, ShadowDetectionGeometry)
+{
+  for (float min_angle = 90.0; min_angle >= 0.0; min_angle -= 5.0)
+  {
+    for (float max_angle = 90.0; max_angle <= 180; max_angle += 5.0)
+    {
+      laser_filters::ScanShadowDetector detector;
+      detector.configure(angles::from_degrees(min_angle), angles::from_degrees(max_angle));
+
+      for (float r1 = 0.1; r1 < 1.0; r1 += 0.1)
+      {
+        for (float r2 = 0.1; r2 < 1.0; r2 += 0.1)
+        {
+          for (float inc = 0.01; inc < 0.1; inc += 0.02)
+          {
+            // Compare with original ScanShadowsFilter implementation
+            EXPECT_EQ(
+                detector.isShadow(r1, r2, inc),
+                isShadowPureImpl(r1, r2, inc, min_angle, max_angle));
+          }
+        }
+      }
+    }
+  }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/laser_filters/test/test_speckle_filter

@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+#
+# Copyright (c) 2020, Eurotec, Netherlands
+# All rights reserved.
+#
+# \author Rein Appeldoorn
+#         Nicolas Limpert
+
+import math
+import unittest
+
+import rospy
+import rostest
+from sensor_msgs.msg import LaserScan
+from std_msgs.msg import Header
+
+
+class TestSpeckleFilter(unittest.TestCase):
+    def __init__(self, *args, **kwargs):
+        super(TestSpeckleFilter, self).__init__(*args, **kwargs)
+        self.msg_euclid = 0
+        self.msg_dist = 0
+
+        rospy.init_node('test_speckle_filter_distance')
+        num_beams = 11
+        rospy.Publisher("scan", LaserScan, queue_size=1, latch=True).publish(LaserScan(
+            header=Header(
+                frame_id="laser",
+                stamp=rospy.Time.now()
+            ),
+            angle_min=-math.pi / 2,
+            angle_max=math.pi / 2,
+            angle_increment=math.pi / (num_beams - 1),
+            ranges=[1, 1, 1, 1, 0.5, 1, 1, 1, 1, 1, 1],
+            range_max=100
+        ))
+
+    def dist_cb(self, msg):
+        self.msg_dist = msg
+        print ("received dist")
+        print (self.msg_dist)
+
+    def euclid_cb(self, msg):
+        self.msg_euclid = msg
+        print ("received euclid")
+        print (self.msg_euclid)
+
+    def test_speckle_filter(self):
+        rospy.Subscriber("scan_filtered_distance", LaserScan, self.dist_cb)
+        rospy.Subscriber("scan_filtered_euclidean", LaserScan, self.euclid_cb)
+
+        now = rospy.Time.now()
+        while (self.msg_euclid == 0 or self.msg_dist == 0):
+            rospy.sleep(0.1)
+
+            if (rospy.Time.now() - now) > rospy.Duration(10):
+                print ("Error: did not receive messages within 10 seconds")
+                self.assert_(False, "Error: did not receive messages within 10 seconds")
+
+        expected_scan_ranges = [1, 1, 1, 1, float('nan'), 1, 1, 1, 1, 1, 1]
+        for scan_range, expected_scan_range in zip(self.msg_dist.ranges, expected_scan_ranges):
+            if math.isnan(expected_scan_range) or math.isnan(scan_range):
+                self.assertEqual(math.isnan(expected_scan_range), math.isnan(scan_range))
+            else:
+                self.assertEqual(scan_range, expected_scan_range)
+
+        expected_scan_ranges = [1, 1, 1, 1, float('nan'), 1, 1, 1, 1, 1, 1]
+        for scan_range, expected_scan_range in zip(self.msg_euclid.ranges, expected_scan_ranges):
+            if math.isnan(expected_scan_range) or math.isnan(scan_range):
+                self.assertEqual(math.isnan(expected_scan_range), math.isnan(scan_range))
+            else:
+                self.assertEqual(scan_range, expected_scan_range)
+
+
+PKG = 'laser_filters'
+NAME = 'test_speckle_filter'
+if __name__ == '__main__':
+    rostest.unitrun(PKG, NAME, TestSpeckleFilter)

src/laser_filters/test/test_speckle_filter.launch

@@ -0,0 +1,13 @@
+<launch>
+  <test test-name="test_speckle_filter" pkg="laser_filters" type="test_speckle_filter" />
+
+  <node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter_distance">
+        <remap from="scan_filtered" to="scan_filtered_distance" />
+        <rosparam command="load" file="$(find laser_filters)/test/test_speckle_filter_distance.yaml" />
+  </node>
+
+  <node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter_euclidean">
+        <remap from="scan_filtered" to="scan_filtered_euclidean" />
+        <rosparam command="load" file="$(find laser_filters)/test/test_speckle_filter_euclidean.yaml" />
+  </node>
+</launch>

src/laser_filters/test/test_speckle_filter_distance.yaml

@@ -0,0 +1,8 @@
+scan_filter_chain:
+- name: speckle_filter
+  type: laser_filters/LaserScanSpeckleFilter
+  params:
+    filter_type: 0
+    max_range: 2.0
+    max_range_difference: 0.1
+    filter_window: 2

src/laser_filters/test/test_speckle_filter_euclidean.yaml

@@ -0,0 +1,8 @@
+scan_filter_chain:
+- name: speckle_filter
+  type: laser_filters/LaserScanSpeckleFilter
+  params:
+    filter_type: 1
+    max_range: 2.0
+    max_range_difference: 0.1
+    filter_window: 2

src/rasp_camera/launch/color_recognition.launch

@@ -0,0 +1,4 @@
+<launch>
+  <node name='color_recognition' pkg="rasp_camera" type="color_recognition.py" output="screen">
+  </node>
+</launch>

src/rasp_camera/launch/mnist_recognition.launch

@@ -0,0 +1,4 @@
+<launch>
+  <node name='mnist_recognition' pkg="rasp_camera" type="mnist_recognition.py" output="screen">
+  </node>
+</launch>

src/rasp_camera/launch/rasp_camera.launch

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="enable_raw" default="false"/>
+  <arg name="enable_raw" default="true"/>
   <arg name="enable_imv" default="false"/>
   <arg name="camera_id" default="0"/>
   <arg name="camera_frame_id" default="raspicam"/>

src/rasp_camera/scripts/color_recognition.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+import rospy, cv2, cv_bridge, math, datetime
+import numpy as np
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64
+
+ball_color = 'red'
+
+color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
+              'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
+              'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
+              'black': {'Lower': np.array([0, 0, 0]), 'Upper': np.array([180, 255, 46])},
+              'yellow': {'Lower': np.array([21, 39, 64]), 'Upper': np.array([34, 255, 255])},
+              }
+class Color_Recognition:
+
+    def __init__(self):
+        self.bridge = cv_bridge.CvBridge()
+        # cv2.namedWindow('window', 1)
+
+        self.image_sub = rospy.Subscriber('/raspicam_node/image', Image, self.image_callback)
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+
+
+        self.twist = Twist()
+        self.err = 0
+        self.cnt = 0
+
+        # self.lift_height_pub.publish(20)
+
+
+    def image_callback(self, msg):
+        # To get the image from the camera and convert it to binary image by using opencv
+        img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+
+        cv2.imshow('window_img',img)
+        cv2.waitKey(3)
+        
+        gs_frame = cv2.GaussianBlur(img, (5, 5), 0)
+        hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)                 # 转化成HSV图像
+        erode_hsv = cv2.erode(hsv, None, iterations=2)                   # 腐蚀 粗的变细
+        inRange_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'], color_dist[ball_color]['Upper'])
+        cnts = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
+
+        if len(cnts) == 0:
+            return
+        c = max(cnts, key=cv2.contourArea)
+        print("area:",cv2.contourArea(c))
+        rect = cv2.minAreaRect(c)
+        box = cv2.boxPoints(rect)
+        cv2.drawContours(img, [np.int0(box)], -1, (0, 255, 255), 2)
+
+        cv2.imshow('window_contours', img)
+        cv2.waitKey(3)
+        x_center = 160
+        current_center = 0.5*(box[2,0]-box[0,0])+(box[0,0])
+        err_center = current_center - x_center
+
+        area = 3000
+        current_area = cv2.contourArea(c)
+        err_area = current_area - area
+        
+        self.twist.linear.x = 0
+        self.twist.angular.z = 0
+
+        if abs(err_area)>500:
+            self.twist.linear.x = 0.0074 * -err_area * 0.006
+            print 'linear.x', self.twist.linear.x
+        if abs(err_center)>5:
+            self.twist.angular.z = 0.74 * -err_center * 0.006
+            print 'angular.z', self.twist.angular.z
+        self.cmd_vel_pub.publish(self.twist)
+
+
+if __name__ == '__main__':
+    rospy.init_node('color_recognition')
+    color_recognition = Color_Recognition()
+    rospy.spin()

src/rasp_camera/scripts/functions.py

@@ -0,0 +1,61 @@
+# coding: utf-8
+import numpy as np
+
+
+def identity_function(x):
+    return x
+
+
+def step_function(x):
+    return np.array(x > 0, dtype=np.int)
+
+
+def sigmoid(x):
+    return 1 / (1 + np.exp(-x))    
+
+
+def sigmoid_grad(x):
+    return (1.0 - sigmoid(x)) * sigmoid(x)
+    
+
+def relu(x):
+    return np.maximum(0, x)
+
+
+def relu_grad(x):
+    grad = np.zeros(x)
+    grad[x>=0] = 1
+    return grad
+    
+
+def softmax(x):
+    if x.ndim == 2:
+        x = x.T
+        x = x - np.max(x, axis=0)
+        y = np.exp(x) / np.sum(np.exp(x), axis=0)
+        return y.T 
+
+    x = x - np.max(x) # 溢出对策
+    return np.exp(x) / np.sum(np.exp(x))
+
+
+def mean_squared_error(y, t):
+    return 0.5 * np.sum((y-t)**2)
+
+
+def cross_entropy_error(y, t):
+    if y.ndim == 1:
+        t = t.reshape(1, t.size)
+        y = y.reshape(1, y.size)
+        
+    # 监督数据是one-hot-vector的情况下，转换为正确解标签的索引
+    if t.size == y.size:
+        t = t.argmax(axis=1)
+             
+    batch_size = y.shape[0]
+    return -np.sum(np.log(y[np.arange(batch_size), t] + 1e-7)) / batch_size
+
+
+def softmax_loss(X, t):
+    y = softmax(X)
+    return cross_entropy_error(y, t)

src/rasp_camera/scripts/gradient.py

@@ -0,0 +1,53 @@
+# coding: utf-8
+import numpy as np
+
+def _numerical_gradient_1d(f, x):
+    h = 1e-4 # 0.0001
+    grad = np.zeros_like(x)
+    
+    for idx in range(x.size):
+        tmp_val = x[idx]
+        x[idx] = float(tmp_val) + h
+        fxh1 = f(x) # f(x+h)
+        
+        x[idx] = tmp_val - h 
+        fxh2 = f(x) # f(x-h)
+        grad[idx] = (fxh1 - fxh2) / (2*h)
+        
+        x[idx] = tmp_val # 还原值
+        
+    return grad
+
+
+def numerical_gradient_2d(f, X):
+    if X.ndim == 1:
+        return _numerical_gradient_1d(f, X)
+    else:
+        grad = np.zeros_like(X)
+        
+        for idx, x in enumerate(X):
+            grad[idx] = _numerical_gradient_1d(f, x)
+        
+        return grad
+
+
+def numerical_gradient(f, x):
+    h = 1e-4 # 0.0001
+    grad = np.zeros_like(x)
+    
+    # 多维迭代
+    it = np.nditer(x, flags=['multi_index'], op_flags=['readwrite'])
+    while not it.finished:
+        idx = it.multi_index
+        tmp_val = x[idx]
+        x[idx] = float(tmp_val) + h
+        fxh1 = f(x) # f(x+h)
+        
+        x[idx] = tmp_val - h 
+        fxh2 = f(x) # f(x-h)
+        grad[idx] = (fxh1 - fxh2) / (2*h)
+        
+        x[idx] = tmp_val # 还原值
+        it.iternext()   
+        
+    return grad

src/rasp_camera/scripts/layers.py

@@ -0,0 +1,284 @@
+# coding: utf-8
+import numpy as np
+from functions import *
+from util import im2col, col2im
+
+
+class Relu:
+    def __init__(self):
+        self.mask = None
+
+    def forward(self, x):
+        self.mask = (x <= 0)
+        out = x.copy()
+        out[self.mask] = 0
+
+        return out
+
+    def backward(self, dout):
+        dout[self.mask] = 0
+        dx = dout
+
+        return dx
+
+
+class Sigmoid:
+    def __init__(self):
+        self.out = None
+
+    def forward(self, x):
+        out = sigmoid(x)
+        self.out = out
+        return out
+
+    def backward(self, dout):
+        dx = dout * (1.0 - self.out) * self.out
+
+        return dx
+
+
+class Affine:
+    def __init__(self, W, b):
+        self.W =W
+        self.b = b
+        
+        self.x = None
+        self.original_x_shape = None
+        # 权重和偏置参数的导数
+        self.dW = None
+        self.db = None
+
+    def forward(self, x):
+        # 对应张量
+        self.original_x_shape = x.shape
+        x = x.reshape(x.shape[0], -1)
+        self.x = x
+
+        out = np.dot(self.x, self.W) + self.b
+
+        return out
+
+    def backward(self, dout):
+        dx = np.dot(dout, self.W.T)
+        self.dW = np.dot(self.x.T, dout)
+        self.db = np.sum(dout, axis=0)
+        
+        dx = dx.reshape(*self.original_x_shape)  # 还原输入数据的形状（对应张量）
+        return dx
+
+
+class SoftmaxWithLoss:
+    def __init__(self):
+        self.loss = None
+        self.y = None # softmax的输出
+        self.t = None # 监督数据
+
+    def forward(self, x, t):
+        self.t = t
+        self.y = softmax(x)
+        self.loss = cross_entropy_error(self.y, self.t)
+        
+        return self.loss
+
+    def backward(self, dout=1):
+        batch_size = self.t.shape[0]
+        if self.t.size == self.y.size: # 监督数据是one-hot-vector的情况
+            dx = (self.y - self.t) / batch_size
+        else:
+            dx = self.y.copy()
+            dx[np.arange(batch_size), self.t] -= 1
+            dx = dx / batch_size
+        
+        return dx
+
+
+class Dropout:
+    """
+    http://arxiv.org/abs/1207.0580
+    """
+    def __init__(self, dropout_ratio=0.5):
+        self.dropout_ratio = dropout_ratio
+        self.mask = None
+
+    def forward(self, x, train_flg=True):
+        if train_flg:
+            self.mask = np.random.rand(*x.shape) > self.dropout_ratio
+            return x * self.mask
+        else:
+            return x * (1.0 - self.dropout_ratio)
+
+    def backward(self, dout):
+        return dout * self.mask
+
+
+class BatchNormalization:
+    """
+    http://arxiv.org/abs/1502.03167
+    """
+    def __init__(self, gamma, beta, momentum=0.9, running_mean=None, running_var=None):
+        self.gamma = gamma
+        self.beta = beta
+        self.momentum = momentum
+        self.input_shape = None # Conv层的情况下为4维，全连接层的情况下为2维  
+
+        # 测试时使用的平均值和方差
+        self.running_mean = running_mean
+        self.running_var = running_var  
+        
+        # backward时使用的中间数据
+        self.batch_size = None
+        self.xc = None
+        self.std = None
+        self.dgamma = None
+        self.dbeta = None
+
+    def forward(self, x, train_flg=True):
+        self.input_shape = x.shape
+        if x.ndim != 2:
+            N, C, H, W = x.shape
+            x = x.reshape(N, -1)
+
+        out = self.__forward(x, train_flg)
+        
+        return out.reshape(*self.input_shape)
+            
+    def __forward(self, x, train_flg):
+        if self.running_mean is None:
+            N, D = x.shape
+            self.running_mean = np.zeros(D)
+            self.running_var = np.zeros(D)
+                        
+        if train_flg:
+            mu = x.mean(axis=0)
+            xc = x - mu
+            var = np.mean(xc**2, axis=0)
+            std = np.sqrt(var + 10e-7)
+            xn = xc / std
+            
+            self.batch_size = x.shape[0]
+            self.xc = xc
+            self.xn = xn
+            self.std = std
+            self.running_mean = self.momentum * self.running_mean + (1-self.momentum) * mu
+            self.running_var = self.momentum * self.running_var + (1-self.momentum) * var            
+        else:
+            xc = x - self.running_mean
+            xn = xc / ((np.sqrt(self.running_var + 10e-7)))
+            
+        out = self.gamma * xn + self.beta 
+        return out
+
+    def backward(self, dout):
+        if dout.ndim != 2:
+            N, C, H, W = dout.shape
+            dout = dout.reshape(N, -1)
+
+        dx = self.__backward(dout)
+
+        dx = dx.reshape(*self.input_shape)
+        return dx
+
+    def __backward(self, dout):
+        dbeta = dout.sum(axis=0)
+        dgamma = np.sum(self.xn * dout, axis=0)
+        dxn = self.gamma * dout
+        dxc = dxn / self.std
+        dstd = -np.sum((dxn * self.xc) / (self.std * self.std), axis=0)
+        dvar = 0.5 * dstd / self.std
+        dxc += (2.0 / self.batch_size) * self.xc * dvar
+        dmu = np.sum(dxc, axis=0)
+        dx = dxc - dmu / self.batch_size
+        
+        self.dgamma = dgamma
+        self.dbeta = dbeta
+        
+        return dx
+
+
+class Convolution:
+    def __init__(self, W, b, stride=1, pad=0):
+        self.W = W
+        self.b = b
+        self.stride = stride
+        self.pad = pad
+        
+        # 中间数据（backward时使用）
+        self.x = None   
+        self.col = None
+        self.col_W = None
+        
+        # 权重和偏置参数的梯度
+        self.dW = None
+        self.db = None
+
+    def forward(self, x):
+        FN, C, FH, FW = self.W.shape
+        N, C, H, W = x.shape
+        out_h = 1 + int((H + 2*self.pad - FH) / self.stride)
+        out_w = 1 + int((W + 2*self.pad - FW) / self.stride)
+
+        col = im2col(x, FH, FW, self.stride, self.pad)
+        col_W = self.W.reshape(FN, -1).T
+
+        out = np.dot(col, col_W) + self.b
+        out = out.reshape(N, out_h, out_w, -1).transpose(0, 3, 1, 2)
+
+        self.x = x
+        self.col = col
+        self.col_W = col_W
+
+        return out
+
+    def backward(self, dout):
+        FN, C, FH, FW = self.W.shape
+        dout = dout.transpose(0,2,3,1).reshape(-1, FN)
+
+        self.db = np.sum(dout, axis=0)
+        self.dW = np.dot(self.col.T, dout)
+        self.dW = self.dW.transpose(1, 0).reshape(FN, C, FH, FW)
+
+        dcol = np.dot(dout, self.col_W.T)
+        dx = col2im(dcol, self.x.shape, FH, FW, self.stride, self.pad)
+
+        return dx
+
+
+class Pooling:
+    def __init__(self, pool_h, pool_w, stride=1, pad=0):
+        self.pool_h = pool_h
+        self.pool_w = pool_w
+        self.stride = stride
+        self.pad = pad
+        
+        self.x = None
+        self.arg_max = None
+
+    def forward(self, x):
+        N, C, H, W = x.shape
+        out_h = int(1 + (H - self.pool_h) / self.stride)
+        out_w = int(1 + (W - self.pool_w) / self.stride)
+
+        col = im2col(x, self.pool_h, self.pool_w, self.stride, self.pad)
+        col = col.reshape(-1, self.pool_h*self.pool_w)
+
+        arg_max = np.argmax(col, axis=1)
+        out = np.max(col, axis=1)
+        out = out.reshape(N, out_h, out_w, C).transpose(0, 3, 1, 2)
+
+        self.x = x
+        self.arg_max = arg_max
+
+        return out
+
+    def backward(self, dout):
+        dout = dout.transpose(0, 2, 3, 1)
+        
+        pool_size = self.pool_h * self.pool_w
+        dmax = np.zeros((dout.size, pool_size))
+        dmax[np.arange(self.arg_max.size), self.arg_max.flatten()] = dout.flatten()
+        dmax = dmax.reshape(dout.shape + (pool_size,)) 
+        
+        dcol = dmax.reshape(dmax.shape[0] * dmax.shape[1] * dmax.shape[2], -1)
+        dx = col2im(dcol, self.x.shape, self.pool_h, self.pool_w, self.stride, self.pad)
+        
+        return dx

src/rasp_camera/scripts/line_follow.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+import rospy, cv2, cv_bridge, numpy, math, datetime
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64
+
+
+class Follower:
+
+    def __init__(self):
+        self.bridge = cv_bridge.CvBridge()
+        # cv2.namedWindow('window', 1)
+
+        self.image_sub = rospy.Subscriber('/raspicam_node/image', Image, self.image_callback)
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+
+
+        self.twist = Twist()
+        self.err = 0
+        self.cnt = 0
+
+        # self.lift_height_pub.publish(20)
+
+
+    def image_callback(self, msg):
+        # To get the image from the camera and convert it to binary image by using opencv
+        img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+
+        cv2.imshow('window_img',img)
+        cv2.waitKey(3)
+
+        
+
+        # ROI
+        # cropped = img[y0:y1, x0:x1]
+        cropped = img[190:240, 20:300]
+
+        cv2.imshow('window_cropped',cropped)
+        cv2.waitKey(3)
+
+        # Binarization
+        gray = cv2.cvtColor(cropped, cv2.COLOR_RGB2GRAY)
+
+        cv2.imshow('window_gray', gray)
+        cv2.waitKey(3)
+
+        ret, binimg = cv2.threshold(gray, 90, 255, cv2.THRESH_BINARY)
+
+        cv2.imshow('window_BIN', binimg)
+        cv2.waitKey(3)
+
+        # calculate center of bounding box
+        y, x = (binimg < 125).nonzero()
+
+        # Judge center of line
+        x0 = 160
+        if x.shape[0] != 0:
+            y0 = (max(y) + min(y))/2
+            x0 = (max(x) + min(x))/2
+            # print '(x0,y0):', x0, y0
+
+        # print 'x.shape[0]:',  x.shape[0]
+        if x.shape[0] < 100:
+            self.cnt  += 1
+            if self.cnt > 20:
+                self.twist.linear.x = 0.0
+                self.twist.angular.z = 0.0
+                print 'Stop!'
+        else:    
+            self.cnt = 0
+            # control
+            err = x0 - 160
+            print 'err:', err
+            self.twist.linear.x = 0.08
+            self.twist.angular.z = 0.0
+            if abs(err) > 5:
+                self.twist.angular.z = 0.74 * -err * 0.006
+                print 'angular.z', self.twist.angular.z
+                print 'Turn'
+            else:
+                print 'Go ahead!'
+        self.cmd_vel_pub.publish(self.twist)
+        # self.lift_height_pub.publish(20)
+
+
+if __name__ == '__main__':
+    rospy.init_node('follow')
+    follow = Follower()
+    rospy.spin()

src/rasp_camera/scripts/mnist_recognition.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+import sys, os,math
+import numpy as np
+from PIL import Image as im
+
+from simple_convnet import SimpleConvNet
+from functions import softmax
+
+import rospy, cv2, cv_bridge
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+network = SimpleConvNet(input_dim=(1,28,28), 
+                        conv_param = {'filter_num': 30, 'filter_size': 5, 'pad': 0, 'stride': 1},
+                        hidden_size=100, output_size=10, weight_init_std=0.01)
+network.load_params("/home/corvin/panda_ws/src/rasp_camera/scripts/params.pkl")
+
+ball_color = 'red'
+
+color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
+              'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
+              'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
+              'black': {'Lower': np.array([0, 0, 0]), 'Upper': np.array([180, 255, 46])},
+              'yellow': {'Lower': np.array([21, 39, 64]), 'Upper': np.array([34, 255, 255])},
+              }
+
+class Mnist_Recognition:
+
+    def __init__(self):
+        self.bridge = cv_bridge.CvBridge()
+        self.image_sub = rospy.Subscriber('/raspicam_node/image', Image, self.image_callback)
+        self.result_pub = rospy.Publisher('/voice_system/iflytek_offline_tts_topic',String,queue_size=1)
+        self.result_string = String()
+        self.confidence_threshold = 0.95
+        self.last_resut = -1
+
+
+    def image_callback(self, msg):
+        # To get the image from the camera and convert it to binary image by using opencv
+        img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        cv2.imshow('window_img',img)
+        cv2.waitKey(3)
+
+        gs_frame = cv2.GaussianBlur(img, (5, 5), 0)
+        hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)                 # 转化成HSV图像
+        erode_hsv = cv2.erode(hsv, None, iterations=2)                   # 腐蚀 粗的变细
+        inRange_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'], color_dist[ball_color]['Upper'])
+        
+        cv2.imshow('window_inrange_img',inRange_hsv)
+        cv2.waitKey(3)
+
+        cnts = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
+        if len(cnts) == 0:
+            print("no region")
+            return
+        c = max(cnts, key=cv2.contourArea)
+        rect = cv2.minAreaRect(c)
+        box = cv2.boxPoints(rect)
+        cv2.drawContours(img, [np.int0(box)], -1, (0, 255, 255), 2)
+
+        cv2.imshow('window_contours', img)
+        cv2.waitKey(3)
+
+        y0 = int(math.floor(min(box[0,1],box[1,1],box[2,1],box[3,1])))-10
+        y1 = int(math.ceil(max(box[0,1],box[1,1],box[2,1],box[3,1])))+10
+        x0 = int(math.floor(min(box[0,0],box[1,0],box[2,0],box[3,0])))-10
+        x1 = int(math.ceil(max(box[0,0],box[1,0],box[2,0],box[3,0])))+10
+
+        x_err = x1 - x0
+        y_err = y1 - y0
+        if y_err > x_err :
+            err = y_err - x_err
+            x0 = x0 - err/2
+            x1 = x1 + err/2
+        else :
+            err = x_err - y_err
+            y0 = y0 - err/2
+            y1 = y1 + err/2
+        if x0<0 or y0<0 or x0 >320 or y0 >240:
+            print("move number")
+            return
+
+        cropped = inRange_hsv[y0:y1, x0:x1]
+        cv2.imshow('window_cropped', cropped)
+        cv2.waitKey(3)
+
+        resized_img = cv2.resize(cropped,(28,28),interpolation = cv2.INTER_AREA)
+        cv2.imshow('window_resizedimg',resized_img)
+        cv2.waitKey(3)
+
+        img_array = resized_img.reshape(1,1,28, 28) / 255.0
+
+        __result = network.predict(img_array)  
+        __result = softmax(__result)
+        result = np.argmax(__result)          # 预测的数字
+        confidence = __result[0,result]
+        if confidence < self.confidence_threshold:
+            print("unvalid result")
+            return
+        print("result and confidence is ",result,confidence)
+        if result != self.last_resut:
+            self.result_string.data = str(result)
+            self.result_pub.publish(self.result_string)
+            self.last_resut = result
+        
+
+
+if __name__ == '__main__':
+    rospy.init_node('mnist_recognition')
+    mnist_recognition = Mnist_Recognition()
+    rospy.spin()

src/rasp_camera/scripts/simple_convnet.py

@@ -0,0 +1,160 @@
+# coding: utf-8
+import sys, os
+sys.path.append(os.pardir)  # 为了导入父目录的文件而进行的设定
+import pickle
+import numpy as np
+from collections import OrderedDict
+from layers import *
+from gradient import numerical_gradient
+
+
+class SimpleConvNet:
+    """简单的ConvNet
+
+    conv - relu - pool - affine - relu - affine - softmax
+    
+    Parameters
+    ----------
+    input_size : 输入大小（MNIST的情况下为784）
+    hidden_size_list : 隐藏层的神经元数量的列表（e.g. [100, 100, 100]）
+    output_size : 输出大小（MNIST的情况下为10）
+    activation : 'relu' or 'sigmoid'
+    weight_init_std : 指定权重的标准差（e.g. 0.01）
+        指定'relu'或'he'的情况下设定“He的初始值”
+        指定'sigmoid'或'xavier'的情况下设定“Xavier的初始值”
+    """
+    def __init__(self, input_dim=(1, 28, 28), 
+                 conv_param={'filter_num':30, 'filter_size':5, 'pad':0, 'stride':1},
+                 hidden_size=100, output_size=10, weight_init_std=0.01):
+        filter_num = conv_param['filter_num']
+        filter_size = conv_param['filter_size']
+        filter_pad = conv_param['pad']
+        filter_stride = conv_param['stride']
+        input_size = input_dim[1]
+        conv_output_size = (input_size - filter_size + 2*filter_pad) / filter_stride + 1
+        pool_output_size = int(filter_num * (conv_output_size/2) * (conv_output_size/2))
+
+        # 初始化权重
+        self.params = {}
+        self.params['W1'] = weight_init_std * \
+                            np.random.randn(filter_num, input_dim[0], filter_size, filter_size)
+        self.params['b1'] = np.zeros(filter_num)
+        self.params['W2'] = weight_init_std * \
+                            np.random.randn(pool_output_size, hidden_size)
+        self.params['b2'] = np.zeros(hidden_size)
+        self.params['W3'] = weight_init_std * \
+                            np.random.randn(hidden_size, output_size)
+        self.params['b3'] = np.zeros(output_size)
+
+        # 生成层
+        self.layers = OrderedDict()
+        self.layers['Conv1'] = Convolution(self.params['W1'], self.params['b1'],
+                                           conv_param['stride'], conv_param['pad'])
+        self.layers['Relu1'] = Relu()
+        self.layers['Pool1'] = Pooling(pool_h=2, pool_w=2, stride=2)
+        self.layers['Affine1'] = Affine(self.params['W2'], self.params['b2'])
+        self.layers['Relu2'] = Relu()
+        self.layers['Affine2'] = Affine(self.params['W3'], self.params['b3'])
+
+        self.last_layer = SoftmaxWithLoss()
+
+    def predict(self, x):
+        for layer in self.layers.values():
+            x = layer.forward(x)
+
+        return x
+
+    def loss(self, x, t):
+        """求损失函数
+        参数x是输入数据、t是教师标签
+        """
+        y = self.predict(x)
+        return self.last_layer.forward(y, t)
+
+    def accuracy(self, x, t, batch_size=100):
+        if t.ndim != 1 : t = np.argmax(t, axis=1)
+        
+        acc = 0.0
+        
+        for i in range(int(x.shape[0] / batch_size)):
+            tx = x[i*batch_size:(i+1)*batch_size]
+            tt = t[i*batch_size:(i+1)*batch_size]
+            y = self.predict(tx)
+            y = np.argmax(y, axis=1)
+            acc += np.sum(y == tt) 
+        
+        return acc / x.shape[0]
+
+    def numerical_gradient(self, x, t):
+        """求梯度（数值微分）
+
+        Parameters
+        ----------
+        x : 输入数据
+        t : 教师标签
+
+        Returns
+        -------
+        具有各层的梯度的字典变量
+            grads['W1']、grads['W2']、...是各层的权重
+            grads['b1']、grads['b2']、...是各层的偏置
+        """
+        loss_w = lambda w: self.loss(x, t)
+
+        grads = {}
+        for idx in (1, 2, 3):
+            grads['W' + str(idx)] = numerical_gradient(loss_w, self.params['W' + str(idx)])
+            grads['b' + str(idx)] = numerical_gradient(loss_w, self.params['b' + str(idx)])
+
+        return grads
+
+    def gradient(self, x, t):
+        """求梯度（误差反向传播法）
+
+        Parameters
+        ----------
+        x : 输入数据
+        t : 教师标签
+
+        Returns
+        -------
+        具有各层的梯度的字典变量
+            grads['W1']、grads['W2']、...是各层的权重
+            grads['b1']、grads['b2']、...是各层的偏置
+        """
+        # forward
+        self.loss(x, t)
+
+        # backward
+        dout = 1
+        dout = self.last_layer.backward(dout)
+
+        layers = list(self.layers.values())
+        layers.reverse()
+        for layer in layers:
+            dout = layer.backward(dout)
+
+        # 设定
+        grads = {}
+        grads['W1'], grads['b1'] = self.layers['Conv1'].dW, self.layers['Conv1'].db
+        grads['W2'], grads['b2'] = self.layers['Affine1'].dW, self.layers['Affine1'].db
+        grads['W3'], grads['b3'] = self.layers['Affine2'].dW, self.layers['Affine2'].db
+
+        return grads
+        
+    def save_params(self, file_name="params.pkl"):
+        params = {}
+        for key, val in self.params.items():
+            params[key] = val
+        with open(file_name, 'wb') as f:
+            pickle.dump(params, f)
+
+    def load_params(self, file_name="params.pkl"):
+        with open(file_name, 'rb') as f:
+            params = pickle.load(f)
+        for key, val in params.items():
+            self.params[key] = val
+
+        for i, key in enumerate(['Conv1', 'Affine1', 'Affine2']):
+            self.layers[key].W = self.params['W' + str(i+1)]
+            self.layers[key].b = self.params['b' + str(i+1)]

src/rasp_camera/scripts/util.py

@@ -0,0 +1,99 @@
+# coding: utf-8
+import numpy as np
+
+
+def smooth_curve(x):
+    """用于使损失函数的图形变圆滑
+
+    参考：http://glowingpython.blogspot.jp/2012/02/convolution-with-numpy.html
+    """
+    window_len = 11
+    s = np.r_[x[window_len-1:0:-1], x, x[-1:-window_len:-1]]
+    w = np.kaiser(window_len, 2)
+    y = np.convolve(w/w.sum(), s, mode='valid')
+    return y[5:len(y)-5]
+
+
+def shuffle_dataset(x, t):
+    """打乱数据集
+
+    Parameters
+    ----------
+    x : 训练数据
+    t : 监督数据
+
+    Returns
+    -------
+    x, t : 打乱的训练数据和监督数据
+    """
+    permutation = np.random.permutation(x.shape[0])
+    x = x[permutation,:] if x.ndim == 2 else x[permutation,:,:,:]
+    t = t[permutation]
+
+    return x, t
+
+def conv_output_size(input_size, filter_size, stride=1, pad=0):
+    return (input_size + 2*pad - filter_size) / stride + 1
+
+
+def im2col(input_data, filter_h, filter_w, stride=1, pad=0):
+    """
+
+    Parameters
+    ----------
+    input_data : 由(数据量, 通道, 高, 长)的4维数组构成的输入数据
+    filter_h : 滤波器的高
+    filter_w : 滤波器的长
+    stride : 步幅
+    pad : 填充
+
+    Returns
+    -------
+    col : 2维数组
+    """
+    N, C, H, W = input_data.shape
+    out_h = (H + 2*pad - filter_h)//stride + 1
+    out_w = (W + 2*pad - filter_w)//stride + 1
+
+    img = np.pad(input_data, [(0,0), (0,0), (pad, pad), (pad, pad)], 'constant')
+    col = np.zeros((N, C, filter_h, filter_w, out_h, out_w))
+
+    for y in range(filter_h):
+        y_max = y + stride*out_h
+        for x in range(filter_w):
+            x_max = x + stride*out_w
+            col[:, :, y, x, :, :] = img[:, :, y:y_max:stride, x:x_max:stride]
+
+    col = col.transpose(0, 4, 5, 1, 2, 3).reshape(N*out_h*out_w, -1)
+    return col
+
+
+def col2im(col, input_shape, filter_h, filter_w, stride=1, pad=0):
+    """
+
+    Parameters
+    ----------
+    col :
+    input_shape : 输入数据的形状（例：(10, 1, 28, 28)）
+    filter_h :
+    filter_w
+    stride
+    pad
+
+    Returns
+    -------
+
+    """
+    N, C, H, W = input_shape
+    out_h = (H + 2*pad - filter_h)//stride + 1
+    out_w = (W + 2*pad - filter_w)//stride + 1
+    col = col.reshape(N, out_h, out_w, C, filter_h, filter_w).transpose(0, 3, 4, 5, 1, 2)
+
+    img = np.zeros((N, C, H + 2*pad + stride - 1, W + 2*pad + stride - 1))
+    for y in range(filter_h):
+        y_max = y + stride*out_h
+        for x in range(filter_w):
+            x_max = x + stride*out_w
+            img[:, :, y:y_max:stride, x:x_max:stride] += col[:, :, y, x, :, :]
+
+    return img[:, :, pad:H + pad, pad:W + pad]

src/robot_ar_track/CMakeLists.txt

@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_ar_track)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES robi_ar_location
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/robi_ar_location.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/robi_ar_location_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_robi_ar_location.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/robot_ar_track/cfg/PID_param.yaml

@@ -0,0 +1,3 @@
+P: [0.7, 0.6] 
+I: [0.5,0.04]
+D: [0.00, 0.000]

src/robot_ar_track/launch/robot_ar_track.launch

@@ -0,0 +1,11 @@
+<launch>
+  <node name='robot_ar_track' pkg="robot_ar_track" type="robot_ar_track.py">
+    <param name='maxSpeed' value='0.2' type='double' />
+    <param name='targetDist' value='0.2' type='double' />
+    <rosparam ns='PID_controller' command='load' file='$(find robot_ar_track)/cfg/PID_param.yaml' />
+  </node>
+  <include file="$(find usb_cam)/launch/usb_cam-test.launch"/>
+  <include file="$(find aruco_ros)/launch/single.launch"/>
+  <!--include file="$(find robi_ar_location)/launch/my_sim_move.launch"/ -->
+</launch>
+

src/robot_ar_track/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_ar_track</name>
+  <version>0.0.0</version>
+  <description>The robi_ar_location package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="xue@todo.todo">xue</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/robi_ar_location</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/robot_ar_track/src/robot_ar_track.py

@@ -0,0 +1,123 @@
+#!/usr/bin/env python
+
+import rospy
+import thread, threading
+import time
+import numpy as np
+import math
+from sensor_msgs.msg import Joy, LaserScan
+from geometry_msgs.msg import Twist, Vector3
+from geometry_msgs.msg import Vector3Stamped
+from std_msgs.msg import String as StringMsg
+from std_msgs.msg import Int16
+from visualization_msgs.msg import Marker
+
+
+class arPose:
+    def __init__(self):
+        self.controllerLossTimer = threading.Timer(1, self.controllerLoss) 
+        self.controllerLossTimer.start()
+        self.max_speed = rospy.get_param('~maxSpeed') 
+
+        self.cmdVelPublisher = rospy.Publisher('/cmd_vel', Twist, queue_size=3)
+        self.positionSubscriber = rospy.Subscriber('/aruco_single/marker', Marker, self.positionUpdateCallback)
+       
+        targetDist = rospy.get_param('~targetDist')
+        PID_param = rospy.get_param('~PID_controller')
+
+        self.PID_controller = simplePID([0, targetDist], PID_param['P'], PID_param['I'], PID_param['D'])
+
+        rospy.on_shutdown(self.controllerLoss)
+
+
+    def positionUpdateCallback(self, position):
+
+        angleX= position.pose.position.x
+        distance = position.pose.position.z
+
+        rospy.loginfo('Angle: {}, Distance: {}, '.format(angleX, distance))
+		
+        [uncliped_ang_speed, uncliped_lin_speed] = self.PID_controller.update([angleX, distance])
+			
+        angularSpeed = -np.clip(-uncliped_ang_speed, -self.max_speed, self.max_speed) * 5
+        linearSpeed  = np.clip(-uncliped_lin_speed, -self.max_speed, self.max_speed)
+		
+
+        velocity = Twist()	
+                
+        velocity.linear = Vector3(linearSpeed,0,0.)
+        velocity.angular= Vector3(0., 0.,angularSpeed)
+        if abs(angleX) < 0.001:
+            velocity.angular.z = 0
+        if 0.14 < distance < 0.16:
+            velocity.linear.x = 0
+        rospy.loginfo('linearSpeed: {}, angularSpeed: {}'.format(linearSpeed, angularSpeed))
+        self.cmdVelPublisher.publish(velocity)
+		
+
+    def stopMoving(self):
+        velocity = Twist()
+        velocity.linear = Vector3(0.,0.,0.)
+        velocity.angular= Vector3(0.,0.,0.)
+        self.cmdVelPublisher.publish(velocity)
+
+    def controllerLoss(self):
+        self.stopMoving()
+        self.active = False
+        rospy.loginfo('lost connection')
+
+
+class simplePID:
+    def __init__(self, target, P, I, D):
+        if(not(np.size(P)==np.size(I)==np.size(D)) or ((np.size(target)==1) and np.size(P)!=1) or (np.size(target )!=1 and (np.size(P) != np.size(target) and (np.size(P) != 1)))):
+            raise TypeError('input parameters shape is not compatable')
+        rospy.loginfo('PID initialised with P:{}, I:{}, D:{}'.format(P,I,D))
+        self.Kp		=np.array(P)
+        self.Ki		=np.array(I)
+        self.Kd		=np.array(D)
+        self.setPoint   =np.array(target)
+		
+        self.last_error=0
+        self.integrator = 0
+        self.integrator_max = float('inf')
+        self.timeOfLastCall = None 
+		
+		
+    def update(self, current_value):
+
+        current_value=np.array(current_value)
+        if(np.size(current_value) != np.size(self.setPoint)):
+            raise TypeError('current_value and target do not have the same shape')
+        if(self.timeOfLastCall is None):
+            self.timeOfLastCall = time.clock()
+            return np.zeros(np.size(current_value))
+
+		
+        error = self.setPoint - current_value
+        P =  error
+		
+        currentTime = time.clock()
+        deltaT      = (currentTime-self.timeOfLastCall)
+
+        self.integrator = self.integrator + (error*deltaT)
+        I = self.integrator
+		
+        D = (error-self.last_error)/deltaT
+		
+        self.last_error = error
+        self.timeOfLastCall = currentTime
+		
+        return self.Kp*P + self.Ki*I + self.Kd*D
+
+
+
+
+
+if __name__ == '__main__':
+    print('starting')
+    rospy.init_node('robot_ar')
+    arpose = arPose()
+    try:
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        print('exception')

src/robot_bringup/launch/robot_bringup.launch

@@ -39,4 +39,7 @@
         <remap from="input" to="/robot_pose_ekf/odom_combined" />
         <remap from="output" to="/odom_ekf" />
     </node>
+
+    <!-- (7) startup voice system -->
+    <include file="$(find voice_system)/launch/startup_voice_system.launch" />
 </launch>

src/robot_bringup/launch/robot_bringup_multi.launch

@@ -0,0 +1,35 @@
+<!--
+  Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+  Description:robot启动时最先需要启动的launch文件，主要加载各基础功能模块启动。
+    首先就需要启动机器人的urdf文件,这样就可以在rviz中可视化显示机器人模型.然后
+    启动下位机arduino的程序,上下位机建立连接,这样上位机就可以发送控制命令.接下来
+    启动IMU模块,开始发布姿态信息.接下来就开始进行信息融合,使用imu信息和轮式里程计
+    这样得出的里程计更为准确.
+  Author: corvin
+  History:
+    20200716:init this file.
+    20201212: add multi robot.
+-->
+<launch>
+    
+    <group ns="robot1">
+    <!-- (1) startup mobilebase arduino launch -->
+    <include file="$(find ros_arduino_python)/launch/arduino_multi.launch" />
+
+    <!-- (2) startup rasp imu-6DOF board-->
+    <include file="$(find rasp_imu_hat_6dof)/launch/imu_data_pub.launch" />
+
+    <!-- (3) startup robot_pose_ekf node -->
+    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+        <param name="output_frame" value="odom_combined"/>
+        <param name="freq" value="10.0"/>
+        <param name="sensor_timeout" value="1.2"/>
+        <param name="odom_used" value="true"/>
+        <param name="imu_used" value="true"/>
+        <param name="vo_used" value="false"/>
+        <param name="debug" value="false"/>
+        <param name="self_diagnose" value="false"/>
+    </node>
+
+    </group>>
+</launch>

src/robot_follower/CMakeLists.txt

@@ -0,0 +1,192 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_follower)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+ add_message_files(
+   FILES
+   position.msg
+ )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+   DEPENDENCIES
+   std_msgs
+   sensor_msgs
+   geometry_msgs
+ )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES simple_follower
+  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(simple_follower
+#   src/${PROJECT_NAME}/simple_follower.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(simple_follower ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(simple_follower_node src/simple_follower_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(simple_follower_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(simple_follower_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS simple_follower simple_follower_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_simple_follower.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/robot_follower/launch/laser_follower.launch

@@ -0,0 +1,5 @@
+<launch>
+  <include file='$(find robot_follower)/launch/robot_lidar.launch' />
+  <include file='$(find robot_follower)/launch/nodes/laserTracker.launch' />
+  <include file='$(find robot_follower)/launch/nodes/follower.launch' />
+</launch>

src/robot_follower/launch/nodes/follower.launch

@@ -0,0 +1,13 @@
+<launch>
+  <node name='follower' pkg="robot_follower" type="follower.py">
+    <!-- switchMode: (if true one button press will change betwenn active, not active. If false the button will have to be kept pressed all the time to be active -->
+    <param name="switchMode" value="True" type="bool" />
+    <!-- maximal speed (angular and linear both), tartet distance: the robot will try to keep this fixed distance -->
+    <param name='maxSpeed' value='0.8' type='double' />
+    <param name='targetDist' value='0.5' type='double' />
+    <!-- index of the button in the buttons field of the joy message from the ps3 controller, that switches active/inactive  -->
+    <param name='controllButtonIndex' value='-4' type='int' />
+    <rosparam ns='PID_controller' command='load' file='$(find robot_follower)/parameters/PID_param.yaml' />
+  </node>
+</launch>
+

src/robot_follower/launch/nodes/laserTracker.launch

@@ -0,0 +1,8 @@
+<launch>
+  <node name='laser_tracker' pkg="robot_follower" type="laserTracker.py">
+    <!-- This is to avoid treating noise in the laser ranges as objects -->
+    <!-- we check for all range measurements around the currently closest distance measurement (size of this window specified by winSize) if they where similarly close in the last scan (threshold specified by deltaDist-->
+    <param name="winSize" value="2" type="int" />
+    <param name="deltaDist" value="0.2" type="double" />
+  </node>
+</launch>

src/robot_follower/launch/nodes/visualTracker.launch

@@ -0,0 +1,17 @@
+<launch>
+  <node name='visual_tracker' pkg="robot_follower" type="visualTracker.py">
+    <!-- color or the target in HSV color space -->
+    <rosparam ns='target'>
+      upper : [0, 120, 100]
+      lower : [9, 255, 255]
+    </rosparam>
+    <rosparam ns='pictureDimensions'>
+      <!-- Picture dimensions in pixel -->
+      pictureHeight: 480
+      pictureWidth: 640
+      <!-- Viewing angle of the camera in one direction in Radians -->
+      verticalAngle: 0.43196898986859655
+      horizontalAngle: 0.5235987755982988
+    </rosparam>
+  </node>
+</launch>

src/robot_follower/launch/robot_lidar.launch

@@ -0,0 +1,11 @@
+<launch>
+    <include file="$(find rplidar_ros)/launch/rplidar.launch" />
+    
+
+    <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
+            args="0.10 0.0 0.15 0 0.0 0.0 /base_link /laser 50">
+    </node>
+    
+    <include file="$(find laser_filters)/launch/laser_scan_angle_filter.launch"> <!--  laser_filters -->
+   </include>
+</launch>

src/robot_follower/launch/visual_follower.launch

@@ -0,0 +1,4 @@
+<launch>
+  <include file='$(find robot_follower)/launch/nodes/visualTracker.launch' />
+  <include file='$(find robot_follower)/launch/nodes/follower.launch' />
+</launch>

src/robot_follower/launch/ydlidar_X2L.launch

@@ -0,0 +1,28 @@
+<!--
+  Copyright: 2016-2020 www.corvin.cn ROS小课堂
+  Description:加载启动EAI的X2L雷达.
+  Author: corvin
+  History:
+    20200325: init this file.
+-->
+<launch>
+  <node name="ydlidar_X2L_node" pkg="ydlidar_ros" type="ydlidar_node" output="screen" respawn="false" >
+    <param name="port"      type="string" value="/dev/ydlidar"/>
+    <param name="baudrate"  type="int"    value="115200"/>
+    <param name="frame_id"  type="string" value="lidar"/>
+
+    <param name="resolution_fixed" type="bool"  value="true"/>
+    <param name="auto_reconnect"   type="bool"  value="true"/>
+
+    <param name="reversion"    type="bool"   value="false"/>
+    <param name="angle_min"    type="double" value="-90" />
+    <param name="angle_max"    type="double" value="90" />
+    <param name="range_min"    type="double" value="0.1" />
+    <param name="range_max"    type="double" value="8.0" />
+    <param name="ignore_array" type="string" value="" />
+
+    <param name="frequency"    type="double" value="7"/>
+    <param name="samp_rate"    type="int"    value="3"/>
+    <param name="isSingleChannel" type="bool"  value="true"/>
+  </node>
+</launch>

src/robot_follower/msg/position.msg

@@ -0,0 +1,3 @@
+float32 angleX
+float32 angleY
+float32 distance

src/robot_follower/package.xml

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<package>
+  <name>robot_follower</name>
+  <version>0.0.0</version>
+  <description>Makes a mobile robot follow a target. either using rgb-d or laser range finder</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="chutter@uos.de">clemens</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/first_experiments</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <build_depend>message_generation</build_depend>
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>message_generation</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/robot_follower/parameters/PID_param.yaml

@@ -0,0 +1,3 @@
+P: [0.7, 0.6] 
+I: [0.07,0.04]
+D: [0.00, 0.000]

src/robot_follower/scripts/follower.py

@@ -0,0 +1,178 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+'''
+Author: adam_zhuo
+Date: 2020-11-23 18:32:56
+LastEditTime: 2020-11-24 10:59:16
+LastEditors: Please set LastEditors
+Description: laser_follow
+FilePath: /hsatsnu_ws/src/robot_follower/scripts/follower.py
+'''
+
+import rospy
+import thread, threading
+import time
+import math
+import numpy as np
+from sensor_msgs.msg import Joy, LaserScan
+from geometry_msgs.msg import Twist, Vector3
+from robot_follower.msg import position as PositionMsg
+from std_msgs.msg import String as StringMsg
+
+class Follower:
+	def __init__(self):
+		# as soon as we stop receiving Joy messages from the ps3 controller we stop all movement:
+		self.switchMode= rospy.get_param('~switchMode') # if this is set to False the O button has to be kept pressed in order for it to move
+		self.max_speed = rospy.get_param('~maxSpeed') 
+		self.controllButtonIndex = rospy.get_param('~controllButtonIndex')
+
+		self.buttonCallbackBusy=False
+		self.active=True
+
+		self.cmdVelPublisher = rospy.Publisher('/cmd_vel', Twist, queue_size =3)
+
+		# the topic for the tracker that gives us the current position of the object we are following
+		self.positionSubscriber = rospy.Subscriber('/object_tracker/current_position', PositionMsg, self.positionUpdateCallback)
+		# an info string from that tracker. E.g. telling us if we lost the object
+		self.trackerInfoSubscriber = rospy.Subscriber('/object_tracker/info', StringMsg, self.trackerInfoCallback)
+
+		# PID parameters first is angular, dist
+		targetDist = rospy.get_param('~targetDist')
+		PID_param = rospy.get_param('~PID_controller')
+		# the first parameter is the angular target (0 degrees always) the second is the target distance (say 1 meter)
+		self.PID_controller = simplePID([0, targetDist], PID_param['P'], PID_param['I'], PID_param['D'])
+
+		# this method gets called when the process is killed with Ctrl+C
+		rospy.on_shutdown(self.controllerLoss)
+
+	def trackerInfoCallback(self, info):
+		# we do not handle any info from the object tracker specifically at the moment. just ignore that we lost the object for example
+		rospy.logwarn(info.data)
+	
+	def positionUpdateCallback(self, position):
+		# gets called whenever we receive a new position. It will then update the motorcomand
+	
+		if(not(self.active)):
+			return #if we are not active we will return imediatly without doing anything
+
+		angleX= position.angleX
+		distance = position.distance
+
+		rospy.logwarn('Angle: {}, Distance: {}, '.format(angleX, distance))
+
+		if distance >= 2:
+			self.stopMoving()
+			return
+		
+		# call the PID controller to update it and get new speeds
+		[uncliped_ang_speed, uncliped_lin_speed] = self.PID_controller.update([angleX, distance])
+			
+		# clip these speeds to be less then the maximal speed specified above
+		angularSpeed = np.clip(-uncliped_ang_speed, -self.max_speed, self.max_speed)
+		linearSpeed  = np.clip(-uncliped_lin_speed, -self.max_speed, self.max_speed)
+		
+		if abs(linearSpeed) > 0.25:
+			linearSpeed = math.copysign(0.25,linearSpeed)
+		if abs(angularSpeed) > 0.8:
+			angularSpeed = math.copysign(0.8,angularSpeed)
+
+		# create the Twist message to send to the cmd_vel topic
+		velocity = Twist()	
+		velocity.linear = Vector3(linearSpeed,0,0.)
+		velocity.angular= Vector3(0., 0.,angularSpeed)
+		rospy.logwarn('linearSpeed: {}, angularSpeed: {}'.format(linearSpeed, angularSpeed))
+		self.cmdVelPublisher.publish(velocity)
+		
+
+	def stopMoving(self):
+		velocity = Twist()
+		velocity.linear = Vector3(0.,0.,0.)
+		velocity.angular= Vector3(0.,0.,0.)
+		self.cmdVelPublisher.publish(velocity)
+
+	def controllerLoss(self):
+		# we lost connection so we will stop moving and become inactive
+		self.stopMoving()
+		self.active = False
+		rospy.loginfo('lost connection')
+
+
+		
+class simplePID:
+	'''very simple discrete PID controller'''
+	def __init__(self, target, P, I, D):
+		'''Create a discrete PID controller
+		each of the parameters may be a vector if they have the same length
+		
+		Args:
+		target (double) -- the target value(s)
+		P, I, D (double)-- the PID parameter
+
+		'''
+
+		# check if parameter shapes are compatabile. 
+		if(not(np.size(P)==np.size(I)==np.size(D)) or ((np.size(target)==1) and np.size(P)!=1) or (np.size(target )!=1 and (np.size(P) != np.size(target) and (np.size(P) != 1)))):
+			raise TypeError('input parameters shape is not compatable')
+		rospy.loginfo('PID initialised with P:{}, I:{}, D:{}'.format(P,I,D))
+		self.Kp		=np.array(P)
+		self.Ki		=np.array(I)
+		self.Kd		=np.array(D)
+		self.setPoint   =np.array(target)
+		
+		self.last_error=0
+		self.integrator = 0
+		self.integrator_max = float('inf')
+		self.timeOfLastCall = None 
+		
+		
+	def update(self, current_value):
+		'''Updates the PID controller. 
+
+		Args:
+			current_value (double): vector/number of same legth as the target given in the constructor
+
+		Returns:
+			controll signal (double): vector of same length as the target
+
+		'''
+		current_value=np.array(current_value)
+		if(np.size(current_value) != np.size(self.setPoint)):
+			raise TypeError('current_value and target do not have the same shape')
+		if(self.timeOfLastCall is None):
+			# the PID was called for the first time. we don't know the deltaT yet
+			# no controll signal is applied
+			self.timeOfLastCall = time.clock()
+			return np.zeros(np.size(current_value))
+
+		
+		error = self.setPoint - current_value
+		P =  error
+		
+		currentTime = time.clock()
+		deltaT      = (currentTime-self.timeOfLastCall)
+
+		# integral of the error is current error * time since last update
+		self.integrator = self.integrator + (error*deltaT)
+		I = self.integrator
+		
+		# derivative is difference in error / time since last update
+		D = (error-self.last_error)/deltaT
+		
+		self.last_error = error
+		self.timeOfLastCall = currentTime
+		
+		# return controll signal
+		return self.Kp*P + self.Ki*I + self.Kd*D
+		
+		
+if __name__ == '__main__':
+	print('starting')
+	rospy.init_node('follower')
+	follower = Follower()
+	try:
+		rospy.spin()
+	except rospy.ROSInterruptException:
+		print('exception')
+
+

src/robot_follower/scripts/laserTracker.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+'''
+Author: adam_zhuo
+Date: 2020-09-14 13:56:07
+LastEditTime: 2020-09-18 15:56:24
+LastEditors: Please set LastEditors
+Description: laser_follow
+FilePath: /blackTornadoRobot/src/robot_follower/scripts/laserTracker.py
+'''
+
+import rospy
+import thread, threading
+import time
+import numpy as np
+from sensor_msgs.msg import Joy, LaserScan
+from geometry_msgs.msg import Twist, Vector3
+from std_msgs.msg import String as StringMsg
+from robot_follower.msg import position as PositionMsg
+		
+class laserTracker:
+	def __init__(self):
+		self.lastScan=None
+		self.winSize = rospy.get_param('~winSize')
+		self.deltaDist = rospy.get_param('~deltaDist')
+		self.scanSubscriber = rospy.Subscriber('/scan_filtered', LaserScan, self.registerScan)
+		self.positionPublisher = rospy.Publisher('/object_tracker/current_position', PositionMsg,queue_size=3)
+		self.infoPublisher = rospy.Publisher('/object_tracker/info', StringMsg, queue_size=3)
+
+	def registerScan(self, scan_data):
+		# registers laser scan and publishes position of closest object (or point rather)
+		ranges = np.array(scan_data.ranges)
+		# sort by distance to check from closer to further away points if they might be something real
+		sortedIndices = np.argsort(ranges)
+		
+		minDistanceID = None
+		minDistance   = float('inf')		
+
+		if(not(self.lastScan is None)):
+			# if we already have a last scan to compare to:
+			for i in sortedIndices:
+				# check all distance measurements starting from the closest one
+				tempMinDistance   = ranges[i]
+				if tempMinDistance == 0:
+					continue
+
+				# now we check if this might be noise:
+				# get a window. in it we will check if there has been a scan with similar distance
+				# in the last scan within that window
+				
+				# we kneed to clip the window so we don't have an index out of bounds
+				windowIndex = np.clip([i-self.winSize, i+self.winSize+1],0,len(self.lastScan))
+				window = self.lastScan[windowIndex[0]:windowIndex[1]]
+
+				with np.errstate(invalid='ignore'):
+					# check if any of the scans in the window (in the last scan) has a distance close enough to the current one
+					if(np.any(abs(window-tempMinDistance)<=self.deltaDist)):
+					# this will also be false for all tempMinDistance = NaN or inf
+
+						# we found a plausible distance
+						minDistanceID = i
+						minDistance = ranges[minDistanceID]
+						break # at least one point was equally close
+						# so we found a valid minimum and can stop the loop
+			
+		self.lastScan=ranges	
+		
+		#catches no scan, no minimum found, minimum is actually inf
+		if(minDistance > scan_data.range_max):
+			#means we did not really find a plausible object
+			
+			# publish warning that we did not find anything
+			rospy.logwarn('laser no object found')
+			self.infoPublisher.publish(StringMsg('laser:nothing found'))
+			
+		else:
+			# calculate angle of the objects location. 0 is straight ahead
+			minDistanceAngle = scan_data.angle_min + minDistanceID * scan_data.angle_increment
+			rospy.logwarn('minDistanceAngle and minDistance is {},{} '.format(minDistanceAngle, minDistance))
+			# here we only have an x angle, so the y is set arbitrarily
+			self.positionPublisher.publish(PositionMsg(minDistanceAngle, 42, minDistance))
+			
+
+
+
+if __name__ == '__main__':
+	print('starting')
+	rospy.init_node('laser_tracker')
+	tracker = laserTracker()
+	print('seems to do something')
+	try:
+		rospy.spin()
+	except rospy.ROSInterruptException:
+		print('exception')
+
+

src/robot_follower/scripts/testCode.py

@@ -0,0 +1,114 @@
+import numpy as np
+import copy
+from unsafe_runaway import *
+import time
+from nose.tools import assert_raises
+
+
+
+
+class mockup:
+	pass
+	
+
+def makeLaserData():
+	laser_data = mockup()
+	laser_data.range_max = 5.6
+	laser_data.angle_min = -2.1
+	laser_data.angle_increment = 0.06136
+	laser_data.ranges = list(1+np.random.rand(69)*5)
+	return laser_data
+
+class Test_simpleTracker:
+
+	
+	def setUp(self):
+		self.tracker = simpleTracker() 
+	
+	def test_ignor_first_scan(self):
+		laser_data = makeLaserData()
+		tracker = simpleTracker()
+		assert_raises(UserWarning, self.tracker.registerScan,laser_data)
+
+	def test_unmuted(self):
+		laser_data = makeLaserData()
+		backup = copy.copy(laser_data)
+		try:
+			angle, distance = self.tracker.registerScan(laser_data)
+		except:
+			pass
+		assert backup.ranges == laser_data.ranges
+		#print(laser_data.ranges)
+		#print('angle: {}, dist: {}'.format(angle, distance))	
+
+	def test_nan(self):
+		laser_data = makeLaserData()
+		assert_raises(UserWarning, self.tracker.registerScan,laser_data)
+		laser_data.ranges[12] = float('nan')
+		angle, dist=self.tracker.registerScan(laser_data)
+		#print('angle: {}, dist: {}'.format(angle, dist))	
+
+	def test_only_nan(self):
+		laser_data = makeLaserData()
+		laser_data.ranges = [float('nan') for _ in laser_data.ranges]
+		assert_raises(UserWarning, self.tracker.registerScan,laser_data)
+		assert_raises(UserWarning, self.tracker.registerScan,laser_data)
+
+		
+
+	def test_real_real_min(self):
+		laser_data = makeLaserData()
+		laser_data.ranges[-1]=0.5 #real min
+		assert_raises(UserWarning, self.tracker.registerScan,laser_data)
+		laser_data.ranges[-1]=0.6 
+		laser_data.ranges[42]=0.1 #fake min
+		ang, dist = self.tracker.registerScan(laser_data)
+		assert dist == 0.6
+		#print('ang: {}, target: {}'.format(ang, (laser_data.angle_min+ 23*laser_data.angle_increment)))	
+		assert ang == laser_data.angle_min+ 68*laser_data.angle_increment
+
+
+class Test_PID:
+	def setUp(self): 
+		pass
+
+	def test_convergence(self):
+		self.pid = simplePID([0,30], 0.8, 0.001, 0.0001)
+		x =np.array([23, 12])
+		for i in range(20):
+			update= self.pid.update(x)
+			print('added {} to current x {}'.format(update, x))
+			x = x+update 
+			time.sleep(0.1)
+		assert np.all(abs(x-[0,30])<=0.01)
+
+	def test_convergence_differentParamShape(self):
+		self.pid = simplePID([0,30],0.8, 0.001, 0.0001)
+		x =np.array([23, 12])
+		for i in range(20):
+			update= self.pid.update(x)
+			print('added {} to current x {}'.format(update, x))
+			x = x+update 
+			time.sleep(0.1)
+		assert np.all(abs(x-[0,30])<=0.01)
+
+	 
+	def test_raises_unequal_param_shape_at_creation(self):
+		assert_raises(TypeError, simplePID, [0,30],[0.8, 0.7, 0.1], 0.001, 0.0001)
+		assert_raises(TypeError, simplePID, [0,30],[0.8, 0.7], 0.001, 0.0001)
+		assert_raises(TypeError, simplePID, 0,[0.8, 0.7], 0.001, 0.0001)
+		assert_raises(TypeError, simplePID, 0, [0.8, 0.7], [0.001, 0.001], [0.0001, 0,0001])
+		_ =  simplePID([0,30],[0.8, 0.7], [0.001, 0.001], [0.0001, 0.0001])
+		_ =  simplePID([0,30],0.8, 0.001, 0.0001)
+		_ =  simplePID(0,0.8, 0.001, 0.0001)
+
+	def test_raise_incompatable_input(self):
+		self.pid = simplePID([0,30], 0.8, 0.001, 0.0001)
+		_ = assert_raises(TypeError, self.pid.update, 3)
+		x =np.array([23, 12])
+		for i in range(50):
+			update= self.pid.update(x)
+			print('added {} to current x {}'.format(update, x))
+			x = x+update 
+			time.sleep(0.1)
+		assert np.all(abs(x-[0,30])<=0.001)

src/robot_follower/scripts/visualTracker.py

@@ -0,0 +1,206 @@
+#!/usr/bin/env python
+
+from __future__ import division
+import rospy
+import message_filters
+import numpy as np
+import cv2
+from matplotlib import pyplot as plt
+from cv_bridge import CvBridge 
+
+from sensor_msgs.msg import Image
+from robot_follower.msg import position as PositionMsg
+from std_msgs.msg import String as StringMsg
+np.seterr(all='raise')  
+displayImage=False
+
+plt.close('all')
+class visualTracker:
+	def __init__(self):
+		self.bridge = CvBridge()
+		self.targetUpper = np.array(rospy.get_param('~target/upper'))
+		self.targetLower = np.array(rospy.get_param('~target/lower'))
+		self.pictureHeight= rospy.get_param('~pictureDimensions/pictureHeight')
+		self.pictureWidth = rospy.get_param('~pictureDimensions/pictureWidth')
+		vertAngle =rospy.get_param('~pictureDimensions/verticalAngle')
+		horizontalAngle =  rospy.get_param('~pictureDimensions/horizontalAngle')
+		# precompute tangens since thats all we need anyways:
+		self.tanVertical = np.tan(vertAngle)
+		self.tanHorizontal = np.tan(horizontalAngle)
+		
+		self.lastPosition =None
+
+		# one callback that deals with depth and rgb at the same time
+		im_sub = message_filters.Subscriber('/orbbec_astra/rgb/image_rect_color', Image)
+		dep_sub = message_filters.Subscriber('/orbbec_astra/depth/image', Image)
+		self.timeSynchronizer = message_filters.ApproximateTimeSynchronizer([im_sub, dep_sub], 10, 0.5)
+		self.timeSynchronizer.registerCallback(self.trackObject)
+
+
+		self.positionPublisher = rospy.Publisher('/object_tracker/current_position', PositionMsg, queue_size=3)
+		self.infoPublisher = rospy.Publisher('/object_tracker/info', StringMsg, queue_size=3)
+		rospy.logwarn(self.targetUpper)
+
+
+	def trackObject(self, image_data, depth_data):
+		if(image_data.encoding != 'rgb8'):
+			raise ValueError('image is not rgb8 as expected')
+
+		#convert both images to numpy arrays
+		frame = self.bridge.imgmsg_to_cv2(image_data, desired_encoding='rgb8')
+		depthFrame = self.bridge.imgmsg_to_cv2(depth_data, desired_encoding='passthrough')#"32FC1")
+
+		if(np.shape(frame)[0:2] != (self.pictureHeight, self.pictureWidth)):
+			raise ValueError('image does not have the right shape. shape(frame): {}, shape parameters:{}'.format(np.shape(frame)[0:2], (self.pictureHeight, self.pictureWidth)))
+
+		# blure a little and convert to HSV color space
+		blurred = cv2.GaussianBlur(frame, (11,11), 0)
+		hsv = cv2.cvtColor(blurred, cv2.COLOR_RGB2HSV)
+		
+		# select all the pixels that are in the range specified by the target
+		org_mask = cv2.inRange(hsv, self.targetUpper, self.targetLower)	
+
+		# clean that up a little, the iterations are pretty much arbitrary
+		mask = cv2.erode(org_mask, None, iterations=4)
+		mask = cv2.dilate(mask,None, iterations=3)
+		
+		# find contours of the object
+		contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
+		newPos = None #if no contour at all was found the last position will again be set to none
+
+		# lets you display the image for debuging. Not in realtime though
+		if displayImage:
+			backConverted = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
+			#cv2.imshow('frame', backConverted)
+			#cv2.waitKey(0)
+			#print(backConverted)
+			plt.figure()
+			plt.subplot(2,2,1)
+			plt.imshow(frame)
+			plt.xticks([]),plt.yticks([])
+			plt.subplot(2,2,2)
+			plt.imshow(org_mask, cmap='gray', interpolation = 'bicubic')
+			plt.xticks([]),plt.yticks([])
+			plt.subplot(2,2,3)
+			plt.imshow(mask, cmap='gray', interpolation = 'bicubic')
+			plt.xticks([]),plt.yticks([])
+			plt.show()
+			rospy.sleep(0.2)
+
+		# go threw all the contours. starting with the bigest one
+		for contour in sorted(contours, key=cv2.contourArea, reverse=True):
+			# get position of object for this contour
+		 	pos = self.analyseContour(contour, depthFrame)
+
+			# if it's the first one we found it will be the fall back for the next scan if we don't find a plausible one
+			if newPos is None:
+				newPos = pos
+			
+			# check if the position is plausible
+			if self.checkPosPlausible(pos):
+				self.lastPosition = pos
+				self.publishPosition(pos)
+				return
+
+		self.lastPosition = newPos #we didn't find a plossible last position, so we just save the biggest contour 
+		# and publish warnings
+		rospy.loginfo('no position found')
+		self.infoPublisher.publish(StringMsg('visual:nothing found'))
+
+			
+			
+			
+
+	def publishPosition(self, pos):
+		# calculate the angles from the raw position
+		angleX = self.calculateAngleX(pos)
+		angleY = self.calculateAngleY(pos)
+		# publish the position (angleX, angleY, distance)
+		posMsg = PositionMsg(angleX, angleY, pos[1])
+		self.positionPublisher.publish(posMsg)
+
+	def checkPosPlausible(self, pos):
+		'''Checks if a position is plausible. i.e. close enough to the last one.'''
+
+		# for the first scan we cant tell
+		if self.lastPosition is None:
+			return False
+
+		# unpack positions
+		((centerX, centerY), dist)=pos	
+		((PcenterX, PcenterY), Pdist)=self.lastPosition
+		
+		if np.isnan(dist):
+			return False
+
+		# distance changed to much
+		if abs(dist-Pdist)>0.5:
+			return False
+
+		# location changed to much (5 is arbitrary)
+		if abs(centerX-PcenterX)>(self.pictureWidth /5):
+			return False
+
+		if abs(centerY-PcenterY)>(self.pictureHeight/5):
+			return False
+		
+		return True
+				
+		
+	def calculateAngleX(self, pos):
+		'''calculates the X angle of displacement from straight ahead'''
+
+		centerX = pos[0][0]
+		displacement = 2*centerX/self.pictureWidth-1
+		angle = -1*np.arctan(displacement*self.tanHorizontal)
+		return angle
+
+	def calculateAngleY(self, pos):
+		centerY = pos[0][1]
+		displacement = 2*centerY/self.pictureHeight-1
+		angle = -1*np.arctan(displacement*self.tanVertical)
+		return angle
+
+	def analyseContour(self, contour, depthFrame):
+		'''Calculates the centers coordinates and distance for a given contour
+
+		Args:
+			contour (opencv contour): contour of the object
+			depthFrame (numpy array): the depth image
+		
+		Returns:
+			centerX, centerY (doubles): center coordinates
+			averageDistance : distance of the object
+		'''
+		# get a rectangle that completely contains the object
+		centerRaw, size, rotation = cv2.minAreaRect(contour)
+
+		# get the center of that rounded to ints (so we can index the image)
+		center = np.round(centerRaw).astype(int)
+
+		# find out how far we can go in x/y direction without leaving the object (min of the extension of the bounding rectangle/2 (here 3 for safety)) 
+		minSize = int(min(size)/3)
+
+		# get all the depth points within this area (that is within the object)
+		depthObject = depthFrame[(center[1]-minSize):(center[1]+minSize), (center[0]-minSize):(center[0]+minSize)]
+
+		# get the average of all valid points (average to have a more reliable distance measure)
+		depthArray = depthObject[~np.isnan(depthObject)]
+		averageDistance = np.mean(depthArray)
+
+		if len(depthArray) == 0:
+			rospy.logwarn('empty depth array. all depth values are nan')
+
+		return (centerRaw, averageDistance)
+		
+
+if __name__ == '__main__':
+        print('starting')
+        rospy.init_node('visual_tracker')
+	tracker=visualTracker()
+        print('seems to do something')
+        try:
+                rospy.spin()
+        except rospy.ROSInterruptException:
+                print('exception')
+

src/ros_arduino_bridge/ros_arduino_python/launch/arduino_multi.launch

@@ -0,0 +1,15 @@
+<!--
+  Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+  Description:
+    启动底盘arduino的上位机launch文件.
+  Author: corvin
+  History:
+    20200706: init this file.
+    20201212: add multi robot.
+-->
+<launch>
+   <node name="mobilebase_arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen" required="true">
+      <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
+      <remap from="/robot1/cmd_vel" to="/cmd_vel" />
+   </node>
+</launch>

src/rplidar_ros/CHANGELOG.rst

@@ -0,0 +1,64 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rplidar_ros
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.9.0 (2018-08-24)
+------------------
+* Update RPLIDAR SDK to 1.9.0
+* [new feature] support baudrate 57600 and 1382400, support HQ scan response
+* [bugfix] TCP channel doesn't work
+* [improvement] Print warning messages when deprecated APIs are called; imporve angular accuracy for ultra capsuled scan points
+* Contributors: tony,kint
+
+1.7.0 (2018-07-19)
+------------------
+* Update RPLIDAR SDK to 1.7.0
+* support scan points farther than 16.38m
+* upport display and set scan mode
+* Contributors: kint
+
+
+1.6.0 (2018-05-21)
+------------------
+* Release 1.6.0.
+* Update RPLIDAR SDK to 1.6.0
+* Support new product RPLIDAR A3(default 16K model and max_distance 25m)
+* Contributors: kint
+
+
+1.5.7 (2016-12-15)
+------------------
+* Release 1.5.7.
+* Update RPLIDAR SDK to 1.5.7
+* Fixed the motor default speed at 10 HZ. Extend the measurement of max_distance from 6m to 8m.
+* Contributors: kint
+
+1.5.5 (2016-08-23)
+------------------
+* Release 1.5.5.
+* Update RPLIDAR SDK to 1.5.5
+* Add RPLIDAR information print, and fix the standard motor speed of RPLIDAR A2.
+* Contributors: kint
+
+1.5.4 (2016-06-02)
+------------------
+* Release 1.5.4.
+* Update RPLIDAR SDK to 1.5.4
+* Support RPLIDAR A2
+* Contributors: kint
+
+1.5.2 (2016-04-29)
+------------------
+* Release 1.5.2.
+* Update RPLIDAR SDK to 1.5.2
+* Support RPLIDAR A2
+* Contributors: kint
+
+1.0.1 (2014-06-03)
+------------------
+* Release 1.0.1.
+* Add angle compensate mechanism to compatible with ROS scan message
+* Add RPLIDAR sdk to the repo.
+* First release of RPLIDAR ROS package (1.0.0)
+* Initial commit
+* Contributors: Ling, RoboPeak Public Repos

src/rplidar_ros/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rplidar_ros)
+
+set(RPLIDAR_SDK_PATH "./sdk/")
+
+FILE(GLOB RPLIDAR_SDK_SRC 
+  "${RPLIDAR_SDK_PATH}/src/arch/linux/*.cpp"
+  "${RPLIDAR_SDK_PATH}/src/hal/*.cpp"
+  "${RPLIDAR_SDK_PATH}/src/*.cpp"
+)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rosconsole
+  sensor_msgs
+)
+
+include_directories(
+  ${RPLIDAR_SDK_PATH}/include
+  ${RPLIDAR_SDK_PATH}/src
+  ${catkin_INCLUDE_DIRS}
+)
+
+catkin_package()
+
+add_executable(rplidarNode src/node.cpp ${RPLIDAR_SDK_SRC})
+target_link_libraries(rplidarNode ${catkin_LIBRARIES})
+
+add_executable(rplidarNodeClient src/client.cpp)
+target_link_libraries(rplidarNodeClient ${catkin_LIBRARIES})
+
+install(TARGETS rplidarNode rplidarNodeClient
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch rviz sdk
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)

src/rplidar_ros/LICENSE

@@ -0,0 +1,24 @@
+Copyright (c) 2009 - 2014 RoboPeak Team
+Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/rplidar_ros/README.md

@@ -0,0 +1,47 @@
+RPLIDAR ROS package
+=====================================================================
+
+ROS node and test application for RPLIDAR
+
+Visit following Website for more details about RPLIDAR:
+
+rplidar roswiki: http://wiki.ros.org/rplidar
+
+rplidar HomePage:   http://www.slamtec.com/en/Lidar
+
+rplidar SDK: https://github.com/Slamtec/rplidar_sdk
+
+rplidar Tutorial:  https://github.com/robopeak/rplidar_ros/wiki
+
+How to build rplidar ros package
+=====================================================================
+    1) Clone this project to your catkin's workspace src folder
+    2) Running catkin_make to build rplidarNode and rplidarNodeClient
+
+How to run rplidar ros package
+=====================================================================
+There're two ways to run rplidar ros package
+
+I. Run rplidar node and view in the rviz
+------------------------------------------------------------
+roslaunch rplidar_ros view_rplidar.launch (for RPLIDAR A1/A2)
+or
+roslaunch rplidar_ros view_rplidar_a3.launch (for RPLIDAR A3)
+
+You should see rplidar's scan result in the rviz.
+
+II. Run rplidar node and view using test application
+------------------------------------------------------------
+roslaunch rplidar_ros rplidar.launch (for RPLIDAR A1/A2)
+or
+roslaunch rplidar_ros rplidar_a3.launch (for RPLIDAR A3)
+
+rosrun rplidar_ros rplidarNodeClient
+
+You should see rplidar's scan result in the console
+
+Notice: the different is serial_baudrate between A1/A2 and A3
+
+RPLidar frame
+=====================================================================
+RPLidar frame must be broadcasted according to picture shown in rplidar-frame.png

src/rplidar_ros/launch/rplidar.launch

@@ -0,0 +1,10 @@
+<launch>
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <param name="serial_port"         type="string" value="/dev/rplidar"/>
+  <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
+  <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
+  <param name="frame_id"            type="string" value="lidar"/>
+  <param name="inverted"            type="bool"   value="false"/>
+  <param name="angle_compensate"    type="bool"   value="true"/>
+  </node>
+</launch>

src/rplidar_ros/launch/rplidar_a3.launch

@@ -0,0 +1,10 @@
+<launch>
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
+  <param name="serial_baudrate"     type="int"    value="256000"/><!--A3 -->
+  <param name="frame_id"            type="string" value="laser"/>
+  <param name="inverted"            type="bool"   value="false"/>
+  <param name="angle_compensate"    type="bool"   value="true"/>
+  <param name="scan_mode"           type="string" value="Sensitivity"/>
+  </node>
+</launch>

src/rplidar_ros/launch/test_rplidar.launch

@@ -0,0 +1,12 @@
+<launch>
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
+  <param name="serial_baudrate"     type="int"    value="115200"/>
+  <param name="frame_id"            type="string" value="laser"/>
+  <param name="inverted"            type="bool"   value="false"/>
+  <param name="angle_compensate"    type="bool"   value="true"/>
+
+ </node>
+  <node name="rplidarNodeClient"          pkg="rplidar_ros"  type="rplidarNodeClient" output="screen">
+  </node>
+</launch>

src/rplidar_ros/launch/test_rplidar_a3.launch

@@ -0,0 +1,13 @@
+<launch>
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
+  <param name="serial_baudrate"     type="int"    value="256000"/>
+  <param name="frame_id"            type="string" value="laser"/>
+  <param name="inverted"            type="bool"   value="false"/>
+  <param name="angle_compensate"    type="bool"   value="true"/>
+  <param name="scan_mode"           type="string" value="Sensitivity"/>
+
+ </node>
+  <node name="rplidarNodeClient"          pkg="rplidar_ros"  type="rplidarNodeClient" output="screen">
+  </node>
+</launch>

src/rplidar_ros/launch/view_rplidar.launch

@@ -0,0 +1,10 @@
+<!--
+  Used for visualising rplidar in action.  
+  
+  It requires rplidar.launch.
+ -->
+<launch>
+  <include file="$(find rplidar_ros)/launch/rplidar.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
+</launch>

src/rplidar_ros/launch/view_rplidar_a3.launch

@@ -0,0 +1,10 @@
+<!--
+  Used for visualising rplidar in action.  
+  
+  It requires rplidar.launch.
+ -->
+<launch>
+  <include file="$(find rplidar_ros)/launch/rplidar_a3.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
+</launch>