diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index e9c01ad8..a1625094 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -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 diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/pcl_ros/include/pcl_ros/filters/crop_box.hpp index 3665521a..159d6e78 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.hpp +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -41,11 +41,9 @@ // PCL includes #include +#include #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> 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 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 & 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 diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp index bcba2a98..93fcf0b6 100644 --- a/pcl_ros/src/pcl_ros/filters/crop_box.cpp +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -37,86 +37,202 @@ */ #include "pcl_ros/filters/crop_box.hpp" -#include -////////////////////////////////////////////////////////////////////////////////////////////// -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>(nh); - dynamic_reconfigure::Server::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 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 & params) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard 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) diff --git a/pcl_ros/tests/filters/CMakeLists.txt b/pcl_ros/tests/filters/CMakeLists.txt index 14f37c6a..69794d02 100644 --- a/pcl_ros/tests/filters/CMakeLists.txt +++ b/pcl_ros/tests/filters/CMakeLists.txt @@ -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 )