refactor(autoware_launch): remove planning_launch, and use tier4_plan…

…ning_launch instead (autowarefoundation#712) * refactor(autoware_launch): remove planning_launch, and use tier4_planning_launch instead Signed-off-by: Takayuki Murooka <[email protected]> * add tier4_planning_component.launch.xml Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> Signed-off-by: Takayuki Murooka <[email protected]>
sykwer · Jan 20, 2023 · 8f447e1 · 8f447e1
1 parent c192ab8
commit 8f447e1
Show file tree

Hide file tree

Showing 60 changed files with 461 additions and 1,811 deletions.
diff --git a/...cenario_planning/common/common.param.yaml → ...cenario_planning/common/common.param.yaml b/...cenario_planning/common/common.param.yaml → ...cenario_planning/common/common.param.yaml
diff --git a/...n_velocity_smoother/Analytical.param.yaml → ...n_velocity_smoother/Analytical.param.yaml b/...n_velocity_smoother/Analytical.param.yaml → ...n_velocity_smoother/Analytical.param.yaml
diff --git a/...velocity_smoother/JerkFiltered.param.yaml → ...velocity_smoother/JerkFiltered.param.yaml b/...velocity_smoother/JerkFiltered.param.yaml → ...velocity_smoother/JerkFiltered.param.yaml
diff --git a/...on/motion_velocity_smoother/L2.param.yaml → ...on/motion_velocity_smoother/L2.param.yaml b/...on/motion_velocity_smoother/L2.param.yaml → ...on/motion_velocity_smoother/L2.param.yaml
diff --git a/.../motion_velocity_smoother/Linf.param.yaml → .../motion_velocity_smoother/Linf.param.yaml b/.../motion_velocity_smoother/Linf.param.yaml → .../motion_velocity_smoother/Linf.param.yaml
diff --git a/...other/motion_velocity_smoother.param.yaml → ...other/motion_velocity_smoother.param.yaml b/...other/motion_velocity_smoother.param.yaml → ...other/motion_velocity_smoother.param.yaml
@@ -9,7 +9,7 @@
 
     # curve parameters
     max_lateral_accel: 1.0             # max lateral acceleration limit [m/ss]
-    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/ss]
+    min_curve_velocity: 0.5            # min velocity at lateral acceleration limit and steering angle rate limit [m/s]
     decel_distance_before_curve: 3.5   # slow speed distance before a curve for lateral acceleration limit
     decel_distance_after_curve: 2.0    # slow speed distance after a curve for lateral acceleration limit
     min_decel_for_lateral_acc_lim_filter: -2.5  # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss]

diff --git a/...planning/common/nearest_search.param.yaml → ...planning/common/nearest_search.param.yaml b/...planning/common/nearest_search.param.yaml → ...planning/common/nearest_search.param.yaml
diff --git a/...th_planner/avoidance/avoidance.param.yaml → ...th_planner/avoidance/avoidance.param.yaml b/...th_planner/avoidance/avoidance.param.yaml → ...th_planner/avoidance/avoidance.param.yaml
diff --git a/..._planner/behavior_path_planner.param.yaml → ..._planner/behavior_path_planner.param.yaml b/..._planner/behavior_path_planner.param.yaml → ..._planner/behavior_path_planner.param.yaml
@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    planning_hz: 10.0
     backward_path_length: 5.0
     forward_path_length: 300.0
     backward_length_buffer_for_end_of_pull_over: 5.0
@@ -14,18 +15,20 @@
     turn_signal_minimum_search_distance: 10.0
     turn_signal_search_time: 3.0
     turn_signal_shift_length_threshold: 0.3
+    turn_signal_on_swerving: true
 
     path_interval: 2.0
 
     visualize_maximum_drivable_area: true
 
     lateral_distance_max_threshold: 2.0
     longitudinal_distance_min_threshold: 3.0
+
     expected_front_deceleration: -1.0
     expected_rear_deceleration: -1.0
 
-    expected_front_deceleration_for_abort: -1.0
-    expected_rear_deceleration_for_abort: -2.0
+    expected_front_deceleration_for_abort: -2.0
+    expected_rear_deceleration_for_abort: -2.5
 
     rear_vehicle_reaction_time: 2.0
-    rear_vehicle_safety_time_margin: 2.0
+    rear_vehicle_safety_time_margin: 1.0
diff --git a/...nning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml b/...nning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml
@@ -0,0 +1,94 @@
+<?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="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>
+    <!-- ////////// -->
+    <BehaviorTree ID="a">
+        <ReactiveFallback>
+            <Condition ID="LaneChange_CheckApproval"/>
+            <KeepRunningUntilFailure>
+                <Action ID="LaneChange_PlanCandidate" output="{output}"/>
+            </KeepRunningUntilFailure>
+        </ReactiveFallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="b">
+        <ReactiveFallback>
+            <Condition ID="Avoidance_CheckApproval"/>
+            <KeepRunningUntilFailure>
+                <Action ID="Avoidance_PlanCandidate" output="{output}"/>
+            </KeepRunningUntilFailure>
+        </ReactiveFallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Condition ID="Avoidance_CheckApproval"/>
+        <Action ID="Avoidance_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="Avoidance_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="Avoidance_Request"/>
+        <Condition ID="ExternalApproval"/>
+        <Condition ID="LaneChange_CheckApproval"/>
+        <Action ID="LaneChange_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="LaneChange_PlanCandidate">
+            <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>
+        <Action ID="LaneFollowing_PlanCandidate">
+            <output_port name="output_candidate" 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"/>
+        <Condition ID="SideShift_CheckApproval"/>
+        <Action ID="SideShift_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="SideShift_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="SideShift_Request"/>
+        <SubTree ID="SubTree"/>
+        <SubTree ID="a"/>
+        <SubTree ID="b"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/...g/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml b/...g/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml
@@ -0,0 +1,86 @@
+<?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="LaneChange_Request"/>
+                <Action ID="LaneChange_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <Action ID="LaneFollowing_Plan" output="{output}"/>
+        </Fallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="a">
+        <ReactiveFallback>
+            <Condition ID="LaneChange_CheckApproval"/>
+            <KeepRunningUntilFailure>
+                <Action ID="LaneChange_PlanCandidate" output="{output}"/>
+            </KeepRunningUntilFailure>
+        </ReactiveFallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="b">
+        <ReactiveFallback>
+            <Condition ID="Avoidance_CheckApproval"/>
+            <KeepRunningUntilFailure>
+                <Action ID="Avoidance_PlanCandidate" output="{output}"/>
+            </KeepRunningUntilFailure>
+        </ReactiveFallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Condition ID="Avoidance_CheckApproval"/>
+        <Action ID="Avoidance_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="Avoidance_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="Avoidance_Request"/>
+        <Condition ID="ExternalApproval"/>
+        <Condition ID="LaneChange_CheckApproval"/>
+        <Action ID="LaneChange_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="LaneChange_PlanCandidate">
+            <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>
+        <Action ID="LaneFollowing_PlanCandidate">
+            <output_port name="output_candidate" 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"/>
+        <Condition ID="SideShift_CheckApproval"/>
+        <Action ID="SideShift_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="SideShift_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="SideShift_Request"/>
+        <SubTree ID="SubTree"/>
+        <SubTree ID="a"/>
+        <SubTree ID="b"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/...e_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml b/...e_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml
@@ -0,0 +1,100 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Fallback>
+            <ReactiveSequence>
+                <Condition ID="PullOver_Request"/>
+                <Condition ID="PullOver_Ready"/>
+                <Action ID="PullOver_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <ReactiveSequence>
+                <Condition ID="PullOut_Request"/>
+                <Condition ID="PullOut_Ready"/>
+                <Action ID="PullOut_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <ReactiveSequence>
+                <Condition ID="LaneChange_Request"/>
+                <Fallback>
+                    <Condition ID="LaneChange_Ready"/>
+                    <Condition ID="ForceLaneChange_ForceApproval"/>
+                </Fallback>
+                <Action ID="LaneChange_Plan" output="{output}"/>
+            </ReactiveSequence>
+            <Action ID="LaneFollowing_Plan" output="{output}"/>
+        </Fallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="a">
+        <ReactiveFallback>
+            <Condition ID="LaneChange_CheckApproval"/>
+            <KeepRunningUntilFailure>
+                <Action ID="LaneChange_PlanCandidate" output="{output}"/>
+            </KeepRunningUntilFailure>
+        </ReactiveFallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="b">
+        <ReactiveFallback>
+            <Condition ID="Avoidance_CheckApproval"/>
+            <KeepRunningUntilFailure>
+                <Action ID="Avoidance_PlanCandidate" output="{output}"/>
+            </KeepRunningUntilFailure>
+        </ReactiveFallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Condition ID="Avoidance_CheckApproval"/>
+        <Action ID="Avoidance_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="Avoidance_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="Avoidance_Ready"/>
+        <Condition ID="Avoidance_Request"/>
+        <Condition ID="ExternalApproval"/>
+        <Condition ID="ForceApproval"/>
+        <Condition ID="ForceLaneChange_ForceApproval"/>
+        <Condition ID="LaneChange_CheckApproval"/>
+        <Action ID="LaneChange_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="LaneChange_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="LaneChange_Ready"/>
+        <Condition ID="LaneChange_Request"/>
+        <Action ID="LaneFollowing_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="LaneFollowing_PlanCandidate">
+            <output_port name="output_candidate" type="boost::optional&lt;tier4_planning_msgs::PathWithLaneId_&lt;std::allocator&lt;void&gt; &gt; &gt;">desc</output_port>
+        </Action>
+        <Condition ID="LaneFollowing_Ready"/>
+        <Condition ID="LaneFollowing_Request"/>
+        <Action ID="PullOut_Plan">
+            <output_port name="output"/>
+        </Action>
+        <Condition ID="PullOut_Ready"/>
+        <Condition ID="PullOut_Request"/>
+        <Action ID="PullOver_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="PullOver_Ready"/>
+        <Condition ID="PullOver_Request"/>
+        <Condition ID="SideShift_CheckApproval"/>
+        <Action ID="SideShift_Plan">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Action ID="SideShift_PlanCandidate">
+            <output_port name="output" type="behavior_path_planner::BehaviorModuleOutput">desc</output_port>
+        </Action>
+        <Condition ID="SideShift_Ready"/>
+        <Condition ID="SideShift_Request"/>
+        <SubTree ID="SubTree"/>
+        <SubTree ID="a"/>
+        <SubTree ID="b"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
diff --git a/...lanner/drivable_area_expansion.param.yaml → ...lanner/drivable_area_expansion.param.yaml b/...lanner/drivable_area_expansion.param.yaml → ...lanner/drivable_area_expansion.param.yaml
diff --git a/...lanner/lane_change/lane_change.param.yaml → ...lanner/lane_change/lane_change.param.yaml b/...lanner/lane_change/lane_change.param.yaml → ...lanner/lane_change/lane_change.param.yaml
@@ -5,9 +5,9 @@
       lane_changing_safety_check_duration: 8.0  # [s]
 
       minimum_lane_change_prepare_distance: 2.0 # [m]
-      minimum_lane_change_length: 16.5
-      backward_length_buffer_for_end_of_lane: 2.0
-      lane_change_finish_judge_buffer: 3.0      # [m]
+      minimum_lane_change_length: 16.5          # [m]
+      backward_length_buffer_for_end_of_lane: 3.0 # [m]
+      lane_change_finish_judge_buffer: 2.0      # [m]
 
       lane_changing_lateral_jerk: 0.5           # [m/s3]
       lane_changing_lateral_acc: 0.5            # [m/s2]

diff --git a/.../lane_following/lane_following.param.yaml → .../lane_following/lane_following.param.yaml b/.../lane_following/lane_following.param.yaml → .../lane_following/lane_following.param.yaml
diff --git a/...path_planner/pull_out/pull_out.param.yaml → ...path_planner/pull_out/pull_out.param.yaml b/...path_planner/pull_out/pull_out.param.yaml → ...path_planner/pull_out/pull_out.param.yaml
@@ -5,18 +5,18 @@
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
       collision_check_margin: 1.0
-      collision_check_distance_from_end: 1.0
+      pull_out_finish_judge_buffer: 1.0
       # shift pull out
       enable_shift_pull_out: true
       shift_pull_out_velocity: 2.0
       pull_out_sampling_num: 4
+      before_pull_out_straight_distance: 0.0
       minimum_shift_pull_out_distance: 20.0
       maximum_lateral_jerk: 2.0
       minimum_lateral_jerk: 0.5
       deceleration_interval: 15.0
       # geometric pull out
       enable_geometric_pull_out: true
-      divide_pull_out_path: false
       geometric_pull_out_velocity: 1.0
       arc_path_interval: 1.0
       lane_departure_margin: 0.2

diff --git a/...th_planner/pull_over/pull_over.param.yaml → ...th_planner/pull_over/pull_over.param.yaml b/...th_planner/pull_over/pull_over.param.yaml → ...th_planner/pull_over/pull_over.param.yaml
@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     pull_over:
-      request_length: 100.0
+      request_length: 200.0
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 2.0  # It must be greater than the state_machine's.
@@ -17,9 +17,6 @@
       backward_goal_search_length: 20.0
       goal_search_interval: 2.0
       longitudinal_margin: 3.0
-      max_lateral_offset: 0.5
-      lateral_offset_interval: 0.25
-      ignore_distance_from_lane_start: 15.0
       # occupancy grid map
       use_occupancy_grid: true
       use_occupancy_grid_for_longitudinal_margin: false
@@ -35,7 +32,8 @@
       maximum_lateral_jerk: 2.0
       minimum_lateral_jerk: 0.5
       deceleration_interval: 15.0
-      after_pull_over_straight_distance: 1.0
+      after_pull_over_straight_distance: 5.0
+      before_pull_over_straight_distance: 5.0
       # parallel parking path
       enable_arc_forward_parking: true
       enable_arc_backward_parking: true

diff --git a/..._planner/side_shift/side_shift.param.yaml → ..._planner/side_shift/side_shift.param.yaml b/..._planner/side_shift/side_shift.param.yaml → ..._planner/side_shift/side_shift.param.yaml
@@ -6,4 +6,3 @@
       shifting_lateral_jerk: 0.2
       min_shifting_distance: 5.0
       min_shifting_speed: 5.56
-      shift_request_time_limit: 1.0