feat(AEB): add height filter for avoiding the ghost brake by false po…

…sitive point clouds Signed-off-by: Shin-kyoto <aquashin0202@gmail.com>
Shin-kyoto · Mar 15, 2024 · 4989e3c · 4989e3c
1 parent 52b2fd6
commit 4989e3c
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 1 deletion.
diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
@@ -3,6 +3,7 @@
     publish_debug_pointcloud: false
     use_predicted_trajectory: true
     use_imu_path: true
+    detection_range_min_height: 0.3
     voxel_grid_x: 0.05
     voxel_grid_y: 0.05
     voxel_grid_z: 100000.0

diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
@@ -175,6 +175,7 @@ class AEB : public rclcpp::Node
   bool publish_debug_pointcloud_;
   bool use_predicted_trajectory_;
   bool use_imu_path_;
+  double detection_range_min_height_;
   double voxel_grid_x_;
   double voxel_grid_y_;
   double voxel_grid_z_;

diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp
@@ -21,6 +21,7 @@
 
 #include <boost/geometry/strategies/agnostic/hull_graham_andrew.hpp>
 
+#include <pcl/filters/passthrough.h>
 #include <pcl/filters/voxel_grid.h>
 #include <tf2/utils.h>
 
@@ -128,6 +129,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   publish_debug_pointcloud_ = declare_parameter<bool>("publish_debug_pointcloud");
   use_predicted_trajectory_ = declare_parameter<bool>("use_predicted_trajectory");
   use_imu_path_ = declare_parameter<bool>("use_imu_path");
+  detection_range_min_height_ = declare_parameter<double>("detection_range_min_height");
   voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
   voxel_grid_y_ = declare_parameter<double>("voxel_grid_y");
   voxel_grid_z_ = declare_parameter<double>("voxel_grid_z");
@@ -221,9 +223,17 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
     pcl::fromROSMsg(transformed_points, *pointcloud_ptr);
   }
 
+  // apply z-axis filter for removing False Positive points
+  PointCloud::Ptr height_filtered_pointcloud_ptr(new PointCloud);
+  pcl::PassThrough<pcl::PointXYZ> height_filter;
+  height_filter.setInputCloud(pointcloud_ptr);
+  height_filter.setFilterFieldName("z");
+  height_filter.setFilterLimits(detection_range_min_height_, 5.0);
+  height_filter.filter(*height_filtered_pointcloud_ptr);
+
   pcl::VoxelGrid<pcl::PointXYZ> filter;
   PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud);
-  filter.setInputCloud(pointcloud_ptr);
+  filter.setInputCloud(height_filtered_pointcloud_ptr);
   filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_);
   filter.filter(*no_height_filtered_pointcloud_ptr);