feat(sensing): create multi_layered_pointcloud_filter pkg

.github/CODEOWNERS

@@ -12,4 +12,5 @@ src/robot/wheel_stuck_robot_utils/** @Autumn60
 src/sample/robot_info_user/** @Autumn60
 src/sample/simple_int_publisher/** @Autumn60
 src/sample/simple_int_subscriber/** @Autumn60
+src/sensing/pointcloud_filter/** @Autumn60
 README.md @RyodoTanaka

src/sensing/pointcloud_filter/multi_layered_pointcloud_filter/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(multi_layered_pointcloud_filter)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(multi_layered_pointcloud_filter SHARED
+  src/multi_layered_pointcloud_filter.cpp
+)
+
+ament_auto_package()

src/sensing/pointcloud_filter/multi_layered_pointcloud_filter/include/multi_layered_pointcloud_filter/multi_layered_pointcloud_filter.hpp

@@ -0,0 +1,41 @@
+// Copyright 2024 Fool Stuck Engineers
+//
+// 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 MULTI_LAYERED_POINTCLOUD_FILTER__MULTI_LAYERED_POINTCLOUD_FILTER_HPP_
+#define MULTI_LAYERED_POINTCLOUD_FILTER__MULTI_LAYERED_POINTCLOUD_FILTER_HPP_
+
+#include "multi_layered_pointcloud_filter/pointcloud_filter_base.hpp"
+
+#include <memory>
+#include <vector>
+
+namespace multi_layered_pointcloud_filter
+{
+
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+
+class MultiLayeredPointCloudFilter
+{
+public:
+  using SharedPtr = std::shared_ptr<MultiLayeredPointCloudFilter>;
+  void add_filter_layer(PointCloudFilterBase::SharedPtr filter_layer);
+  bool is_ready();
+  bool filter(const PointCloud2::SharedPtr input_points, PointCloud2::SharedPtr output_points);
+
+private:
+  std::vector<PointCloudFilterBase::SharedPtr> filter_layers_;
+};
+}  // namespace multi_layered_pointcloud_filter
+
+#endif  // MULTI_LAYERED_POINTCLOUD_FILTER__MULTI_LAYERED_POINTCLOUD_FILTER_HPP_

src/sensing/pointcloud_filter/multi_layered_pointcloud_filter/include/multi_layered_pointcloud_filter/pointcloud_filter_base.hpp

@@ -0,0 +1,36 @@
+// Copyright 2024 Fool Stuck Engineers
+//
+// 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 MULTI_LAYERED_POINTCLOUD_FILTER__POINTCLOUD_FILTER_BASE_HPP_
+#define MULTI_LAYERED_POINTCLOUD_FILTER__POINTCLOUD_FILTER_BASE_HPP_
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <memory>
+
+namespace multi_layered_pointcloud_filter
+{
+
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+
+class PointCloudFilterBase
+{
+public:
+  using SharedPtr = std::shared_ptr<PointCloudFilterBase>;
+  virtual bool is_ready() = 0;
+  virtual bool filter(PointCloud2::SharedPtr points) = 0;
+};
+}  // namespace multi_layered_pointcloud_filter
+
+#endif  // MULTI_LAYERED_POINTCLOUD_FILTER__POINTCLOUD_FILTER_BASE_HPP_

src/sensing/pointcloud_filter/multi_layered_pointcloud_filter/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>multi_layered_pointcloud_filter</name>
+  <version>0.0.0</version>
+  <description>The multi_layered_pointcloud_filter pkg</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>sensor_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/sensing/pointcloud_filter/multi_layered_pointcloud_filter/src/multi_layered_pointcloud_filter.cpp

@@ -0,0 +1,42 @@
+// Copyright 2024 Fool Stuck Engineers
+//
+// 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 "multi_layered_pointcloud_filter/multi_layered_pointcloud_filter.hpp"
+
+namespace multi_layered_pointcloud_filter
+{
+void MultiLayeredPointCloudFilter::add_filter_layer(PointCloudFilterBase::SharedPtr filter_layer)
+{
+  filter_layers_.push_back(filter_layer);
+}
+
+bool MultiLayeredPointCloudFilter::is_ready()
+{
+  for (auto filter_layer : filter_layers_) {
+    if (!filter_layer->is_ready()) return false;
+  }
+  return true;
+}
+
+bool MultiLayeredPointCloudFilter::filter(
+  const PointCloud2::SharedPtr input_points, PointCloud2::SharedPtr output_points)
+{
+  PointCloud2::SharedPtr current_points = input_points;
+  for (auto filter_layer : filter_layers_) {
+    if (!filter_layer->filter(current_points)) return false;
+  }
+  output_points = current_points;
+  return true;
+}
+}  // namespace multi_layered_pointcloud_filter