From 5d3883d0d1e6118888b2118e65498030a34702c7 Mon Sep 17 00:00:00 2001
From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Date: Thu, 10 Jun 2021 00:13:07 +0900
Subject: [PATCH] Add use foa option (#106)

* Add use foa option

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Cosmetic change

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>
---
 autoware_launch/launch/autoware.launch.xml       |  2 ++
 .../launch/logging_simulator.launch.xml          |  2 ++
 .../launch/planning_simulator.launch.xml         |  2 ++
 .../obstacle_avoidance_planner.param.yaml        |  4 ++--
 planning_launch/launch/planning.launch.xml       |  4 ++++
 .../behavior_planning.launch.py                  | 14 ++++++++------
 .../behavior_planning.launch.xml                 | 16 +++++++++-------
 .../motion_planning/motion_planning.launch.py    |  1 +
 .../motion_planning/motion_planning.launch.xml   |  1 +
 9 files changed, 31 insertions(+), 15 deletions(-)

diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
index b36abfc84..e0e939fde 100644
--- a/autoware_launch/launch/autoware.launch.xml
+++ b/autoware_launch/launch/autoware.launch.xml
@@ -8,6 +8,7 @@
   <arg name="rviz" default="true" description="launch rviz"/>
   <arg name="lanelet2_map_file" default="lanelet2_map.osm" />
   <arg name="pointcloud_map_file" default="pointcloud_map.pcd" />
+  <arg name="use_foa" default="false"/>
 
   <!-- Global parameters -->
   <!-- Vehicle info -->
@@ -53,6 +54,7 @@
 
   <!-- Planning -->
   <include file="$(find-pkg-share planning_launch)/launch/planning.launch.xml">
+    <arg name="use_foa" value="$(var use_foa)" />
   </include>
 
   <!-- Control -->
diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
index ca1784889..c48fd5b22 100644
--- a/autoware_launch/launch/logging_simulator.launch.xml
+++ b/autoware_launch/launch/logging_simulator.launch.xml
@@ -19,6 +19,7 @@
   <arg name="rviz" default="true" description="launch rviz" />
   <arg name="lanelet2_map_file" default="lanelet2_map.osm" />
   <arg name="pointcloud_map_file" default="pointcloud_map.pcd" />
+  <arg name="use_foa" default="false" />
   <set_parameter name="use_sim_time" value="true" />
 
   <!-- Global parameters -->
@@ -83,6 +84,7 @@
   <!-- Planning -->
   <group>
     <include file="$(find-pkg-share planning_launch)/launch/planning.launch.xml" if="$(var planning)">
+      <arg name="use_foa" value="$(var use_foa)" />
     </include>
   </group>
 
diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml
index 3e1308905..6be63c800 100644
--- a/autoware_launch/launch/planning_simulator.launch.xml
+++ b/autoware_launch/launch/planning_simulator.launch.xml
@@ -15,6 +15,7 @@
   <arg name="perception/enable_object_recognition" default="true"/>
   <arg name="sensing/visible_range" default="300.0"/>
   <arg name="scenario_simulation" default="false"/>
+  <arg name="use_foa" default="false" />
   <arg name="use_sim_time" default="false" />
   <set_parameter name="use_sim_time" value="$(var use_sim_time)" />
   <!-- Optional parameters for vcu emulation -->
@@ -58,6 +59,7 @@
 
   <!-- Planning -->
   <include file="$(find-pkg-share planning_launch)/launch/planning.launch.xml">
+    <arg name="use_foa" value="$(var use_foa)" />
   </include>
 
   <!-- Control -->
diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml
index 809eb19c6..18e99272d 100644
--- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml
+++ b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml
@@ -14,7 +14,7 @@
     # clearance for unique points
     clearance_for_straight_line: 0.05 # minimum optimizing range around straight points
     clearance_for_joint: 0.1 # minimum optimizing range around joint points
-    clearance_for_only_smoothing: 0.1 # minimum optimizing range when applygin only smoothing
+    clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing
     range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending
     clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line
 
@@ -44,7 +44,7 @@
 
     is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid
     is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid
-    enable_avoidance: false # enable avoidance function
+    # enable_avoidance: false # enable avoidance function
     is_using_vehicle_config: true # use vehicle config
     num_sampling_points: 100 # number of optimizing points
     num_joint_buffer_points: 3 # number of joint buffer points
diff --git a/planning_launch/launch/planning.launch.xml b/planning_launch/launch/planning.launch.xml
index 003af6fca..76151717b 100644
--- a/planning_launch/launch/planning.launch.xml
+++ b/planning_launch/launch/planning.launch.xml
@@ -1,6 +1,10 @@
 <launch>
+  <arg name="use_foa" default="false" />
   <!-- planning module -->
   <group>
+    <let name="disuse_foa" value="false" if="$(var use_foa)"/>
+    <let name="disuse_foa" value="true" unless="$(var use_foa)"/>
+
     <push-ros-namespace namespace="planning"/>
     <!-- mission planning module -->
     <group>
diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
index f35986b09..084b427d7 100644
--- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
+++ b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -67,11 +67,11 @@ def generate_launch_description():
         parameters=[
             lane_change_planner_param,
             {
-                'enable_abort_lane_change': False,
-                'enable_collision_check_at_prepare_phase': False,
-                'use_predicted_path_outside_lanelet': False,
-                'use_all_predicted_path': False,
-                'enable_blocked_by_obstacle': False,
+                'enable_abort_lane_change': LaunchConfiguration('disuse_foa'),
+                'enable_collision_check_at_prepare_phase': LaunchConfiguration('disuse_foa'),
+                'use_predicted_path_outside_lanelet': LaunchConfiguration('disuse_foa'),
+                'use_all_predicted_path': LaunchConfiguration('disuse_foa'),
+                'enable_blocked_by_obstacle': LaunchConfiguration('disuse_foa'),
             }
         ],
         extra_arguments=[
@@ -240,5 +240,7 @@ def generate_launch_description():
             cmd=['ros2', 'topic', 'pub',
                  '/planning/scenario_planning/lane_driving/lane_change_approval',
                  'autoware_planning_msgs/msg/LaneChangeCommand', '{command: true}',
-                 '-r', '10']),
+                 '-r', '10'],
+            condition=IfCondition(LaunchConfiguration('disuse_foa'))
+        ),
     ])
diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 708fe5158..7ef336ac5 100644
--- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -15,15 +15,17 @@
     <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons" />
     <remap from="~/debug/lane_change_candidate_path" to="/planning/scenario_planning/lane_driving/lane_change_candidate_path" />
     <param from="$(find-pkg-share planning_launch)/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml" />
-    <param name="enable_abort_lane_change" value="false" />
-    <param name="enable_collision_check_at_prepare_phase" value="false" />
-    <param name="use_predicted_path_outside_lanelet" value="false" />
-    <param name="use_all_predicted_path" value="false" />
-    <param name="enable_blocked_by_obstacle" value="false" />
+    <param name="enable_abort_lane_change" value="$(var disuse_foa)" />
+    <param name="enable_collision_check_at_prepare_phase" value="$(var disuse_foa)" />
+    <param name="use_predicted_path_outside_lanelet" value="$(var disuse_foa)" />
+    <param name="use_all_predicted_path" value="$(var disuse_foa)" />
+    <param name="enable_blocked_by_obstacle" value="$(var disuse_foa)" />
   </node>
 
-  <executable cmd="ros2 topic pub /planning/scenario_planning/lane_driving/lane_change_approval
-                   autoware_planning_msgs/msg/LaneChangeCommand 'command: true' -r 10"/>
+  <group if="$(var disuse_foa)">
+    <executable cmd="ros2 topic pub /planning/scenario_planning/lane_driving/lane_change_approval
+                    autoware_planning_msgs/msg/LaneChangeCommand 'command: true' -r 10"/>
+  </group>
 
   <node pkg="turn_signal_decider" exec="turn_signal_decider" name="turn_signal_decider" output="screen">
     <remap from="input/path_with_lane_id" to="path_with_lane_id" />
diff --git a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
index 5abfdbfc6..6313ef0ca 100644
--- a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
+++ b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
@@ -52,6 +52,7 @@ def generate_launch_description():
         ],
         parameters=[
             obstacle_avoidance_planner_param,
+            {'enable_avoidance': LaunchConfiguration('disuse_foa')},
             {'is_showing_debug_info': False},
             {'is_stopping_if_outside_drivable_area': True},
         ],
diff --git a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
index 05892f5a2..7bf68aa96 100644
--- a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
+++ b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -4,6 +4,7 @@
 
   <!-- path planning for avoiding obstacle with dynamic object info -->
   <group>
+    <set_parameter name="enable_avoidance" value="$(var disuse_foa)" />
     <include file="$(find-pkg-share obstacle_avoidance_planner)/launch/obstacle_avoidance_planner.launch.xml">
       <arg name="param_path" value="$(find-pkg-share planning_launch)/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml"/>
       <arg name="input_path_topic" value="$(var input_path_topic)" />