From b52e7a7ab1e2b4295253ea88153f770f747af4d0 Mon Sep 17 00:00:00 2001 From: Andrew Symington Date: Tue, 21 Feb 2023 14:07:04 -0800 Subject: [PATCH] Migrate the ROS1 pcl::VoxelGrid filter to ROS2 pcl_ros::VoxelGrid (#398) * Add the VoxelGrid filter (squash commit) * Revert the change that brings in separate leaf sizes --- pcl_ros/CMakeLists.txt | 6 +- .../include/pcl_ros/filters/voxel_grid.hpp | 48 ++-- pcl_ros/src/pcl_ros/filters/filter.cpp | 11 +- pcl_ros/src/pcl_ros/filters/passthrough.cpp | 16 +- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 210 +++++++++++------- pcl_ros/tests/filters/CMakeLists.txt | 12 + 6 files changed, 185 insertions(+), 118 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index a1625094..577d239b 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -88,7 +88,7 @@ add_library(pcl_ros_filters SHARED src/pcl_ros/filters/project_inliers.cpp src/pcl_ros/filters/radius_outlier_removal.cpp src/pcl_ros/filters/statistical_outlier_removal.cpp -# src/pcl_ros/filters/voxel_grid.cpp + src/pcl_ros/filters/voxel_grid.cpp src/pcl_ros/filters/crop_box.cpp ) target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES}) @@ -117,6 +117,10 @@ rclcpp_components_register_node(pcl_ros_filters PLUGIN "pcl_ros::CropBox" EXECUTABLE filter_crop_box_node ) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::VoxelGrid" + EXECUTABLE filter_voxel_grid_node +) class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index 54c7c459..af206262 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/VoxelGridConfig.hpp" - namespace pcl_ros { /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. @@ -53,38 +51,42 @@ namespace pcl_ros class VoxelGrid : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - - /** \brief The PCL filter implementation used. */ - pcl::VoxelGrid impl_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ - virtual void + inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output); + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override + { + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); + } - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void - config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::VoxelGrid impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit VoxelGrid(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 9ea418f3..fb167bb4 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -247,20 +247,11 @@ pcl_ros::Filter::add_common_params() "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max]."; declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc); - rcl_interfaces::msg::ParameterDescriptor keep_organized_desc; - keep_organized_desc.name = "keep_organized"; - keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - keep_organized_desc.description = - "Set whether the filtered points should be kept and set to NaN, " - "or removed from the PointCloud, thus potentially breaking its organized structure."; - declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc); - return std::vector { ffn_desc.name, flmin_desc.name, flmax_desc.name, - flneg_desc.name, - keep_organized_desc.name + flneg_desc.name }; } diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index 461966b5..d7a90e78 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -41,7 +41,20 @@ pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options) : Filter("PassThroughNode", options) { use_frame_params(); - std::vector param_names = add_common_params(); + std::vector common_param_names = add_common_params(); + + rcl_interfaces::msg::ParameterDescriptor keep_organized_desc; + keep_organized_desc.name = "keep_organized"; + keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + keep_organized_desc.description = + "Set whether the filtered points should be kept and set to NaN, " + "or removed from the PointCloud, thus potentially breaking its organized structure."; + declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc); + + std::vector param_names { + keep_organized_desc.name, + }; + param_names.insert(param_names.end(), common_param_names.begin(), common_param_names.end()); callback_handle_ = add_on_set_parameters_callback( @@ -50,6 +63,7 @@ pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options) std::placeholders::_1)); config_callback(get_parameters(param_names)); + // TODO(daisukes): lazy subscription after rclcpp#2060 subscribe(); } diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 39b797a0..18a4f0a8 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -35,100 +35,144 @@ * */ -#include #include "pcl_ros/filters/voxel_grid.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::VoxelGrid::child_init(ros::NodeHandle & nh, bool & has_service) + +pcl_ros::VoxelGrid::VoxelGrid(const rclcpp::NodeOptions & options) +: Filter("VoxelGridNode", options) { - // Enable the dynamic reconfigure service - has_service = true; - srv_ = boost::make_shared>(nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &VoxelGrid::config_callback, this, _1, _2); - srv_->setCallback(f); + std::vector common_param_names = add_common_params(); - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::VoxelGrid::filter( - const PointCloud2::ConstPtr & input, - const IndicesPtr & indices, - PointCloud2 & output) -{ - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level) -{ - boost::mutex::scoped_lock lock(mutex_); - - Eigen::Vector3f leaf_size = impl_.getLeafSize(); - - if (leaf_size[0] != config.leaf_size) { - leaf_size.setConstant(config.leaf_size); - NODELET_DEBUG("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]); - impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]); + rcl_interfaces::msg::ParameterDescriptor leaf_size_desc; + leaf_size_desc.name = "leaf_size"; + leaf_size_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + leaf_size_desc.description = + "The size of a leaf (on x,y,z) used for downsampling"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 1.0; + leaf_size_desc.floating_point_range.push_back(float_range); } + declare_parameter(leaf_size_desc.name, rclcpp::ParameterValue(0.01), leaf_size_desc); + + rcl_interfaces::msg::ParameterDescriptor min_points_per_voxel_desc; + min_points_per_voxel_desc.name = "min_points_per_voxel"; + min_points_per_voxel_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + min_points_per_voxel_desc.description = + "The minimum number of points required for a voxel to be used."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 1; + int_range.to_value = 100000; + min_points_per_voxel_desc.integer_range.push_back(int_range); + } + declare_parameter( + min_points_per_voxel_desc.name, rclcpp::ParameterValue(2), min_points_per_voxel_desc); + + std::vector param_names { + leaf_size_desc.name, + min_points_per_voxel_desc.name, + }; + param_names.insert(param_names.end(), common_param_names.begin(), common_param_names.end()); + + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &VoxelGrid::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::VoxelGrid::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); double filter_min, filter_max; impl_.getFilterLimits(filter_min, filter_max); - if (filter_min != config.filter_limit_min) { - filter_min = config.filter_limit_min; - NODELET_DEBUG( - "[config_callback] Setting the minimum filtering value a point will be considered " - "from to: %f.", - filter_min); - } - if (filter_max != config.filter_limit_max) { - filter_max = config.filter_limit_max; - NODELET_DEBUG( - "[config_callback] Setting the maximum filtering value a point will be considered " - "from to: %f.", - filter_max); - } - impl_.setFilterLimits(filter_min, filter_max); - if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) { - impl_.setFilterLimitsNegative(config.filter_limit_negative); - NODELET_DEBUG( - "[%s::config_callback] Setting the filter negative flag to: %s.", - getName().c_str(), config.filter_limit_negative ? "true" : "false"); + Eigen::Vector3f leaf_size = impl_.getLeafSize(); + + unsigned int minPointsPerVoxel = impl_.getMinimumPointsNumberPerVoxel(); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "filter_field_name") { + // Check the current value for the filter field + if (impl_.getFilterFieldName() != param.as_string()) { + // Set the filter field if different + impl_.setFilterFieldName(param.as_string()); + RCLCPP_DEBUG( + get_logger(), "Setting the filter field name to: %s.", + param.as_string().c_str()); + } + } + if (param.get_name() == "filter_limit_min") { + // Check the current values for filter min-max + if (filter_min != param.as_double()) { + filter_min = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum filtering value a point will be considered from to: %f.", + filter_min); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_max") { + // Check the current values for filter min-max + if (filter_max != param.as_double()) { + filter_max = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the maximum filtering value a point will be considered from to: %f.", + filter_max); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_negative") { + bool new_filter_limits_negative = param.as_bool(); + if (impl_.getFilterLimitsNegative() != new_filter_limits_negative) { + RCLCPP_DEBUG( + get_logger(), + "Setting the filter negative flag to: %s.", + (new_filter_limits_negative ? "true" : "false")); + impl_.setFilterLimitsNegative(new_filter_limits_negative); + } + } + if (param.get_name() == "min_points_per_voxel") { + if (minPointsPerVoxel != ((unsigned int) param.as_int())) { + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum points per voxel to: %u.", + minPointsPerVoxel); + impl_.setMinimumPointsNumberPerVoxel(param.as_int()); + } + } + if (param.get_name() == "leaf_size") { + leaf_size.setConstant(param.as_double()); + if (impl_.getLeafSize() != leaf_size) { + RCLCPP_DEBUG( + get_logger(), "Setting the downsampling leaf size to: %f %f %f.", + leaf_size[0], leaf_size[1], leaf_size[2]); + impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]); + } + } } - if (impl_.getFilterFieldName() != config.filter_field_name) { - impl_.setFilterFieldName(config.filter_field_name); - NODELET_DEBUG( - "[config_callback] Setting the filter field name to: %s.", - config.filter_field_name.c_str()); - } - - // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, - // we'll remove them and inherit from Filter - if (tf_input_frame_ != config.input_frame) { - tf_input_frame_ = config.input_frame; - NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); - } - if (tf_output_frame_ != config.output_frame) { - tf_output_frame_ = config.output_frame; - NODELET_DEBUG( - "[config_callback] Setting the output TF frame to: %s.", - tf_output_frame_.c_str()); - } - // ]--- + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } -typedef pcl_ros::VoxelGrid VoxelGrid; -PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet); +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::VoxelGrid) diff --git a/pcl_ros/tests/filters/CMakeLists.txt b/pcl_ros/tests/filters/CMakeLists.txt index 69794d02..669b94eb 100644 --- a/pcl_ros/tests/filters/CMakeLists.txt +++ b/pcl_ros/tests/filters/CMakeLists.txt @@ -59,6 +59,12 @@ ament_add_pytest_test(test_pcl_ros::CropBox FILTER_PLUGIN=pcl_ros::CropBox APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index ) +ament_add_pytest_test(test_pcl_ros::VoxelGrid + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::VoxelGrid + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) # test executables ament_add_pytest_test(test_filter_extract_indices_node @@ -98,3 +104,9 @@ ament_add_pytest_test(test_filter_crop_box_node FILTER_EXECUTABLE=filter_crop_box_node APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index ) +ament_add_pytest_test(test_filter_voxel_grid_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_voxel_grid_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +)