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

Migrate the ROS1 pcl_ros::CropBox filter to ROS2 (#401)

Browse Source 
* Add squashed commit for CropBox filter

* Lint
This commit is contained in:
Andrew Symington 2023-02-21 12:51:13 -08:00 committed by GitHub
parent de15639154
commit bb871ac7f0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 207 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
pcl_ros/CMakeLists.txt
View File

@ -89,7 +89,7 @@ add_library(pcl_ros_filters SHARED
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/crop_box.cpp
src/pcl_ros/filters/crop_box.cpp
)
target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES})
ament_target_dependencies(pcl_ros_filters ${dependencies})
@ -113,6 +113,10 @@ rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::StatisticalOutlierRemoval"
EXECUTABLE filter_statistical_outlier_removal_node
)
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::CropBox"
EXECUTABLE filter_crop_box_node
)
class_loader_hide_library_symbols(pcl_ros_filters)
#
### Declare the pcl_ros_segmentation library

31
pcl_ros/include/pcl_ros/filters/crop_box.hpp
View File

@ -41,11 +41,9 @@
// PCL includes
#include <pcl/filters/crop_box.h>
#include <vector>
#include "pcl_ros/filters/filter.hpp"
// Dynamic reconfigure
#include "pcl_ros/CropBoxConfig.hpp"
namespace pcl_ros
{
/** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box.
@ -57,9 +55,6 @@ namespace pcl_ros
class CropBox : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>> srv_; // TODO(xxx)
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
@ -67,10 +62,10 @@ protected:
*/
inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override
{
boost::mutex::scoped_lock lock(mutex_);
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);
@ -80,19 +75,13 @@ protected:
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 service callback.
* \param config the dynamic reconfigure CropBoxConfig object
* \param level the dynamic reconfigure level
*/
void
config_callback(pcl_ros::CropBoxConfig & config, uint32_t level);
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
private:
/** \brief The PCL filter implementation used. */
@ -100,6 +89,8 @@ private:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit CropBox(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros

242
pcl_ros/src/pcl_ros/filters/crop_box.cpp
View File

@ -37,86 +37,202 @@
*/
#include "pcl_ros/filters/crop_box.hpp"
#include <pluginlib/class_list_macros.h>
//////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl_ros::CropBox::child_init(ros::NodeHandle & nh, bool & has_service)
pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options)
: Filter("CropBoxNode", options)
{
// Enable the dynamic reconfigure service
has_service = true;
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>>(nh);
dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>::CallbackType f = boost::bind(
&CropBox::config_callback, this, _1, _2);
srv_->setCallback(f);
// This both declares and initializes the input and output frames
use_frame_params();
return true;
rcl_interfaces::msg::ParameterDescriptor min_x_desc;
min_x_desc.name = "min_x";
min_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
min_x_desc.description =
"Minimum x value below which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
min_x_desc.floating_point_range.push_back(float_range);
}
declare_parameter(min_x_desc.name, rclcpp::ParameterValue(-1.0), min_x_desc);
rcl_interfaces::msg::ParameterDescriptor max_x_desc;
max_x_desc.name = "max_x";
max_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
max_x_desc.description =
"Maximum x value above which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
max_x_desc.floating_point_range.push_back(float_range);
}
declare_parameter(max_x_desc.name, rclcpp::ParameterValue(1.0), max_x_desc);
rcl_interfaces::msg::ParameterDescriptor min_y_desc;
min_y_desc.name = "min_y";
min_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
min_y_desc.description =
"Minimum y value below which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
min_y_desc.floating_point_range.push_back(float_range);
}
declare_parameter(min_y_desc.name, rclcpp::ParameterValue(-1.0), min_y_desc);
rcl_interfaces::msg::ParameterDescriptor max_y_desc;
max_y_desc.name = "max_y";
max_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
max_y_desc.description =
"Maximum y value above which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
max_y_desc.floating_point_range.push_back(float_range);
}
declare_parameter(max_y_desc.name, rclcpp::ParameterValue(1.0), max_y_desc);
rcl_interfaces::msg::ParameterDescriptor min_z_desc;
min_z_desc.name = "min_z";
min_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
min_z_desc.description =
"Minimum z value below which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
min_z_desc.floating_point_range.push_back(float_range);
}
declare_parameter(min_z_desc.name, rclcpp::ParameterValue(-1.0), min_z_desc);
rcl_interfaces::msg::ParameterDescriptor max_z_desc;
max_z_desc.name = "max_z";
max_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
max_z_desc.description =
"Maximum z value above which points will be removed";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = -1000.0;
float_range.to_value = 1000.0;
max_z_desc.floating_point_range.push_back(float_range);
}
declare_parameter(max_z_desc.name, rclcpp::ParameterValue(1.0), max_z_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);
rcl_interfaces::msg::ParameterDescriptor negative_desc;
negative_desc.name = "negative";
negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
negative_desc.description =
"Set whether the inliers should be returned (true) or the outliers (false).";
declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc);
const std::vector<std::string> param_names {
min_x_desc.name,
max_x_desc.name,
min_y_desc.name,
max_y_desc.name,
min_z_desc.name,
max_z_desc.name,
keep_organized_desc.name,
negative_desc.name,
};
callback_handle_ =
add_on_set_parameters_callback(
std::bind(
&CropBox::config_callback, this,
std::placeholders::_1));
config_callback(get_parameters(param_names));
// TODO(daisukes): lazy subscription after rclcpp#2060
subscribe();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::CropBox::config_callback(pcl_ros::CropBoxConfig & config, uint32_t level)
rcl_interfaces::msg::SetParametersResult
pcl_ros::CropBox::config_callback(const std::vector<rclcpp::Parameter> & params)
{
boost::mutex::scoped_lock lock(mutex_);
std::lock_guard<std::mutex> lock(mutex_);
Eigen::Vector4f min_point, max_point;
min_point = impl_.getMin();
max_point = impl_.getMax();
Eigen::Vector4f new_min_point, new_max_point;
new_min_point << config.min_x, config.min_y, config.min_z, 0.0;
new_max_point << config.max_x, config.max_y, config.max_z, 0.0;
for (const rclcpp::Parameter & param : params) {
if (param.get_name() == "min_x") {
min_point(0) = param.as_double();
}
if (param.get_name() == "max_x") {
max_point(0) = param.as_double();
}
if (param.get_name() == "min_y") {
min_point(1) = param.as_double();
}
if (param.get_name() == "max_y") {
max_point(1) = param.as_double();
}
if (param.get_name() == "min_z") {
min_point(2) = param.as_double();
}
if (param.get_name() == "max_z") {
max_point(2) = param.as_double();
}
if (param.get_name() == "negative") {
// Check the current value for the negative flag
if (impl_.getNegative() != param.as_bool()) {
RCLCPP_DEBUG(
get_logger(), "Setting the filter negative flag to: %s.",
param.as_bool() ? "true" : "false");
// Call the virtual method in the child
impl_.setNegative(param.as_bool());
}
}
if (param.get_name() == "keep_organized") {
// Check the current value for keep_organized
if (impl_.getKeepOrganized() != param.as_bool()) {
RCLCPP_DEBUG(
get_logger(), "Setting the filter keep_organized value to: %s.",
param.as_bool() ? "true" : "false");
// Call the virtual method in the child
impl_.setKeepOrganized(param.as_bool());
}
}
}
// Check the current values for minimum point
if (min_point != new_min_point) {
NODELET_DEBUG(
"[%s::config_callback] Setting the minimum point to: %f %f %f.",
getName().c_str(), new_min_point(0), new_min_point(1), new_min_point(2));
// Set the filter min point if different
impl_.setMin(new_min_point);
if (min_point != impl_.getMin()) {
RCLCPP_DEBUG(
get_logger(), "Setting the minimum point to: %f %f %f.",
min_point(0), min_point(1), min_point(2));
impl_.setMin(min_point);
}
// Check the current values for the maximum point
if (max_point != new_max_point) {
NODELET_DEBUG(
"[%s::config_callback] Setting the maximum point to: %f %f %f.",
getName().c_str(), new_max_point(0), new_max_point(1), new_max_point(2));
// Set the filter max point if different
impl_.setMax(new_max_point);
if (max_point != impl_.getMax()) {
RCLCPP_DEBUG(
get_logger(), "Setting the maximum point to: %f %f %f.",
max_point(0), max_point(1), max_point(2));
impl_.setMax(max_point);
}
// Check the current value for keep_organized
if (impl_.getKeepOrganized() != config.keep_organized) {
NODELET_DEBUG(
"[%s::config_callback] Setting the filter keep_organized value to: %s.",
getName().c_str(), config.keep_organized ? "true" : "false");
// Call the virtual method in the child
impl_.setKeepOrganized(config.keep_organized);
}
// Check the current value for the negative flag
if (impl_.getNegative() != config.negative) {
NODELET_DEBUG(
"[%s::config_callback] Setting the filter negative flag to: %s.",
getName().c_str(), config.negative ? "true" : "false");
// Call the virtual method in the child
impl_.setNegative(config.negative);
}
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters
// as they are inexistent in PCL
if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame;
NODELET_DEBUG(
"[%s::config_callback] Setting the input TF frame to: %s.",
getName().c_str(), tf_input_frame_.c_str());
}
if (tf_output_frame_ != config.output_frame) {
tf_output_frame_ = config.output_frame;
NODELET_DEBUG(
"[%s::config_callback] Setting the output TF frame to: %s.",
getName().c_str(), 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::CropBox CropBox;
PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet);
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::CropBox)

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

@ -53,6 +53,12 @@ ament_add_pytest_test(test_pcl_ros::StatisticalOutlierRemoval
FILTER_PLUGIN=pcl_ros::StatisticalOutlierRemoval
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
)
ament_add_pytest_test(test_pcl_ros::CropBox
test_filter_component.py
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
FILTER_PLUGIN=pcl_ros::CropBox
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
)
# test executables
ament_add_pytest_test(test_filter_extract_indices_node
@ -84,5 +90,11 @@ ament_add_pytest_test(test_filter_statistical_outlier_removal_node
test_filter_executable.py
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
FILTER_EXECUTABLE=filter_statistical_outlier_removal_node
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
)
ament_add_pytest_test(test_filter_crop_box_node
test_filter_executable.py
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
FILTER_EXECUTABLE=filter_crop_box_node
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
)