Merge pull request #81 from OUXT-Polaris/feature/matsuzaki

[add] intensity_filter

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

@@ -0,0 +1,44 @@
+// 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

@@ -0,0 +1,30 @@
+// 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.
+
+// Headers in this package
+#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;
+}