feat(pointcloud_preprocessor): add crop_boxes_filter_node to handle multiple cropbox inputs

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/docs/crop-box-filter.md

@@ -4,9 +4,15 @@
 
 The `crop_box_filter` is a node that removes points with in a given box region. This filter is used to remove the points that hit the vehicle itself.
 
+There are two nodes belongs to this package.
+
+- `crop_box_filter_node`: This node is a ROS2 node that subscribes to a point cloud topic and publishes the filtered point cloud topic.
+- `crop_boxes_filter_node`: This node has same functionality as `crop_box_filter_node`, but it can handle multiple crop boxes.
+
 ## Inner-workings / Algorithms
 
-`pcl::CropBox` is used, which filters all points inside a given box.
+Crop box filter is a simple filter that removes points inside a given box.
+Remember cropbox coordinate is defined in the `input_frame` frame.
 
 ## Inputs / Outputs
 
@@ -20,6 +26,8 @@
 
 ### Core Parameters
 
+For `crop_box_filter_node` and `crop_boxes_filter_node`, the following parameters are used to describe each cropbox.
+
 | Name    | Type   | Default Value | Description                               |
 | ------- | ------ | ------------- | ----------------------------------------- |
 | `min_x` | double | -1.0          | x-coordinate minimum value for crop range |
@@ -29,6 +37,34 @@
 | `min_z` | double | -1.0          | z-coordinate minimum value for crop range |
 | `max_z` | double | 1.0           | z-coordinate maximum value for crop range |
 
+For multiple crop boxes in `crop_boxes_filter_node`, the following parameters are used to describe each cropbox.
+
+| Name               | Type     | Default Value | Description                                                           |
+| ------------------ | -------- | ------------- | --------------------------------------------------------------------- |
+| `crop_boxes_names` | string[] | []            | Names of crop boxes                                                   |
+| `crop_boxes`       | map      | {}            | Map of crop boxes. Each crop box is described by the above parameters |
+
+Example of `crop_boxes` parameter is:
+
+```yaml
+crop_boxes_names: ["right", "left"]
+crop_boxes:
+  right:
+    min_longitudinal_offset: 4.8
+    max_longitudinal_offset: 5.3
+    min_lateral_offset: -1.3
+    max_lateral_offset: -1.05
+    min_height_offset: 1.85
+    max_height_offset: 2.59
+  left:
+    min_longitudinal_offset: 4.83
+    max_longitudinal_offset: 6.0
+    min_lateral_offset: 0.95
+    max_lateral_offset: 1.28
+    min_height_offset: 1.95
+    max_height_offset: 2.6
+```
+
 ## Assumptions / Known limits
 
 ## (Optional) Error detection and handling

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_