├── .gitignore ├── src ├── FilterPointCloud2.cpp ├── FilterBasePointCloud2.cpp ├── FilterIndicesPointCloud2.cpp ├── CropBoxFilterPointCloud2.cpp ├── PassThroughFilterPointCloud2.cpp ├── VoxelGridFilterPointCloud2.cpp └── SacSegmentationExtractFilterPointCloud2.cpp ├── launch └── d435_example.launch ├── cfg ├── PassThroughPointCloud2.cfg ├── FilterIndicesPointCloud2.cfg ├── FilterBasePointCloud2.cfg ├── CropBoxPointCloud2.cfg ├── VoxelGridPointCloud2.cfg └── SacSegmentationExtractPointCloud2.cfg ├── include └── point_cloud2_filters │ ├── FilterPointCloud2.hpp │ ├── FilterIndicesPointCloud2.hpp │ ├── PassThroughFilterPointCloud2.hpp │ ├── CropBoxFilterPointCloud2.hpp │ ├── FilterBasePointCloud2.hpp │ ├── VoxelGridFilterPointCloud2.hpp │ └── SacSegmentationExtractFilterPointCloud2.hpp ├── package.xml ├── CHANGELOG.rst ├── LICENSE ├── point_cloud2_filters_plugins.xml ├── config └── d435_example.yaml ├── CMakeLists.txt └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | *.kdev4 2 | *.vscode 3 | -------------------------------------------------------------------------------- /src/FilterPointCloud2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | -------------------------------------------------------------------------------- /src/FilterBasePointCloud2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | -------------------------------------------------------------------------------- /src/FilterIndicesPointCloud2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | -------------------------------------------------------------------------------- /src/CropBoxFilterPointCloud2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | PLUGINLIB_EXPORT_CLASS(point_cloud2_filters::CropBoxFilterPointCloud2, filters::FilterBase) 5 | -------------------------------------------------------------------------------- /src/PassThroughFilterPointCloud2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | PLUGINLIB_EXPORT_CLASS(point_cloud2_filters::PassThroughFilterPointCloud2, filters::FilterBase) 5 | -------------------------------------------------------------------------------- /src/VoxelGridFilterPointCloud2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | PLUGINLIB_EXPORT_CLASS(point_cloud2_filters::VoxelGridFilterPointCloud2, filters::FilterBase) 5 | 6 | -------------------------------------------------------------------------------- /src/SacSegmentationExtractFilterPointCloud2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | PLUGINLIB_EXPORT_CLASS(point_cloud2_filters::SacSegmentationExtractFilterPointCloud2, filters::FilterBase) 5 | 6 | -------------------------------------------------------------------------------- /launch/d435_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /cfg/PassThroughPointCloud2.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE = 'point_cloud2_filters' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator () 10 | # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): 11 | gen.add ("filter_field_name", str_t, 0, "The field name used for filtering", "z") 12 | gen.add ("filter_limit_min", double_t, 0, "The minimum allowed field value a point will be considered from", 0, -1000, 1000) 13 | gen.add ("filter_limit_max", double_t, 0, "The maximum allowed field value a point will be considered from", 1, -1000, 1000) 14 | 15 | exit (gen.generate (PACKAGE, "point_cloud2_filters", "PassThroughPointCloud2")) 16 | 17 | -------------------------------------------------------------------------------- /cfg/FilterIndicesPointCloud2.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE = 'point_cloud2_filters' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator () 10 | # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): 11 | gen.add ("keep_organized", bool_t, 0, "Set whether the filtered points should be kept and set to the value given through setUserFilterValue (default: NaN), or removed from the PointCloud, thus potentially breaking its organized structure.", False) 12 | gen.add ("negative", bool_t, 0, "Set whether the regular conditions for points filtering should apply, or the inverted conditions.", False) 13 | 14 | exit (gen.generate (PACKAGE, "point_cloud2_filters", "FilterIndicesPointCloud2")) 15 | 16 | -------------------------------------------------------------------------------- /cfg/FilterBasePointCloud2.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE = 'point_cloud2_filters' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator () 10 | # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): 11 | gen.add ("active", bool_t, 0, "Activate or not the filter.", True) 12 | gen.add ("input_frame", str_t, 0, "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.", "") 13 | gen.add ("output_frame", str_t, 0, "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different.", "") 14 | gen.add ("pub_cloud", bool_t, 0, "Publish the cloud immediately after this filter (note: this will be a duplicate if this is the last filter of the chain", False) 15 | 16 | exit (gen.generate (PACKAGE, "point_cloud2_filters", "FilterBasePointCloud2")) 17 | 18 | -------------------------------------------------------------------------------- /include/point_cloud2_filters/FilterPointCloud2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FILTER_POINT_CLOUD_HPP 2 | #define FILTER_POINT_CLOUD_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace point_cloud2_filters { 8 | 9 | class FilterPointCloud2 : public FilterBasePointCloud2 10 | { 11 | public: 12 | FilterPointCloud2(); 13 | ~FilterPointCloud2(); 14 | 15 | public: 16 | 17 | protected: 18 | std::shared_ptr> filter_; 19 | 20 | virtual bool execute() override; 21 | 22 | private: 23 | }; 24 | 25 | FilterPointCloud2::FilterPointCloud2() { 26 | 27 | }; 28 | 29 | FilterPointCloud2::~FilterPointCloud2() 30 | { 31 | }; 32 | 33 | 34 | 35 | bool FilterPointCloud2::execute() 36 | { 37 | 38 | filter_->setInputCloud (cloud_out_); 39 | filter_->filter (*cloud_out_); 40 | 41 | return true; 42 | 43 | }; 44 | 45 | 46 | } //namespace point_cloud2_filters 47 | 48 | 49 | #endif //FILTER_POINT_CLOUD_HPP 50 | 51 | -------------------------------------------------------------------------------- /cfg/CropBoxPointCloud2.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE = 'point_cloud2_filters' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator () 10 | # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): 11 | gen.add ("min_x", double_t, 0, "X coordinate of the minimum point of the box.", -1, -20, 20) 12 | gen.add ("max_x", double_t, 0, "X coordinate of the maximum point of the box.", 1, -20, 20) 13 | gen.add ("min_y", double_t, 0, "Y coordinate of the minimum point of the box.", -1, -20, 20) 14 | gen.add ("max_y", double_t, 0, "Y coordinate of the maximum point of the box.", 1, -20, 20) 15 | gen.add ("min_z", double_t, 0, "Z coordinate of the minimum point of the box.", -1, -20, 20) 16 | gen.add ("max_z", double_t, 0, "Z coordinate of the maximum point of the box.", 1, -20, 20) 17 | 18 | exit (gen.generate (PACKAGE, "point_cloud2_filters", "CropBoxPointCloud2")) 19 | 20 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | point_cloud2_filters 4 | 1.0.3 5 | Filters for the sensor_msgs/PointCloud2 based on the filters and sensor_filters chains 6 | 7 | Davide Torielli 8 | Davide Torielli 9 | 10 | BSD 11 | 12 | 13 | 14 | 15 | 16 | 17 | roscpp 18 | pluginlib 19 | tf2_ros 20 | pcl_ros 21 | pcl_conversions 22 | filters 23 | sensor_msgs 24 | 25 | catkin 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /cfg/VoxelGridPointCloud2.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE = 'point_cloud2_filters' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator () 10 | # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): 11 | gen.add ("leaf_size_x", double_t, 0, "The size of a leaf (on x) used for downsampling", 0.01, 0, 1.0) 12 | gen.add ("leaf_size_y", double_t, 0, "The size of a leaf (on y) used for downsampling", 0.01, 0, 1.0) 13 | gen.add ("leaf_size_z", double_t, 0, "The size of a leaf (on z) used for downsampling", 0.01, 0, 1.0) 14 | gen.add ("min_points_per_voxel", int_t, 0, "The minimum number of points required for a voxel to be used.", 1, 1, 100000) 15 | gen.add ("downsample_all_data", bool_t, 0, "Set to true if all fields need to be downsampled, or false if just XYZ.", True) 16 | gen.add ("filter_field_name", str_t, 0, "The field name used for filtering", "z") 17 | gen.add ("filter_limit_min", double_t, 0, "The minimum allowed field value a point will be considered from", 0, -1000, 1000) 18 | gen.add ("filter_limit_max", double_t, 0, "The maximum allowed field value a point will be considered from", 1, -1000, 1000) 19 | gen.add ("negative", bool_t, 0, "Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max)", False) 20 | 21 | exit (gen.generate (PACKAGE, "point_cloud2_filters", "VoxelGridPointCloud2")) 22 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package point_cloud2_filters 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.3 (2024-08-06) 6 | ------------------ 7 | 8 | * Updated README.md 9 | * option to pub cloud immediately after the filter execution 10 | * Contributors: Davide Torielli, torydebra 11 | 12 | 1.0.2 (2023-08-04) 13 | ------------------ 14 | * install example 15 | * Contributors: Davide Torielli 16 | 17 | 1.0.1 (2023-07-24) 18 | ------------------ 19 | * Create LICENSE 20 | * Contributors: torydebra 21 | 22 | 1.0.0 (2023-07-24) 23 | ------------------ 24 | * Merge pull request `#5 `_ from ADVRHumanoids/devel 25 | merging into master 26 | * Sac Segmentation filter with additional base class 27 | * added dynamic reconfigure && derivation from FilterIndices 28 | * Merge branch 'devel' of https://github.com/torydebra/pass_through_filter into devel 29 | * better readme 30 | * wrong interpretation of ROS_INFO_NAMED corrected 31 | * other filters and reamde better 32 | * derived class 33 | * cropbox filter 34 | * keep organized option 35 | * solved bug 36 | * corrected filename 37 | * fixing `#2 `_ 38 | * Merge pull request `#4 `_ from peci1/patch-1 39 | Improve CMakeLists.txt 40 | * Modified License 41 | * Improve CMakeLists.txt 42 | * first version 43 | * first commit 44 | * Contributors: Davide Torielli, Martin Pecka 45 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, Davide Torielli 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /point_cloud2_filters_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | A simple Pass Through Filter for sensor_msgs/PointCloud2 messages 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | A Crop Box filter for sensor_msgs/PointCloud2 messages 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | A Voxel Grid filter for sensor_msgs/PointCloud2 messages 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | Extract a model with segmentation methods for sensor_msgs/PointCloud2 messages 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /config/d435_example.yaml: -------------------------------------------------------------------------------- 1 | output_queue_size: 1 2 | 3 | cloud_filter_chain: 4 | - name: PassThroughFilter 5 | type: point_cloud2_filters/PassThroughFilterPointCloud2 6 | params: 7 | active: True 8 | input_frame: "D435_camera_link" 9 | output_frame: "D435_camera_depth_optical_frame" 10 | keep_organized: True 11 | negative: False 12 | #user_filter_value: 3 13 | filter_field_name: "z" 14 | filter_limit_min: -5 15 | filter_limit_max: 0.2 16 | log_time: True 17 | 18 | - name: CropBoxFilter 19 | type: point_cloud2_filters/CropBoxFilterPointCloud2 20 | params: 21 | active: True 22 | input_frame: "D435_camera_link" 23 | output_frame: "D435_camera_depth_optical_frame" 24 | keep_organized: True 25 | negative: False 26 | #user_filter_value: 5 27 | min_x: 0 28 | max_x: 1 29 | min_y: 0 30 | max_y: 0.4 31 | min_z: -1 32 | max_z: 0.8 33 | 34 | - name: VoxelGridFilter 35 | type: point_cloud2_filters/VoxelGridFilterPointCloud2 36 | params: 37 | active: False 38 | input_frame: "D435_camera_link" 39 | output_frame: "D435_camera_depth_optical_frame" 40 | leaf_size_x: 0.1 41 | leaf_size_y: 0.2 42 | leaf_size_z: 0.1 43 | min_points_per_voxel: 1 44 | downsample_all_data: True 45 | filter_field_name: "y" 46 | filter_limit_min: -1 47 | filter_limit_max: 1 48 | negative: False 49 | 50 | - name: ExtractPlane 51 | type: point_cloud2_filters/SacSegmentationExtractFilterPointCloud2 52 | params: 53 | active: True 54 | input_frame: "D435_camera_link" 55 | output_frame: "D435_camera_depth_optical_frame" 56 | model_type: 15 57 | method_type: 0 58 | axis_x: 0 59 | axis_y: 0 60 | axis_z: 1 61 | eps_angle: 0.15 62 | distance_threshold: 0.01 63 | negative: True 64 | optimize_coefficents: True 65 | 66 | -------------------------------------------------------------------------------- /cfg/SacSegmentationExtractPointCloud2.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE = 'point_cloud2_filters' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator () 10 | # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): 11 | gen.add ("axis_x", double_t, 0, "The x component of the normal to the plane to be removed.", 0, 0, 1) 12 | gen.add ("axis_y", double_t, 0, "The y component of the normal to the plane to be removed.", 0, 0, 1) 13 | gen.add ("axis_z", double_t, 0, "The z component of the normal to the plane to be removed.", 1, 0, 1) 14 | gen.add ("eps_angle", double_t, 0, "Tolerance angle (rad) to the plane to be considered normal to the axis", 0.15, -3.15, 3.15) 15 | gen.add ("distance_threshold", double_t, 0, "", 0.01, 0, 10) 16 | gen.add ("optimize_coefficents", bool_t, 0, "Optimize the coefficents or not", True) 17 | gen.add ("max_iterations", int_t, 0, "", 50, 1, 200) 18 | gen.add ("probability", double_t, 0, "", 0.99, 0.01, 1) 19 | gen.add ("min_radius", double_t, 0, "", -1, 0, 1000) 20 | gen.add ("max_radius", double_t, 0, "", 1000, 0, 1000) 21 | gen.add ("negative", bool_t, 0, "Set whether to filter out (remove) the plane (true) or all the rest (false).", True) 22 | 23 | # from pcl https://pointclouds.org/documentation/group__sample__consensus.html 24 | # For the commented ones a SACSegmentationFromNormals 25 | # (see https://github.com/PointCloudLibrary/pcl/blob/master/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp) 26 | #is required, which is currently not implemented in this package 27 | model_type = gen.enum([ gen.const("SACMODEL_PLANE", int_t, 0, ""), 28 | gen.const("SACMODEL_LINE", int_t, 1, ""), 29 | gen.const("SACMODEL_CIRCLE2D", int_t, 2, ""), 30 | gen.const("SACMODEL_CIRCLE3D", int_t, 3, ""), 31 | gen.const("SACMODEL_SPHERE ", int_t, 4, ""), 32 | # gen.const("SACMODEL_CONE", int_t, 6, ""), 33 | # gen.const("SACMODEL_TORUS", int_t, 7, ""), 34 | gen.const("SACMODEL_PARALLEL_LINE", int_t, 8, ""), 35 | gen.const("SACMODEL_PERPENDICULAR_PLANE", int_t, 9, ""), 36 | # gen.const("SACMODEL_PARALLEL_LINES ", int_t, 10, ""), 37 | # gen.const("SACMODEL_NORMAL_PLANE", int_t, 11, ""), 38 | # gen.const("SACMODEL_NORMAL_SPHERE", int_t, 12, ""), 39 | # gen.const("SACMODEL_REGISTRATION", int_t, 13, ""), 40 | # gen.const("SACMODEL_REGISTRATION_2D", int_t, 14, ""), 41 | gen.const("SACMODEL_PARALLEL_PLANE", int_t, 15, ""), 42 | # gen.const("SACMODEL_NORMAL_PARALLEL_PLANE ", int_t, 16, ""), 43 | gen.const("SACMODEL_STICK", int_t, 17, ""), 44 | gen.const("SACMODEL_ELLIPSE3D", int_t, 18, "")], 45 | "Geometric model to look for") 46 | gen.add("model_type", int_t, 0, "Geometric model to look for", 16, 0, 18, edit_method=model_type) 47 | 48 | method_type = gen.enum([ gen.const("SAC_RANSAC", int_t, 0, ""), 49 | gen.const("SAC_LMEDS", int_t, 1, ""), 50 | gen.const("SAC_MSAC", int_t, 2, ""), 51 | gen.const("SAC_RRANSAC", int_t, 3, ""), 52 | gen.const("SAC_RMSAC ", int_t, 4, ""), 53 | gen.const("SAC_MLESAC", int_t, 5, ""), 54 | gen.const("SAC_PROSAC", int_t, 6, "")], 55 | "Sample consensus estimator see pcl doc") 56 | gen.add("method_type", int_t, 0, "Sample consensus estimator", 0, 0, 6, edit_method=method_type) 57 | 58 | exit (gen.generate (PACKAGE, "point_cloud2_filters", "SacSegmentationExtractPointCloud2")) 59 | 60 | -------------------------------------------------------------------------------- /include/point_cloud2_filters/FilterIndicesPointCloud2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FILTER_INDICES_POINT_CLOUD_HPP 2 | #define FILTER_INDICES_POINT_CLOUD_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace point_cloud2_filters { 10 | 11 | class FilterIndicesPointCloud2 : public FilterPointCloud2 12 | { 13 | public: 14 | FilterIndicesPointCloud2(); 15 | ~FilterIndicesPointCloud2(); 16 | 17 | public: 18 | virtual bool configure() override; 19 | 20 | protected: 21 | std::shared_ptr> filter_indices_; 22 | 23 | private: 24 | bool negative_ = false; 25 | bool keep_organized_ = true; 26 | //Provide a value that the filtered points should be set to instead of removing them 27 | double user_filter_value_ = std::numeric_limits::quiet_NaN (); 28 | 29 | /** \brief Pointer to a dynamic reconfigure service. */ 30 | std::unique_ptr> dynamic_reconfigure_srv_; 31 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_clbk_; 32 | void dynamicReconfigureClbk(point_cloud2_filters::FilterIndicesPointCloud2Config &config, uint32_t level); 33 | boost::recursive_mutex dynamic_reconfigure_mutex_; 34 | }; 35 | 36 | FilterIndicesPointCloud2::FilterIndicesPointCloud2() : FilterPointCloud2() 37 | { 38 | }; 39 | 40 | FilterIndicesPointCloud2::~FilterIndicesPointCloud2() 41 | { 42 | }; 43 | 44 | bool FilterIndicesPointCloud2::configure() 45 | { 46 | FilterPointCloud2::configure(); 47 | 48 | filter_indices_ = std::dynamic_pointer_cast>(filter_); 49 | 50 | filters::FilterBase::getParam(std::string("keep_organized"), keep_organized_); 51 | ROS_INFO_NAMED(getName(), "[%s] Using keep organized='%d'", getName().c_str(), keep_organized_); 52 | 53 | filters::FilterBase::getParam(std::string("negative"), negative_); 54 | ROS_INFO_NAMED(getName(), "[%s] Using negative='%d'", getName().c_str(), negative_); 55 | 56 | if (filters::FilterBase::getParam(std::string("user_filter_value"), user_filter_value_)) { 57 | ROS_INFO_NAMED(getName(), "[%s] Using user_filter_value='%f'", getName().c_str(), user_filter_value_); 58 | 59 | } 60 | 61 | filter_indices_->setKeepOrganized(keep_organized_); 62 | filter_indices_->setNegative(negative_); 63 | 64 | if (user_filter_value_ != std::numeric_limits::quiet_NaN ()) { 65 | filter_indices_->setUserFilterValue(user_filter_value_); 66 | } 67 | 68 | //dynamic reconfigure 69 | dynamic_reconfigure_srv_ = std::make_unique>( 70 | dynamic_reconfigure_mutex_, 71 | ros::NodeHandle(dynamic_reconfigure_namespace_root_ + "/filter_indices")); 72 | 73 | dynamic_reconfigure_clbk_ = boost::bind(&FilterIndicesPointCloud2::dynamicReconfigureClbk, this, _1, _2); 74 | 75 | point_cloud2_filters::FilterIndicesPointCloud2Config initial_config; 76 | initial_config.keep_organized = keep_organized_; 77 | initial_config.negative = negative_; 78 | 79 | dynamic_reconfigure_srv_->setConfigDefault(initial_config); 80 | dynamic_reconfigure_srv_->updateConfig(initial_config); 81 | 82 | //put this after updateConfig! 83 | dynamic_reconfigure_srv_->setCallback(dynamic_reconfigure_clbk_); 84 | 85 | return true; 86 | 87 | }; 88 | 89 | void FilterIndicesPointCloud2::dynamicReconfigureClbk (point_cloud2_filters::FilterIndicesPointCloud2Config &config, uint32_t /*level*/) 90 | { 91 | 92 | boost::recursive_mutex::scoped_lock lock(dynamic_reconfigure_mutex_); 93 | 94 | 95 | if (keep_organized_ != config.keep_organized) 96 | { 97 | keep_organized_ = config.keep_organized; 98 | filter_indices_->setKeepOrganized(keep_organized_); 99 | ROS_DEBUG_NAMED (getName(), "[%s] Setting keep_organized to: %d.", getName().c_str(), keep_organized_); 100 | } 101 | 102 | if (negative_ != config.negative) 103 | { 104 | negative_ = config.negative; 105 | filter_indices_->setNegative(negative_); 106 | ROS_DEBUG_NAMED (getName(), "[%s] Setting negative to: %d.", getName().c_str(), negative_); 107 | } 108 | 109 | } 110 | 111 | 112 | } //namespace point_cloud2_filters 113 | 114 | 115 | #endif //FILTER_INDICES_POINT_CLOUD_HPP 116 | 117 | -------------------------------------------------------------------------------- /include/point_cloud2_filters/PassThroughFilterPointCloud2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PASS_THROUGH_FILTER_HPP 2 | #define PASS_THROUGH_FILTER_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace point_cloud2_filters { 10 | 11 | 12 | class PassThroughFilterPointCloud2 : public FilterIndicesPointCloud2 13 | { 14 | public: 15 | PassThroughFilterPointCloud2(); 16 | ~PassThroughFilterPointCloud2(); 17 | 18 | public: 19 | virtual bool configure() override; 20 | 21 | private: 22 | std::shared_ptr> pass_through_; 23 | 24 | std::string filter_field_name_ = "z"; 25 | double filter_limit_min_ = 0; 26 | double filter_limit_max_ = 1; 27 | 28 | /** \brief Pointer to a dynamic reconfigure service. */ 29 | std::unique_ptr> dynamic_reconfigure_srv_; 30 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_clbk_; 31 | void dynamicReconfigureClbk(point_cloud2_filters::PassThroughPointCloud2Config &config, uint32_t level); 32 | boost::recursive_mutex dynamic_reconfigure_mutex_; 33 | 34 | }; 35 | 36 | PassThroughFilterPointCloud2::PassThroughFilterPointCloud2() : FilterIndicesPointCloud2() { 37 | 38 | filter_ = std::make_shared>(); 39 | 40 | }; 41 | 42 | PassThroughFilterPointCloud2::~PassThroughFilterPointCloud2() 43 | { 44 | }; 45 | 46 | 47 | bool PassThroughFilterPointCloud2::configure() 48 | { 49 | 50 | FilterIndicesPointCloud2::configure(); 51 | 52 | pass_through_ = std::dynamic_pointer_cast>(filter_); 53 | 54 | filters::FilterBase::getParam(std::string("filter_field_name"), filter_field_name_); 55 | ROS_INFO_NAMED(getName(),"[%s] Using field name='%s'", getName().c_str(), filter_field_name_.c_str()); 56 | 57 | filters::FilterBase::getParam(std::string("filter_limit_min"), filter_limit_min_); 58 | ROS_INFO_NAMED(getName(),"[%s] Using limit min='%f'", getName().c_str(), filter_limit_min_); 59 | 60 | filters::FilterBase::getParam(std::string("filter_limit_max"), filter_limit_max_); 61 | ROS_INFO_NAMED(getName(),"[%s] Using limit max='%f'", getName().c_str(), filter_limit_max_); 62 | 63 | pass_through_->setFilterFieldName(filter_field_name_); 64 | pass_through_->setFilterLimits(filter_limit_min_, filter_limit_max_); 65 | 66 | //dynamic reconfigure 67 | dynamic_reconfigure_srv_ = std::make_unique>( 68 | dynamic_reconfigure_mutex_, 69 | ros::NodeHandle( dynamic_reconfigure_namespace_root_ + "/" + getName())); 70 | 71 | dynamic_reconfigure_clbk_ = boost::bind(&PassThroughFilterPointCloud2::dynamicReconfigureClbk, this, _1, _2); 72 | 73 | point_cloud2_filters::PassThroughPointCloud2Config initial_config; 74 | initial_config.filter_field_name = filter_field_name_; 75 | initial_config.filter_limit_min = filter_limit_min_; 76 | initial_config.filter_limit_max = filter_limit_max_; 77 | 78 | dynamic_reconfigure_srv_->setConfigDefault(initial_config); 79 | dynamic_reconfigure_srv_->updateConfig(initial_config); 80 | 81 | //put this after updateConfig! 82 | dynamic_reconfigure_srv_->setCallback(dynamic_reconfigure_clbk_); 83 | 84 | return true; 85 | 86 | }; 87 | 88 | void PassThroughFilterPointCloud2::dynamicReconfigureClbk (point_cloud2_filters::PassThroughPointCloud2Config &config, uint32_t /*level*/) 89 | { 90 | 91 | boost::recursive_mutex::scoped_lock lock(dynamic_reconfigure_mutex_); 92 | bool to_update_limits = false; 93 | 94 | if (filter_field_name_.compare(config.filter_field_name) != 0) 95 | { 96 | filter_field_name_ = config.filter_field_name; 97 | pass_through_->setFilterFieldName(filter_field_name_); 98 | ROS_DEBUG_NAMED (getName(), "[%s] Setting filter_field_name to: %s.", getName().c_str(), filter_field_name_.c_str()); 99 | } 100 | 101 | if (filter_limit_min_ != config.filter_limit_min) 102 | { 103 | to_update_limits = true; 104 | filter_limit_min_ = config.filter_limit_min; 105 | ROS_DEBUG_NAMED (getName(), "[%s] Setting filter_limit_min to: %f.", getName().c_str(), filter_limit_min_); 106 | } 107 | 108 | if (filter_limit_max_ != config.filter_limit_max) 109 | { 110 | to_update_limits = true; 111 | filter_limit_max_ = config.filter_limit_max; 112 | ROS_DEBUG_NAMED (getName(), "[%s] Setting filter_limit_max to: %f.", getName().c_str(), filter_limit_max_); 113 | } 114 | 115 | if (to_update_limits) { 116 | pass_through_->setFilterLimits(filter_limit_min_, filter_limit_max_); 117 | } 118 | 119 | } 120 | 121 | 122 | } //namespace point_cloud2_filters 123 | 124 | 125 | #endif //PASS_THROUGH_FILTER_HPP 126 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(point_cloud2_filters) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED 8 | roscpp 9 | pluginlib 10 | filters 11 | tf2_ros 12 | pcl_conversions 13 | pcl_ros 14 | sensor_msgs 15 | dynamic_reconfigure 16 | ) 17 | 18 | find_package(Eigen3 REQUIRED) 19 | 20 | generate_dynamic_reconfigure_options( 21 | cfg/FilterBasePointCloud2.cfg 22 | cfg/FilterIndicesPointCloud2.cfg 23 | cfg/PassThroughPointCloud2.cfg 24 | cfg/CropBoxPointCloud2.cfg 25 | cfg/VoxelGridPointCloud2.cfg 26 | cfg/SacSegmentationExtractPointCloud2.cfg 27 | ) 28 | 29 | ################################### 30 | ## catkin specific configuration ## 31 | ################################### 32 | ## The catkin_package macro generates cmake config files for your package 33 | ## Declare things to be passed to dependent projects 34 | ## INCLUDE_DIRS: uncomment this if your package contains header files 35 | ## LIBRARIES: libraries you create in this project that dependent projects also need 36 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 37 | ## DEPENDS: system dependencies of this project that dependent projects also need 38 | catkin_package() 39 | 40 | ########### 41 | ## Build ## 42 | ########### 43 | 44 | ## Specify additional locations of header files 45 | ## Your package locations should be listed before other locations 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${EIGEN3_INCLUDE_DIRS} 50 | ) 51 | 52 | add_library(filter_base_point_cloud2 53 | src/FilterBasePointCloud2.cpp 54 | ) 55 | target_link_libraries(filter_base_point_cloud2 56 | ${catkin_LIBRARIES} 57 | ) 58 | add_dependencies(filter_base_point_cloud2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 59 | class_loader_hide_library_symbols(filter_base_point_cloud2) 60 | 61 | add_library(filter_point_cloud2 62 | src/FilterPointCloud2.cpp 63 | ) 64 | target_link_libraries(filter_point_cloud2 65 | ${catkin_LIBRARIES} 66 | filter_base_point_cloud2 67 | ) 68 | add_dependencies(filter_point_cloud2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 69 | class_loader_hide_library_symbols(filter_point_cloud2) 70 | 71 | add_library(filter_indices_point_cloud2 72 | src/FilterIndicesPointCloud2.cpp 73 | ) 74 | target_link_libraries(filter_indices_point_cloud2 75 | ${catkin_LIBRARIES} 76 | filter_point_cloud2 77 | ) 78 | add_dependencies(filter_indices_point_cloud2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 79 | class_loader_hide_library_symbols(filter_indices_point_cloud2) 80 | 81 | add_library(pass_through_filter_point_cloud2 82 | src/PassThroughFilterPointCloud2.cpp 83 | ) 84 | target_link_libraries(pass_through_filter_point_cloud2 85 | ${catkin_LIBRARIES} 86 | ) 87 | add_dependencies(pass_through_filter_point_cloud2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 88 | class_loader_hide_library_symbols(pass_through_filter_point_cloud2) 89 | 90 | add_library(crop_box_filter_point_cloud2 91 | src/CropBoxFilterPointCloud2.cpp 92 | ) 93 | 94 | target_link_libraries(crop_box_filter_point_cloud2 95 | ${catkin_LIBRARIES} 96 | filter_point_cloud2 97 | ) 98 | add_dependencies(crop_box_filter_point_cloud2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 99 | class_loader_hide_library_symbols(crop_box_filter_point_cloud2) 100 | 101 | add_library(voxel_grid_filter_point_cloud2 102 | src/VoxelGridFilterPointCloud2.cpp 103 | ) 104 | 105 | target_link_libraries(voxel_grid_filter_point_cloud2 106 | ${catkin_LIBRARIES} 107 | filter_point_cloud2 108 | ) 109 | add_dependencies(voxel_grid_filter_point_cloud2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 110 | class_loader_hide_library_symbols(voxel_grid_filter_point_cloud2) 111 | 112 | add_library(sac_segmentation_extract_filter_point_cloud2 113 | src/SacSegmentationExtractFilterPointCloud2.cpp 114 | ) 115 | 116 | target_link_libraries(sac_segmentation_extract_filter_point_cloud2 117 | ${catkin_LIBRARIES} 118 | filter_base_point_cloud2 119 | ) 120 | add_dependencies(sac_segmentation_extract_filter_point_cloud2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 121 | class_loader_hide_library_symbols(sac_segmentation_extract_filter_point_cloud2) 122 | 123 | ############# 124 | ## Install ## 125 | ############# 126 | 127 | install(TARGETS 128 | filter_base_point_cloud2 129 | filter_point_cloud2 130 | filter_indices_point_cloud2 131 | pass_through_filter_point_cloud2 132 | crop_box_filter_point_cloud2 133 | voxel_grid_filter_point_cloud2 134 | sac_segmentation_extract_filter_point_cloud2 135 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 136 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 137 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 138 | ) 139 | 140 | # Install plugins xml file 141 | install(FILES point_cloud2_filters_plugins.xml 142 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 143 | ) 144 | 145 | install(DIRECTORY launch/ 146 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 147 | FILES_MATCHING PATTERN "*.launch" 148 | ) 149 | 150 | install(DIRECTORY config/ 151 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 152 | ) 153 | 154 | ############# 155 | ## Testing ## 156 | ############# 157 | 158 | ## Add gtest based cpp test target and link libraries 159 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pass_through_filter.cpp) 160 | # if(TARGET ${PROJECT_NAME}-test) 161 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 162 | # endif() 163 | 164 | ## Add folders to be run by python nosetests 165 | # catkin_add_nosetests(test) 166 | -------------------------------------------------------------------------------- /include/point_cloud2_filters/CropBoxFilterPointCloud2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CROP_BOX_FILTER_HPP 2 | #define CROP_BOX_FILTER_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace point_cloud2_filters { 10 | 11 | class CropBoxFilterPointCloud2 : public FilterIndicesPointCloud2 12 | { 13 | public: 14 | CropBoxFilterPointCloud2(); 15 | ~CropBoxFilterPointCloud2(); 16 | 17 | public: 18 | virtual bool configure() override; 19 | 20 | private: 21 | 22 | std::shared_ptr> crop_box_; 23 | 24 | double min_x_, min_y_, min_z_ = -1; 25 | double max_x_, max_y_, max_z_ = 1; 26 | 27 | /** \brief Pointer to a dynamic reconfigure service. */ 28 | std::unique_ptr> dynamic_reconfigure_srv_; 29 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_clbk_; 30 | void dynamicReconfigureClbk(point_cloud2_filters::CropBoxPointCloud2Config &config, uint32_t level); 31 | boost::recursive_mutex dynamic_reconfigure_mutex_; 32 | }; 33 | 34 | CropBoxFilterPointCloud2::CropBoxFilterPointCloud2() : FilterIndicesPointCloud2() { 35 | 36 | filter_ = std::make_shared>(); 37 | }; 38 | 39 | CropBoxFilterPointCloud2::~CropBoxFilterPointCloud2() 40 | { 41 | }; 42 | 43 | 44 | bool CropBoxFilterPointCloud2::configure() 45 | { 46 | 47 | FilterIndicesPointCloud2::configure(); 48 | 49 | crop_box_ = std::dynamic_pointer_cast>(filter_); 50 | 51 | if (filters::FilterBase::getParam(std::string("min_x"), min_x_)) 52 | { 53 | 54 | ROS_INFO_NAMED(getName(), "[%s] Using min_x=%f", getName().c_str(), min_x_); 55 | } 56 | 57 | if (filters::FilterBase::getParam(std::string("max_x"), max_x_)) 58 | { 59 | 60 | ROS_INFO_NAMED(getName(), "[%s] Using max_x=%f", getName().c_str(), max_x_); 61 | } 62 | 63 | if (filters::FilterBase::getParam(std::string("min_y"), min_y_)) 64 | { 65 | 66 | ROS_INFO_NAMED(getName(), "[%s] Using min_y=%f", getName().c_str(), min_y_); 67 | } 68 | 69 | if (filters::FilterBase::getParam(std::string("max_y"), max_y_)) 70 | { 71 | 72 | ROS_INFO_NAMED(getName(), "[%s] Using max_y=%f", getName().c_str(), max_y_); 73 | } 74 | 75 | if (filters::FilterBase::getParam(std::string("min_z"), min_z_)) 76 | { 77 | 78 | ROS_INFO_NAMED(getName(), "[%s] Using min_z=%f", getName().c_str(), min_z_); 79 | } 80 | 81 | if (filters::FilterBase::getParam(std::string("max_z"), max_z_)) 82 | { 83 | 84 | ROS_INFO_NAMED(getName(), "[%s] Using max_z=%f", getName().c_str(), max_z_); 85 | } 86 | 87 | Eigen::Vector4f min_point, max_point; 88 | min_point << min_x_, min_y_, min_z_, 0; 89 | max_point << max_x_, max_y_, max_z_, 0; 90 | 91 | crop_box_->setMin(min_point); 92 | crop_box_->setMax(max_point); 93 | 94 | //dynamic reconfigure 95 | dynamic_reconfigure_srv_ = std::make_unique>( 96 | dynamic_reconfigure_mutex_, 97 | ros::NodeHandle( dynamic_reconfigure_namespace_root_ + "/" + getName())); 98 | 99 | dynamic_reconfigure_clbk_ = boost::bind(&CropBoxFilterPointCloud2::dynamicReconfigureClbk, this, _1, _2); 100 | 101 | point_cloud2_filters::CropBoxPointCloud2Config initial_config; 102 | initial_config.min_x = min_x_; 103 | initial_config.max_x = max_x_; 104 | initial_config.min_y = min_y_; 105 | initial_config.max_y = max_y_; 106 | initial_config.min_z = min_z_; 107 | initial_config.max_z = max_z_; 108 | 109 | dynamic_reconfigure_srv_->setConfigDefault(initial_config); 110 | dynamic_reconfigure_srv_->updateConfig(initial_config); 111 | 112 | //put this after updateConfig! 113 | dynamic_reconfigure_srv_->setCallback(dynamic_reconfigure_clbk_); 114 | 115 | return true; 116 | 117 | }; 118 | 119 | void CropBoxFilterPointCloud2::dynamicReconfigureClbk (point_cloud2_filters::CropBoxPointCloud2Config &config, uint32_t /*level*/) 120 | { 121 | 122 | boost::recursive_mutex::scoped_lock lock(dynamic_reconfigure_mutex_); 123 | bool max_to_update = false; 124 | bool min_to_update = false; 125 | 126 | if (min_x_ != config.min_x) 127 | { 128 | min_to_update = true; 129 | min_x_ = config.min_x; 130 | ROS_DEBUG_NAMED (getName(), "[%s] Setting min_x to: %f.", getName().c_str(), min_x_); 131 | } 132 | if (min_y_ != config.min_y) 133 | { 134 | min_to_update = true; 135 | min_y_ = config.min_y; 136 | ROS_DEBUG_NAMED (getName(), "[%s] Setting min_y to: %f.", getName().c_str(), min_y_); 137 | } 138 | if (min_z_ != config.min_z) 139 | { 140 | min_to_update = true; 141 | min_z_ = config.min_z; 142 | ROS_DEBUG_NAMED (getName(), "[%s] Setting min_z to: %f.", getName().c_str(), min_z_); 143 | } 144 | 145 | if (max_x_ != config.max_x) 146 | { 147 | max_to_update = true; 148 | max_x_ = config.max_x; 149 | ROS_DEBUG_NAMED (getName(), "[%s] Setting max_x to: %f.", getName().c_str(), max_x_); 150 | } 151 | if (max_y_ != config.max_y) 152 | { 153 | max_to_update = true; 154 | max_y_ = config.max_y; 155 | ROS_DEBUG_NAMED (getName(), "[%s] Setting max_y to: %f.", getName().c_str(), max_y_); 156 | } 157 | if (max_z_ != config.max_z) 158 | { 159 | max_to_update = true; 160 | max_z_ = config.max_z; 161 | ROS_DEBUG_NAMED (getName(), "[%s] Setting max_z to: %f.", getName().c_str(), max_z_); 162 | } 163 | 164 | if (min_to_update) { 165 | Eigen::Vector4f min_point; 166 | min_point << min_x_, min_y_, min_z_, 0; 167 | crop_box_->setMin(min_point); 168 | } 169 | if (max_to_update) { 170 | Eigen::Vector4f max_point; 171 | max_point << max_x_, max_y_, max_z_, 0; 172 | crop_box_->setMax(max_point); 173 | } 174 | } 175 | 176 | 177 | } //namespace point_cloud2_filters 178 | 179 | 180 | #endif //CROP_BOX_FILTER_HPP 181 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # point_cloud_2_filters 2 | [![Build Status](https://build.ros.org/buildStatus/icon?job=Ndev__point_cloud2_filters__ubuntu_focal_amd64&subject=Noetic%20Build)](https://build.ros.org/job/Ndev__point_cloud2_filters__ubuntu_focal_amd64/) 3 | 4 | Wrappers for some of the [pcl filters](https://pointclouds.org/documentation/group__filters.html) for `sensor_msgs/PointCloud2` ROS messages. The implementation and usage is based on the [filter](https://wiki.ros.org/filters) and [sensor_filter](https://wiki.ros.org/sensor_filters) packages, so it is different from the wrappers of the PCL filters provided by the package [pcl_ros](https://wiki.ros.org/pcl_ros/Tutorials/filters). 5 | 6 | All the parameters are settable from the config file, but also online through the `dynamic_reconfigure` server. 7 | Note that changing params with the `dynamic_reconfigure` server may take some seconds to have effect. 8 | 9 | No ROS2 version (yet). 10 | 11 | ## Usage example 12 | See launch and config folders 13 | 14 | ## Filters list 15 | ### PassThroughFilterPointCloud2 16 | Wrapper for the [pcl::PassThrough](https://pointclouds.org/documentation/classpcl_1_1_pass_through_3_01pcl_1_1_p_c_l_point_cloud2_01_4.html) filter 17 | #### Params 18 | - `active`*(bool, default: true)* Activate the filter or not. 19 | - `input_frame`*(str, default: "")* The input TF frame the data should be transformed into before processing 20 | - `output_frame`*(str, default: "")* The output TF frame the data should be transformed into after processing 21 | - `pub_cloud`*(bool, default: false)* Publish the cloud immediately after this filter. Note that this is a duplicate if the filter is the last in the chain. Useful for debug purposes and it will publish even if `active` is *false*. 22 | - `keep_organized`*(bool, default: true)* Keep the point cloud organized ([`pcl::FilterIndices::setKeepOrganized (bool keep_organized)`](https://pointclouds.org/documentation/classpcl_1_1_filter_indices.html#a21eb00357056c0cc432cd03afa84d08c) 23 | - `negative`*(bool, default: false)* Set to true to return the data outside the min max limits 24 | - `filter_field_name`*(str, default: z)* The field to be used for filtering data 25 | - `filter_limit_min`*(double, default: 0)* The minimum allowed field value a point will be considered 26 | - `filter_limit_max`*(double, default: 1)* The maximum allowed field value a point will be considered 27 | 28 | ### CropBoxFilterPointCloud2 29 | Wrapper for the [pcl::CropBox](https://pointclouds.org/documentation/classpcl_1_1_crop_box_3_01pcl_1_1_p_c_l_point_cloud2_01_4.html) filter. 30 | **Warning** `pcl::CrobBox` parameter `keep_organized` is broken on ROS melodic (on noetic it is ok). 31 | #### Params 32 | - `active`*(bool, default: true)* Activate the filter or not. 33 | - `input_frame`*(str, default: "")* The input TF frame the data should be transformed into before processing 34 | - `output_frame`*(str, default: "")* The output TF frame the data should be transformed into after processing 35 | - `pub_cloud`*(bool, default: false)* Publish the cloud immediately after this filter. Note that this is a duplicate if the filter is the last in the chain. Useful for debug purposes and it will publish even if `active` is *false*. 36 | - `keep_organized`*(bool, default: true)* Keep the point cloud organized ([`pcl::FilterIndices::setKeepOrganized (bool keep_organized)`](https://pointclouds.org/documentation/classpcl_1_1_filter_indices.html#a21eb00357056c0cc432cd03afa84d08c) 37 | - `negative`*(bool, default: false)* Set to true to return the data outside the min max limits 38 | - `min_x`*(double, default: -1.0)* The minimum allowed x value a point will be considered from. Range: -1000.0 to 1000.0 39 | - `max_x`*(double, default: -1.0)* The maximum allowed x value a point will be considered from. Range: -1000.0 to 1000.0 40 | - `min_y`*(double, default: -1.0)* The minimum allowed y value a point will be considered from. Range: -1000.0 to 1000.0 41 | - `max_y`*(double, default: -1.0)* The maximum allowed y value a point will be considered from. Range: -1000.0 to 1000.0 42 | - `min_z`*(double, default: -1.0)* The minimum allowed z value a point will be considered from. Range: -1000.0 to 1000.0 43 | - `max_z`*(double, default: -1.0)* The maximum allowed z value a point will be considered from. Range: -1000.0 to 1000.0 44 | 45 | ### VoxelGridFilterPointCloud2 46 | Wrapper for the [pcl::VoxelGrid](https://pointclouds.org/documentation/classpcl_1_1_voxel_grid.html) filter. 47 | #### Params 48 | - `active`*(bool, default: true)* Activate the filter or not. 49 | - `input_frame`*(str, default: "")* The input TF frame the data should be transformed into before processing 50 | - `output_frame`*(str, default: "")* The output TF frame the data should be transformed into after processing 51 | - `pub_cloud`*(bool, default: false)* Publish the cloud immediately after this filter. Note that this is a duplicate if the filter is the last in the chain. Useful for debug purposes and it will publish even if `active` is *false*. 52 | - `negative`*(bool, default: false)* Set to true to return the data outside the min max limits 53 | - `leaf_size_x`*(double, default: 0.01)* The size of a leaf (on x) used for downsampling. Range: 0.0 to 1.0 54 | - `leaf_size_y`*(double, default: 0.01)* The size of a leaf (on y) used for downsampling. Range: 0.0 to 1.0 55 | - `leaf_size_z`*(double, default: 0.01)* The size of a leaf (on z) used for downsampling. Range: 0.0 to 1.0 56 | - `min_points_per_voxel`*(int, default:0)* Set the minimum number of points required for a voxel to be used 57 | - `downsample_all_data`*(int, default:0)* Set to true if all fields need to be downsampled, or false if just XYZ 58 | - `filter_field_name`*(str, default: "")* The field to be used for filtering data, acting like a passthrough. Empty for not using 59 | - `filter_limit_min`*(double, default: -FLT_MAX)* The minimum allowed field value a point will be considered 60 | - `filter_limit_max`*(double, default: FLT_MAX)* The maximum allowed field value a point will be considered 61 | 62 | ### SacSegmentationExtractFilterPointCloud2 63 | Wrapper to extract a geometric model with [pcl::SACSegmentation](https://pointclouds.org/documentation/classpcl_1_1_s_a_c_segmentation.html) and [pcl::ExtractIndices](https://pointclouds.org/documentation/classpcl_1_1_extract_indices.html). 64 | #### Params 65 | - `active`*(bool, default: true)* Activate the filter or not. 66 | - `input_frame`*(str, default: "")* The input TF frame the data should be transformed into before processing 67 | - `output_frame`*(str, default: "")* The output TF frame the data should be transformed into after processing 68 | - `pub_cloud`*(bool, default: false)* Publish the cloud immediately after this filter. Note that this is a duplicate if the filter is the last in the chain. Useful for debug purposes and it will publish even if `active` is *false*. 69 | - `negative`*(bool, default: false)* Set whether to filter out (remove) the model (true) or all the rest (false). 70 | - `model_type` *(int, default: 16)* Geometric model to look for. Default to `SACMODEL_PARALLEL_PLANE` (15). Please use integers according to the linked enum. 71 | Some types require a SacSegmentationFromNormals which is currently not implemented. Check `SacSegmentationExtractPointCloud2.cfg` for the ones available. See also [pcl official doc](https://pointclouds.org/documentation/group__sample__consensus.html) for params info. 72 | - `method_type` *(int, default: 0)* Segmentation model to use for. Default to `SAC_RANSAC` (0) . Check [pcl official doc](https://pointclouds.org/documentation/group__sample__consensus.html). Please use integers according to the linked enum. 73 | - `axis_x`*(double, default: 0.0)* The x component of the normal to the model to be removed. Range: 0.0 to 1.0 74 | - `axis_y`*(double, default: 0.0)* The y component of the normal to the model to be removed. Range: 0.0 to 1.0 75 | - `axis_z`*(double, default: 1.0)* The z component of the normal to the model to be removed. Range: 0.0 to 1.0 76 | - `eps_angle`*(double, default: 0.15)* Tolerance angle (rad) to the model to be considered normal to the axis. Range: -3.15 to 3.15 77 | - `distance_threshold`*(double, default: 0.01)* Range: 0 to 10 78 | - `optimize_coefficents`*(bool, default: 0.01)* Optimize the coefficents or not. 79 | - `max_iterations`*(bool, default: 50)* 80 | - `probability`*(bool, default: 0.99)* 81 | - `min_radius`*(bool, default: -1)* 82 | - `max_radius`*(bool, default: 1000)* 83 | 84 | -------------------------------------------------------------------------------- /include/point_cloud2_filters/FilterBasePointCloud2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FILTER_BASE_POINT_CLOUD_HPP 2 | #define FILTER_BASE_POINT_CLOUD_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | #if ROS_VERSION_MINIMUM(1, 15, 0) 9 | #include 10 | #else 11 | #include 12 | #endif 13 | #include 14 | #include 15 | 16 | // Dynamic reconfigure 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | namespace point_cloud2_filters { 26 | 27 | typedef pcl::PointXYZ Point; 28 | typedef pcl::PointCloud PointCloud; 29 | 30 | class FilterBasePointCloud2 : public filters::FilterBase 31 | { 32 | public: 33 | FilterBasePointCloud2(); 34 | ~FilterBasePointCloud2(); 35 | 36 | public: 37 | virtual bool configure() override; 38 | 39 | /** \brief Update the filter and return the data seperately 40 | * \param data_in T array with length width 41 | * \param data_out T array with length width 42 | */ 43 | virtual bool update( const sensor_msgs::PointCloud2& data_in, sensor_msgs::PointCloud2& data_out) override final; 44 | 45 | protected: 46 | std::string dynamic_reconfigure_namespace_root_; 47 | 48 | virtual bool execute() = 0; 49 | 50 | PointCloud::Ptr cloud_out_; 51 | 52 | private: 53 | 54 | tf2_ros::Buffer tf_buffer_; 55 | std::unique_ptr tf_listener_; 56 | std::unique_ptr nh_; 57 | ros::Publisher pc_pub_; 58 | 59 | /** \brief Pointer to a dynamic reconfigure service. */ 60 | std::unique_ptr> dynamic_reconfigure_srv_; 61 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_clbk_; 62 | void dynamicReconfigureClbk(point_cloud2_filters::FilterBasePointCloud2Config &config, uint32_t level); 63 | boost::recursive_mutex dynamic_reconfigure_mutex_; 64 | 65 | bool active_ = true; 66 | std::string input_frame_ = ""; 67 | std::string output_frame_ = ""; 68 | bool pub_cloud_ = false; 69 | 70 | bool log_time_ = false; 71 | std::chrono::high_resolution_clock::time_point start_time_, stop_time_; 72 | }; 73 | 74 | FilterBasePointCloud2::FilterBasePointCloud2() { 75 | 76 | cloud_out_ = boost::make_shared(); 77 | 78 | tf_listener_ = std::make_unique(tf_buffer_); 79 | 80 | }; 81 | 82 | FilterBasePointCloud2::~FilterBasePointCloud2() 83 | { 84 | }; 85 | 86 | bool FilterBasePointCloud2::configure() 87 | { 88 | 89 | nh_ = std::make_unique("~"); 90 | 91 | if (filters::FilterBase::getParam(std::string("active"), active_)) 92 | { 93 | ROS_INFO_NAMED(getName(), "[%s] Using active='%d'", getName().c_str(), active_); 94 | } 95 | 96 | if (filters::FilterBase::getParam(std::string("input_frame"), input_frame_)) 97 | { 98 | ROS_INFO_NAMED(getName(), "[%s] Using input_frame='%s'", getName().c_str(), input_frame_.c_str()); 99 | } 100 | 101 | if (filters::FilterBase::getParam(std::string("output_frame"), output_frame_)) 102 | { 103 | ROS_INFO_NAMED(getName(), "[%s] Using output_frame='%s'", getName().c_str(), output_frame_.c_str()); 104 | } 105 | 106 | if (filters::FilterBase::getParam(std::string("pub_cloud"), pub_cloud_)) 107 | { 108 | ROS_INFO_NAMED(getName(), "[%s] Using pub_cloud='%d'", getName().c_str(), pub_cloud_); 109 | pc_pub_ = nh_->advertise(getName()+"/points", 10); 110 | } 111 | if (filters::FilterBase::getParam(std::string("log_time"), log_time_)) 112 | { 113 | ROS_INFO_NAMED(getName(), "[%s] Using log_time_='%d'", getName().c_str(), log_time_); 114 | } 115 | 116 | //WARNING dynamic reconfigure, the base class one. Children can have their own server for their specific values, but 117 | //be sure to use another namespace to be passed to the dyn server constructor (eg ros::NodeHandle(dynamic_reconfigure_namespace_root_ + "/" + getName()) 118 | dynamic_reconfigure_namespace_root_ = "/filter/" + getName(); 119 | dynamic_reconfigure_srv_ = std::make_unique>( 120 | dynamic_reconfigure_mutex_, 121 | ros::NodeHandle(dynamic_reconfigure_namespace_root_ + "/base")); 122 | 123 | dynamic_reconfigure_clbk_ = boost::bind(&FilterBasePointCloud2::dynamicReconfigureClbk, this, _1, _2); 124 | 125 | point_cloud2_filters::FilterBasePointCloud2Config initial_config; 126 | initial_config.active = active_; 127 | initial_config.input_frame = input_frame_; 128 | initial_config.output_frame = output_frame_; 129 | initial_config.pub_cloud = pub_cloud_; 130 | dynamic_reconfigure_srv_->setConfigDefault(initial_config); 131 | dynamic_reconfigure_srv_->updateConfig(initial_config); 132 | 133 | //put this after updateConfig! 134 | dynamic_reconfigure_srv_->setCallback(dynamic_reconfigure_clbk_); 135 | 136 | return true; 137 | 138 | }; 139 | 140 | bool FilterBasePointCloud2::update( const sensor_msgs::PointCloud2& data_in, sensor_msgs::PointCloud2& data_out) 141 | { 142 | 143 | if (log_time_) { 144 | start_time_ = std::chrono::high_resolution_clock::now(); 145 | } 146 | 147 | if (active_) { 148 | pcl::fromROSMsg(data_in, *cloud_out_); 149 | 150 | //TODO do not transform if already that frame 151 | if (input_frame_.length() > 0) { 152 | 153 | pcl_ros::transformPointCloud (input_frame_, *cloud_out_, *cloud_out_, tf_buffer_); 154 | } 155 | 156 | if (!execute()) { 157 | data_out = data_in; 158 | return false; 159 | } 160 | 161 | //TODO do not transform if already that frame 162 | if (output_frame_.length() > 0) { 163 | 164 | pcl_ros::transformPointCloud (output_frame_, *cloud_out_, *cloud_out_, tf_buffer_); 165 | } 166 | 167 | pcl::toROSMsg(*cloud_out_, data_out); 168 | 169 | } else { 170 | data_out = data_in; 171 | } 172 | 173 | if (pub_cloud_) { 174 | pc_pub_.publish(data_out); 175 | } 176 | if (log_time_) { 177 | stop_time_ = std::chrono::high_resolution_clock::now(); 178 | ROS_INFO_NAMED(getName(), "[%s] took %ld ms", getName().c_str(), std::chrono::duration_cast(stop_time_ - start_time_).count()); 179 | } 180 | 181 | return true; 182 | 183 | }; 184 | 185 | void FilterBasePointCloud2::dynamicReconfigureClbk (point_cloud2_filters::FilterBasePointCloud2Config &config, uint32_t /*level*/) 186 | { 187 | 188 | boost::recursive_mutex::scoped_lock lock(dynamic_reconfigure_mutex_); 189 | 190 | if (active_ != config.active) 191 | { 192 | active_ = config.active; 193 | ROS_DEBUG_NAMED (getName(), "[%s] Setting active to: %d.", getName().c_str(), active_); 194 | } 195 | 196 | if (input_frame_.compare(config.input_frame) != 0 ) 197 | { 198 | input_frame_ = config.input_frame; 199 | ROS_DEBUG_NAMED (getName(), "[%s] Setting the input TF frame to: %s.", getName().c_str(), input_frame_.c_str()); 200 | } 201 | 202 | if (output_frame_.compare(config.output_frame) != 0 ) 203 | { 204 | output_frame_ = config.output_frame; 205 | ROS_DEBUG_NAMED (getName(), "[%s] Setting the output TF frame to: %s.", getName().c_str(), output_frame_.c_str()); 206 | } 207 | 208 | if (pub_cloud_ != config.pub_cloud) 209 | { 210 | pub_cloud_ = config.pub_cloud; 211 | if (pub_cloud_) { 212 | pc_pub_ = nh_->advertise(getName()+"/points", 10); 213 | } else { 214 | pc_pub_.shutdown(); 215 | } 216 | ROS_DEBUG_NAMED (getName(), "[%s] Setting pub_cloud to: %d.", getName().c_str(), pub_cloud_); 217 | } 218 | } 219 | 220 | 221 | } //namespace point_cloud2_filters 222 | 223 | 224 | #endif //FILTER_BASE_POINT_CLOUD_HPP 225 | 226 | -------------------------------------------------------------------------------- /include/point_cloud2_filters/VoxelGridFilterPointCloud2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VOXEL_GRID_FILTER_HPP 2 | #define VOXEL_GRID_FILTER_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace point_cloud2_filters { 10 | 11 | class VoxelGridFilterPointCloud2 : public FilterPointCloud2 12 | { 13 | public: 14 | VoxelGridFilterPointCloud2(); 15 | ~VoxelGridFilterPointCloud2(); 16 | 17 | public: 18 | virtual bool configure() override; 19 | 20 | private: 21 | std::shared_ptr> voxel_grid_; 22 | 23 | double leaf_size_x_, leaf_size_y_, leaf_size_z_ = 0.01; 24 | unsigned int min_points_per_voxel_ = 0; 25 | bool downsample_all_data_ = true; 26 | std::string filter_field_name_ = ""; 27 | double filter_limit_min_ = -1000; 28 | double filter_limit_max_ = 1000; 29 | bool negative_ = false; 30 | 31 | /** \brief Pointer to a dynamic reconfigure service. */ 32 | std::unique_ptr> dynamic_reconfigure_srv_; 33 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_clbk_; 34 | void dynamicReconfigureClbk(point_cloud2_filters::VoxelGridPointCloud2Config &config, uint32_t level); 35 | boost::recursive_mutex dynamic_reconfigure_mutex_; 36 | 37 | }; 38 | 39 | VoxelGridFilterPointCloud2::VoxelGridFilterPointCloud2() : FilterPointCloud2() { 40 | 41 | filter_ = std::make_shared>(); 42 | 43 | }; 44 | 45 | VoxelGridFilterPointCloud2::~VoxelGridFilterPointCloud2() 46 | { 47 | }; 48 | 49 | 50 | bool VoxelGridFilterPointCloud2::configure() 51 | { 52 | 53 | FilterPointCloud2::configure(); 54 | 55 | voxel_grid_ = std::dynamic_pointer_cast>(filter_); 56 | 57 | filters::FilterBase::getParam(std::string("leaf_size_x"), leaf_size_x_); 58 | filters::FilterBase::getParam(std::string("leaf_size_y"), leaf_size_y_); 59 | filters::FilterBase::getParam(std::string("leaf_size_z"), leaf_size_z_); 60 | 61 | ROS_INFO_NAMED(getName(), "[%s] Using leaf_size='[%f, %f, %f]'", getName().c_str(), leaf_size_x_, leaf_size_y_, leaf_size_z_); 62 | 63 | filters::FilterBase::getParam(std::string("min_points_per_voxel"), min_points_per_voxel_); 64 | ROS_INFO_NAMED(getName(), "[%s] Using min_points_per_voxel=%d", getName().c_str(), min_points_per_voxel_); 65 | 66 | filters::FilterBase::getParam(std::string("downsample_all_data"), downsample_all_data_); 67 | ROS_INFO_NAMED(getName(), "[%s] Using downsample_all_data=%d", getName().c_str(), downsample_all_data_); 68 | 69 | if (filters::FilterBase::getParam(std::string("filter_field_name"), filter_field_name_)) 70 | { 71 | 72 | ROS_INFO_NAMED(getName(), "[%s] Using filter_field_name=%s", getName().c_str(), filter_field_name_.c_str()); 73 | } 74 | 75 | if (filters::FilterBase::getParam(std::string("filter_limit_min"), filter_limit_min_)) 76 | { 77 | 78 | ROS_INFO_NAMED(getName(), "[%s] Using filter_limit_min=%f", getName().c_str(), filter_limit_min_); 79 | } 80 | 81 | if (filters::FilterBase::getParam(std::string("filter_limit_max"), filter_limit_max_)) 82 | { 83 | 84 | ROS_INFO_NAMED(getName(), "[%s] Using filter_limit_max=%f", getName().c_str(), filter_limit_max_); 85 | } 86 | 87 | filters::FilterBase::getParam(std::string("negative"), negative_); 88 | ROS_INFO_NAMED(getName(), "[%s] Using negative='%d'", getName().c_str(), negative_); 89 | 90 | voxel_grid_->setLeafSize(leaf_size_x_, leaf_size_y_, leaf_size_z_); 91 | voxel_grid_->setMinimumPointsNumberPerVoxel(min_points_per_voxel_); 92 | voxel_grid_->setDownsampleAllData(downsample_all_data_); 93 | 94 | if (filter_field_name_.length() > 0) { 95 | voxel_grid_->setFilterFieldName(filter_field_name_); 96 | voxel_grid_->setFilterLimits(filter_limit_min_, filter_limit_max_); 97 | } 98 | voxel_grid_->setFilterLimitsNegative(negative_); 99 | 100 | //dynamic reconfigure 101 | dynamic_reconfigure_srv_ = std::make_unique>( 102 | dynamic_reconfigure_mutex_, 103 | ros::NodeHandle( dynamic_reconfigure_namespace_root_ + "/" + getName())); 104 | 105 | dynamic_reconfigure_clbk_ = boost::bind(&VoxelGridFilterPointCloud2::dynamicReconfigureClbk, this, _1, _2); 106 | 107 | point_cloud2_filters::VoxelGridPointCloud2Config initial_config; 108 | initial_config.negative = negative_; 109 | initial_config.leaf_size_x = leaf_size_x_; 110 | initial_config.leaf_size_y = leaf_size_y_; 111 | initial_config.leaf_size_z = leaf_size_z_; 112 | initial_config.min_points_per_voxel = min_points_per_voxel_; 113 | initial_config.downsample_all_data = downsample_all_data_; 114 | initial_config.filter_field_name = filter_field_name_; 115 | initial_config.filter_limit_min = filter_limit_min_; 116 | initial_config.filter_limit_max = filter_limit_max_; 117 | 118 | dynamic_reconfigure_srv_->setConfigDefault(initial_config); 119 | dynamic_reconfigure_srv_->updateConfig(initial_config); 120 | 121 | //put this after updateConfig! 122 | dynamic_reconfigure_srv_->setCallback(dynamic_reconfigure_clbk_); 123 | 124 | return true; 125 | 126 | }; 127 | 128 | void VoxelGridFilterPointCloud2::dynamicReconfigureClbk (point_cloud2_filters::VoxelGridPointCloud2Config &config, uint32_t /*level*/) 129 | { 130 | 131 | boost::recursive_mutex::scoped_lock lock(dynamic_reconfigure_mutex_); 132 | bool to_update_limits = false; 133 | bool to_update_leaf_size = false; 134 | 135 | if (leaf_size_x_ != config.leaf_size_x) 136 | { 137 | leaf_size_x_ = config.leaf_size_x; 138 | to_update_leaf_size = true; 139 | ROS_DEBUG_NAMED (getName(), "[%s] Setting leaf_size_x to: %f.", getName().c_str(), leaf_size_x_); 140 | } 141 | if (leaf_size_y_ != config.leaf_size_y) 142 | { 143 | leaf_size_y_ = config.leaf_size_y; 144 | to_update_leaf_size = true; 145 | ROS_DEBUG_NAMED (getName(), "[%s] Setting leaf_size_y to: %f.", getName().c_str(), leaf_size_y_); 146 | } 147 | if (leaf_size_z_ != config.leaf_size_z) 148 | { 149 | leaf_size_z_ = config.leaf_size_z; 150 | to_update_leaf_size = true; 151 | ROS_DEBUG_NAMED (getName(), "[%s] Setting leaf_size_z to: %f.", getName().c_str(), leaf_size_z_); 152 | } 153 | 154 | if (min_points_per_voxel_ != config.min_points_per_voxel) 155 | { 156 | min_points_per_voxel_ = config.min_points_per_voxel; 157 | voxel_grid_->setMinimumPointsNumberPerVoxel(min_points_per_voxel_); 158 | ROS_DEBUG_NAMED (getName(), "[%s] Setting min_points_per_voxel to: %d.", getName().c_str(), min_points_per_voxel_); 159 | } 160 | 161 | if (downsample_all_data_ != config.downsample_all_data) 162 | { 163 | downsample_all_data_ = config.downsample_all_data; 164 | voxel_grid_->setDownsampleAllData(downsample_all_data_); 165 | ROS_DEBUG_NAMED (getName(), "[%s] Setting downsample_all_data to: %d.", getName().c_str(), downsample_all_data_); 166 | } 167 | if (filter_field_name_.compare(config.filter_field_name) != 0) 168 | { 169 | filter_field_name_ = config.filter_field_name; 170 | voxel_grid_->setFilterFieldName(filter_field_name_); 171 | ROS_DEBUG_NAMED (getName(), "[%s] Setting filter_field_name to: %s.", getName().c_str(), filter_field_name_.c_str()); 172 | } 173 | if (filter_limit_min_ != config.filter_limit_min) 174 | { 175 | filter_limit_min_ = config.filter_limit_min; 176 | to_update_limits = true; 177 | ROS_DEBUG_NAMED (getName(), "[%s] Setting filter_limit_min to: %f.", getName().c_str(), filter_limit_min_); 178 | } 179 | if (filter_limit_max_ != config.filter_limit_max) 180 | { 181 | filter_limit_max_ = config.filter_limit_max; 182 | to_update_limits = true; 183 | ROS_DEBUG_NAMED (getName(), "[%s] Setting filter_limit_max to: %f.", getName().c_str(), filter_limit_max_); 184 | } 185 | if (negative_ != config.negative) 186 | { 187 | negative_ = config.negative; 188 | voxel_grid_->setFilterLimitsNegative(negative_); 189 | ROS_DEBUG_NAMED (getName(), "[%s] Setting negative to: %d.", getName().c_str(), negative_); 190 | } 191 | 192 | 193 | if (to_update_limits) { 194 | voxel_grid_->setFilterLimits(filter_limit_min_, filter_limit_max_); 195 | } 196 | if (to_update_leaf_size) { 197 | voxel_grid_->setLeafSize(leaf_size_x_, leaf_size_y_, leaf_size_z_); 198 | } 199 | 200 | 201 | 202 | } 203 | 204 | 205 | } //namespace point_cloud2_filters 206 | 207 | 208 | #endif //VOXEL_GRID_FILTER_HPP 209 | -------------------------------------------------------------------------------- /include/point_cloud2_filters/SacSegmentationExtractFilterPointCloud2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SAC_SEGMENTATION_EXTRACT_FILTER_POINT_CLOUD_HPP 2 | #define SAC_SEGMENTATION_EXTRACT_FILTER_POINT_CLOUD_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace point_cloud2_filters { 13 | 14 | class SacSegmentationExtractFilterPointCloud2 : public FilterBasePointCloud2 15 | { 16 | public: 17 | SacSegmentationExtractFilterPointCloud2(); 18 | ~SacSegmentationExtractFilterPointCloud2(); 19 | 20 | public: 21 | virtual bool configure() override; 22 | 23 | protected: 24 | virtual bool execute() override; 25 | 26 | private: 27 | pcl::ModelCoefficients::Ptr coefficients_; 28 | pcl::PointIndices::Ptr inliers_; 29 | pcl::SACSegmentation sac_segmentation_; 30 | pcl::ExtractIndices extract_indices_; 31 | 32 | double axis_x_ = 0; 33 | double axis_y_ = 0; 34 | double axis_z_ = 1; 35 | double eps_angle_ = 0.15; 36 | double distance_threshold_ = 0.01; 37 | 38 | bool optimize_coefficents_ = true; 39 | bool negative_ = true; //true: remove the model, false: remove the rest 40 | int max_iterations_ = 50; 41 | double probability_ = 0.99; 42 | double min_radius_ = -10000; //dyn param server wants -2147483648 to 2147483647 range, cant use std::numeric_limits::max and min 43 | double max_radius_ = 10000; 44 | 45 | //https://pointclouds.org/documentation/model__types_8h_source.html 46 | int model_type_ = pcl::SacModel::SACMODEL_PERPENDICULAR_PLANE; 47 | 48 | //https://pointclouds.org/documentation/method__types_8h_source.html 49 | int method_type_ = pcl::SAC_RANSAC; 50 | 51 | /** \brief Pointer to a dynamic reconfigure service. */ 52 | std::unique_ptr> dynamic_reconfigure_srv_; 53 | dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_clbk_; 54 | void dynamicReconfigureClbk(point_cloud2_filters::SacSegmentationExtractPointCloud2Config &config, uint32_t level); 55 | boost::recursive_mutex dynamic_reconfigure_mutex_; 56 | 57 | }; 58 | 59 | SacSegmentationExtractFilterPointCloud2::SacSegmentationExtractFilterPointCloud2() 60 | { 61 | 62 | coefficients_ = boost::make_shared(); 63 | inliers_ = boost::make_shared(); 64 | 65 | }; 66 | 67 | SacSegmentationExtractFilterPointCloud2::~SacSegmentationExtractFilterPointCloud2() 68 | { 69 | }; 70 | 71 | bool SacSegmentationExtractFilterPointCloud2::configure() 72 | { 73 | 74 | FilterBasePointCloud2::configure(); 75 | 76 | if (filters::FilterBase::getParam(std::string("optimize_coefficents"), optimize_coefficents_)) 77 | { 78 | sac_segmentation_.setOptimizeCoefficients (optimize_coefficents_); 79 | ROS_INFO_NAMED(getName(), "[%s] Using optimize_coefficents=%d", getName().c_str(), optimize_coefficents_); 80 | } 81 | 82 | if (filters::FilterBase::getParam(std::string("max_iterations"), max_iterations_)) 83 | { 84 | sac_segmentation_.setMaxIterations (max_iterations_); 85 | ROS_INFO_NAMED(getName(), "[%s] Using max_iterations=%d", getName().c_str(), max_iterations_); 86 | } 87 | 88 | if (filters::FilterBase::getParam(std::string("probability"), probability_)) 89 | { 90 | sac_segmentation_.setProbability (probability_); 91 | ROS_INFO_NAMED(getName(), "[%s] Using probability=%f", getName().c_str(), probability_); 92 | } 93 | 94 | filters::FilterBase::getParam(std::string("axis_x"), axis_x_); 95 | filters::FilterBase::getParam(std::string("axis_y"), axis_y_); 96 | filters::FilterBase::getParam(std::string("axis_z"), axis_z_); 97 | sac_segmentation_.setAxis(Eigen::Vector3f(axis_x_, axis_y_, axis_z_)); 98 | ROS_INFO_NAMED(getName(), "[%s] Using axis=[%f, %f, %f]", getName().c_str(), axis_x_, axis_y_, axis_z_); 99 | 100 | bool using_radius = false; 101 | if (filters::FilterBase::getParam(std::string("min_radius"), min_radius_)) { 102 | using_radius = true; 103 | } 104 | if (filters::FilterBase::getParam(std::string("max_radius"), max_radius_)) { 105 | using_radius = true; 106 | } 107 | if (using_radius) { 108 | sac_segmentation_.setRadiusLimits(min_radius_, max_radius_); 109 | ROS_INFO_NAMED(getName(), "[%s] Using radius limits=[%f, %f]", getName().c_str(), min_radius_, max_radius_); 110 | } 111 | 112 | filters::FilterBase::getParam(std::string("eps_angle"), eps_angle_); 113 | sac_segmentation_.setEpsAngle(eps_angle_); 114 | ROS_INFO_NAMED(getName(), "[%s] Using eps_angle=%f", getName().c_str(), eps_angle_); 115 | 116 | filters::FilterBase::getParam(std::string("distance_threshold"), distance_threshold_); 117 | sac_segmentation_.setDistanceThreshold (distance_threshold_); 118 | ROS_INFO_NAMED(getName(), "[%s] Using distance_threshold=%f", getName().c_str(), distance_threshold_); 119 | 120 | filters::FilterBase::getParam(std::string("negative"), negative_); 121 | extract_indices_.setNegative (negative_); 122 | ROS_INFO_NAMED(getName(), "[%s] Using negative='%d'", getName().c_str(), negative_); 123 | 124 | filters::FilterBase::getParam(std::string("model_type"), model_type_); 125 | sac_segmentation_.setModelType (model_type_); 126 | ROS_INFO_NAMED(getName(), "[%s] Using model_type='%d'", getName().c_str(), model_type_); 127 | 128 | filters::FilterBase::getParam(std::string("method_type"), method_type_); 129 | sac_segmentation_.setMethodType (method_type_); 130 | ROS_INFO_NAMED(getName(), "[%s] Using method_type='%d'", getName().c_str(), method_type_); 131 | 132 | 133 | 134 | //dynamic reconfigure 135 | dynamic_reconfigure_srv_ = std::make_unique>( 136 | dynamic_reconfigure_mutex_, 137 | ros::NodeHandle( dynamic_reconfigure_namespace_root_ + "/" + getName())); 138 | 139 | dynamic_reconfigure_clbk_ = boost::bind(&SacSegmentationExtractFilterPointCloud2::dynamicReconfigureClbk, this, _1, _2); 140 | 141 | point_cloud2_filters::SacSegmentationExtractPointCloud2Config initial_config; 142 | initial_config.optimize_coefficents = optimize_coefficents_; 143 | initial_config.axis_x = axis_x_; 144 | initial_config.axis_y = axis_y_; 145 | initial_config.axis_z = axis_z_; 146 | initial_config.eps_angle = eps_angle_; 147 | initial_config.distance_threshold = distance_threshold_; 148 | initial_config.negative = negative_; 149 | initial_config.max_iterations = max_iterations_; 150 | initial_config.probability = probability_; 151 | initial_config.min_radius = min_radius_; 152 | initial_config.max_radius = max_radius_; 153 | initial_config.method_type = method_type_; 154 | initial_config.model_type = model_type_; 155 | 156 | dynamic_reconfigure_srv_->setConfigDefault(initial_config); 157 | dynamic_reconfigure_srv_->updateConfig(initial_config); 158 | 159 | //put this after updateConfig! 160 | dynamic_reconfigure_srv_->setCallback(dynamic_reconfigure_clbk_); 161 | 162 | return true; 163 | }; 164 | 165 | 166 | 167 | bool SacSegmentationExtractFilterPointCloud2::execute() 168 | { 169 | 170 | sac_segmentation_.setInputCloud (cloud_out_); 171 | sac_segmentation_.segment (*inliers_, *coefficients_); 172 | 173 | extract_indices_.setInputCloud (cloud_out_); 174 | extract_indices_.setIndices (inliers_); 175 | 176 | extract_indices_.filter (*cloud_out_); 177 | 178 | 179 | return true; 180 | 181 | }; 182 | 183 | void SacSegmentationExtractFilterPointCloud2::dynamicReconfigureClbk (point_cloud2_filters::SacSegmentationExtractPointCloud2Config &config, uint32_t /*level*/) 184 | { 185 | 186 | boost::recursive_mutex::scoped_lock lock(dynamic_reconfigure_mutex_); 187 | bool axis_to_update = false; 188 | bool radius_to_update = false; 189 | 190 | if (axis_x_ != config.axis_x) 191 | { 192 | axis_to_update = true; 193 | axis_x_ = config.axis_x; 194 | ROS_DEBUG_NAMED (getName(), "[%s] Setting axis_x to: %f.", getName().c_str(), axis_x_); 195 | } 196 | 197 | if (axis_y_ != config.axis_y) 198 | { 199 | axis_to_update = true; 200 | axis_y_ = config.axis_y; 201 | ROS_DEBUG_NAMED (getName(), "[%s] Setting axis_y to: %f.", getName().c_str(), axis_y_); 202 | } 203 | 204 | if (axis_z_ != config.axis_z) 205 | { 206 | axis_to_update = true; 207 | axis_z_ = config.axis_z; 208 | ROS_DEBUG_NAMED (getName(), "[%s] Setting axis_z to: %f.", getName().c_str(), axis_z_); 209 | } 210 | 211 | if (eps_angle_ != config.eps_angle) 212 | { 213 | eps_angle_ = config.eps_angle; 214 | sac_segmentation_.setEpsAngle(eps_angle_); 215 | ROS_DEBUG_NAMED (getName(), "[%s] Setting eps_angle to: %f.", getName().c_str(), eps_angle_); 216 | } 217 | if (distance_threshold_ != config.distance_threshold) 218 | { 219 | distance_threshold_ = config.distance_threshold; 220 | sac_segmentation_.setDistanceThreshold (distance_threshold_); 221 | ROS_DEBUG_NAMED (getName(), "[%s] Setting distance_threshold to: %f.", getName().c_str(), distance_threshold_); 222 | } 223 | if (optimize_coefficents_ != config.optimize_coefficents) 224 | { 225 | optimize_coefficents_ = config.optimize_coefficents; 226 | sac_segmentation_.setOptimizeCoefficients (optimize_coefficents_); 227 | ROS_DEBUG_NAMED (getName(), "[%s] Setting optimize_coefficents to: %d.", getName().c_str(), optimize_coefficents_); 228 | } 229 | if (negative_ != config.negative) 230 | { 231 | negative_ = config.negative; 232 | extract_indices_.setNegative (negative_); 233 | ROS_DEBUG_NAMED (getName(), "[%s] Setting negative to: %d.", getName().c_str(), negative_); 234 | } 235 | 236 | if (max_iterations_ != config.max_iterations) 237 | { 238 | max_iterations_ = config.max_iterations; 239 | sac_segmentation_.setMaxIterations (max_iterations_); 240 | ROS_DEBUG_NAMED (getName(), "[%s] Setting max_iterations to: %d.", getName().c_str(), max_iterations_); 241 | } 242 | 243 | if (probability_ != config.probability) 244 | { 245 | probability_ = config.probability; 246 | sac_segmentation_.setProbability (probability_); 247 | ROS_DEBUG_NAMED (getName(), "[%s] Setting probability to: %f.", getName().c_str(), probability_); 248 | } 249 | 250 | if (min_radius_ != config.min_radius) 251 | { 252 | min_radius_ = config.min_radius; 253 | radius_to_update = true; 254 | ROS_DEBUG_NAMED (getName(), "[%s] Setting min_radius to: %f.", getName().c_str(), min_radius_); 255 | } 256 | 257 | if (max_radius_ != config.max_radius) 258 | { 259 | max_radius_ = config.max_radius; 260 | radius_to_update = true; 261 | ROS_DEBUG_NAMED (getName(), "[%s] Setting max_radius to: %f.", getName().c_str(), max_radius_); 262 | } 263 | 264 | if (model_type_ != config.model_type) 265 | { 266 | model_type_ = config.model_type; 267 | sac_segmentation_.setModelType(model_type_); 268 | ROS_DEBUG_NAMED (getName(), "[%s] Setting model_type to: %d.", getName().c_str(), model_type_); 269 | } 270 | 271 | if (method_type_ != config.method_type) 272 | { 273 | method_type_ = config.method_type; 274 | sac_segmentation_.setMethodType(method_type_); 275 | ROS_DEBUG_NAMED (getName(), "[%s] Setting method_type to: %d.", getName().c_str(), method_type_); 276 | } 277 | 278 | if (axis_to_update) { 279 | 280 | sac_segmentation_.setAxis(Eigen::Vector3f(axis_x_, axis_y_, axis_z_)); 281 | } 282 | 283 | if (radius_to_update) { 284 | 285 | sac_segmentation_.setRadiusLimits(min_radius_, max_radius_); 286 | } 287 | 288 | } 289 | 290 | 291 | } //namespace point_cloud2_filters 292 | 293 | 294 | #endif //SAC_SEGMENTATION_EXTRACT_FILTER_POINT_CLOUD_HPP 295 | 296 | --------------------------------------------------------------------------------