Skip to content

Commit 1b2c7a5

Browse files
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>
1 parent 3a62c18 commit 1b2c7a5

File tree

3 files changed

+76
-8
lines changed

3 files changed

+76
-8
lines changed
+65
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
# autoware_crop_box_filter
2+
3+
## Overview
4+
5+
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.
6+
7+
## Design
8+
9+
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.
10+
11+
## Inputs / Outputs
12+
13+
### Input
14+
15+
| Name | Type | Description |
16+
| ----------------- | ------------------------------- | ----------------- |
17+
| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points |
18+
19+
### Output
20+
21+
| Name | Type | Description |
22+
| ----------------- | ------------------------------- | --------------- |
23+
| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
24+
| `~/crop_box_polygon` | `geometry_msgs::msg::PolygonStamped` | bounding box polygon |
25+
26+
## Parameters
27+
28+
### Launch file Parameters
29+
30+
| Name | Type | Default Value | Description |
31+
| ------------------ | ------ | ------------- | ------------------------------------- |
32+
| `input_frame` | string | " " | the frame id in which filtering is performed |
33+
| `output_frame` | string | " " | output frame id of the filtered points |
34+
| `input_pointcloud_frame` | string | " " | frame id of input pointcloud |
35+
| `max_queue_size` | int | 5 | max buffer size of input/output topics |
36+
| `crop_box_filter_param_file` | string | " " | path to the parameter file for the node |
37+
38+
39+
### Node Parameters
40+
41+
| Name | Type | Default Value | Description |
42+
| ------------------ | ------ | ------------- | ------------------------------------- |
43+
| `min_x` | double | -5.0 | minimum x value of the crop box |
44+
| `min_y` | double | -5.0 | minimum y value of the crop box |
45+
| `min_z` | double | -5.0 | minimum z value of the crop box |
46+
| `max_x` | double | 5.0 | maximum x value of the crop box |
47+
| `max_y` | double | 5.0 | maximum y value of the crop box |
48+
| `max_z` | double | 5.0 | maximum z value of the crop box |
49+
| `negative` | bool | true | if true, points inside the box are removed, otherwise points outside the box are removed |
50+
51+
## Usage
52+
53+
### 1.publish static tf from input pointcloud to target frame that is used for filtering
54+
55+
```cpp
56+
ros2 run tf2_ros static_transform_publisher 2.0 3.2 1.3 0 0 0 1 velodyne_top_base_link base_link
57+
```
58+
59+
### 2.launch node
60+
61+
```cpp
62+
ros2 launch autoware_crop_box_filter crop_box_filter_node.launch.xml
63+
```
64+
65+
### 3. launch rviz2 and AWSIM

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

+7-3
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
// Copyright(c) 2025 AutoCore Technology (Nanjing) Co., Ltd. All rights reserved.
2-
2+
//
33
// Copyright 2020 Tier IV, Inc.
44
//
55
// Licensed under the Apache License, Version 2.0 (the "License");
@@ -57,6 +57,7 @@
5757

5858
#include <geometry_msgs/msg/polygon_stamped.hpp>
5959
#include <sensor_msgs/msg/point_cloud2.hpp>
60+
#include <sensor_msgs/point_cloud2_iterator.hpp>
6061

6162
#include <autoware_utils/ros/debug_publisher.hpp>
6263
#include <autoware_utils/ros/managed_transform_buffer.hpp>
@@ -65,7 +66,10 @@
6566

6667
#include <autoware/point_types/types.hpp>
6768

69+
#include <memory>
6870
#include <vector>
71+
#include <string>
72+
#include <utility>
6973

7074
namespace autoware::crop_box_filter
7175
{
@@ -81,7 +85,7 @@ using PointCloudConstPtr = PointCloud::ConstPtr;
8185

8286
private:
8387

84-
// member variable declearation & definitions *************************************
88+
// member variable declaration & definitions *************************************
8589

8690
/** \brief The managed transform buffer. */
8791
std::unique_ptr<autoware_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
@@ -123,7 +127,7 @@ using PointCloudConstPtr = PointCloud::ConstPtr;
123127
/** \brief Parameter service callback result : needed to be hold */
124128
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
125129

126-
// publiser and subscriber declaration *********************
130+
// publisher and subscriber declaration *********************
127131

128132
/** \brief The input PointCloud2 subscriber. */
129133
rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;

sensing/autoware_crop_box_filter/src/crop_box_filter_node.cpp

+4-5
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
// Copyright(c) 2025 AutoCore Technology (Nanjing) Co., Ltd. All rights reserved.
2-
2+
//
33
// Copyright 2024 TIER IV, Inc.
44
//
55
// Licensed under the Apache License, Version 2.0 (the "License");
@@ -52,11 +52,10 @@
5252
*/
5353

5454
#include "autoware/crop_box_filter/crop_box_filter_node.hpp"
55-
56-
#include <sensor_msgs/point_cloud2_iterator.hpp>
57-
5855
#include <memory>
5956
#include <vector>
57+
#include <string>
58+
#include <utility>
6059

6160
namespace autoware::crop_box_filter
6261
{
@@ -191,7 +190,7 @@ CropBoxFilter::CropBoxFilter(const rclcpp::NodeOptions & node_options)
191190

192191
void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
193192
{
194-
// chechk if the pointcloud is valid
193+
// check whether the pointcloud is valid
195194
if (
196195
!is_data_layout_compatible_with_point_xyzircaedt(*cloud) &&
197196
!is_data_layout_compatible_with_point_xyzirc(*cloud))

0 commit comments

Comments
 (0)