From af4c5a6af261262840b647e1d22a63e5e92e17d6 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Sat, 29 Jun 2024 11:26:49 +0900 Subject: [PATCH] add component Signed-off-by: Masaya Kataoka --- pcl_apps/CMakeLists.txt | 2 + .../intensity_filter_component.hpp | 43 +++++++++++ .../intensity_filter_component.cpp | 76 +++++++++++-------- .../intensity_filter_node.cpp | 19 ++--- 4 files changed, 99 insertions(+), 41 deletions(-) create mode 100644 pcl_apps/include/pcl_apps/filter/intensity_filter/intensity_filter_component.hpp diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index 4ebe024..c2c5392 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -81,6 +81,8 @@ add_pcl_apps_component(filter pointcloud_to_laserscan) rclcpp_components_register_nodes(pointcloud_to_laserscan_component "pcl_apps::PointCloudToLaserScanComponent") add_pcl_apps_component(filter voxelgrid_filter) rclcpp_components_register_nodes(voxelgrid_filter_component "pcl_apps::VoxelgridFilterComponent") +add_pcl_apps_component(filter intensity_filter) +rclcpp_components_register_nodes(intensity_filter_component "pcl_apps::IntensityFilterComponent") # Matching Modules add_pcl_apps_component(matching ndt_matching) diff --git a/pcl_apps/include/pcl_apps/filter/intensity_filter/intensity_filter_component.hpp b/pcl_apps/include/pcl_apps/filter/intensity_filter/intensity_filter_component.hpp new file mode 100644 index 0000000..a29f1ec --- /dev/null +++ b/pcl_apps/include/pcl_apps/filter/intensity_filter/intensity_filter_component.hpp @@ -0,0 +1,43 @@ +// Copyright (c) 2019 OUXT Polaris +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PCL_APPS__FILTER__INTENSITY_FILTER__INTENSITY_FILTER_COMPONENT_HPP_ +#define PCL_APPS__FILTER__INTENSITY_FILTER__INTENSITY_FILTER_COMPONENT_HPP_ + +#include + +#include +#include +#include +#include + +namespace pcl_apps +{ +class IntensityFilterComponent : public rclcpp::Node +{ +public: + PCL_APPS_PUBLIC + explicit IntensityFilterComponent(const rclcpp::NodeOptions & options); + ~IntensityFilterComponent(){}; + void pointsCallback(const PCLPointCloudTypePtr & msg); + +private: + PointCloudPublisher pub_; + PointCloudSubscriber sub_; + float min_intensity_; + float max_intensity_; +}; +} // namespace pcl_apps + +#endif // PCL_APPS__FILTER__INTENSITY_FILTER__INTENSITY_FILTER_COMPONENT_HPP_ diff --git a/pcl_apps/src/filter/intensity_filter/intensity_filter_component.cpp b/pcl_apps/src/filter/intensity_filter/intensity_filter_component.cpp index e28367e..6ab9e8e 100644 --- a/pcl_apps/src/filter/intensity_filter/intensity_filter_component.cpp +++ b/pcl_apps/src/filter/intensity_filter/intensity_filter_component.cpp @@ -1,32 +1,44 @@ -#include -#include -#include -#include -#include - -using namespace pcl; -using namespace std; - -int main() { - PointCloud::Ptr cloud(new PointCloud()); - PointCloud::Ptr cloud_filtered(new PointCloud()); - - if (io::loadPCDFile("input.pcd", *cloud) == -1) { - PCL_ERROR("Couldn't read file input.pcd\n"); - return -1; - } - - cout << "Loaded " << cloud->points.size() << "data points from input.pcd with the following fields: x y z intensity\n"; - - PassThrough pass; - pass.setInputCloud(cloud); - pass.setFilterFieldName("intensity"); - pass.setFilterLimits(0.0, 1.0); - pass.filter(*cloud_filtered); - - cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << endl; - - io::savePCDFileASCII("output.pcd", *cloud_filtered); - - return 0; -} \ No newline at end of file +// Copyright (c) 2019 OUXT Polaris +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace pcl_apps +{ +IntensityFilterComponent::IntensityFilterComponent(const rclcpp::NodeOptions & options) +: Node("intensity_filter_node", options) +{ + declare_parameter("min_intensity", 0.0); + get_parameter("min_intensity", min_intensity_); + declare_parameter("max_intensity", 1.0); + get_parameter("max_intensity", max_intensity_); + pub_ = create_publisher("~/points_filtered", 1); + sub_ = create_subscription( + "~/points", 1, [this](const PCLPointCloudTypePtr & msg) { pointsCallback(msg); }); +} + +void IntensityFilterComponent::pointsCallback(const PCLPointCloudTypePtr & msg) +{ + pcl::PassThrough pass; + pass.setInputCloud(msg); + pass.setFilterFieldName("intensity"); + pass.setFilterLimits(min_intensity_, max_intensity_); + PCLPointCloudTypePtr filtered_cloud(new PCLPointCloudType()); + pass.filter(*filtered_cloud); + pub_->publish(filtered_cloud); +} +} // namespace pcl_apps + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_apps::IntensityFilterComponent) diff --git a/pcl_apps/src/filter/intensity_filter/intensity_filter_node.cpp b/pcl_apps/src/filter/intensity_filter/intensity_filter_node.cpp index 2006c26..ae83d4c 100644 --- a/pcl_apps/src/filter/intensity_filter/intensity_filter_node.cpp +++ b/pcl_apps/src/filter/intensity_filter/intensity_filter_node.cpp @@ -13,17 +13,18 @@ // limitations under the License. // Headers in this package -#include +#include // Headers in RCLCPP #include // Headers in STL #include -int main(int argc, char * argv[]){ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +}