feat: add use_external_lane_change option (autowarefoundation#413)

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
soblin · Jan 30, 2023 · 49980ac · 49980ac
1 parent 5f90390
commit 49980ac
Show file tree

Hide file tree

Showing 4 changed files with 90 additions and 7 deletions.
diff --git a/...or_planning/behavior_path_planner/behavior_path_planner_tree_with_external_request_LC.xml b/...or_planning/behavior_path_planner/behavior_path_planner_tree_with_external_request_LC.xml
@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Fallback>
+            <ReactiveSequence>
+                <Condition ID="PullOver_Request"/>
+                <Action ID="PullOver_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <ReactiveSequence>
+                <Condition ID="PullOut_Request"/>
+                <Action ID="PullOut_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <ReactiveSequence>
+                <Condition ID="SideShift_Request"/>
+                <Action ID="SideShift_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <ReactiveSequence>
+                <Condition ID="ExternalRequestLaneChangeRight_Request"/>
+                <Action ID="ExternalRequestLaneChangeRight_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <ReactiveSequence>
+                <Condition ID="ExternalRequestLaneChangeLeft_Request"/>
+                <Action ID="ExternalRequestLaneChangeLeft_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <ReactiveSequence>
+                <Condition ID="LaneChange_Request"/>
+                <Action ID="LaneChange_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <ReactiveSequence>
+                <Condition ID="Avoidance_Request"/>
+                <Action ID="Avoidance_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <Action ID="LaneFollowing_Plan" output="{output}"/>
+        </Fallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="Avoidance_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="Avoidance_Request"/>
+        <Condition ID="ExternalApproval"/>
+        <Action ID="ExternalRequestLaneChangeRight_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="ExternalRequestLaneChangeRight_Request"/>
+        <Action ID="ExternalRequestLaneChangeLeft_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="ExternalRequestLaneChangeLeft_Request"/>
+        <Action ID="LaneChange_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="LaneChange_Request"/>
+        <Action ID="LaneFollowing_Plan">
+            <output_port name="output" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
+        </Action>
+        <Condition ID="LaneFollowing_Request"/>
+        <Action ID="PullOut_Plan">
+            <output_port name="output"/>
+        </Action>
+        <Condition ID="PullOut_Request"/>
+        <Action ID="PullOver_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="PullOver_Request"/>
+        <Action ID="SideShift_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="SideShift_Request"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
@@ -36,13 +36,19 @@
   <arg name="perception_mode" default="camera_lidar_fusion" description="select perception mode. camera_lidar_radar_fusion, camera_lidar_fusion, lidar_radar_fusion, lidar, radar"/>
   <arg name="traffic_light_recognition/enable_fine_detection" default="true" description="enable traffic light fine detection"/>
   <!-- Planning -->
-  <arg name="use_experimental_lane_change_function" default="false" />
+  <arg name="use_experimental_lane_change_function" default="false"/>
+  <arg name="use_external_lane_change" default="true"/>
   <!-- Others -->
   <arg name="use_foa" default="false"/>
 
+  <!-- rtc settings (auto/manual) -->
   <let name="rtc_auto_mode_manager_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml" unless="$(var use_foa)"/>
   <let name="rtc_auto_mode_manager_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager_foa.param.yaml" if="$(var use_foa)"/>
 
+  <!-- behavior path planner settings (use external lane change function or not) -->
+  <let name="behavior_path_planner_tree_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml" if="$(var use_external_lane_change)"/>
+  <let name="behavior_path_planner_tree_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_with_external_request_LC.xml" unless="$(var use_external_lane_change)"/>
+
   <!-- Global parameters -->
   <group scoped="false">
     <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
@@ -120,10 +126,7 @@
       <arg name="vehicle_param_file" value="$(find-pkg-share $(var vehicle_model)_description)/config/vehicle_info.param.yaml"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
-      <arg
-        name="behavior_path_planner_tree_param_path"
-        value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml"
-      />
+      <arg name="behavior_path_planner_tree_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml" />
       <arg name="cruise_planner_type" value="$(var cruise_planner_type)"/>
       <arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)"/>
       <arg name="use_surround_obstacle_check" value="false"/>

diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
@@ -28,9 +28,11 @@
   <arg name="rviz" default="true" description="launch rviz"/>
   <!-- Scenario simulation -->
   <arg name="scenario_simulation" default="false" description="use scenario simulation"/>
+  <!-- Planning -->
+  <arg name="use_experimental_lane_change_function" default="false"/>
+  <arg name="use_external_lane_change" default="true"/>
   <!-- Others -->
   <arg name="use_foa" default="false"/>
-  <arg name="use_experimental_lane_change_function" default="false" />
 
   <group scoped="false">
     <include file="$(find-pkg-share autoware_launch)/launch/autoware.launch.xml">
@@ -64,6 +66,7 @@
       <arg name="launch_sensing_driver" value="false"/>
       <!-- Planning -->
       <arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)" />
+      <arg name="use_external_lane_change" value="$(var use_external_lane_change)" />
       <!-- Others -->
       <arg name="use_foa" value="$(var use_foa)"/>
     </include>

diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml
@@ -20,9 +20,11 @@
   <arg name="scenario_simulation" default="false" description="use scenario simulation"/>
   <!-- Vcu emulation -->
   <arg name="vehicle_simulation" default="true" description="use vehicle simulation"/>
+  <!-- Planning -->
+  <arg name="use_experimental_lane_change_function" default="true"/>
+  <arg name="use_external_lane_change" default="false"/>
   <!-- Others -->
   <arg name="use_foa" default="false"/>
-  <arg name="use_experimental_lane_change_function" default="true" />
 
   <group scoped="false">
     <!-- Vehicle -->
@@ -52,6 +54,7 @@
       <arg name="rviz_config" value="$(var rviz_config)"/>
       <!-- Planning -->
       <arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)" />
+      <arg name="use_external_lane_change" value="$(var use_external_lane_change)" />
       <!-- Others -->
       <arg name="use_foa" value="$(var use_foa)"/>
     </include>