Skip to content

Commit 4989e3c

Browse files
committed
feat(AEB): add height filter for avoiding the ghost brake by false positive point clouds
Signed-off-by: Shin-kyoto <aquashin0202@gmail.com>
1 parent 52b2fd6 commit 4989e3c

File tree

3 files changed

+13
-1
lines changed

3 files changed

+13
-1
lines changed

control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
publish_debug_pointcloud: false
44
use_predicted_trajectory: true
55
use_imu_path: true
6+
detection_range_min_height: 0.3
67
voxel_grid_x: 0.05
78
voxel_grid_y: 0.05
89
voxel_grid_z: 100000.0

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -175,6 +175,7 @@ class AEB : public rclcpp::Node
175175
bool publish_debug_pointcloud_;
176176
bool use_predicted_trajectory_;
177177
bool use_imu_path_;
178+
double detection_range_min_height_;
178179
double voxel_grid_x_;
179180
double voxel_grid_y_;
180181
double voxel_grid_z_;

control/autonomous_emergency_braking/src/node.cpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121

2222
#include <boost/geometry/strategies/agnostic/hull_graham_andrew.hpp>
2323

24+
#include <pcl/filters/passthrough.h>
2425
#include <pcl/filters/voxel_grid.h>
2526
#include <tf2/utils.h>
2627

@@ -128,6 +129,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
128129
publish_debug_pointcloud_ = declare_parameter<bool>("publish_debug_pointcloud");
129130
use_predicted_trajectory_ = declare_parameter<bool>("use_predicted_trajectory");
130131
use_imu_path_ = declare_parameter<bool>("use_imu_path");
132+
detection_range_min_height_ = declare_parameter<double>("detection_range_min_height");
131133
voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
132134
voxel_grid_y_ = declare_parameter<double>("voxel_grid_y");
133135
voxel_grid_z_ = declare_parameter<double>("voxel_grid_z");
@@ -221,9 +223,17 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
221223
pcl::fromROSMsg(transformed_points, *pointcloud_ptr);
222224
}
223225

226+
// apply z-axis filter for removing False Positive points
227+
PointCloud::Ptr height_filtered_pointcloud_ptr(new PointCloud);
228+
pcl::PassThrough<pcl::PointXYZ> height_filter;
229+
height_filter.setInputCloud(pointcloud_ptr);
230+
height_filter.setFilterFieldName("z");
231+
height_filter.setFilterLimits(detection_range_min_height_, 5.0);
232+
height_filter.filter(*height_filtered_pointcloud_ptr);
233+
224234
pcl::VoxelGrid<pcl::PointXYZ> filter;
225235
PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud);
226-
filter.setInputCloud(pointcloud_ptr);
236+
filter.setInputCloud(height_filtered_pointcloud_ptr);
227237
filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_);
228238
filter.filter(*no_height_filtered_pointcloud_ptr);
229239

0 commit comments

Comments
 (0)