feat(autoware_crop_box_filter): porting autoware_crop_box_filter, autoware_crop_box_filter, add readme: v0.1

Signed-off-by: liuXinGangChina <lxg19892021@gmail.com>

Author: liuXinGangChina

sensing/autoware_crop_box_filter/README.md

@@ -0,0 +1,65 @@
+# autoware_crop_box_filter
+
+## Overview
+
+The `autoware_crop_box_filter` is a package that crops the input pointcloud to a specified bounding box. This is useful for reducing the computational load and improving the performance of the system.
+
+## Design
+
+The `autoware_crop_box_filter` is implemented as a autoware core node that subscribes to the input pointcloud, and publishes the filtered pointcloud. The bounding box is specified using the `min_point` and `max_point` parameters.
+
+## Inputs / Outputs
+
+### Input
+
+| Name              | Type                            | Description       |
+| ----------------- | ------------------------------- | ----------------- |
+| `~/input/points`  | `sensor_msgs::msg::PointCloud2` | reference points  |
+
+### Output
+
+| Name              | Type                            | Description     |
+| ----------------- | ------------------------------- | --------------- |
+| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
+| `~/crop_box_polygon` | `geometry_msgs::msg::PolygonStamped` | bounding box polygon |
+
+## Parameters
+
+### Launch file Parameters
+
+| Name               | Type   | Default Value | Description                           |
+| ------------------ | ------ | ------------- | ------------------------------------- |
+| `input_frame`      | string | " "           | the frame id in which filtering is performed                         |
+| `output_frame`     | string | " "           | output frame id of the filtered points                                                |
+| `input_pointcloud_frame`     | string | " "           | frame id of input  pointcloud  |                                              
+| `max_queue_size`   | int    | 5             | max buffer size of input/output topics |
+| `crop_box_filter_param_file` | string | " "           | path to the parameter file for the node |
+
+
+### Node Parameters
+
+| Name               | Type   | Default Value | Description                           |
+| ------------------ | ------ | ------------- | ------------------------------------- |
+| `min_x`            | double | -5.0          | minimum x value of the crop box       |
+| `min_y`            | double | -5.0          | minimum y value of the crop box       |
+| `min_z`            | double | -5.0          | minimum z value of the crop box       |
+| `max_x`            | double | 5.0           | maximum x value of the crop box       |
+| `max_y`            | double | 5.0           | maximum y value of the crop box       |
+| `max_z`            | double | 5.0           | maximum z value of the crop box       |
+| `negative`         | bool   | true          | if true, points inside the box are removed, otherwise points outside the box are removed |
+
+## Usage
+
+### 1.publish static tf from input pointcloud to target frame that is used for filtering
+
+```cpp
+ros2 run tf2_ros static_transform_publisher 2.0 3.2 1.3 0 0 0 1  velodyne_top_base_link  base_link
+```
+
+### 2.launch node
+
+```cpp
+ros2 launch autoware_crop_box_filter crop_box_filter_node.launch.xml
+```
+
+### 3. launch rviz2 and AWSIM

sensing/autoware_crop_box_filter/include/autoware/crop_box_filter/crop_box_filter_node.hpp

@@ -1,5 +1,5 @@
 // Copyright(c) 2025 AutoCore Technology (Nanjing) Co., Ltd. All rights reserved.
-
+//
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
@@ -57,6 +57,7 @@
 
 #include <geometry_msgs/msg/polygon_stamped.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
 
 #include <autoware_utils/ros/debug_publisher.hpp>
 #include <autoware_utils/ros/managed_transform_buffer.hpp>
@@ -65,7 +66,10 @@
 
 #include <autoware/point_types/types.hpp>
 
+#include <memory>
 #include <vector>
+#include <string>
+#include <utility>
 
 namespace autoware::crop_box_filter
 {
@@ -81,7 +85,7 @@ using PointCloudConstPtr = PointCloud::ConstPtr;
 
 private:
 
-  // member variable declearation & definitions *************************************
+  // member variable declaration & definitions *************************************
 
   /** \brief The managed transform buffer. */
   std::unique_ptr<autoware_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
@@ -123,7 +127,7 @@ using PointCloudConstPtr = PointCloud::ConstPtr;
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
 
-  // publiser and subscriber declaration *********************
+  // publisher and subscriber declaration *********************
 
   /** \brief The input PointCloud2 subscriber. */
   rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;

sensing/autoware_crop_box_filter/src/crop_box_filter_node.cpp

@@ -1,5 +1,5 @@
 // Copyright(c) 2025 AutoCore Technology (Nanjing) Co., Ltd. All rights reserved.
-
+//
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
@@ -52,11 +52,10 @@
  */
 
 #include "autoware/crop_box_filter/crop_box_filter_node.hpp"
-
-#include <sensor_msgs/point_cloud2_iterator.hpp>
-
 #include <memory>
 #include <vector>
+#include <string>
+#include <utility>
 
 namespace autoware::crop_box_filter
 {
@@ -191,32 +190,7 @@ CropBoxFilter::CropBoxFilter(const rclcpp::NodeOptions & node_options)
 
 void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
 {
-  // chechk if the pointcloud is valid
-  if (
-    !is_data_layout_compatible_with_point_xyzircaedt(*cloud) &&
-    !is_data_layout_compatible_with_point_xyzirc(*cloud)) 
-  {
-    RCLCPP_ERROR(
-      get_logger(),
-      "The pointcloud layout is not compatible with PointXYZIRCAEDT or PointXYZIRC. Aborting");
-
-    if (is_data_layout_compatible_with_point_xyziradrt(*cloud)) {
-      RCLCPP_ERROR(
-        get_logger(),
-        "The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy "
-        "code/data");
-    }
-
-    if (is_data_layout_compatible_with_point_xyzi(*cloud)) {
-      RCLCPP_ERROR(
-        get_logger(),
-        "The pointcloud layout is compatible with PointXYZI. You may be using legacy "
-        "code/data");
-    }
-
-    return;
-  }
-
+  // check whether the pointcloud is valid
   if (!is_valid(cloud)) {
     RCLCPP_ERROR(this->get_logger(), "[input_pointcloud_callback] Invalid input pointcloud!");
     return;
@@ -249,27 +223,30 @@ void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
   for (size_t global_offset = 0; global_offset + cloud->point_step <= cloud->data.size();
        global_offset += cloud->point_step) 
   {
+    // extract point data from point cloud data buffer
     Eigen::Vector4f point;
-    Eigen::Vector4f point_preprocessed;
+    
     std::memcpy(&point[0], &cloud->data[global_offset + x_offset], sizeof(float));
     std::memcpy(&point[1], &cloud->data[global_offset + y_offset], sizeof(float));
     std::memcpy(&point[2], &cloud->data[global_offset + z_offset], sizeof(float));
     point[3] = 1;
-    point_preprocessed[3] = 1;
 
     if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || !std::isfinite(point[2])) {
       skipped_count++;
       continue;
     }
 
+    // preprocess point for filtering
+    Eigen::Vector4f point_preprocessed = point;
+
     // apply pre-transform if needed
     if (need_preprocess_transform_) {
       point_preprocessed = eigen_transform_preprocess_ * point;
     }
 
-    bool point_is_inside = point[2] > param_.min_z && point[2] < param_.max_z &&
-                           point[1] > param_.min_y && point[1] < param_.max_y &&
-                           point[0] > param_.min_x && point[0] < param_.max_x;
+    bool point_is_inside = point_preprocessed[2] > param_.min_z && point_preprocessed[2] < param_.max_z &&
+                            point_preprocessed[1] > param_.min_y && point_preprocessed[1] < param_.max_y &&
+                            point_preprocessed[0] > param_.min_x && point_preprocessed[0] < param_.max_x;
     if ((!param_.negative && point_is_inside) || (param_.negative && !point_is_inside)) {
 
       // apply post-transform if needed
@@ -627,6 +604,33 @@ bool CropBoxFilter::is_data_layout_compatible_with_point_xyzircaedt(const PointC
 
 bool CropBoxFilter::is_valid( const PointCloud2ConstPtr & cloud)
 {
+  // firstly check the fields of the point cloud
+  if (
+    !is_data_layout_compatible_with_point_xyzircaedt(*cloud) &&
+    !is_data_layout_compatible_with_point_xyzirc(*cloud)) 
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "The pointcloud layout is not compatible with PointXYZIRCAEDT or PointXYZIRC. Aborting");
+
+    if (is_data_layout_compatible_with_point_xyziradrt(*cloud)) {
+      RCLCPP_ERROR(
+        get_logger(),
+        "The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy "
+        "code/data");
+    }
+
+    if (is_data_layout_compatible_with_point_xyzi(*cloud)) {
+      RCLCPP_ERROR(
+        get_logger(),
+        "The pointcloud layout is compatible with PointXYZI. You may be using legacy "
+        "code/data");
+    }
+
+    return false;
+  }
+
+  // secondly, verify the total size of the point cloud
   if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
     RCLCPP_WARN(
       this->get_logger(),