apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Migrate the ROS1 pcl::VoxelGrid filter to ROS2 pcl_ros::VoxelGrid (#398)

Browse Source 
* Add the VoxelGrid filter (squash commit)

* Revert the change that brings in separate leaf sizes
This commit is contained in:
Andrew Symington 2023-02-21 14:07:04 -08:00 committed by GitHub
parent bb871ac7f0
commit b52e7a7ab1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 185 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
pcl_ros/CMakeLists.txt
View File

@ -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

48
pcl_ros/include/pcl_ros/filters/voxel_grid.hpp
View File

@ -40,11 +40,9 @@
// PCL includes
#include <pcl/filters/voxel_grid.h>
#include <vector>
#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<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>> srv_;
/** \brief The PCL filter implementation used. */
pcl::VoxelGrid<pcl::PCLPointCloud2> 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<std::mutex> 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<rclcpp::Parameter> & 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<pcl::PCLPointCloud2> impl_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit VoxelGrid(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros

11
pcl_ros/src/pcl_ros/filters/filter.cpp
View File

@ -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<std::string> {
ffn_desc.name,
flmin_desc.name,
flmax_desc.name,
flneg_desc.name,
keep_organized_desc.name
flneg_desc.name
};
}

16
pcl_ros/src/pcl_ros/filters/passthrough.cpp
View File

@ -41,7 +41,20 @@ pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options)
: Filter("PassThroughNode", options)
{
use_frame_params();
std::vector<std::string> param_names = add_common_params();
std::vector<std::string> 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<std::string> 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();
}

210
pcl_ros/src/pcl_ros/filters/voxel_grid.cpp
View File

@ -35,100 +35,144 @@
*
*/
#include <pluginlib/class_list_macros.h>
#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<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>::CallbackType f = boost::bind(
&VoxelGrid::config_callback, this, _1, _2);
srv_->setCallback(f);
std::vector<std::string> 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<std::string> 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<rclcpp::Parameter> & params)
{
std::lock_guard<std::mutex> 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)

12
pcl_ros/tests/filters/CMakeLists.txt
View File

@ -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
)