diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md
index 28b7636ba82ac..7a7bf212320e0 100644
--- a/control/autonomous_emergency_braking/README.md
+++ b/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:
diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
index 82d5a4f12e002..68a42383feb1f 100644
--- a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml
+++ b/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
diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
index b8aad1c80dfea..43fb310b17416 100644
--- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
+++ b/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
 
diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp
index 8c830812078b6..d8886672a8ecd 100644
--- a/control/autonomous_emergency_braking/src/node.cpp
+++ b/control/autonomous_emergency_braking/src/node.cpp
@@ -14,17 +14,27 @@
 
 #include "autonomous_emergency_braking/node.hpp"
 
-#include <motion_utils/trajectory/trajectory.hpp>
+#include <pcl_ros/transforms.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
+#include <tier4_autoware_utils/ros/update_param.hpp>
 
+#include <boost/geometry/algorithms/convex_hull.hpp>
+#include <boost/geometry/algorithms/within.hpp>
 #include <boost/geometry/strategies/agnostic/hull_graham_andrew.hpp>
 
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/filters/crop_hull.h>
+#include <pcl/filters/extract_indices.h>
 #include <pcl/filters/passthrough.h>
 #include <pcl/filters/voxel_grid.h>
+#include <pcl/point_types.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/surface/convex_hull.h>
 #include <tf2/utils.h>
-
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -34,8 +44,6 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
-#include <boost/geometry/algorithms/convex_hull.hpp>
-#include <boost/geometry/algorithms/within.hpp>
 namespace autoware::motion::control::autonomous_emergency_braking
 {
 using diagnostic_msgs::msg::DiagnosticStatus;
@@ -98,37 +106,42 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   collision_data_keeper_(this->get_clock())
 {
   // Subscribers
-  sub_point_cloud_ = this->create_subscription<PointCloud2>(
-    "~/input/pointcloud", rclcpp::SensorDataQoS(),
-    std::bind(&AEB::onPointCloud, this, std::placeholders::_1));
+  {
+    sub_point_cloud_ = this->create_subscription<PointCloud2>(
+      "~/input/pointcloud", rclcpp::SensorDataQoS(),
+      std::bind(&AEB::onPointCloud, this, std::placeholders::_1));
 
-  sub_velocity_ = this->create_subscription<VelocityReport>(
-    "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1));
+    sub_velocity_ = this->create_subscription<VelocityReport>(
+      "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1));
 
-  sub_imu_ = this->create_subscription<Imu>(
-    "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1));
+    sub_imu_ = this->create_subscription<Imu>(
+      "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1));
 
-  sub_predicted_traj_ = this->create_subscription<Trajectory>(
-    "~/input/predicted_trajectory", rclcpp::QoS{1},
-    std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1));
+    sub_predicted_traj_ = this->create_subscription<Trajectory>(
+      "~/input/predicted_trajectory", rclcpp::QoS{1},
+      std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1));
 
-  sub_autoware_state_ = this->create_subscription<AutowareState>(
-    "/autoware/state", rclcpp::QoS{1},
-    std::bind(&AEB::onAutowareState, this, std::placeholders::_1));
+    sub_autoware_state_ = this->create_subscription<AutowareState>(
+      "/autoware/state", rclcpp::QoS{1},
+      std::bind(&AEB::onAutowareState, this, std::placeholders::_1));
+  }
 
   // Publisher
-  pub_obstacle_pointcloud_ =
-    this->create_publisher<sensor_msgs::msg::PointCloud2>("~/debug/obstacle_pointcloud", 1);
-  debug_ego_path_publisher_ = this->create_publisher<MarkerArray>("~/debug/markers", 1);
-
+  {
+    pub_obstacle_pointcloud_ =
+      this->create_publisher<sensor_msgs::msg::PointCloud2>("~/debug/obstacle_pointcloud", 1);
+    debug_ego_path_publisher_ = this->create_publisher<MarkerArray>("~/debug/markers", 1);
+  }
   // Diagnostics
-  updater_.setHardwareID("autonomous_emergency_braking");
-  updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision);
-
+  {
+    updater_.setHardwareID("autonomous_emergency_braking");
+    updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision);
+  }
   // parameter
   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");
+  path_footprint_extra_margin_ = declare_parameter<double>("path_footprint_extra_margin");
   detection_range_min_height_ = declare_parameter<double>("detection_range_min_height");
   detection_range_max_height_margin_ =
     declare_parameter<double>("detection_range_max_height_margin");
@@ -141,18 +154,74 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   t_response_ = declare_parameter<double>("t_response");
   a_ego_min_ = declare_parameter<double>("a_ego_min");
   a_obj_min_ = declare_parameter<double>("a_obj_min");
+
+  cluster_tolerance_ = declare_parameter<double>("cluster_tolerance");
+  minimum_cluster_size_ = declare_parameter<int>("minimum_cluster_size");
+  maximum_cluster_size_ = declare_parameter<int>("maximum_cluster_size");
+
   imu_prediction_time_horizon_ = declare_parameter<double>("imu_prediction_time_horizon");
   imu_prediction_time_interval_ = declare_parameter<double>("imu_prediction_time_interval");
   mpc_prediction_time_horizon_ = declare_parameter<double>("mpc_prediction_time_horizon");
   mpc_prediction_time_interval_ = declare_parameter<double>("mpc_prediction_time_interval");
 
-  const auto collision_keeping_sec = declare_parameter<double>("collision_keeping_sec");
-  collision_data_keeper_.setTimeout(collision_keeping_sec);
+  {  // Object history data keeper setup
+    const auto previous_obstacle_keep_time =
+      declare_parameter<double>("previous_obstacle_keep_time");
+    const auto collision_keeping_sec = declare_parameter<double>("collision_keeping_sec");
+    collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time);
+  }
+
+  // Parameter Callback
+  set_param_res_ =
+    add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1));
 
   // start time
   const double aeb_hz = declare_parameter<double>("aeb_hz");
   const auto period_ns = rclcpp::Rate(aeb_hz).period();
-  timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&AEB::onTimer, this));
+  timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this));
+}
+
+rcl_interfaces::msg::SetParametersResult AEB::onParameter(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  using tier4_autoware_utils::updateParam;
+  updateParam<bool>(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_);
+  updateParam<bool>(parameters, "use_predicted_trajectory", use_predicted_trajectory_);
+  updateParam<bool>(parameters, "use_imu_path", use_imu_path_);
+  updateParam<double>(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_);
+  updateParam<double>(parameters, "detection_range_min_height", detection_range_min_height_);
+  updateParam<double>(
+    parameters, "detection_range_max_height_margin", detection_range_max_height_margin_);
+  updateParam<double>(parameters, "voxel_grid_x", voxel_grid_x_);
+  updateParam<double>(parameters, "voxel_grid_y", voxel_grid_y_);
+  updateParam<double>(parameters, "voxel_grid_z", voxel_grid_z_);
+  updateParam<double>(parameters, "min_generated_path_length", min_generated_path_length_);
+  updateParam<double>(parameters, "expand_width", expand_width_);
+  updateParam<double>(parameters, "longitudinal_offset", longitudinal_offset_);
+  updateParam<double>(parameters, "t_response", t_response_);
+  updateParam<double>(parameters, "a_ego_min", a_ego_min_);
+  updateParam<double>(parameters, "a_obj_min", a_obj_min_);
+
+  updateParam<double>(parameters, "cluster_tolerance", cluster_tolerance_);
+  updateParam<int>(parameters, "minimum_cluster_size", minimum_cluster_size_);
+  updateParam<int>(parameters, "maximum_cluster_size", maximum_cluster_size_);
+
+  updateParam<double>(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_);
+  updateParam<double>(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_);
+  updateParam<double>(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_);
+  updateParam<double>(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_);
+
+  {  // Object history data keeper setup
+    auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout();
+    updateParam<double>(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time);
+    updateParam<double>(parameters, "collision_keeping_sec", collision_keeping_sec);
+    collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time);
+  }
+
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+  return result;
 }
 
 void AEB::onTimer()
@@ -242,11 +311,9 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
   filter.filter(*no_height_filtered_pointcloud_ptr);
 
   obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
+
   pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
   obstacle_ros_pointcloud_ptr_->header = input_msg->header;
-  if (publish_debug_pointcloud_) {
-    pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
-  }
 }
 
 bool AEB::isDataReady()
@@ -284,13 +351,14 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
   MarkerArray debug_markers;
   checkCollision(debug_markers);
 
-  if (!collision_data_keeper_.checkExpired()) {
+  if (!collision_data_keeper_.checkCollisionExpired()) {
     const std::string error_msg = "[AEB]: Emergency Brake";
     const auto diag_level = DiagnosticStatus::ERROR;
     stat.summary(diag_level, error_msg);
     const auto & data = collision_data_keeper_.get();
     stat.addf("RSS", "%.2f", data.rss);
     stat.addf("Distance", "%.2f", data.distance_to_object);
+    stat.addf("Object Speed", "%.2f", data.velocity);
     addCollisionMarker(data, debug_markers);
   } else {
     const std::string error_msg = "[AEB]: No Collision";
@@ -304,6 +372,8 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
 
 bool AEB::checkCollision(MarkerArray & debug_markers)
 {
+  using colorTuple = std::tuple<double, double, double, double>;
+
   // step1. check data
   if (!isDataReady()) {
     return false;
@@ -320,86 +390,114 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
     return false;
   }
 
-  // step3. create ego path based on sensor data
-  bool has_collision_ego = false;
-  if (use_imu_path_) {
-    std::vector<Polygon2d> ego_polys;
-    const double current_w = angular_velocity_ptr_->z;
-    constexpr double color_r = 0.0 / 256.0;
-    constexpr double color_g = 148.0 / 256.0;
-    constexpr double color_b = 205.0 / 256.0;
-    constexpr double color_a = 0.999;
-    const auto current_time = get_clock()->now();
-    const auto ego_path = generateEgoPath(current_v, current_w, ego_polys);
-    std::vector<ObjectData> objects;
-    createObjectData(ego_path, ego_polys, current_time, objects);
-    has_collision_ego = hasCollision(current_v, ego_path, objects);
-
-    std::string ns = "ego";
-    addMarker(
-      current_time, ego_path, ego_polys, objects, color_r, color_g, color_b, color_a, ns,
-      debug_markers);
-  }
-
-  // step4. transform predicted trajectory from control module
-  bool has_collision_predicted = false;
-  if (use_predicted_trajectory_) {
-    std::vector<Polygon2d> predicted_polys;
-    const auto predicted_traj_ptr = predicted_traj_ptr_;
-    constexpr double color_r = 0.0;
-    constexpr double color_g = 100.0 / 256.0;
-    constexpr double color_b = 0.0;
-    constexpr double color_a = 0.999;
-    const auto current_time = predicted_traj_ptr->header.stamp;
-    const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr, predicted_polys);
-    if (predicted_path_opt) {
-      const auto & predicted_path = predicted_path_opt.value();
-      std::vector<ObjectData> objects;
-      createObjectData(predicted_path, predicted_polys, current_time, objects);
-      has_collision_predicted = hasCollision(current_v, predicted_path, objects);
-
-      std::string ns = "predicted";
+  auto check_collision = [&](
+                           const auto & path, const colorTuple & debug_colors,
+                           const std::string & debug_ns,
+                           pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) {
+    // Crop out Pointcloud using an extra wide ego path
+    const auto expanded_ego_polys =
+      generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_);
+    cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects);
+
+    // Check which points of the cropped point cloud are on the ego path, and get the closest one
+    std::vector<ObjectData> objects_from_point_clusters;
+    const auto ego_polys = generatePathFootprint(path, expand_width_);
+    const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp;
+    createObjectDataUsingPointCloudClusters(
+      path, ego_polys, current_time, objects_from_point_clusters, filtered_objects);
+
+    // Get only the closest object and calculate its speed
+    const auto closest_object_point = std::invoke([&]() -> std::optional<ObjectData> {
+      const auto closest_object_point_itr = std::min_element(
+        objects_from_point_clusters.begin(), objects_from_point_clusters.end(),
+        [](const auto & o1, const auto & o2) {
+          return o1.distance_to_object < o2.distance_to_object;
+        });
+
+      if (closest_object_point_itr == objects_from_point_clusters.end()) {
+        return std::nullopt;
+      }
+      const auto closest_object_speed = collision_data_keeper_.calcObjectSpeedFromHistory(
+        *closest_object_point_itr, path, current_v);
+      if (!closest_object_speed.has_value()) {
+        return std::nullopt;
+      }
+      closest_object_point_itr->velocity = closest_object_speed.value();
+      return std::make_optional<ObjectData>(*closest_object_point_itr);
+    });
+
+    // Add debug markers
+    {
+      const auto [color_r, color_g, color_b, color_a] = debug_colors;
       addMarker(
-        current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
-        ns, debug_markers);
+        this->get_clock()->now(), path, ego_polys, objects_from_point_clusters,
+        closest_object_point, color_r, color_g, color_b, color_a, debug_ns, debug_markers);
     }
-  }
+    // check collision using rss distance
+    return (closest_object_point.has_value())
+             ? hasCollision(current_v, closest_object_point.value())
+             : false;
+  };
+
+  // step3. make function to check collision with ego path created with sensor data
+  const auto has_collision_ego = [&](pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) -> bool {
+    if (!use_imu_path_) return false;
+    const double current_w = angular_velocity_ptr_->z;
+    constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999};
+    const std::string ns = "ego";
+    const auto ego_path = generateEgoPath(current_v, current_w);
+
+    return check_collision(ego_path, debug_color, ns, filtered_objects);
+  };
 
-  return has_collision_ego || has_collision_predicted;
+  // step4. make function to check collision with predicted trajectory from control module
+  const auto has_collision_predicted =
+    [&](pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) -> bool {
+    if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false;
+    const auto predicted_traj_ptr = predicted_traj_ptr_;
+    const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr);
+
+    if (!predicted_path_opt) return false;
+    constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999};
+    const std::string ns = "predicted";
+    const auto & predicted_path = predicted_path_opt.value();
+
+    return check_collision(predicted_path, debug_color, ns, filtered_objects);
+  };
+
+  // Data of filtered point cloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects(new PointCloud);
+  // evaluate if there is a collision for both paths
+  const bool has_collision =
+    has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects);
+
+  // Debug print
+  if (!filtered_objects->empty() && publish_debug_pointcloud_) {
+    const auto filtered_objects_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
+    pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr_);
+    pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr_);
+  }
+  return has_collision;
 }
 
-bool AEB::hasCollision(
-  const double current_v, const Path & ego_path, const std::vector<ObjectData> & objects)
+bool AEB::hasCollision(const double current_v, const ObjectData & closest_object)
 {
-  // calculate RSS
-  const auto current_p = tier4_autoware_utils::createPoint(
-    ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z);
+  const double & obj_v = closest_object.velocity;
   const double & t = t_response_;
-  for (const auto & obj : objects) {
-    const double & obj_v = obj.velocity;
-    const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) -
-                            obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_;
-
-    // check the object is front the ego or not
-    if ((obj.position.x - ego_path[0].position.x) > 0) {
-      const double dist_ego_to_object =
-        motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) -
-        vehicle_info_.max_longitudinal_offset_m;
-      if (dist_ego_to_object < rss_dist) {
-        // collision happens
-        ObjectData collision_data = obj;
-        collision_data.rss = rss_dist;
-        collision_data.distance_to_object = dist_ego_to_object;
-        collision_data_keeper_.update(collision_data);
-        return true;
-      }
-    }
+  const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) -
+                          obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_;
+  if (closest_object.distance_to_object < rss_dist) {
+    // collision happens
+    ObjectData collision_data = closest_object;
+    collision_data.rss = rss_dist;
+    collision_data.distance_to_object = closest_object.distance_to_object;
+    collision_data_keeper_.setCollisionData(collision_data);
+    return true;
   }
   return false;
 }
 
-Path AEB::generateEgoPath(
-  const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons)
+Path AEB::generateEgoPath(const double curr_v, const double curr_w)
 {
   Path path;
   double curr_x = 0.0;
@@ -444,17 +542,10 @@ Path AEB::generateEgoPath(
     }
     path.push_back(current_pose);
   }
-
-  // generate ego polygons
-  polygons.resize(path.size() - 1);
-  for (size_t i = 0; i < path.size() - 1; ++i) {
-    polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
-  }
   return path;
 }
 
-std::optional<Path> AEB::generateEgoPath(
-  const Trajectory & predicted_traj, std::vector<tier4_autoware_utils::Polygon2d> & polygons)
+std::optional<Path> AEB::generateEgoPath(const Trajectory & predicted_traj)
 {
   if (predicted_traj.points.empty()) {
     return std::nullopt;
@@ -482,35 +573,82 @@ std::optional<Path> AEB::generateEgoPath(
       break;
     }
   }
-  // create polygon
-  polygons.resize(path.size());
+  return path;
+}
+
+std::vector<Polygon2d> AEB::generatePathFootprint(
+  const Path & path, const double extra_width_margin)
+{
+  std::vector<Polygon2d> polygons;
   for (size_t i = 0; i < path.size() - 1; ++i) {
-    polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
+    polygons.push_back(
+      createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin));
   }
-  return path;
+  return polygons;
 }
 
-void AEB::createObjectData(
-  const Path & ego_path, const std::vector<tier4_autoware_utils::Polygon2d> & ego_polys,
-  const rclcpp::Time & stamp, std::vector<ObjectData> & objects)
+void AEB::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)
 {
   // check if the predicted path has valid number of points
-  if (ego_path.size() < 2 || ego_polys.empty()) {
+  if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) {
     return;
   }
 
-  PointCloud::Ptr obstacle_points_ptr(new PointCloud);
-  pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_points_ptr);
-  for (const auto & point : obstacle_points_ptr->points) {
+  // eliminate noisy points by only considering points belonging to clusters of at least a certain
+  // size
+  const std::vector<pcl::PointIndices> cluster_indices = std::invoke([&]() {
+    std::vector<pcl::PointIndices> cluster_idx;
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    tree->setInputCloud(obstacle_points_ptr);
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+    ec.setClusterTolerance(cluster_tolerance_);
+    ec.setMinClusterSize(minimum_cluster_size_);
+    ec.setMaxClusterSize(maximum_cluster_size_);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(obstacle_points_ptr);
+    ec.extract(cluster_idx);
+    return cluster_idx;
+  });
+
+  PointCloud::Ptr points_belonging_to_cluster_hulls(new PointCloud);
+  for (const auto & indices : cluster_indices) {
+    PointCloud::Ptr cluster(new PointCloud);
+    for (const auto & index : indices.indices) {
+      cluster->push_back((*obstacle_points_ptr)[index]);
+    }
+    // Make a 2d convex hull for the objects
+    pcl::ConvexHull<pcl::PointXYZ> hull;
+    hull.setDimension(2);
+    hull.setInputCloud(cluster);
+    std::vector<pcl::Vertices> polygons;
+    PointCloud::Ptr surface_hull(new pcl::PointCloud<pcl::PointXYZ>);
+    hull.reconstruct(*surface_hull, polygons);
+    for (const auto & p : *surface_hull) {
+      points_belonging_to_cluster_hulls->push_back(p);
+    }
+  }
+
+  // select points inside the ego footprint path
+  const auto current_p = tier4_autoware_utils::createPoint(
+    ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z);
+
+  for (const auto & p : *points_belonging_to_cluster_hulls) {
+    const auto obj_position = tier4_autoware_utils::createPoint(p.x, p.y, p.z);
+    const double dist_ego_to_object =
+      motion_utils::calcSignedArcLength(ego_path, current_p, obj_position) -
+      vehicle_info_.max_longitudinal_offset_m;
+    // objects behind ego are ignored
+    if (dist_ego_to_object < 0.0) continue;
+
     ObjectData obj;
     obj.stamp = stamp;
-    obj.position = tier4_autoware_utils::createPoint(point.x, point.y, point.z);
+    obj.position = obj_position;
     obj.velocity = 0.0;
-    const Point2d obj_point(point.x, point.y);
-    const double lat_dist = motion_utils::calcLateralOffset(ego_path, obj.position);
-    if (lat_dist > 5.0) {
-      continue;
-    }
+    obj.distance_to_object = dist_ego_to_object;
+
+    const Point2d obj_point(p.x, p.y);
     for (const auto & ego_poly : ego_polys) {
       if (bg::within(obj_point, ego_poly)) {
         objects.push_back(obj);
@@ -520,10 +658,41 @@ void AEB::createObjectData(
   }
 }
 
+void AEB::cropPointCloudWithEgoFootprintPath(
+  const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects)
+{
+  PointCloud::Ptr full_points_ptr(new PointCloud);
+  pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr);
+  // Create a Point cloud with the points of the ego footprint
+  PointCloud::Ptr path_polygon_points(new PointCloud);
+  std::for_each(ego_polys.begin(), ego_polys.end(), [&](const auto & poly) {
+    std::for_each(poly.outer().begin(), poly.outer().end(), [&](const auto & p) {
+      pcl::PointXYZ point(p.x(), p.y(), 0.0);
+      path_polygon_points->push_back(point);
+    });
+  });
+  // Make a surface hull with the ego footprint to filter out points
+  pcl::ConvexHull<pcl::PointXYZ> hull;
+  hull.setDimension(2);
+  hull.setInputCloud(path_polygon_points);
+  std::vector<pcl::Vertices> polygons;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull(new pcl::PointCloud<pcl::PointXYZ>);
+  hull.reconstruct(*surface_hull, polygons);
+  // Filter out points outside of the path's convex hull
+  pcl::CropHull<pcl::PointXYZ> path_polygon_hull_filter;
+  path_polygon_hull_filter.setDim(2);
+  path_polygon_hull_filter.setInputCloud(full_points_ptr);
+  path_polygon_hull_filter.setHullIndices(polygons);
+  path_polygon_hull_filter.setHullCloud(surface_hull);
+  path_polygon_hull_filter.filter(*filtered_objects);
+  filtered_objects->header.stamp = full_points_ptr->header.stamp;
+}
+
 void AEB::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)
 {
   auto path_marker = tier4_autoware_utils::createDefaultMarker(
     "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP,
@@ -553,12 +722,29 @@ void AEB::addMarker(
 
   auto object_data_marker = tier4_autoware_utils::createDefaultMarker(
     "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST,
-    tier4_autoware_utils::createMarkerScale(0.05, 0.05, 0.05),
+    tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5),
     tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a));
   for (const auto & e : objects) {
     object_data_marker.points.push_back(e.position);
   }
   debug_markers.markers.push_back(object_data_marker);
+
+  // Visualize planner type text
+  if (closest_object.has_value()) {
+    const auto & obj = closest_object.value();
+    const auto color = tier4_autoware_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999);
+    auto closest_object_velocity_marker_array = tier4_autoware_utils::createDefaultMarker(
+      "base_link", obj.stamp, ns + "_closest_object_velocity", 0,
+      visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.7), color);
+    closest_object_velocity_marker_array.pose.position = obj.position;
+    const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity;
+    closest_object_velocity_marker_array.text =
+      "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n";
+    closest_object_velocity_marker_array.text +=
+      "Object relative velocity to ego: " + std::to_string(obj.velocity - ego_velocity) + " [m/s]";
+    debug_markers.markers.push_back(closest_object_velocity_marker_array);
+  }
 }
 
 void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers)
diff --git a/evaluator/control_evaluator/CMakeLists.txt b/evaluator/control_evaluator/CMakeLists.txt
new file mode 100644
index 0000000000000..b97a14e29cdcd
--- /dev/null
+++ b/evaluator/control_evaluator/CMakeLists.txt
@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.14)
+project(control_evaluator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(pluginlib REQUIRED)
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/${PROJECT_NAME}_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "control_diagnostics::controlEvaluatorNode"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    param
+    launch
+)
diff --git a/evaluator/control_evaluator/README.md b/evaluator/control_evaluator/README.md
new file mode 100644
index 0000000000000..71bd5c0142570
--- /dev/null
+++ b/evaluator/control_evaluator/README.md
@@ -0,0 +1,5 @@
+# Planning Evaluator
+
+## Purpose
+
+This package provides nodes that generate metrics to evaluate the quality of control.
diff --git a/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp b/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp
new file mode 100644
index 0000000000000..3a3ba47d88e03
--- /dev/null
+++ b/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp
@@ -0,0 +1,61 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
+#define CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+
+#include <array>
+#include <deque>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace control_diagnostics
+{
+
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+
+/**
+ * @brief Node for control evaluation
+ */
+class controlEvaluatorNode : public rclcpp::Node
+{
+public:
+  explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options);
+
+  /**
+   * @brief publish the given metric statistic
+   */
+  DiagnosticStatus generateDiagnosticStatus(const bool is_emergency_brake) const;
+  void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
+  void onTimer();
+
+private:
+  rclcpp::Subscription<DiagnosticArray>::SharedPtr control_diag_sub_;
+  rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
+
+  // Calculator
+  // Metrics
+  std::deque<rclcpp::Time> stamps_;
+  DiagnosticArray metrics_msg_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+}  // namespace control_diagnostics
+
+#endif  // CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
diff --git a/evaluator/control_evaluator/launch/control_evaluator.launch.xml b/evaluator/control_evaluator/launch/control_evaluator.launch.xml
new file mode 100644
index 0000000000000..75012791888a6
--- /dev/null
+++ b/evaluator/control_evaluator/launch/control_evaluator.launch.xml
@@ -0,0 +1,12 @@
+<launch>
+  <arg name="input/diagnostics" default="/diagnostics"/>
+
+  <!-- control evaluator -->
+  <group>
+    <node name="control_evaluator" exec="control_evaluator" pkg="control_evaluator">
+      <param from="$(find-pkg-share control_evaluator)/param/control_evaluator.defaults.yaml"/>
+      <remap from="~/input/diagnostics" to="$(var input/diagnostics)"/>
+      <remap from="~/metrics" to="/diagnostic/control_evaluator/metrics"/>
+    </node>
+  </group>
+</launch>
diff --git a/evaluator/control_evaluator/package.xml b/evaluator/control_evaluator/package.xml
new file mode 100644
index 0000000000000..10686e9b73761
--- /dev/null
+++ b/evaluator/control_evaluator/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>control_evaluator</name>
+  <version>0.1.0</version>
+  <description>ROS 2 node for evaluating control</description>
+  <maintainer email="daniel.sanchez@tier4.jp">Daniel SANCHEZ</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">takayuki MUROOKA</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="daniel.sanchez@tier4.jp">Daniel SANCHEZ</author>
+  <author email="takayuki.murooka@tier4.jp">takayuki MUROOKA</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/evaluator/control_evaluator/param/control_evaluator.defaults.yaml b/evaluator/control_evaluator/param/control_evaluator.defaults.yaml
new file mode 100644
index 0000000000000..a20dbd7ffd3d9
--- /dev/null
+++ b/evaluator/control_evaluator/param/control_evaluator.defaults.yaml
@@ -0,0 +1,2 @@
+/**:
+  ros__parameters:
diff --git a/evaluator/control_evaluator/src/control_evaluator_node.cpp b/evaluator/control_evaluator/src/control_evaluator_node.cpp
new file mode 100644
index 0000000000000..d575a35a2389f
--- /dev/null
+++ b/evaluator/control_evaluator/src/control_evaluator_node.cpp
@@ -0,0 +1,86 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "control_evaluator/control_evaluator_node.hpp"
+
+#include <fstream>
+#include <iostream>
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace control_diagnostics
+{
+controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options)
+: Node("control_evaluator", node_options)
+{
+  using std::placeholders::_1;
+
+  control_diag_sub_ = create_subscription<DiagnosticArray>(
+    "~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1));
+
+  // Publisher
+  metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);
+
+  // Timer callback to publish evaluator diagnostics
+  using namespace std::literals::chrono_literals;
+  timer_ =
+    rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this));
+}
+
+DiagnosticStatus controlEvaluatorNode::generateDiagnosticStatus(const bool is_emergency_brake) const
+{
+  DiagnosticStatus status;
+  status.level = status.OK;
+  status.name = "autonomous_emergency_braking";
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "decision";
+  key_value.value = (is_emergency_brake) ? "stop" : "none";
+  status.values.push_back(key_value);
+  return status;
+}
+
+void controlEvaluatorNode::onTimer()
+{
+  if (!metrics_msg_.status.empty()) {
+    metrics_pub_->publish(metrics_msg_);
+    metrics_msg_.status.clear();
+  }
+}
+
+void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg)
+{
+  const auto start = now();
+  const auto aeb_status =
+    std::find_if(diag_msg->status.begin(), diag_msg->status.end(), [](const auto & status) {
+      const bool aeb_found = status.name.find("autonomous_emergency_braking") != std::string::npos;
+      return aeb_found;
+    });
+
+  if (aeb_status == diag_msg->status.end()) return;
+
+  const bool is_emergency_brake = (aeb_status->level == DiagnosticStatus::ERROR);
+  metrics_msg_.header.stamp = now();
+  metrics_msg_.status.emplace_back(generateDiagnosticStatus(is_emergency_brake));
+
+  const auto runtime = (now() - start).seconds();
+  RCLCPP_DEBUG(get_logger(), "control evaluation calculation time: %2.2f ms", runtime * 1e3);
+}
+
+}  // namespace control_diagnostics
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode)
diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py
index a63e7f547fef1..2ef2e07367f81 100644
--- a/launch/tier4_control_launch/launch/control.launch.py
+++ b/launch/tier4_control_launch/launch/control.launch.py
@@ -343,6 +343,22 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
+    # control evaluator
+    control_evaluator_component = ComposableNode(
+        package="control_evaluator",
+        plugin="control_diagnostics::controlEvaluatorNode",
+        name="control_evaluator",
+        remappings=[
+            ("~/input/diagnostics", "/diagnostics"),
+            ("~/output/metrics", "~/metrics"),
+        ],
+    )
+
+    control_evaluator_loader = LoadComposableNodes(
+        composable_node_descriptions=[control_evaluator_component],
+        target_container="/control/control_container",
+    )
+
     # control validator checker
     control_validator_component = ComposableNode(
         package="control_validator",
@@ -369,6 +385,7 @@ def launch_setup(context, *args, **kwargs):
             obstacle_collision_checker_loader,
             autonomous_emergency_braking_loader,
             predicted_path_checker_loader,
+            control_evaluator_loader,
         ]
     )
 
diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml
index 4bfefa4d93747..801fa274dd086 100644
--- a/launch/tier4_control_launch/package.xml
+++ b/launch/tier4_control_launch/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <exec_depend>control_evaluator</exec_depend>
   <exec_depend>external_cmd_converter</exec_depend>
   <exec_depend>external_cmd_selector</exec_depend>
   <exec_depend>lane_departure_checker</exec_depend>