From 283a38acc94c51ae81d1950ba05cf611d761ec4a Mon Sep 17 00:00:00 2001
From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
Date: Fri, 17 Mar 2023 11:08:57 +0900
Subject: [PATCH] feat(control_launch): add parameter for aeb (#232)

---
 .../autonomous_emergency_braking.param.yaml      | 16 ++++++++++++++++
 .../tier4_control_component.launch.xml           |  3 +++
 autoware_launch/rviz/autoware.rviz               | 15 +++++++++++++++
 3 files changed, 34 insertions(+)
 create mode 100644 autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml

diff --git a/autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml b/autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml
new file mode 100644
index 000000000..6019eaae5
--- /dev/null
+++ b/autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml
@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    use_predicted_trajectory: true
+    use_imu_path: true
+    voxel_grid_x: 0.05
+    voxel_grid_y: 0.05
+    voxel_grid_z: 100000.0
+    min_generated_path_length: 0.5
+    expand_width: 0.1
+    longitudinal_offset: 2.0
+    t_response: 1.0
+    a_ego_min: -3.0
+    a_obj_min: -1.0
+    prediction_time_horizon: 1.5
+    prediction_time_interval: 0.1
+    aeb_hz: 10.0
diff --git a/autoware_launch/launch/components/tier4_control_component.launch.xml b/autoware_launch/launch/components/tier4_control_component.launch.xml
index bae586510..3af7b43df 100644
--- a/autoware_launch/launch/components/tier4_control_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_control_component.launch.xml
@@ -4,6 +4,7 @@
   <arg name="lateral_controller_mode" default="mpc"/>
   <arg name="longitudinal_controller_mode" default="pid"/>
   <arg name="use_individual_control_param" default="false"/>
+  <arg name="enable_autonomous_emergency_braking" default="false"/>
 
   <let name="latlon_controller_param_path_dir" value="$(var vehicle_id)" if="$(var use_individual_control_param)"/>
   <let name="latlon_controller_param_path_dir" value="" unless="$(var use_individual_control_param)"/>
@@ -37,5 +38,7 @@
     <arg name="shift_decider_param_path" value="$(find-pkg-share autoware_launch)/config/control/shift_decider/shift_decider.param.yaml"/>
     <arg name="obstacle_collision_checker_param_path" value="$(find-pkg-share autoware_launch)/config/control/obstacle_collision_checker/obstacle_collision_checker.param.yaml"/>
     <arg name="external_cmd_selector_param_path" value="$(find-pkg-share autoware_launch)/config/control/external_cmd_selector/external_cmd_selector.param.yaml"/>
+    <arg name="aeb_param_path" value="$(find-pkg-share autoware_launch)/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml"/>
+    <arg name="enable_autonomous_emergency_braking" value="$(var enable_autonomous_emergency_braking)"/>
   </include>
 </launch>
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index e1674c7d7..f694b1d2c 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -1976,6 +1976,21 @@ Visualization Manager:
             Reliability Policy: Reliable
             Value: /control/trajectory_follower/controller_node_exe/debug/markers
           Value: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Debug/AEB
+          Namespaces:
+            ego_path: true
+            ego_polygons: true
+            predicted_path: true
+            predicted_polygons: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/autonomous_emergency_braking/debug/markers
+          Value: false
       Enabled: true
       Name: Control
   Enabled: true