Merge pull request #656 from tier4/sync-awf-latest

chore: sync awf-latest

autoware_launch/launch/autoware.launch.xml

@@ -119,9 +119,12 @@
 
   <!-- Control -->
   <group if="$(var launch_control)">
-    <include file="$(find-pkg-share control_launch)/launch/control.launch.xml">
-      <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
-      <arg name="lateral_controller_mode" value="mpc_follower"/>
+    <include file="$(find-pkg-share control_launch)/launch/control.launch.py">
+      <!-- options for lateral_controller_mode: mpc, pure_pursuit -->
+      <arg name="lateral_controller_mode" value="mpc"/>
+      <arg name="longitudinal_controller_mode" value="pid"/>
+      <arg name="lat_controller_param_path" value="$(find-pkg-share control_launch)/config/trajectory_follower/lateral/$(var lateral_controller_mode).param.yaml"/>
+      <arg name="lon_controller_param_path" value="$(find-pkg-share control_launch)/config/trajectory_follower/longitudinal/$(var longitudinal_controller_mode).param.yaml"/>
       <arg name="check_external_emergency_heartbeat" value="$(var check_external_emergency_heartbeat)"/>
     </include>
   </group>

control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    ctrl_period: 0.03
+    timeout_thr_sec: 0.5

control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml

@@ -12,9 +12,11 @@
       lon_jerk_lim: 5.0
       lat_acc_lim: 5.0
       lat_jerk_lim: 5.0
+      actual_steer_diff_lim: 1.0
     on_transition:
       vel_lim: 50.0
       lon_acc_lim: 1.0
       lon_jerk_lim: 0.5
       lat_acc_lim: 1.2
       lat_jerk_lim: 0.75
+      actual_steer_diff_lim: 0.05

control_launch/launch/control.launch.py

@@ -39,6 +39,11 @@ def launch_setup(context, *args, **kwargs):
     lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context)
     with open(lon_controller_param_path, "r") as f:
         lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    trajectory_follower_node_param_path = LaunchConfiguration(
+        "trajectory_follower_node_param_path"
+    ).perform(context)
+    with open(trajectory_follower_node_param_path, "r") as f:
+        trajectory_follower_node_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform(
         context
     )
@@ -61,8 +66,8 @@ def launch_setup(context, *args, **kwargs):
         shift_decider_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
     controller_component = ComposableNode(
-        package="trajectory_follower_nodes",
-        plugin="autoware::motion::control::trajectory_follower_nodes::Controller",
+        package="trajectory_follower_node",
+        plugin="autoware::motion::control::trajectory_follower_node::Controller",
         name="controller_node_exe",
         namespace="trajectory_follower",
         remappings=[
@@ -83,6 +88,7 @@ def launch_setup(context, *args, **kwargs):
                 "longitudinal_controller_mode": LaunchConfiguration("longitudinal_controller_mode"),
             },
             nearest_search_param,
+            trajectory_follower_node_param,
             lon_controller_param,
             lat_controller_param,
         ],
@@ -208,7 +214,12 @@ def launch_setup(context, *args, **kwargs):
         launch_arguments=[
             ("use_intra_process", LaunchConfiguration("use_intra_process")),
             ("target_container", "/control/control_container"),
-            ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")),
+            # This is temporary uncomment. I will replace control_launch with tier4_control_launch soon.
+            # ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")),
+            (
+                "external_cmd_selector_param_path",
+                LaunchConfiguration("external_cmd_selector_param_path"),
+            ),
         ],
     )
 
@@ -260,8 +271,8 @@ def add_launch_arg(name: str, default_value=None, description=None):
     # lateral controller mode
     add_launch_arg(
         "lateral_controller_mode",
-        "mpc_follower",
-        "lateral controller mode: `mpc_follower` or `pure_pursuit`",
+        "mpc",
+        "lateral controller mode: `mpc` or `pure_pursuit`",
     )
 
     # longitudinal controller mode
@@ -284,15 +295,23 @@ def add_launch_arg(name: str, default_value=None, description=None):
         "lat_controller_param_path",
         [
             FindPackageShare("control_launch"),
-            "/config/trajectory_follower/mpc_follower.param.yaml",
+            "/config/trajectory_follower/lateral/mpc.param.yaml",
         ],
         "path to the parameter file of lateral controller. default is `mpc_follower`",
     )
     add_launch_arg(
         "lon_controller_param_path",
         [
             FindPackageShare("control_launch"),
-            "/config/trajectory_follower/longitudinal_controller.param.yaml",
+            "/config/trajectory_follower/longitudinal/pid.param.yaml",
+        ],
+        "path to the parameter file of longitudinal controller",
+    )
+    add_launch_arg(
+        "trajectory_follower_node_param_path",
+        [
+            FindPackageShare("control_launch"),
+            "/config/trajectory_follower/trajectory_follower_node.param.yaml",
         ],
         "path to the parameter file of longitudinal controller",
     )
@@ -332,6 +351,13 @@ def add_launch_arg(name: str, default_value=None, description=None):
 
     # external cmd selector
     add_launch_arg("initial_selector_mode", "remote", "local or remote")
+    add_launch_arg(
+        "external_cmd_selector_param_path",
+        [
+            FindPackageShare("external_cmd_selector"),
+            "/config/external_cmd_selector.param.yaml",
+        ],
+    )
 
     # component
     add_launch_arg("use_intra_process", "false", "use ROS2 component container communication")

control_launch/package.xml

@@ -14,8 +14,7 @@
   <exec_depend>external_cmd_converter</exec_depend>
   <exec_depend>lane_departure_checker</exec_depend>
   <exec_depend>shift_decider</exec_depend>
-  <exec_depend>trajectory_follower</exec_depend>
-  <exec_depend>trajectory_follower_nodes</exec_depend>
+  <exec_depend>trajectory_follower_node</exec_depend>
   <exec_depend>vehicle_cmd_gate</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -2,63 +2,26 @@
 /**:
   ros__parameters:
     avoidance:
-      resample_interval_for_planning: 0.3
-      resample_interval_for_output: 4.0
-      detection_area_right_expand_dist: 0.0
-      detection_area_left_expand_dist: 1.0
-
+      resample_interval_for_planning: 0.3               # [m]
+      resample_interval_for_output: 4.0                 # [m]
+      detection_area_right_expand_dist: 0.0             # [m]
+      detection_area_left_expand_dist: 1.0              # [m]
+      drivable_area_right_bound_offset: 0.0             # [m]
+      drivable_area_left_bound_offset: 0.0              # [m]
+      object_envelope_buffer: 0.3                       # [m]
+
+      # avoidance module common setting
+      enable_bound_clipping: false
       enable_avoidance_over_same_direction: true
       enable_avoidance_over_opposite_direction: true
-
-      # For target object filtering
-      threshold_speed_object_is_stopped: 1.0      # [m/s]
-      threshold_time_object_is_moving: 1.0        # [s]
-
-      object_check_forward_distance: 150.0        # [m]
-      object_check_backward_distance: 2.0         # [m]
-
-      threshold_distance_object_is_on_center: 1.0 # [m]
-
-      object_check_shiftable_ratio: 0.6           # [-]
-      object_check_min_road_shoulder_width: 0.5   # [m]
-
-      # For lateral margin
-      object_envelope_buffer: 0.3                 # [m]
-      lateral_collision_margin: 1.0               # [m]
-      lateral_collision_safety_buffer: 0.7        # [m]
-
-      prepare_time: 2.0                           # [s]
-      min_prepare_distance: 1.0                   # [m]
-      min_avoidance_distance: 10.0                # [m]
-
-      min_nominal_avoidance_speed: 7.0  # [m/s]
-      min_sharp_avoidance_speed: 1.0    # [m/s]
-
-      road_shoulder_safety_margin: 0.0 # [m]
-
-      max_right_shift_length: 5.0
-      max_left_shift_length: 5.0
-
-      nominal_lateral_jerk: 0.2   # [m/s3]
-      max_lateral_jerk: 1.0       # [m/s3]
-
-      object_last_seen_threshold: 2.0
-
-      # For prevention of large acceleration while avoidance
-      min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
-      max_avoidance_acceleration: 0.5  # [m/ss]
-
-      # bound clipping for objects
-      enable_bound_clipping: false
+      enable_update_path_when_object_is_gone: false
+      enable_safety_check: false
+      enable_yield_maneuver: false
 
       # for debug
       publish_debug_marker: false
       print_debug_info: false
 
-      # not enabled yet
-      longitudinal_collision_margin_min_distance: 0.0          # [m]
-      longitudinal_collision_margin_time: 0.0
-
       # avoidance is performed for the object type with true
       target_object:
         car: true
@@ -70,5 +33,80 @@
         motorcycle: false
         pedestrian: false
 
-      # ---------- advanced parameters ----------
-      avoidance_execution_lateral_threshold: 0.499
+      # For target object filtering
+      target_filtering:
+        # filtering moving objects
+        threshold_speed_object_is_stopped: 1.0          # [m/s]
+        threshold_time_object_is_moving: 1.0            # [s]
+        # detection range
+        object_check_forward_distance: 150.0            # [m]
+        object_check_backward_distance: 2.0             # [m]
+        object_check_goal_distance: 20.0                # [m]
+        # filtering parking objects
+        threshold_distance_object_is_on_center: 1.0     # [m]
+        object_check_shiftable_ratio: 0.6               # [-]
+        object_check_min_road_shoulder_width: 0.5       # [m]
+        # lost object compensation
+        object_last_seen_threshold: 2.0
+
+      # For safety check
+      safety_check:
+        safety_check_backward_distance: 50.0            # [m]
+        safety_check_time_horizon: 10.0                 # [s]
+        safety_check_idling_time: 1.5                   # [s]
+        safety_check_accel_for_rss: 2.5                 # [m/ss]
+        safety_check_hysteresis_factor: 2.0             # [-]
+
+      # For avoidance maneuver
+      avoidance:
+        # avoidance lateral parameters
+        lateral:
+          lateral_collision_margin: 1.0                 # [m]
+          lateral_collision_safety_buffer: 0.7          # [m]
+          lateral_passable_safety_buffer: 0.0           # [m]
+          road_shoulder_safety_margin: 0.0              # [m]
+          avoidance_execution_lateral_threshold: 0.499
+          max_right_shift_length: 5.0
+          max_left_shift_length: 5.0
+        # avoidance distance parameters
+        longitudinal:
+          prepare_time: 2.0                             # [s]
+          longitudinal_collision_safety_buffer: 0.0     # [m]
+          min_prepare_distance: 1.0                     # [m]
+          min_avoidance_distance: 10.0                  # [m]
+          min_nominal_avoidance_speed: 7.0              # [m/s]
+          min_sharp_avoidance_speed: 1.0                # [m/s]
+
+      # For yield maneuver
+      yield:
+        yield_velocity: 2.78                            # [m/s]
+
+      # For stop maneuver
+      stop:
+        min_distance: 10.0                              # [m]
+        max_distance: 20.0                              # [m]
+
+      constraints:
+        # vehicle slows down under longitudinal constraints
+        use_constraints_for_decel: false                # [-]
+
+        # lateral constraints
+        lateral:
+          nominal_lateral_jerk: 0.2                     # [m/s3]
+          max_lateral_jerk: 1.0                         # [m/s3]
+
+        # longitudinal constraints
+        longitudinal:
+          nominal_deceleration: -1.0                    # [m/ss]
+          nominal_jerk: 0.5                             # [m/sss]
+          max_deceleration: -2.0                        # [m/ss]
+          max_jerk: 1.0
+          # For prevention of large acceleration while avoidance
+          min_avoidance_speed_for_acc_prevention: 3.0   # [m/s]
+          max_avoidance_acceleration: 0.5               # [m/ss]
+
+      target_velocity_matrix:
+        col_size: 2
+        matrix:
+          [2.78, 13.9,                                  # velocity [m/s]
+           0.50, 1.00]                                  # margin [m]

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml

@@ -2,10 +2,8 @@
   ros__parameters:
     backward_path_length: 5.0
     forward_path_length: 300.0
-    backward_length_buffer_for_end_of_lane: 5.0
     backward_length_buffer_for_end_of_pull_over: 5.0
     backward_length_buffer_for_end_of_pull_out: 5.0
-    minimum_lane_change_length: 12.0
     minimum_pull_over_length: 16.0
 
     refine_goal_search_radius_range: 7.5
@@ -23,11 +21,11 @@
 
     lateral_distance_max_threshold: 2.0
     longitudinal_distance_min_threshold: 3.0
-    expected_front_deceleration: -0.5
+    expected_front_deceleration: -1.0
     expected_rear_deceleration: -1.0
 
-    expected_front_deceleration_for_abort: -2.0
-    expected_rear_deceleration_for_abort: -2.5
+    expected_front_deceleration_for_abort: -1.0
+    expected_rear_deceleration_for_abort: -2.0
 
     rear_vehicle_reaction_time: 2.0
     rear_vehicle_safety_time_margin: 2.0

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml

@@ -2,18 +2,33 @@
   ros__parameters:
     lane_change:
       lane_change_prepare_duration: 4.0         # [s]
-      lane_changing_duration: 8.0               # [s]
+      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_velocity: 5.6         # [m/s]
+
+      lane_changing_lateral_jerk: 0.5           # [m/s3]
+      lane_changing_lateral_acc: 0.5            # [m/s2]
+
+      minimum_lane_change_velocity: 2.78        # [m/s]
       prediction_time_resolution: 0.5           # [s]
       maximum_deceleration: 1.0                 # [m/s2]
       lane_change_sampling_num: 10
-      abort_lane_change_velocity_thresh: 0.5
-      abort_lane_change_angle_thresh: 10.0      # [deg]
-      abort_lane_change_distance_thresh: 0.3    # [m]
-      enable_abort_lane_change: true
+
+      # collision check
       enable_collision_check_at_prepare_phase: true
+      prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s]
       use_predicted_path_outside_lanelet: true
       use_all_predicted_path: false
+
+      # abort
+      enable_cancel_lane_change: true
+      enable_abort_lane_change: false
+
+      abort_delta_time: 3.0                      # [s]
+      abort_max_lateral_jerk: 1000.0             # [m/s3]
+
+      # debug
       publish_debug_marker: false

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     pull_over:
-      request_length: 200.0
+      request_length: 100.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,7 +17,7 @@
       backward_goal_search_length: 20.0
       goal_search_interval: 2.0
       longitudinal_margin: 3.0
-      max_lateral_offset: 1.0
+      max_lateral_offset: 0.5
       lateral_offset_interval: 0.25
       # occupancy grid map
       use_occupancy_grid: true
@@ -34,7 +34,7 @@
       maximum_lateral_jerk: 2.0
       minimum_lateral_jerk: 0.5
       deceleration_interval: 15.0
-      after_pull_over_straight_distance: 5.0
+      after_pull_over_straight_distance: 1.0
       # parallel parking path
       enable_arc_forward_parking: true
       enable_arc_backward_parking: true

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml

@@ -6,3 +6,4 @@
       backward_length: 15.0 # [m]
       ignore_width_from_center_line: 0.7 # [m]
       max_future_movement_time: 10.0 # [second]
+      threshold_yaw_diff: 0.523 # [rad]