feat(aeb): update aeb module and add control evaluator

control/autonomous_emergency_braking/README.md

@@ -86,20 +86,55 @@ 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.
+
+#### Obstacle velocity estimation
+
+Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object speed using the following equations:
+
+$$
+d_{t} = o_{time stamp} - prev_{time stamp}
+$$
+
+$$
+d_{pos} = norm(o_{pos} - prev_{pos})
+$$
+
+$$
+v_{norm} = d_{pos} / d_{t}
+$$
+
+Where $o_{time stamp}$ and $prev_{time stamp}$ are the timestamps of the point clouds used to detect the current closest object and the closest object of the previous point cloud frame, and $o_{pos}$ and $prev_{pos}$ are the positions of those objects, respectively.
+
+Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$:
+
+$$
+v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego}
+$$
+
+where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's speed, which accounts for the movement of points caused by the ego moving and not the object.
+All these equations are performed disregarding the z axis (in 2D).
+
 ### 4. Collision check with target obstacles
 
 In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as:

control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

@@ -1,22 +1,38 @@
 /**:
   ros__parameters:
-    publish_debug_pointcloud: false
+    # Ego path calculation
     use_predicted_trajectory: true
-    use_imu_path: true
+    use_imu_path: false
+    min_generated_path_length: 0.5
+    imu_prediction_time_horizon: 1.5
+    imu_prediction_time_interval: 0.1
+    mpc_prediction_time_horizon: 1.5
+    mpc_prediction_time_interval: 0.1
+
+    # Debug
+    publish_debug_pointcloud: false
+
+    # Point cloud partitioning
     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
-    min_generated_path_length: 0.5
+
+    # Point cloud cropping
     expand_width: 0.1
+    path_footprint_extra_margin: 4.0
+
+    # Point cloud clustering
+    cluster_tolerance: 0.1 #[m]
+    minimum_cluster_size: 10
+    maximum_cluster_size: 10000
+
+    # RSS distance collision check
     longitudinal_offset: 2.0
     t_response: 1.0
     a_ego_min: -3.0
     a_obj_min: -1.0
-    imu_prediction_time_horizon: 1.5
-    imu_prediction_time_interval: 0.1
-    mpc_prediction_time_horizon: 1.5
-    mpc_prediction_time_interval: 0.1
-    collision_keeping_sec: 0.0
+    collision_keeping_sec: 2.0
+    previous_obstacle_keep_time: 1.0
     aeb_hz: 10.0

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -16,8 +16,10 @@
 #define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
+#include <motion_utils/trajectory/trajectory.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>
@@ -39,12 +41,13 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <deque>
+#include <limits>
 #include <memory>
-#include <mutex>
 #include <optional>
 #include <string>
+#include <utility>
 #include <vector>
-
 namespace autoware::motion::control::autonomous_emergency_braking
 {
 
@@ -64,7 +67,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;
@@ -79,30 +81,146 @@ class CollisionDataKeeper
 public:
   explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; }
 
-  void setTimeout(const double timeout_sec) { timeout_sec_ = timeout_sec; }
+  void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time)
+  {
+    collision_keep_time_ = collision_keep_time;
+    previous_obstacle_keep_time_ = previous_obstacle_keep_time;
+  }
+
+  std::pair<double, double> getTimeout()
+  {
+    return {collision_keep_time_, previous_obstacle_keep_time_};
+  }
 
-  bool checkExpired()
+  bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout)
   {
-    if (data_ && (clock_->now() - data_->stamp).seconds() > timeout_sec_) {
-      data_.reset();
+    if (!data.has_value()) return true;
+    const auto now = clock_->now();
+    const auto & prev_obj = data.value();
+    const auto & data_time_stamp = prev_obj.stamp;
+    if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) {
+      data = std::nullopt;
+      return true;
     }
-    return (data_ == nullptr);
+    return false;
+  }
+
+  bool checkCollisionExpired()
+  {
+    return this->checkObjectDataExpired(closest_object_, collision_keep_time_);
+  }
+
+  bool checkPreviousObjectDataExpired()
+  {
+    return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_);
+  }
+
+  ObjectData get() const
+  {
+    return (closest_object_.has_value()) ? closest_object_.value() : ObjectData();
+  }
+
+  ObjectData getPreviousObjectData() const
+  {
+    return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData();
+  }
+
+  void setCollisionData(const ObjectData & data)
+  {
+    closest_object_ = std::make_optional<ObjectData>(data);
+  }
+
+  void setPreviousObjectData(const ObjectData & data)
+  {
+    prev_closest_object_ = std::make_optional<ObjectData>(data);
   }
 
-  void update(const ObjectData & data) { data_.reset(new ObjectData(data)); }
+  void resetVelocityHistory() { obstacle_velocity_history_.clear(); }
+
+  void updateVelocityHistory(
+    const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp)
+  {
+    // remove old msg from deque
+    const auto now = clock_->now();
+    obstacle_velocity_history_.erase(
+      std::remove_if(
+        obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(),
+        [&](const auto & velocity_time_pair) {
+          const auto & vel_time = velocity_time_pair.second;
+          return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_);
+        }),
+      obstacle_velocity_history_.end());
+    obstacle_velocity_history_.emplace_back(
+      std::make_pair(current_object_velocity, current_object_velocity_time_stamp));
+  }
 
-  ObjectData get()
+  std::optional<double> getMedianObstacleVelocity() const
   {
-    if (data_) {
-      return *data_;
-    } else {
-      return ObjectData();
+    if (obstacle_velocity_history_.empty()) return std::nullopt;
+    std::vector<double> raw_velocities;
+    for (const auto & vel_time_pair : obstacle_velocity_history_) {
+      raw_velocities.emplace_back(vel_time_pair.first);
     }
+
+    const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1
+                                                         : (raw_velocities.size()) / 2.0;
+    const size_t med2 = (raw_velocities.size()) / 2.0;
+    std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end());
+    const double vel1 = raw_velocities.at(med1);
+    std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end());
+    const double vel2 = raw_velocities.at(med2);
+    return (vel1 + vel2) / 2.0;
+  }
+
+  std::optional<double> calcObjectSpeedFromHistory(
+    const ObjectData & closest_object, const Path & path, const double current_ego_speed)
+  {
+    if (this->checkPreviousObjectDataExpired()) {
+      this->setPreviousObjectData(closest_object);
+      this->resetVelocityHistory();
+      return std::nullopt;
+    }
+
+    const auto estimated_velocity_opt = std::invoke([&]() -> std::optional<double> {
+      const auto & prev_object = this->getPreviousObjectData();
+      const double p_dt =
+        (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9;
+      if (p_dt < std::numeric_limits<double>::epsilon()) return std::nullopt;
+      const auto & nearest_collision_point = closest_object.position;
+      const auto & prev_collision_point = prev_object.position;
+
+      const double p_dx = nearest_collision_point.x - prev_collision_point.x;
+      const double p_dy = nearest_collision_point.y - prev_collision_point.y;
+      const double p_dist = std::hypot(p_dx, p_dy);
+      const double p_yaw = std::atan2(p_dy, p_dx);
+      const double p_vel = p_dist / p_dt;
+
+      const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point);
+      const auto & nearest_path_pose = path.at(nearest_idx);
+      const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation);
+      const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed;
+
+      // Current RSS distance calculation does not account for negative velocities
+      return (estimated_velocity > 0.0) ? estimated_velocity : 0.0;
+    });
+
+    if (!estimated_velocity_opt.has_value()) {
+      return std::nullopt;
+    }
+
+    const auto & estimated_velocity = estimated_velocity_opt.value();
+    this->setPreviousObjectData(closest_object);
+    this->updateVelocityHistory(estimated_velocity, closest_object.stamp);
+    return this->getMedianObstacleVelocity();
   }
 
 private:
-  std::unique_ptr<ObjectData> data_;
-  double timeout_sec_{0.0};
+  std::optional<ObjectData> prev_closest_object_{std::nullopt};
+  std::optional<ObjectData> closest_object_{std::nullopt};
+  double collision_keep_time_{0.0};
+  double previous_obstacle_keep_time_{0.0};
+
+  std::deque<std::pair<double, rclcpp::Time>> obstacle_velocity_history_;
   rclcpp::Clock::SharedPtr clock_;
 };
 
@@ -132,30 +250,44 @@ class AEB : public rclcpp::Node
   void onTimer();
   void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg);
   void onAutowareState(const AutowareState::ConstSharedPtr input_msg);
+  rcl_interfaces::msg::SetParametersResult onParameter(
+    const std::vector<rclcpp::Parameter> & parameters);
 
   bool isDataReady();
 
   // 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::vector<Polygon2d> & polygons);
-  std::optional<Path> generateEgoPath(
-    const Trajectory & predicted_traj, std::vector<Polygon2d> & polygons);
-  void createObjectData(
+  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);
+
+  void createObjectDataUsingPointCloudClusters(
+    const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
+    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 createObjectDataUsingPointCloudClusters(
     const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
     std::vector<ObjectData> & objects);
+  void cropPointCloudWithEgoFootprintPath(const std::vector<Polygon2d> & ego_polys);
 
   void addMarker(
     const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
-    const std::vector<ObjectData> & objects, const double color_r, const double color_g,
-    const double color_b, const double color_a, const std::string & ns,
-    MarkerArray & debug_markers);
+    const std::vector<ObjectData> & objects, const std::optional<ObjectData> & closest_object,
+    const double color_r, const double color_g, const double color_b, const double color_a,
+    const std::string & ns, MarkerArray & debug_markers);
 
   void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers);
 
+  std::optional<double> calcObjectSpeedFromHistory(
+    const ObjectData & closest_object, const Path & path, const double current_ego_speed);
+
   PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
   VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
   Vector3::SharedPtr angular_velocity_ptr_{nullptr};
@@ -175,6 +307,7 @@ 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_;
@@ -186,11 +319,16 @@ 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_;
   double mpc_prediction_time_interval_;
   CollisionDataKeeper collision_data_keeper_;
+  // Parameter callback
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
 };
 }  // namespace autoware::motion::control::autonomous_emergency_braking