diff --git a/autoware_launch/config/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml
index ebdf2828fc..4cc71e0904 100644
--- a/autoware_launch/config/localization/ekf_localizer.param.yaml
+++ b/autoware_launch/config/localization/ekf_localizer.param.yaml
@@ -1,37 +1,46 @@
/**:
ros__parameters:
- show_debug_info: false
- enable_yaw_bias_estimation: true
- predict_frequency: 50.0
- tf_rate: 50.0
- publish_tf: true
- extend_state_step: 50
+ node:
+ show_debug_info: false
+ enable_yaw_bias_estimation: true
+ predict_frequency: 50.0
+ tf_rate: 50.0
+ publish_tf: true
+ extend_state_step: 50
- # for Pose measurement
- pose_additional_delay: 0.0
- pose_measure_uncertainty_time: 0.01
- pose_smoothing_steps: 5
- pose_gate_dist: 10000.0
+ pose_measurement:
+ # for Pose measurement
+ pose_additional_delay: 0.0
+ pose_measure_uncertainty_time: 0.01
+ pose_smoothing_steps: 5
+ pose_gate_dist: 10000.0
- # for twist measurement
- twist_additional_delay: 0.0
- twist_smoothing_steps: 2
- twist_gate_dist: 10000.0
+ twist_measurement:
+ # for twist measurement
+ twist_additional_delay: 0.0
+ twist_smoothing_steps: 2
+ twist_gate_dist: 10000.0
- # for process model
- proc_stddev_yaw_c: 0.005
- proc_stddev_vx_c: 10.0
- proc_stddev_wz_c: 5.0
+ process_noise:
+ # for process model
+ proc_stddev_yaw_c: 0.005
+ proc_stddev_vx_c: 10.0
+ proc_stddev_wz_c: 5.0
- #Simple1DFilter parameters
- z_filter_proc_dev: 1.0
- roll_filter_proc_dev: 0.01
- pitch_filter_proc_dev: 0.01
- # for diagnostics
- pose_no_update_count_threshold_warn: 50
- pose_no_update_count_threshold_error: 100
- twist_no_update_count_threshold_warn: 50
- twist_no_update_count_threshold_error: 100
+ simple_1d_filter_parameters:
+ #Simple1DFilter parameters
+ z_filter_proc_dev: 1.0
+ roll_filter_proc_dev: 0.01
+ pitch_filter_proc_dev: 0.01
- # for velocity measurement limitation (Set 0.0 if you want to ignore)
- threshold_observable_velocity_mps: 0.0 # [m/s]
+ diagnostics:
+ # for diagnostics
+ pose_no_update_count_threshold_warn: 50
+ pose_no_update_count_threshold_error: 100
+ twist_no_update_count_threshold_warn: 50
+ twist_no_update_count_threshold_error: 100
+
+ misc:
+ # for velocity measurement limitation (Set 0.0 if you want to ignore)
+ threshold_observable_velocity_mps: 0.0 # [m/s]
+ pose_frame_id: "map"
diff --git a/autoware_launch/config/localization/pose_initializer.param.yaml b/autoware_launch/config/localization/pose_initializer.param.yaml
index a05cc7c35c..88acbfb526 100644
--- a/autoware_launch/config/localization/pose_initializer.param.yaml
+++ b/autoware_launch/config/localization/pose_initializer.param.yaml
@@ -2,6 +2,11 @@
ros__parameters:
gnss_pose_timeout: 3.0 # [sec]
stop_check_duration: 3.0 # [sec]
+ ekf_enabled: $(var ekf_enabled)
+ gnss_enabled: $(var gnss_enabled)
+ yabloc_enabled: $(var yabloc_enabled)
+ ndt_enabled: $(var ndt_enabled)
+ stop_check_enabled: $(var stop_check_enabled)
# from gnss
gnss_particle_covariance:
diff --git a/autoware_launch/config/map/map_tf_generator.param.yaml b/autoware_launch/config/map/map_tf_generator.param.yaml
new file mode 100644
index 0000000000..90790808ac
--- /dev/null
+++ b/autoware_launch/config/map/map_tf_generator.param.yaml
@@ -0,0 +1,4 @@
+/**:
+ ros__parameters:
+ map_frame: map
+ viewer_frame: viewer
diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml
index 0b29a87965..a9c3174846 100644
--- a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml
+++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml
@@ -13,3 +13,14 @@
iou_nms_search_distance_2d: 10.0
iou_nms_threshold: 0.1
yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
+ score_threshold: 0.35
+ has_twist: false
+ trt_precision: fp16
+ densification_num_past_frames: 1
+ densification_world_frame_id: map
+
+ # weight files
+ encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"
+ encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine"
+ head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
+ head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"
diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_tiny.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_tiny.param.yaml
index 8252fde827..5eb560dd7b 100644
--- a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_tiny.param.yaml
+++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_tiny.param.yaml
@@ -13,3 +13,15 @@
iou_nms_search_distance_2d: 10.0
iou_nms_threshold: 0.1
yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
+ score_threshold: 0.35
+ build_only: false # shutdown node after TensorRT engine file is built
+ has_twist: false
+ trt_precision: fp16
+ densification_num_past_frames: 1
+ densification_world_frame_id: map
+
+ # weight files
+ encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"
+ encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine"
+ head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
+ head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"
diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/pointpainting.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/pointpainting.param.yaml
index 8fb88da291..8ccd469786 100755
--- a/autoware_launch/config/perception/object_recognition/detection/lidar_model/pointpainting.param.yaml
+++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/pointpainting.param.yaml
@@ -1,7 +1,6 @@
/**:
ros__parameters:
trt_precision: fp16
- build_only: false
encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"
encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine"
head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
diff --git a/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml
index fdd64174de..ed1193f1c4 100644
--- a/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml
+++ b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml
@@ -19,6 +19,7 @@
use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not
speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value
acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds
+ use_crosswalk_signal: true
# parameter for shoulder lane prediction
prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml
index 3512d722ec..69328a9b79 100644
--- a/autoware_launch/config/planning/preset/default_preset.yaml
+++ b/autoware_launch/config/planning/preset/default_preset.yaml
@@ -9,6 +9,9 @@ launch:
- arg:
name: launch_dynamic_avoidance_module
default: "false"
+ - arg:
+ name: launch_sampling_planner_module
+ default: "false" # Warning, experimental module, use only in simulations
- arg:
name: launch_lane_change_right_module
default: "true"
diff --git a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml
index 235c76a5c1..4b6693f150 100644
--- a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml
@@ -9,17 +9,17 @@
# -- curve parameters --
# common parameters
- curvature_calculation_distance: 5.0 # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m]
+ 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.74 # min velocity at lateral acceleration limit and steering angle rate limit [m/s]
+ min_curve_velocity: 2.0 # 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]
# 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: 40.0 # maximum steering angle rate [degree/s]
+ max_steering_angle_rate: 11.5 # maximum steering angle rate [degree/s]
resample_ds: 0.1 # distance between trajectory points [m]
curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml
new file mode 100644
index 0000000000..9a6b30c219
--- /dev/null
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml
@@ -0,0 +1,40 @@
+/**:
+ ros__parameters:
+ common:
+ output_delta_arc_length: 0.5 # [m] delta arc length for output trajectory
+
+ debug:
+ enable_calculation_time_info: false # flag to print calculation times
+ id: 0 # id of the candidate paths for which to print/show details (e.g., footprint in rviz)
+
+ preprocessing:
+ force_zero_initial_deviation: True # if true, initial planning starts from the reference path
+ force_zero_initial_heading: True # if true, initial planning starts with a heading aligned with the reference path
+ smooth_reference_trajectory: False # if true, the reference trajectory is smoothed before being used for planning
+ constraints:
+ hard:
+ max_curvature: 3.0 # [m⁻¹] maximum curvature of a sampled path
+ min_curvature: -3.0 # [m⁻¹] minimum curvature of a sampled path
+ soft:
+ lateral_deviation_weight: 1.0 # cost weight for lateral deviation between the end of a sampled path and the reference path
+ length_weight: 1.0 # cost weight for the length of a sampled path
+ curvature_weight: 2000.0 # cost weight for the curvature of a sampled path
+ weights: [0.5,1.0,20.0]
+ sampling:
+ enable_frenet: True
+ enable_bezier: False
+ resolution: 0.5 # [m] target distance between sampled path points
+ previous_path_reuse_points_nb: 2 # number of points reused from the previously generated path (0:no reuse, 1:replan from end of prev path, 2: end and mid of prev path, etc)
+ target_lengths: [20.0, 40.0] # [m] target lengths of the sampled paths
+ nb_target_lateral_positions: 0 # number of lateral positions to use for sampling (in addition to 0.0 and the current lateral deviation)
+ target_lateral_positions: [-4.5,-2.5, 0.0, 2.5,4.5] # manual values that are only used if nb_target_lateral_positions = 0
+ frenet: # target values for the sampled "lateral deviation over longitudinal position" polynomial
+ target_lateral_velocities: [-0.5, 0.0, 0.5]
+ target_lateral_accelerations: [0.0]
+ # bezier:
+ # nb_k: 3 # number of sampled curvature values
+ # mk_min: 0.0 # minimum curvature value
+ # mk_max: 10.0 # maximum curvature value
+ # nb_t: 5 # number of sampled acceleration values
+ # mt_min: 0.3 # minimum acceleration value
+ # mt_max: 1.7 # maximum acceleration value
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml
index 9853e220bc..8d4fe057b4 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml
@@ -82,3 +82,12 @@
keep_last: true
priority: 7
max_module_size: 1
+
+ sampling_planner:
+ enable_module: true
+ enable_rtc: false
+ enable_simultaneous_execution_as_approved_module: false
+ enable_simultaneous_execution_as_candidate_module: false
+ keep_last: false
+ priority: 16
+ max_module_size: 1
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
index 1ac32ba562..2ff87e9b01 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
@@ -95,7 +95,12 @@
obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s]
max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
-
+ yield:
+ enable_yield: false
+ lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding
+ max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it
+ max_obstacles_collision_time: 10.0 # how far the blocking obstacle
+ stopped_obstacle_velocity_threshold: 0.5
slow_down:
max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width
lat_hysteresis_margin: 0.2
diff --git a/autoware_launch/launch/components/tier4_map_component.launch.xml b/autoware_launch/launch/components/tier4_map_component.launch.xml
index 455b848c63..ba5611b6c0 100644
--- a/autoware_launch/launch/components/tier4_map_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_map_component.launch.xml
@@ -8,6 +8,7 @@
+
diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml
index a84911a414..3519d5780d 100644
--- a/autoware_launch/launch/components/tier4_perception_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml
@@ -14,7 +14,6 @@
-
@@ -156,6 +155,11 @@
+
+
+
+
+
diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml
index 4608427134..3d57f6198e 100644
--- a/autoware_launch/launch/components/tier4_planning_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml
@@ -26,6 +26,7 @@
+