From a0a1d9524f01434c28417c0eb09e67d93424f511 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 13 Apr 2023 02:59:23 +0000 Subject: [PATCH 1/7] chore: sync files (#292) Signed-off-by: GitHub Co-authored-by: shmpwk --- .github/PULL_REQUEST_TEMPLATE/small-change.md | 6 ++++++ .github/PULL_REQUEST_TEMPLATE/standard-change.md | 8 ++++++++ 2 files changed, 14 insertions(+) diff --git a/.github/PULL_REQUEST_TEMPLATE/small-change.md b/.github/PULL_REQUEST_TEMPLATE/small-change.md index 527c8ed81f..2ff933c43a 100644 --- a/.github/PULL_REQUEST_TEMPLATE/small-change.md +++ b/.github/PULL_REQUEST_TEMPLATE/small-change.md @@ -9,6 +9,12 @@ Not applicable. +## Effects on system behavior + + + +Not applicable. + ## Pre-review checklist for the PR author The PR author **must** check the checkboxes below when creating the PR. diff --git a/.github/PULL_REQUEST_TEMPLATE/standard-change.md b/.github/PULL_REQUEST_TEMPLATE/standard-change.md index cfdf7101b5..7aedefd0a7 100644 --- a/.github/PULL_REQUEST_TEMPLATE/standard-change.md +++ b/.github/PULL_REQUEST_TEMPLATE/standard-change.md @@ -14,6 +14,14 @@ +## Interface changes + + + +## Effects on system behavior + + + ## Pre-review checklist for the PR author The PR author **must** check the checkboxes below when creating the PR. From 9f677fc94f867277bf89febfe872322dac871165 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 13 Apr 2023 20:42:42 +0900 Subject: [PATCH 2/7] feat(avoidance): margin can be set independently for each class (#286) Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 51 +++++++++++++++---- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 35ac377924..0183c227bc 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -8,7 +8,6 @@ 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 @@ -26,14 +25,46 @@ # avoidance is performed for the object type with true target_object: - car: true - truck: true - bus: true - trailer: true - unknown: false - bicycle: false - motorcycle: false - pedestrian: false + car: + enable: true + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + truck: + enable: true + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + bus: + enable: true + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + trailer: + enable: true + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + unknown: + enable: false + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + bicycle: + enable: false + envelope_buffer_margin: 0.8 + safety_buffer_lateral: 1.0 + safety_buffer_longitudinal: 1.0 + motorcycle: + enable: false + envelope_buffer_margin: 0.8 + safety_buffer_lateral: 1.0 + safety_buffer_longitudinal: 1.0 + pedestrian: + enable: false + envelope_buffer_margin: 0.8 + safety_buffer_lateral: 1.0 + safety_buffer_longitudinal: 1.0 # For target object filtering target_filtering: @@ -66,7 +97,6 @@ # avoidance lateral parameters lateral: lateral_collision_margin: 1.0 # [m] - lateral_collision_safety_buffer: 0.7 # [m] lateral_passable_safety_buffer: 1.0 # [m] road_shoulder_safety_margin: 0.3 # [m] avoidance_execution_lateral_threshold: 0.499 @@ -75,7 +105,6 @@ # 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] From 6b97c8c3679b3ae31a74c51c442438c90f23c5b7 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 14 Apr 2023 10:21:24 +0900 Subject: [PATCH 3/7] feat(rviz): add rough goal (#295) Signed-off-by: kosuke55 --- autoware_launch/rviz/autoware.rviz | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 9148a67b52..25808ea688 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2141,6 +2141,13 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/mission_planning/goal + - Class: tier4_adapi_rviz_plugins::RouteTool + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz/routing/rough_goal - Class: rviz_plugins/PedestrianInitialPoseTool Pose Topic: /simulation/dummy_perception_publisher/object_info Theta std deviation: 0.0872664600610733 From d4eda8d931f07d46aaf01fe898c4eaf27ebbbbb8 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 14 Apr 2023 15:17:14 +0900 Subject: [PATCH 4/7] refactor(behavior_velocity_planner): removed external input from behavior_velocity (#296) removed external input from behavior_velocity Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/crosswalk.param.yaml | 2 -- .../behavior_velocity_planner/intersection.param.yaml | 6 +----- .../behavior_velocity_planner/traffic_light.param.yaml | 1 - 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 444912b60e..2bbc5d31fc 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -29,7 +29,6 @@ ego_yield_query_stop_duration: 0.1 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for input data - external_input_timeout: 1.0 tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal # param for target area & object @@ -43,4 +42,3 @@ walkway: stop_duration_sec: 1.0 # [s] stop time at stop position stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists - external_input_timeout: 1.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 9f6d39189f..ab221e4750 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -22,10 +22,6 @@ assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning enable_front_car_decel_prediction: false # By default this feature is disabled stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection - external_input_timeout: 1.0 - merge_from_private_road: - stop_duration_sec: 1.0 merge_from_private: - merge_from_private_area: - stop_duration_sec: 1.0 + stop_duration_sec: 1.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml index f5db276c9e..444fa5ca65 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml @@ -3,6 +3,5 @@ traffic_light: stop_margin: 0.0 tl_state_timeout: 1.0 - external_tl_state_timeout: 1.0 yellow_lamp_period: 2.75 enable_pass_judge: true From 57e5eda6d42eebe82dfa5a1d5161d08416c9db04 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 14 Apr 2023 16:37:13 +0900 Subject: [PATCH 5/7] refactor(behavior_velocity_planner::intersection): organize param intersection (#297) reorganize intersection param for readability Signed-off-by: Mamoru Sobue --- .../intersection.param.yaml | 47 ++++++++++--------- 1 file changed, 26 insertions(+), 21 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index ab221e4750..8a69d80394 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -1,27 +1,32 @@ /**: ros__parameters: intersection: - state_transit_margin_time: 1.0 - stop_line_margin: 3.0 - keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr - stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) - stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h - intersection_velocity: 2.778 # 2.778m/s = 10.0km/h - intersection_max_accel: 0.5 # m/ss - detection_area_margin: 0.5 # [m] - detection_area_right_margin: 0.5 # [m] - detection_area_left_margin: 0.5 # [m] - detection_area_length: 200.0 # [m] - detection_area_angle_threshold: 0.785 # [rad] - min_predicted_path_confidence: 0.05 - minimum_ego_predicted_velocity: 1.388 # [m/s] - collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object - collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object - use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck - assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning - enable_front_car_decel_prediction: false # By default this feature is disabled - stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection + common: + detection_area_margin: 0.5 # [m] + detection_area_right_margin: 0.5 # [m] + detection_area_left_margin: 0.5 # [m] + detection_area_length: 200.0 # [m] + detection_area_angle_threshold: 0.785 # [rad] + stop_line_margin: 3.0 + intersection_velocity: 2.778 # 2.778m/s = 10.0km/h + intersection_max_accel: 0.5 # m/ss + stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection + + stuck_vehicle: + use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck + stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h + stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. + enable_front_car_decel_prediction: false # By default this feature is disabled + assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning + + collision_detection: + state_transit_margin_time: 1.0 + min_predicted_path_confidence: 0.05 + minimum_ego_predicted_velocity: 1.388 # [m/s] + collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object + collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object + keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr merge_from_private: stop_duration_sec: 1.0 From fbf9607a339408e0cf39bc03f3ad77ea0557247f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 14 Apr 2023 22:09:26 +0900 Subject: [PATCH 6/7] feat(obstacle_cruise_planner): implement slow down planner (#288) * feat(obstacle_cruise_planner): add param for slow down Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../obstacle_cruise_planner.param.yaml | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) 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 f5621257ae..63a5157ca1 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 @@ -6,7 +6,7 @@ enable_debug_info: false enable_calculation_time_info: false - enable_slow_down_planning: false + enable_slow_down_planning: true # longitudinal info idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] @@ -93,7 +93,7 @@ max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego slow_down: - max_lat_margin: 10.0 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width cruise: pid_based_planner: @@ -160,11 +160,11 @@ over_j_weight: 10000.0 slow_down: - # parameters to calculate slow down velocity - max_lateral_margin: 5.0 - min_lateral_margin: 3.0 - max_ego_velocity: 10.0 - min_ego_velocity: 3.0 - - # parameter for where to insert slow down point - max_deceleration: -2.0 + # parameters to calculate slow down velocity by linear interpolation + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + + # max deceleration during slow down + max_deceleration: -1.0 From fc12862b2b907ce16ee389e35a69fc8e315ea065 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 17 Apr 2023 23:13:39 +0900 Subject: [PATCH 7/7] feat(behavior_path_planner): pull over support road_lane and right_hand_traffic (#300) Signed-off-by: kosuke55 --- .../behavior_path_planner/pull_over/pull_over.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 83a548bb46..f777581150 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -11,9 +11,9 @@ decide_path_distance: 10.0 maximum_deceleration: 1.0 maximum_jerk: 1.0 - # goal research - enable_goal_research: true + # goal search search_priority: "efficient_path" # "efficient_path" or "close_goal" + parking_policy: "left_side" # "left_side" or "right_side" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 goal_search_interval: 2.0