diff --git a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml
index 1a6c26cd9c..9bc62d3f91 100644
--- a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml
+++ b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml
@@ -1,8 +1,5 @@
/**:
ros__parameters:
- # Use dynamic map loading
- use_dynamic_map_loading: true
-
# Vehicle reference frame
base_frame: "base_link"
diff --git a/autoware_launch/config/map/pointcloud_map_loader.param.yaml b/autoware_launch/config/map/pointcloud_map_loader.param.yaml
index ba4c032d3e..b4efbec970 100644
--- a/autoware_launch/config/map/pointcloud_map_loader.param.yaml
+++ b/autoware_launch/config/map/pointcloud_map_loader.param.yaml
@@ -3,7 +3,6 @@
enable_whole_load: true
enable_downsampled_whole_load: false
enable_partial_load: true
- enable_differential_load: true
enable_selected_load: false
# only used when downsample_whole_load enabled
diff --git a/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_sync.param.yaml b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_sync.param.yaml
index 0e4c52ba92..99d85089be 100644
--- a/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_sync.param.yaml
+++ b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_sync.param.yaml
@@ -3,6 +3,8 @@
input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0]
timeout_ms: 70.0
match_threshold_ms: 50.0
+ image_buffer_size: 15
+ debug_mode: false
filter_scope_min_x: -100.0
filter_scope_min_y: -100.0
filter_scope_min_z: -100.0
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 e1be5426cb..21d31f2163 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,18 +1,26 @@
/**:
ros__parameters:
- class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
- paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"]
- point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle
- max_voxel_size: 40000
- point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0]
- voxel_size: [0.32, 0.32, 8.0]
- downsample_factor: 1
- encoder_in_feature_size: 12
- yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
- # post-process params
- circle_nms_dist_threshold: 0.3
- iou_nms_target_class_names: ["CAR"]
- iou_nms_search_distance_2d: 10.0
- iou_nms_threshold: 0.1
- # omp params
- omp_num_threads: 1
+ model_params:
+ class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
+ paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"]
+ point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle
+ max_voxel_size: 40000
+ point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0]
+ voxel_size: [0.32, 0.32, 8.0]
+ downsample_factor: 1
+ encoder_in_feature_size: 12
+ yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
+ has_twist: false
+ densification_params:
+ world_frame_id: "map"
+ num_past_frames: 0
+ post_process_params:
+ # post-process params
+ circle_nms_dist_threshold: 0.3
+ iou_nms_target_class_names: ["CAR"]
+ iou_nms_search_distance_2d: 10.0
+ iou_nms_threshold: 0.1
+ score_threshold: 0.4
+ omp_params:
+ # omp params
+ num_threads: 1
diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml
index 7bd3dd26b3..3df13a108d 100644
--- a/autoware_launch/config/planning/preset/default_preset.yaml
+++ b/autoware_launch/config/planning/preset/default_preset.yaml
@@ -77,6 +77,9 @@ launch:
- arg:
name: launch_no_drivable_lane_module
default: "false"
+ - arg:
+ name: launch_dynamic_obstacle_stop_module
+ default: "true"
# motion planning modules
- arg:
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml
index 29fdca78e5..0a563498be 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml
@@ -38,7 +38,7 @@
crossing_object:
min_overtaking_object_vel: 1.0
max_overtaking_object_angle: 1.05
- min_oncoming_object_vel: 0.0
+ min_oncoming_object_vel: 1.0
max_oncoming_object_angle: 0.523
front_object:
@@ -54,20 +54,24 @@
object_path_base:
min_longitudinal_polygon_margin: 3.0 # [m]
- lat_offset_from_obstacle: 0.8 # [m]
+ lat_offset_from_obstacle: 1.0 # [m]
max_lat_offset_to_avoid: 0.5 # [m]
max_time_for_object_lat_shift: 0.0 # [s]
lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]
+ max_ego_lat_acc: 0.3 # [m/ss]
+ max_ego_lat_jerk: 0.3 # [m/sss]
+ delay_time_ego_shift: 1.0 # [s]
+
# for same directional object
overtaking_object:
max_time_to_collision: 40.0 # [s]
- start_duration_to_avoid: 2.0 # [s]
- end_duration_to_avoid: 4.0 # [s]
+ start_duration_to_avoid: 1.0 # [s]
+ end_duration_to_avoid: 1.0 # [s]
duration_to_hold_avoidance: 3.0 # [s]
# for opposite directional object
oncoming_object:
max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles
- start_duration_to_avoid: 12.0 # [s]
+ start_duration_to_avoid: 1.0 # [s]
end_duration_to_avoid: 0.0 # [s]
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml
index 366c093901..57899fada8 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml
@@ -57,14 +57,14 @@
# lane expansion for object filtering
lane_expansion:
- left_offset: 0.0 # [m]
- right_offset: 0.0 # [m]
+ left_offset: 1.0 # [m]
+ right_offset: 1.0 # [m]
# lateral acceleration map
lateral_acceleration:
velocity: [0.0, 4.0, 10.0]
- min_values: [0.15, 0.15, 0.15]
- max_values: [0.5, 0.5, 0.5]
+ min_values: [0.4, 0.4, 0.4]
+ max_values: [0.65, 0.65, 0.65]
# target object
target_object:
@@ -72,7 +72,7 @@
truck: true
bus: true
trailer: true
- unknown: true
+ unknown: false
bicycle: true
motorcycle: true
pedestrian: true
@@ -80,14 +80,14 @@
# collision check
enable_prepare_segment_collision_check: false
prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s]
- check_objects_on_current_lanes: true
- check_objects_on_other_lanes: true
+ check_objects_on_current_lanes: false
+ check_objects_on_other_lanes: false
use_all_predicted_path: true
# lane change regulations
regulation:
- crosswalk: false
- intersection: false
+ crosswalk: true
+ intersection: true
traffic_light: true
# ego vehicle stuck detection
@@ -107,4 +107,4 @@
finish_judge_lateral_threshold: 0.2 # [m]
# debug
- publish_debug_marker: false
+ publish_debug_marker: true
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml
index aa51c38b55..b31506918a 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml
@@ -2,6 +2,7 @@
ros__parameters:
forward_path_length: 1000.0
backward_path_length: 5.0
+ behavior_output_path_interval: 1.0
stop_line_extend_length: 5.0
max_accel: -2.8
max_jerk: -5.0
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/dynamic_obstacle_stop.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/dynamic_obstacle_stop.param.yaml
new file mode 100644
index 0000000000..14483093e8
--- /dev/null
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/dynamic_obstacle_stop.param.yaml
@@ -0,0 +1,10 @@
+/**:
+ ros__parameters:
+ dynamic_obstacle_stop: # module to stop or before entering the immediate path of a moving object
+ extra_object_width: 1.0 # [m] extra width around detected objects
+ minimum_object_velocity: 0.5 # [m/s] objects with a velocity bellow this value are ignored
+ stop_distance_buffer: 0.5 # [m] extra distance to add between the stop point and the collision point
+ time_horizon: 5.0 # [s] time horizon used for collision checks
+ hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection
+ decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled
+ minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision
diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml
index 49156fba51..3a968f0f27 100644
--- a/autoware_launch/launch/components/tier4_perception_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml
@@ -127,8 +127,9 @@
-
+
-
+
+
diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml
index 5fb17541cd..06740146f5 100644
--- a/autoware_launch/launch/components/tier4_planning_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml
@@ -8,6 +8,7 @@
+
@@ -49,6 +50,7 @@
+
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index bc22eadb70..a0ade56821 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -397,6 +397,7 @@ Visualization Manager:
shoulder_road_lanelets: false
traffic_light: true
traffic_light_id: false
+ traffic_light_reg_elem_id: false
traffic_light_triangle: true
walkway_lanelets: true
hatched_road_markings_bound: true
@@ -1676,6 +1677,18 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_drivable_lane
Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: VirtualWall (DynamicObstacleStop)
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/dynamic_obstacle_stop
+ Value: true
Enabled: true
Name: VirtualWall
- Class: rviz_common/Group
@@ -1944,6 +1957,18 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_drivable_lane
Value: false
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: false
+ Name: DynamicObstacleStop
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/dynamic_obstacle_stop
+ Value: false
Enabled: false
Name: DebugMarker
- Class: rviz_common/Group