From a3a6793c561ed9361ee13cc79ae07d69337a9a40 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Thu, 6 Apr 2023 11:49:45 +0900
Subject: [PATCH] feat(avoidance_by_lc): add new module to avoid obstacle by
 lane change (#261)

* feat(avoidance_by_lc): add config

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(launch): add avoidance by lc param path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(rviz): add marker for avoidance by lc

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../avoidance_by_lc.param.yaml                |  6 +++
 .../scene_module_manager.param.yaml           |  9 +++-
 .../tier4_planning_component.launch.xml       |  4 ++
 autoware_launch/rviz/autoware.rviz            | 46 +++++++++++++++++++
 4 files changed, 64 insertions(+), 1 deletion(-)
 create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml

diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml
new file mode 100644
index 0000000000..a27841f28d
--- /dev/null
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml
@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    avoidance_by_lane_change:
+      execute_object_num: 1
+      execute_object_longitudinal_margin: 0.0
+      execute_only_when_lane_change_finish_before_object: true
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml
index 31b1d99348..df3d5d14eb 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml
@@ -24,7 +24,7 @@
     lane_change_right:
       enable_module: true
       enable_simultaneous_execution: false
-      priority: 4
+      priority: 5
       max_module_size: 1
 
     pull_out:
@@ -47,6 +47,13 @@
 
     avoidance:
       enable_module: true
+      enable_simultaneous_execution: true
+      priority: 4
+      max_module_size: 1
+
+    # NOTE: This module is unstable. Deprecated for now.
+    avoidance_by_lc:
+      enable_module: false
       enable_simultaneous_execution: false
       priority: 3
       max_module_size: 1
diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml
index 43189b12e3..a520ef4bc5 100644
--- a/autoware_launch/launch/components/tier4_planning_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml
@@ -33,6 +33,10 @@
       value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml"
     />
     <arg name="avoidance_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml"/>
+    <arg
+      name="avoidance_by_lc_param_path"
+      value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml"
+    />
     <arg
       name="lane_change_param_path"
       value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml"
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index e7ce7b6371..7fc53dbb3f 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -1011,6 +1011,29 @@ Visualization Manager:
                         Constant Color: false
                         Scale: 0.30000001192092896
                         Value: true
+                    - Class: rviz_plugins/Path
+                      Color Border Vel Max: 3
+                      Enabled: false
+                      Name: PathReference_AvoidanceByLC
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/path_reference/avoidance_by_lane_change
+                      Value: true
+                      View Path:
+                        Alpha: 0.3
+                        Color: 210; 110; 10
+                        Constant Color: true
+                        Value: true
+                        Width: 2
+                      View Velocity:
+                        Alpha: 0.30000001192092896
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: false
                     - Class: rviz_plugins/Path
                       Color Border Vel Max: 3
                       Enabled: false
@@ -1126,6 +1149,29 @@ Visualization Manager:
                         Constant Color: false
                         Scale: 0.30000001192092896
                         Value: false
+                    - Class: rviz_plugins/Path
+                      Color Border Vel Max: 3
+                      Enabled: true
+                      Name: PathChangeCandidate_AvoidanceByLC
+                      Topic:
+                        Depth: 5
+                        Durability Policy: Volatile
+                        History Policy: Keep Last
+                        Reliability Policy: Reliable
+                        Value: /planning/path_candidate/avoidance_by_lane_change
+                      Value: true
+                      View Path:
+                        Alpha: 0.30000001192092896
+                        Color: 115; 210; 22
+                        Constant Color: false
+                        Value: true
+                        Width: 2
+                      View Velocity:
+                        Alpha: 0.30000001192092896
+                        Color: 0; 0; 0
+                        Constant Color: false
+                        Scale: 0.30000001192092896
+                        Value: false
                     - Class: rviz_plugins/Path
                       Color Border Vel Max: 3
                       Enabled: true