feat: add cropboxes filter nodelet

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -62,6 +62,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/concatenate_data/concatenate_pointclouds.cpp
   src/time_synchronizer/time_synchronizer_nodelet.cpp
   src/crop_box_filter/crop_box_filter_nodelet.cpp
+  src/crop_box_filter/crop_boxes_filter_nodelet.cpp
   src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp
   src/downsample_filter/random_downsample_filter_nodelet.cpp
   src/downsample_filter/approximate_downsample_filter_nodelet.cpp
@@ -108,6 +109,10 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::CropBoxFilterComponent"
   EXECUTABLE crop_box_filter_node)
 
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::CropBoxesFilterComponent"
+  EXECUTABLE crop_boxes_filter_node)
+
 # ========== Down Sampler Filter ==========
 # -- Voxel Grid Downsample Filter --
 rclcpp_components_register_node(pointcloud_preprocessor_filter

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_boxes_filter_nodelet.hpp

@@ -0,0 +1,114 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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.
+
+/*
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: cropboxes.cpp
+ *
+ */
+
+#ifndef POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOXES_FILTER_NODELET_HPP_
+#define POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOXES_FILTER_NODELET_HPP_
+
+#include "pointcloud_preprocessor/filter.hpp"
+#include "pointcloud_preprocessor/transform_info.hpp"
+
+#include <geometry_msgs/msg/polygon_stamped.hpp>
+
+#include <pcl/filters/crop_box.h>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace pointcloud_preprocessor
+{
+class CropBoxesFilterComponent : public pointcloud_preprocessor::Filter
+{
+protected:
+  bool load_crop_boxes_parameters();
+
+  virtual void filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
+
+  // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform
+  // to new API
+  virtual void faster_filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
+    const TransformInfo & transform_info);
+
+  void publishCropBoxPolygons();
+
+private:
+  struct CropBoxParam
+  {
+    float min_x;
+    float max_x;
+    float min_y;
+    float max_y;
+    float min_z;
+    float max_z;
+    bool negative{false};
+  };
+
+  std::unordered_map<std::string, CropBoxParam> crop_box_params_;
+
+  std::unordered_map<std::string, rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr>
+    crop_box_polygon_pubs_;
+
+  /** \brief Parameter service callback result : needed to be hold */
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  /** \brief Parameter service callback */
+  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+  explicit CropBoxesFilterComponent(const rclcpp::NodeOptions & options);
+};
+}  // namespace pointcloud_preprocessor
+
+#endif  // POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOXES_FILTER_NODELET_HPP_