Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[pull] main from autowarefoundation:main #376

Merged
merged 3 commits into from
Oct 7, 2023
Merged

[pull] main from autowarefoundation:main #376

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
057d3d4
feat(lane_change): separate execution and cancel safety check param (…
zulfaqar-azmi-t4 Oct 6, 2023
0c48944
feat(intersection): yield initially on green light (#5234)
soblin Oct 6, 2023
83821d4
feat(rtc_auto_mode_manager): eliminate rtc auto mode manager (#5235)
kyoichi-sugahara Oct 7, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions launch/tier4_planning_launch/launch/planning.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@
<!-- common -->
<arg name="common_param_path"/>
<arg name="nearest_search_param_path"/>
<!-- rtc -->
<arg name="rtc_auto_mode_manager_param_path"/>
<!-- mission planner -->
<arg name="mission_planner_param_path"/>
<!-- behavior path planner -->
Expand Down
5 changes: 0 additions & 5 deletions launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,6 @@
<arg name="use_experimental_lane_change_function" value="$(var use_experimental_lane_change_function)"/>
</include>
</group>
<group>
<include file="$(find-pkg-share rtc_auto_mode_manager)/launch/rtc_auto_mode_manager.launch.xml">
<arg name="param_path" value="$(var rtc_auto_mode_manager_param_path)"/>
</include>
</group>
</group>

<!-- motion planning module -->
Expand Down
1 change: 0 additions & 1 deletion launch/tier4_planning_launch/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,6 @@
<exec_depend>obstacle_stop_planner</exec_depend>
<exec_depend>planning_evaluator</exec_depend>
<exec_depend>planning_validator</exec_depend>
<exec_depend>rtc_auto_mode_manager</exec_depend>
<exec_depend>scenario_selector</exec_depend>
<exec_depend>surround_obstacle_checker</exec_depend>

Expand Down
26 changes: 17 additions & 9 deletions planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,15 +28,23 @@

# safety check
safety_check:
expected_front_deceleration: -1.0
expected_rear_deceleration: -1.0
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: 1.0
lateral_distance_max_threshold: 2.0
longitudinal_distance_min_threshold: 3.0
longitudinal_velocity_delta_time: 0.8
allow_loose_check_for_cancel: true
execution:
expected_front_deceleration: -1.0
expected_rear_deceleration: -1.0
rear_vehicle_reaction_time: 2.0
rear_vehicle_safety_time_margin: 1.0
lateral_distance_max_threshold: 2.0
longitudinal_distance_min_threshold: 3.0
longitudinal_velocity_delta_time: 0.8
cancel:
expected_front_deceleration: -1.0
expected_rear_deceleration: -2.0
rear_vehicle_reaction_time: 1.5
rear_vehicle_safety_time_margin: 0.8
lateral_distance_max_threshold: 1.0
longitudinal_distance_min_threshold: 2.5
longitudinal_velocity_delta_time: 0.6

# lane expansion for object filtering
lane_expansion:
Expand Down
20 changes: 10 additions & 10 deletions planning/behavior_path_planner/config/scene_module_manager.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
ros__parameters:
external_request_lane_change_left:
enable_module: false
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
Expand All @@ -14,7 +14,7 @@

external_request_lane_change_right:
enable_module: false
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
Expand All @@ -23,7 +23,7 @@

lane_change_left:
enable_module: true
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
Expand All @@ -32,7 +32,7 @@

lane_change_right:
enable_module: true
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
Expand All @@ -41,7 +41,7 @@

start_planner:
enable_module: true
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: false
Expand All @@ -50,7 +50,7 @@

side_shift:
enable_module: true
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: false
Expand All @@ -59,7 +59,7 @@

goal_planner:
enable_module: true
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: true
Expand All @@ -68,7 +68,7 @@

avoidance:
enable_module: true
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: false
keep_last: false
Expand All @@ -77,7 +77,7 @@

avoidance_by_lc:
enable_module: false
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: false
Expand All @@ -86,7 +86,7 @@

dynamic_avoidance:
enable_module: false
enable_rtc: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
Expand Down
1 change: 1 addition & 0 deletions ..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ struct LaneChangeParameters
utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;

// safety check
bool allow_loose_check_for_cancel{true};
utils::path_safety_checker::RSSparams rss_params{};
utils::path_safety_checker::RSSparams rss_params_for_abort{};

Expand Down
70 changes: 48 additions & 22 deletions planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,35 +92,41 @@ LaneChangeModuleManager::LaneChangeModuleManager(
p.stop_time_threshold =
getOrDeclareParameter<double>(*node, parameter("stuck_detection.stop_time"));

// safety check
p.allow_loose_check_for_cancel =
getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));

p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
*node, parameter("safety_check.longitudinal_distance_min_threshold"));
*node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));
p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));
p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
*node, parameter("safety_check.longitudinal_velocity_delta_time"));
p.rss_params.front_vehicle_deceleration =
getOrDeclareParameter<double>(*node, parameter("safety_check.expected_front_deceleration"));
p.rss_params.rear_vehicle_deceleration =
getOrDeclareParameter<double>(*node, parameter("safety_check.expected_rear_deceleration"));
p.rss_params.rear_vehicle_reaction_time =
getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_reaction_time"));
p.rss_params.rear_vehicle_safety_time_margin =
getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_safety_time_margin"));
p.rss_params.lateral_distance_max_threshold =
getOrDeclareParameter<double>(*node, parameter("safety_check.lateral_distance_max_threshold"));
*node, parameter("safety_check.execution.longitudinal_velocity_delta_time"));
p.rss_params.front_vehicle_deceleration = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.expected_front_deceleration"));
p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.expected_rear_deceleration"));
p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.rear_vehicle_reaction_time"));
p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.rear_vehicle_safety_time_margin"));
p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter<double>(
*node, parameter("safety_check.execution.lateral_distance_max_threshold"));

p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
*node, parameter("safety_check.longitudinal_distance_min_threshold"));
*node, parameter("safety_check.cancel.longitudinal_distance_min_threshold"));
p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
*node, parameter("safety_check.longitudinal_velocity_delta_time"));
*node, parameter("safety_check.cancel.longitudinal_velocity_delta_time"));
p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter<double>(
*node, parameter("safety_check.expected_front_deceleration_for_abort"));
*node, parameter("safety_check.cancel.expected_front_deceleration"));
p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter<double>(
*node, parameter("safety_check.expected_rear_deceleration_for_abort"));
p.rss_params_for_abort.rear_vehicle_reaction_time =
getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_reaction_time"));
p.rss_params_for_abort.rear_vehicle_safety_time_margin =
getOrDeclareParameter<double>(*node, parameter("safety_check.rear_vehicle_safety_time_margin"));
p.rss_params_for_abort.lateral_distance_max_threshold =
getOrDeclareParameter<double>(*node, parameter("safety_check.lateral_distance_max_threshold"));
*node, parameter("safety_check.cancel.expected_rear_deceleration"));
p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
*node, parameter("safety_check.cancel.rear_vehicle_reaction_time"));
p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
*node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin"));
p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter<double>(
*node, parameter("safety_check.cancel.lateral_distance_max_threshold"));

// target object
{
Expand Down Expand Up @@ -165,6 +171,26 @@ LaneChangeModuleManager::LaneChangeModuleManager(
exit(EXIT_FAILURE);
}

// validation of safety check parameters
// if loosely check is not allowed, lane change module will keep on chattering and canceling, and
// false positive situation might occur
if (!p.allow_loose_check_for_cancel) {
if (
p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration ||
p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration ||
p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time ||
p.rss_params.rear_vehicle_safety_time_margin >
p.rss_params_for_abort.rear_vehicle_safety_time_margin ||
p.rss_params.lateral_distance_max_threshold >
p.rss_params_for_abort.lateral_distance_max_threshold ||
p.rss_params.longitudinal_distance_min_threshold >
p.rss_params_for_abort.longitudinal_distance_min_threshold ||
p.rss_params.longitudinal_velocity_delta_time >
p.rss_params_for_abort.longitudinal_velocity_delta_time) {
RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program...");
exit(EXIT_FAILURE);
}
}
if (p.cancel.delta_time < 1.0) {
RCLCPP_WARN_STREAM(
logger_, "cancel.delta_time: " << p.cancel.delta_time
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,4 @@
max_future_movement_time: 10.0 # [second]
threshold_yaw_diff: 0.523 # [rad]
adjacent_extend_width: 1.5 # [m]
enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
2 changes: 1 addition & 1 deletion planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
show_processing_time: false # [-] whether to show processing time
# param for input data
traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal
enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc.
enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc.

# param for stop position
stop_position:
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,4 @@
state_clear_time: 2.0
hold_stop_margin_distance: 0.0
distance_to_judge_over_stop_line: 0.5
enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
8 changes: 6 additions & 2 deletions planning/behavior_velocity_intersection_module/config/intersection.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,10 @@
keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity
yield_on_green_traffic_light:
distance_to_assigned_lanelet_start: 5.0
duration: 2.0
range: 30.0 # [m]

occlusion:
enable: false
Expand All @@ -62,8 +66,8 @@
attention_lane_curvature_calculation_ds: 0.5

enable_rtc:
intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
intersection_to_occlusion: true
intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
intersection_to_occlusion: false

merge_from_private:
stop_line_margin: 3.0
Expand Down
8 changes: 8 additions & 0 deletions planning/behavior_velocity_intersection_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
getOrDeclareParameter<bool>(node, ns + ".collision_detection.use_upstream_velocity");
ip.collision_detection.minimum_upstream_velocity =
getOrDeclareParameter<double>(node, ns + ".collision_detection.minimum_upstream_velocity");
ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start =
getOrDeclareParameter<double>(
node,
ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start");
ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter<double>(
node, ns + ".collision_detection.yield_on_green_traffic_light.duration");
ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter<double>(
node, ns + ".collision_detection.yield_on_green_traffic_light.range");

ip.occlusion.enable = getOrDeclareParameter<bool>(node, ns + ".occlusion.enable");
ip.occlusion.occlusion_attention_area_length =
Expand Down
Loading