Changes to fix planning error in narrow corners

autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml

@@ -11,20 +11,21 @@
     curvature_calculation_distance: 2.0         # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m]
     # lateral acceleration limit parameters
     enable_lateral_acc_limit: true     # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime.
-    max_lateral_accel: 1.0             # max lateral acceleration limit [m/ss]
-    min_curve_velocity: 2.0            # min velocity at lateral acceleration limit and steering angle rate limit [m/s]
+    max_lateral_accel: 2.0             # max lateral acceleration limit [m/ss] #SJ default 1.0  #slightly increased it to be able to take tighter curves/avoiding action. However, it is above the possible maximum.
+    min_curve_velocity: 0.25            # min velocity at lateral acceleration limit and steering angle rate limit [m/s] #SJ default 2.0 #The lateral acceleration limit and max steering angle rate will only be applied at velocities above this.
     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]
     # steering angle rate limit parameters
     enable_steering_rate_limit: true         # To toggle the steer rate filter on and off. You can switch it dynamically at runtime.
-    max_steering_angle_rate: 11.5            # maximum steering angle rate [degree/s]
+    max_steering_angle_rate: 10.0            # maximum steering angle rate [degree/s]
+    #reduced it to be within the planning validators limit, or it will trip off the validator. 
     resample_ds: 0.1                         # distance between trajectory points [m]
-    curvature_threshold: 0.02                # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
+    curvature_threshold: 0.05                # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] #SJ default 0.02 #The default curvature threshold value in the start planner shft pullout is 0.07. Might get in the way of that.
 
     # engage & replan parameters
     replan_vel_deviation: 5.53         # velocity deviation to replan initial velocity [m/s]
-    engage_velocity: 0.15 # 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
+    engage_velocity: 0.25 #0.15 # 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) #SJ set it back to default, because vehicle wheels wont turn if lower in the sim.
     engage_acceleration: 0.5           # engage acceleration [m/ss] (use this acceleration when engagement)
     engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
     stop_dist_to_prohibit_engage: 0.2  # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
@@ -34,8 +35,9 @@
     stopping_distance: 1.2    # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied.
 
     # path extraction parameters
-    extract_ahead_dist: 200.0         # forward trajectory distance used for planning [m]
-    extract_behind_dist: 5.0          # backward trajectory distance used for planning [m]
+    extract_ahead_dist: 30.0         # forward trajectory distance used for planning [m] #SJ default 200.00  #Set it to the distance planned by the behaviour_path_planner. No use smoothing out velocity for a path that is not planned.
+    extract_behind_dist: 2.5          # backward trajectory distance used for planning [m] #SJ default 5.0 #IMPORTANT change - The higher the value, the larger the arc length of the smoothed out curve. 
+                                      # Higher values mean any maneuvers will have a much smoother and shallower curve. Lower values mean the curve arc length is smaller and can have a smaller radius. 
     delta_yaw_threshold: 1.0472       # Allowed delta yaw between ego pose and trajectory pose [radian]
 
     # resampling parameters for optimization

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -271,7 +271,7 @@
         # lateral constraints
         lateral:
           velocity: [1.0, 1.38, 11.1]                   # [m/s]
-          max_accel_values: [10.5, 10.5, 10.5]             # [m/ss] |default all 0.5 YH
+          max_accel_values: [2.0, 2.0, 2.0]             # [m/ss] |default all 0.5 YH all 2.0 |SJ #SJ reduced it to a lower value to stop possible unfollowable paths from being planned
           min_jerk_values: [0.2, 0.2, 0.2]              # [m/sss] |default all 0.2 YH
           max_jerk_values: [10.0, 10.0, 10.0]              # [m/sss] |default all 1.0 YH
 

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml

@@ -5,7 +5,7 @@
     traffic_light_signal_timeout: 1.0
 
     planning_hz: 10.0
-    backward_path_length: 5.0
+    backward_path_length: 0.5 #SJ default 5.0 #Reduced backward path length that is planned - can cause noodle like path to be planned. 
     forward_path_length: 30.0 #default 300 YH
     backward_length_buffer_for_end_of_pull_over: 5.0
     backward_length_buffer_for_end_of_pull_out: 5.0

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml

@@ -183,7 +183,7 @@
           # hysteresis factor to expand/shrink polygon with the value
           hysteresis_factor_expand_rate: 1.0
           # temporary
-          backward_path_length: 10.0
+          backward_path_length: 0.5 #SJ default 10.0 #Reduced backward path length that is planned - can cause noodle like path to be planned. 
           forward_path_length: 50.0
 
       # debug

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml

@@ -18,7 +18,7 @@
         check_motorcycle: true
         check_pedestrian: true
         check_unknown: true
-      th_distance_to_middle_of_the_road: 0.0 #default 0.5 YH
+      th_distance_to_middle_of_the_road: 0.00 #default 0.5 YH #SJ- just to make sure added another decimal point.
       center_line_path_interval: 1.0
       # shift pull out
       enable_shift_pull_out: true
@@ -31,17 +31,17 @@
       lateral_jerk: 0.5
       lateral_acceleration_sampling_num: 3
       minimum_lateral_acc: 0.15
-      maximum_lateral_acc: 0.5
+      maximum_lateral_acc: 2.0 #SJ default 0.5 #SJ slightly increased it to allow for a tighter pull out if stopped in narrow places.
       maximum_curvature: 0.07
       # geometric pull out
       enable_geometric_pull_out: true
       geometric_collision_check_distance_from_end: 0.0
       divide_pull_out_path: true
-      geometric_pull_out_velocity: 1.0
-      arc_path_interval: 1.0
+      geometric_pull_out_velocity: 0.5 #SJ default 1.0 #Reduced it to ParcelPal  specs
+      arc_path_interval: 0.5 #SJ default 1.0 #Reduced arc length since our vehicle has a shorter wheelbase compared to a normal caar.
       lane_departure_margin: 0.2
-      backward_velocity: -1.0
-      pull_out_max_steer_angle: 0.50  # 15deg #default 0.26 YH
+      backward_velocity: -0.5 #SJ default -1.0 #Reduced it to ParcelPal specs
+      pull_out_max_steer_angle: 0.70 #SJ default 0.26 YH 0.50 #Increased it to ParcelPal specs
       # search start pose backward
       enable_back: true
       search_priority: "efficient_path"  # "efficient_path" or "short_back_distance"

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     forward_path_length: 100.0
-    backward_path_length: 5.0
+    backward_path_length: 0.5 #SJ default 5.0 #Reduced backward path length that is planned - can cause noodle like path to be planned. 
     behavior_output_path_interval: 1.0
     stop_line_extend_length: 5.0
     max_accel: -2.8

autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml

@@ -18,7 +18,8 @@
     common:
       # output
       output_delta_arc_length: 0.5     #  delta arc length for output trajectory [m]
-      output_backward_traj_length: 5.0  # backward length for backward trajectory from base_link [m]
+      output_backward_traj_length: 2.5  # backward length for backward trajectory from base_link [m] #SJ default 5.0 #IMPORTANT change - The higher the value, the larger the arc length of the smoothed out curve. 
+                                      # Higher values mean any maneuvers will have a much smoother and shallower curve. Lower values mean the curve arc length is smaller and can have a smaller radius. 
 
       vehicle_stop_margin_outside_drivable_area: 0.0 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] .
                                                      # This margin will be realized with delta_arc_length_for_mpt_points m precision.

autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml

@@ -3,7 +3,8 @@
     common:
       # output
       output_delta_arc_length: 0.5     #  delta arc length for output trajectory [m]
-      output_backward_traj_length: 5.0  # backward length for backward trajectory from base_link [m]
+      output_backward_traj_length: 2.5  # backward length for backward trajectory from base_link [m] #SJ default 5.0 #IMPORTANT change - The higher the value, the larger the arc length of the smoothed out curve. 
+                                      # Higher values mean any maneuvers will have a much smoother and shallower curve. Lower values mean the curve arc length is smaller and can have a smaller radius. 
     # elastic band
     elastic_band:
       option: