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

@@ -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 -->

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>
 

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 -->

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

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>

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'))
+        ),
     ])

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" />

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},
         ],

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)" />