feat(autoware_crop_box_filter): reimplementation in core repo

sensing/autoware_crop_box_filter/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_crop_box_filter)
+
+find_package(autoware_cmake REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED)
+
+autoware_package()
+
+include_directories(
+  include
+  SYSTEM
+  ${PCL_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+ament_auto_add_library(crop_box_filter_node SHARED
+  src/crop_box_filter_node.cpp
+)
+
+rclcpp_components_register_node(crop_box_filter_node
+  PLUGIN "autoware::crop_box_filter::CropBoxFilter"
+  EXECUTABLE crop_box_filter)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_auto_add_gtest(test_crop_box_filter_node
+    test/test_crop_box_filter_node.cpp
+  )
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+  test
+)

sensing/autoware_crop_box_filter/README.md

@@ -0,0 +1,64 @@
+# 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/config/crop_box_filter_node.param.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    min_x: -5.0
+    min_y: -5.0
+    min_z: -5.0
+    max_x: 5.0
+    max_y: 5.0
+    max_z: 5.0
+    negative: true

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

@@ -0,0 +1,151 @@
+// Copyright(c) 2025 AutoCore Technology (Nanjing) Co., Ltd. All rights reserved.
+//
+// Copyright 2025 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.
+
+#ifndef AUTOWARE__CROP_BOX_FILTER__CROP_BOX_FILTER_NODE_HPP_
+#define AUTOWARE__CROP_BOX_FILTER__CROP_BOX_FILTER_NODE_HPP_
+
+#include <autoware/point_types/types.hpp>
+#include <autoware_utils/ros/debug_publisher.hpp>
+#include <autoware_utils/ros/managed_transform_buffer.hpp>
+#include <autoware_utils/ros/published_time_publisher.hpp>
+#include <autoware_utils/system/stop_watch.hpp>
+
+#include <geometry_msgs/msg/polygon_stamped.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
+
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using PointCloudPtr = PointCloud::Ptr;
+using PointCloudConstPtr = PointCloud::ConstPtr;
+
+namespace autoware::crop_box_filter
+{
+
+class CropBoxFilter : public rclcpp::Node
+{
+private:
+  // member variable declaration & definitions *************************************
+
+  /** \brief The managed transform buffer. */
+  std::unique_ptr<autoware_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
+
+  /** \brief The input TF frame the data should be transformed into,
+   * if input.header.frame_id is different. */
+  std::string tf_input_frame_;
+
+  /** \brief The original data input TF frame. */
+  std::string tf_input_orig_frame_;
+
+  /** \brief The output TF frame the data should be transformed into,
+   * if input.header.frame_id is different. */
+  std::string tf_output_frame_;
+
+  /** \brief The maximum queue size (default: 3). */
+  size_t max_queue_size_ = 3;
+
+  /** \brief Internal mutex. */
+  std::mutex mutex_;
+
+  bool need_preprocess_transform_ = false;
+  bool need_postprocess_transform_ = false;
+
+  Eigen::Matrix4f eigen_transform_preprocess_ = Eigen::Matrix4f::Identity(4, 4);
+  Eigen::Matrix4f eigen_transform_postprocess_ = Eigen::Matrix4f::Identity(4, 4);
+
+  struct CropBoxParam
+  {
+    float min_x;
+    float max_x;
+    float min_y;
+    float max_y;
+    float min_z;
+    float max_z;
+    bool negative{false};
+  } param_;
+
+  /** \brief Parameter service callback result : needed to be hold */
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  // publisher and subscriber declaration *********************
+
+  /** \brief The input PointCloud2 subscriber. */
+  rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;
+
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_output_;
+  rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr crop_box_polygon_pub_;
+
+  /** \brief processing time publisher. **/
+  std::unique_ptr<autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<autoware_utils::DebugPublisher> debug_publisher_;
+  std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_;
+
+  // function declaration *************************************
+
+  void publish_crop_box_polygon();
+
+  void pointcloud_callback(const PointCloud2ConstPtr cloud);
+
+  /** \brief Parameter service callback */
+  rcl_interfaces::msg::SetParametersResult param_callback(const std::vector<rclcpp::Parameter> & p);
+
+  /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is
+   * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */
+  bool is_data_layout_compatible_with_point_xyzi(const PointCloud2 & input);
+
+  /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRC. That is
+   * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRC */
+  bool is_data_layout_compatible_with_point_xyzirc(const PointCloud2 & input);
+
+  /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That
+   * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */
+  bool is_data_layout_compatible_with_point_xyziradrt(const PointCloud2 & input);
+
+  /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT.
+   * That is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */
+  bool is_data_layout_compatible_with_point_xyzircaedt(const PointCloud2 & input);
+
+  bool is_valid(const PointCloud2ConstPtr & cloud);
+
+  /** \brief For parameter service callback */
+  template <typename T>
+  bool get_param(const std::vector<rclcpp::Parameter> & p, const std::string & name, T & value)
+  {
+    auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) {
+      return parameter.get_name() == name;
+    });
+    if (it != p.cend()) {
+      value = it->template get_value<T>();
+      return true;
+    }
+    return false;
+  }
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+  explicit CropBoxFilter(const rclcpp::NodeOptions & options);
+  void filter_pointcloud(const PointCloud2ConstPtr & cloud, PointCloud2 & output);
+};
+}  // namespace autoware::crop_box_filter
+
+#endif  // AUTOWARE__CROP_BOX_FILTER__CROP_BOX_FILTER_NODE_HPP_

sensing/autoware_crop_box_filter/launch/crop_box_filter_node.launch.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input_topic_name" default="/sensing/lidar/top/pointcloud_raw_ex"/>
+  <arg name="output_topic_name" default="/sensing/lidar/top/self_cropped/pointcloud_ex"/>
+  <arg name="input_frame" default="base_link"/>
+  <arg name="output_frame" default="base_link"/>
+  <arg name="input_pointcloud_frame" default="velodyne_top_base_link"/>
+  <arg name="crop_box_filter_param_file" default="$(find-pkg-share autoware_crop_box_filter)/config/crop_box_filter_node.param.yaml"/>
+  <node pkg="autoware_crop_box_filter" exec="crop_box_filter" name="crop_box_filter">
+    <param from="$(var crop_box_filter_param_file)"/>
+    <remap from="input" to="$(var input_topic_name)"/>
+    <remap from="output" to="$(var output_topic_name)"/>
+    <param name="input_pointcloud_frame" value="$(var input_pointcloud_frame)"/>
+    <param name="input_frame" value="$(var input_frame)"/>
+    <param name="output_frame" value="$(var output_frame)"/>
+  </node>
+</launch>

sensing/autoware_crop_box_filter/package.xml

@@ -0,0 +1,36 @@
+<?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>autoware_crop_box_filter</name>
+  <version>0.0.0</version>
+  <description>The ROS 2 autoware_crop_box_filter package</description>
+  <maintainer email="abrahammonrroy@yahoo.com">amc-nu</maintainer>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+  <maintainer email="dai.nguyen@tier4.jp">Dai Nguyen</maintainer>
+  <maintainer email="kenzo.lobos@tier4.jp">Kenzo Lobos-Tsunekawa</maintainer>
+  <maintainer email="yihsiang.fang@tier4.jp">Yihsiang Fang</maintainer>
+  <maintainer email="yoshi.ri@tier4.jp">Yoshi Ri</maintainer>
+  <maintainer email="david.wong@tier4.jp">David Wong</maintainer>
+  <maintainer email="melike@leodrive.ai">Melike Tanrikulu</maintainer>
+  <maintainer email="max.schmeller@tier4.jp">Max Schmeller</maintainer>
+  <maintainer email="lxg19892021@gmail.com">Xingang Liu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_point_types</depend>
+  <depend>autoware_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>