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.

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

refactor(lane_change): add missing safety check parameter #9928

Prev Previous commit
update readme
Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
zulfaqar-azmi-t4 committed Jan 16, 2025
commit 21f6154aa16fa4a9ee56edcf37984d6fe2af0bd3
Original file line number Diff line number Diff line change
@@ -1148,8 +1148,8 @@ The following parameters are used to configure terminal lane change path feature
| `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false |
| `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true |
| `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 |
| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between predicted ego pose and predicted object pose when executing rss-based collision checking | 3.1416 |
| `collision_check.th_incoming_object_yaw` | [rad] | double | Maximum yaw difference between current ego pose and current object pose. Objects with a yaw difference exceeding this value are excluded from the safety check. | 2.3562 |
| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between predicted ego pose and predicted object pose when executing rss-based collision checking | 3.1416 |
| `collision_check.th_incoming_object_yaw` | [rad] | double | Maximum yaw difference between current ego pose and current object pose. Objects with a yaw difference exceeding this value are excluded from the safety check. | 2.3562 |

#### safety constraints during lane change path is computed

@@ -1162,6 +1162,7 @@ The following parameters are used to configure terminal lane change path feature
| `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 |
| `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 |
| `safety_check.execution.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 |
| `safety_check.execution.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` |

#### safety constraints specifically for stopped or parked vehicles

@@ -1174,6 +1175,7 @@ The following parameters are used to configure terminal lane change path feature
| `safety_check.parked.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 |
| `safety_check.parked.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 |
| `safety_check.parked.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 |
| `safety_check.parked.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` |

##### safety constraints to cancel lane change path

@@ -1186,6 +1188,7 @@ The following parameters are used to configure terminal lane change path feature
| `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 |
| `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 2.5 |
| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.6 |
| `safety_check.cancel.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` |

##### safety constraints used during lane change path is computed when ego is stuck

@@ -1198,6 +1201,7 @@ The following parameters are used to configure terminal lane change path feature
| `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 |
| `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 |
| `safety_check.stuck.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 |
| `safety_check.stuck.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` |

(\*1) the value must be negative.


Unchanged files with check annotations Beta

getOrDeclareParameter<double>(*node, parameter("collision_check.prediction_time_resolution"));
p.safety.collision_check.th_yaw_diff =
getOrDeclareParameter<double>(*node, parameter("collision_check.yaw_diff_threshold"));
p.safety.collision_check.th_incoming_object_yaw =
getOrDeclareParameter<double>(*node, parameter("collision_check.th_incoming_object_yaw"));
// rss check
auto set_rss_params = [&](auto & params, const std::string & prefix) {
params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
*node, parameter(prefix + ".longitudinal_distance_min_threshold"));
params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
*node, parameter(prefix + ".longitudinal_velocity_delta_time"));
params.front_vehicle_deceleration =
getOrDeclareParameter<double>(*node, parameter(prefix + ".expected_front_deceleration"));
params.rear_vehicle_deceleration =
getOrDeclareParameter<double>(*node, parameter(prefix + ".expected_rear_deceleration"));
params.rear_vehicle_reaction_time =
getOrDeclareParameter<double>(*node, parameter(prefix + ".rear_vehicle_reaction_time"));
params.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
*node, parameter(prefix + ".rear_vehicle_safety_time_margin"));
params.lateral_distance_max_threshold =
getOrDeclareParameter<double>(*node, parameter(prefix + ".lateral_distance_max_threshold"));
params.extended_polygon_policy =
getOrDeclareParameter<std::string>(*node, parameter(prefix + ".extended_polygon_policy"));

Check warning on line 175 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

LaneChangeModuleManager::set_params already has high cyclomatic complexity, and now it increases in Lines of Code from 213 to 217. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
};
set_rss_params(p.safety.rss_params, "safety_check.execution");
set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked");
p->safety.collision_check.prediction_time_resolution);
updateParam<double>(
parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff);
auto th_incoming_object_yaw = p->safety.collision_check.th_incoming_object_yaw;
updateParam<double>(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw);
if (th_incoming_object_yaw >= M_PI_2) {
p->safety.collision_check.th_incoming_object_yaw = th_incoming_object_yaw;
} else {
RCLCPP_WARN_THROTTLE(
node_->get_logger(), *node_->get_clock(), 5000,
"The value of th_incoming_object_yaw (%.3f rad) is less than the minimum possible value "
"(%.3f "
"rad).",
th_incoming_object_yaw, M_PI_2);
}
}
{
const std::string ns = "lane_change.target_object.";
updateParam<bool>(parameters, ns + "car", p->safety.target_object_types.check_car);
updateParam<bool>(parameters, ns + "truck", p->safety.target_object_types.check_truck);
updateParam<bool>(parameters, ns + "bus", p->safety.target_object_types.check_bus);
updateParam<bool>(parameters, ns + "trailer", p->safety.target_object_types.check_trailer);
updateParam<bool>(parameters, ns + "unknown", p->safety.target_object_types.check_unknown);
updateParam<bool>(parameters, ns + "bicycle", p->safety.target_object_types.check_bicycle);
updateParam<bool>(
parameters, ns + "motorcycle", p->safety.target_object_types.check_motorcycle);
updateParam<bool>(
parameters, ns + "pedestrian", p->safety.target_object_types.check_pedestrian);
}
{
const std::string ns = "lane_change.regulation.";
updateParam<bool>(parameters, ns + "crosswalk", p->regulate_on_crosswalk);
updateParam<bool>(parameters, ns + "intersection", p->regulate_on_intersection);
updateParam<bool>(parameters, ns + "traffic_light", p->regulate_on_traffic_light);
}
{
const std::string ns = "lane_change.stuck_detection.";
updateParam<double>(parameters, ns + "velocity", p->th_stop_velocity);
updateParam<double>(parameters, ns + "stop_time", p->th_stop_time);
}
auto update_rss_params = [&parameters, this](const std::string & prefix, auto & params) {
using autoware::universe_utils::updateParam;
updateParam<double>(
parameters, prefix + "longitudinal_distance_min_threshold",
params.longitudinal_distance_min_threshold);
updateParam<double>(
parameters, prefix + "longitudinal_velocity_delta_time",
params.longitudinal_velocity_delta_time);
updateParam<double>(
parameters, prefix + "expected_front_deceleration", params.front_vehicle_deceleration);
updateParam<double>(
parameters, prefix + "expected_rear_deceleration", params.rear_vehicle_deceleration);
updateParam<double>(
parameters, prefix + "rear_vehicle_reaction_time", params.rear_vehicle_reaction_time);
updateParam<double>(
parameters, prefix + "rear_vehicle_safety_time_margin",
params.rear_vehicle_safety_time_margin);
updateParam<double>(
parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold);
auto extended_polygon_policy = params.extended_polygon_policy;
updateParam<std::string>(
parameters, prefix + "extended_polygon_policy", extended_polygon_policy);
if (extended_polygon_policy == "rectangle" || extended_polygon_policy == "along_path") {
params.extended_polygon_policy = extended_polygon_policy;
} else {
RCLCPP_WARN_THROTTLE(
node_->get_logger(), *node_->get_clock(), 1000,
"Policy %s not supported or there's typo. Make sure you choose either 'rectangle' or "
"'along_path'",
extended_polygon_policy.c_str());
}

Check warning on line 520 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

LaneChangeModuleManager::updateModuleParams increases in cyclomatic complexity from 11 to 14, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
};
update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params);
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File

The lines of code increases from 1529 to 1531, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.