add component

Signed-off-by: Masaya Kataoka <[email protected]>

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)

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 <pcl/filters/passthrough.h>
+
+#include <memory>
+#include <pcl_apps/adapter.hpp>
+#include <pcl_apps/visibility_control.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+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_

pcl_apps/src/filter/intensity_filter/intensity_filter_component.cpp

@@ -1,32 +1,44 @@
-#include <iostream>
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/filters/passthrough.h>
-
-using namespace pcl;
-using namespace std;
-
-int main() {
-    PointCloud<PointXYZI>::Ptr cloud(new PointCloud<PointXYZI>());
-    PointCloud<PointXYZI>::Ptr cloud_filtered(new PointCloud<PointXYZI>());
-
-    if (io::loadPCDFile<PointXYZI>("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<PointXYZI> 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;
-}
+// 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 <pcl_apps/filter/intensity_filter/intensity_filter_component.hpp>
+
+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<PointCloudAdapterType>("~/points_filtered", 1);
+  sub_ = create_subscription<PointCloudAdapterType>(
+    "~/points", 1, [this](const PCLPointCloudTypePtr & msg) { pointsCallback(msg); });
+}
+
+void IntensityFilterComponent::pointsCallback(const PCLPointCloudTypePtr & msg)
+{
+  pcl::PassThrough<PCLPointType> 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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_apps::IntensityFilterComponent)

pcl_apps/src/filter/intensity_filter/intensity_filter_node.cpp

@@ -13,17 +13,18 @@
 // limitations under the License.
 
 // Headers in this package
-#include <pcl_apps/filter/intensity_filter/intensity_filter_component.cpp>
+#include <pcl_apps/filter/intensity_filter/intensity_filter_component.hpp>
 // Headers in RCLCPP
 #include <rclcpp/rclcpp.hpp>
 // Headers in STL
 #include <memory>
 
-int main(int argc, char * argv[]){
-    rclcpp::init(argc, argv);
-    rclcpp::NodeOptions options;
-    auto component = std::make_shared<pcl_apps::IntensityFilterComponent>(options);
-    rclcpp::spin(component);
-    rclcpp::shutdown();
-    return 0;
-}
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions options;
+  auto component = std::make_shared<pcl_apps::IntensityFilterComponent>(options);
+  rclcpp::spin(component);
+  rclcpp::shutdown();
+  return 0;
+}