feat(autonomous_emergency_braking): filter and crop aeb pointcloud (a…

…utowarefoundation#6875) * enable aeb, fix topic problem Signed-off-by: Daniel Sanchez <[email protected]> * eliminate unused var Signed-off-by: Daniel Sanchez <[email protected]> * enable aeb, fix topic problem Signed-off-by: Daniel Sanchez <[email protected]> * eliminate unused var Signed-off-by: Daniel Sanchez <[email protected]> * add clustering method to eliminate small objects/noise Signed-off-by: Daniel Sanchez <[email protected]> * remove comments Signed-off-by: Daniel Sanchez <[email protected]> * Crop points outside of EGO predicted path Signed-off-by: Daniel Sanchez <[email protected]> * remove offset Signed-off-by: Daniel Sanchez <[email protected]> * Update library use Signed-off-by: Daniel Sanchez <[email protected]> * add check for empty cloud Signed-off-by: Daniel Sanchez <[email protected]> * add extra width for pointcloud cropping Signed-off-by: Daniel Sanchez <[email protected]> * Use single PC ptr Signed-off-by: Daniel Sanchez <[email protected]> * remove problematic option Signed-off-by: Daniel Sanchez <[email protected]> * Revert "Use single PC ptr" This reverts commit b5091fc. Signed-off-by: Daniel Sanchez <[email protected]> * refactoring Signed-off-by: Daniel Sanchez <[email protected]> * Add back timestamp Signed-off-by: Daniel Sanchez <[email protected]> * consider all points in clusters Signed-off-by: Daniel Sanchez <[email protected]> * USe object hull to detect collisions Signed-off-by: Daniel Sanchez <[email protected]> * Use only closest object point Signed-off-by: Daniel Sanchez <[email protected]> * remove unused functions Signed-off-by: Daniel Sanchez <[email protected]> * remove debug timer out of code Signed-off-by: Daniel Sanchez <[email protected]> * make it so the clustering uses parameters Signed-off-by: Daniel Sanchez <[email protected]> * solve type problem Signed-off-by: Daniel Sanchez <[email protected]> * update comment Signed-off-by: Daniel Sanchez <[email protected]> * update README Signed-off-by: Daniel Sanchez <[email protected]> * eliminate member var in favor of local pointcloud ptr Signed-off-by: Daniel Sanchez <[email protected]> * remove unused chrono dependency Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
tier4 · May 22, 2024 · 72acb0a · 72acb0a
1 parent 31ecdb4
commit 72acb0a
Show file tree

Hide file tree

Showing 4 changed files with 236 additions and 108 deletions.
diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md
@@ -84,20 +84,28 @@ where $v$ and $\omega$ are current longitudinal velocity and angular velocity re
 
 ### 3. Get target obstacles from the input point cloud
 
-After generating the ego predicted path, we select target obstacles from the input point cloud. This obstacle filtering has two major steps, which are rough filtering and rigorous filtering.
+After generating the ego predicted path, we select target obstacles from the input point cloud. This obstacle filtering has three major steps, which are rough filtering, noise filtering with clustering and rigorous filtering.
 
 #### Rough filtering
 
-In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default 5[m]) away from the predicted path of the ego vehicle and ignore the point cloud (obstacles) that are not within it. The image of the rough filtering is illustrated below.
+In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default is half of the ego vehicle width plus the `path_footprint_extra_margin` parameter) away from the predicted path of the ego vehicle and ignore the point cloud that are not within it. The image of the rough filtering is illustrated below.
 
 ![rough_filtering](./image/obstacle_filtering_1.drawio.svg)
 
+#### Noise filtering with clustering and convex hulls
+
+To prevent the AEB from considering noisy points, euclidean clustering is performed on the filtered point cloud. The points in the point cloud that are not close enough to other points to form a cluster are discarded. The parameters `cluster_tolerance`, `minimum_cluster_size` and `maximum_cluster_size` can be used to tune the clustering and the size of objects to be ignored, for more information about the clustering method used by the AEB module, please check the official documentation on euclidean clustering of the PCL library: <https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html>.
+
+Furthermore, a 2D convex hull is created around each detected cluster, the vertices of each hull represent the most extreme/outside points of the cluster. These vertices are then checked in the next step.
+
 #### Rigorous filtering
 
-After rough filtering, it performs a geometric collision check to determine whether the filtered obstacles actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points.
+After Noise filtering, it performs a geometric collision check to determine whether the filtered obstacles/hull vertices actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. Only the vertices with a possibility of collision are kept.
 
 ![rigorous_filtering](./image/obstacle_filtering_2.drawio.svg)
 
+Finally, the vertex that is closest to the ego vehicle is chosen as the candidate for collision checking: Since rss distance is used to judge if a collision will happen or not, if the closest vertex to the ego is deemed to be safe, the rest of the vertices (and the points in the clusters) will also be safe.
+
 ### 4. Collision check with target obstacles
 
 In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as:

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,9 @@
     publish_debug_pointcloud: false
     use_predicted_trajectory: true
     use_imu_path: true
+    path_footprint_extra_margin: 1.0
+    detection_range_min_height: 0.0
+    detection_range_max_height_margin: 0.0
     voxel_grid_x: 0.05
     voxel_grid_y: 0.05
     voxel_grid_z: 100000.0
@@ -12,6 +15,9 @@
     t_response: 1.0
     a_ego_min: -3.0
     a_obj_min: -1.0
+    cluster_tolerance: 0.1 #[m]
+    minimum_cluster_size: 10
+    maximum_cluster_size: 10000
     imu_prediction_time_horizon: 1.5
     imu_prediction_time_interval: 0.1
     mpc_prediction_time_horizon: 1.5

diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
@@ -18,6 +18,7 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
@@ -64,7 +65,6 @@ using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 using Path = std::vector<geometry_msgs::msg::Pose>;
 using Vector3 = geometry_msgs::msg::Vector3;
-
 struct ObjectData
 {
   rclcpp::Time stamp;
@@ -138,15 +138,19 @@ class AEB : public rclcpp::Node
   // main function
   void onCheckCollision(DiagnosticStatusWrapper & stat);
   bool checkCollision(MarkerArray & debug_markers);
-  bool hasCollision(
-    const double current_v, const Path & ego_path, const std::vector<ObjectData> & objects);
+  bool hasCollision(const double current_v, const ObjectData & closest_object);
+
+  Path generateEgoPath(const double curr_v, const double curr_w);
+  std::optional<Path> generateEgoPath(const Trajectory & predicted_traj);
+  std::vector<Polygon2d> generatePathFootprint(const Path & path, const double extra_width_margin);
 
-  Path generateEgoPath(const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons);
-  std::optional<Path> generateEgoPath(
-    const Trajectory & predicted_traj, std::vector<Polygon2d> & polygons);
-  void createObjectData(
+  void createObjectDataUsingPointCloudClusters(
     const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
-    std::vector<ObjectData> & objects);
+    std::vector<ObjectData> & objects,
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_ptr);
+
+  void cropPointCloudWithEgoFootprintPath(
+    const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects);
 
   void addMarker(
     const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
@@ -175,6 +179,9 @@ class AEB : public rclcpp::Node
   bool publish_debug_pointcloud_;
   bool use_predicted_trajectory_;
   bool use_imu_path_;
+  double path_footprint_extra_margin_;
+  double detection_range_min_height_;
+  double detection_range_max_height_margin_;
   double voxel_grid_x_;
   double voxel_grid_y_;
   double voxel_grid_z_;
@@ -184,6 +191,9 @@ class AEB : public rclcpp::Node
   double t_response_;
   double a_ego_min_;
   double a_obj_min_;
+  double cluster_tolerance_;
+  int minimum_cluster_size_;
+  int maximum_cluster_size_;
   double imu_prediction_time_horizon_;
   double imu_prediction_time_interval_;
   double mpc_prediction_time_horizon_;