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

zulfaqar-azmi-t4 · maxime-clem · mkquda · web-flow · commit 0bf689c2dc5a · 2025-01-16T23:39:33.000+09:00
* refactor(lane_change): parameterize incoming object yaw threshold

Signed-off-by: Zulfaqar Azmi &lt;zulfaqar.azmi@tier4.jp&gt;

* Readme

Signed-off-by: Zulfaqar Azmi &lt;zulfaqar.azmi@tier4.jp&gt;

* Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

Co-authored-by: Maxime CLEMENT &lt;78338830+maxime-clem@users.noreply.github.com&gt;

* Add missing parameters

Signed-off-by: Zulfaqar Azmi &lt;zulfaqar.azmi@tier4.jp&gt;

* missing dot

Signed-off-by: Zulfaqar Azmi &lt;zulfaqar.azmi@tier4.jp&gt;

* Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

Co-authored-by: mkquda &lt;168697710+mkquda@users.noreply.github.com&gt;

* update readme

Signed-off-by: Zulfaqar Azmi &lt;zulfaqar.azmi@tier4.jp&gt;

---------

Signed-off-by: Zulfaqar Azmi &lt;zulfaqar.azmi@tier4.jp&gt;
Co-authored-by: Maxime CLEMENT &lt;78338830+maxime-clem@users.noreply.github.com&gt;
Co-authored-by: mkquda &lt;168697710+mkquda@users.noreply.github.com&gt;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
@@ -1149,7 +1149,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 ego and object when executing rss-based collision checking                                                                                                                  | 3.1416        |
+| `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 +1163,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 +1176,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 +1189,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 +1202,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.
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml
@@ -58,6 +58,7 @@
           lateral_distance_max_threshold: 2.0
           longitudinal_distance_min_threshold: 3.0
           longitudinal_velocity_delta_time: 0.0
+          extended_polygon_policy: "rectangle"
         parked:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -2.0
@@ -66,6 +67,7 @@
           lateral_distance_max_threshold: 1.0
           longitudinal_distance_min_threshold: 3.0
           longitudinal_velocity_delta_time: 0.0
+          extended_polygon_policy: "rectangle"
         cancel:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -2.0
@@ -74,6 +76,7 @@
           lateral_distance_max_threshold: 1.0
           longitudinal_distance_min_threshold: 2.5
           longitudinal_velocity_delta_time: 0.0
+          extended_polygon_policy: "rectangle"
         stuck:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -1.0
@@ -82,6 +85,7 @@
           lateral_distance_max_threshold: 2.0
           longitudinal_distance_min_threshold: 3.0
           longitudinal_velocity_delta_time: 0.0
+          extended_polygon_policy: "rectangle"
 
         # lane expansion for object filtering
         lane_expansion:
@@ -123,7 +127,8 @@
           intersection: true
           turns: true
         prediction_time_resolution: 0.5
-        yaw_diff_threshold: 3.1416
+        th_incoming_object_yaw: 2.3562 # [rad]
+        yaw_diff_threshold: 3.1416 # [rad]
         check_current_lanes: false
         check_other_lanes: false
         use_all_predicted_paths: false
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp
@@ -94,6 +94,7 @@ struct CollisionCheckParameters
   bool check_current_lane{true};
   bool check_other_lanes{true};
   bool use_all_predicted_paths{false};
+  double th_incoming_object_yaw{2.3562};
   double th_yaw_diff{3.1416};
   double prediction_time_resolution{0.5};
 };
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
@@ -152,6 +152,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
       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) {
@@ -169,6 +171,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
         *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"));
     };
     set_rss_params(p.safety.rss_params, "safety_check.execution");
     set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked");
@@ -454,6 +458,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
       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);
+    }
   }
 
   {
@@ -483,7 +500,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
     updateParam<double>(parameters, ns + "stop_time", p->th_stop_time);
   }
 
-  auto update_rss_params = [&parameters](const std::string & prefix, auto & params) {
+  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",
@@ -502,6 +519,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
       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());
+    }
   };
 
   update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -1051,7 +1051,9 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const
 
   const auto is_same_direction = [&](const PredictedObject & object) {
     const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
-    return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose);
+    return !utils::path_safety_checker::isTargetObjectOncoming(
+      current_pose, object_pose,
+      common_data_ptr_->lc_param_ptr->safety.collision_check.th_incoming_object_yaw);
   };
 
   //  Perception noise could make stationary objects seem opposite the ego vehicle; check the
@@ -1792,7 +1794,6 @@ bool NormalLaneChange::is_colliding(
 
   constexpr auto is_safe{true};
   auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
-  constexpr auto collision_check_yaw_diff_threshold{M_PI};
   constexpr auto hysteresis_factor{1.0};
   const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
     obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths);
@@ -1817,7 +1818,8 @@ bool NormalLaneChange::is_colliding(
     const auto collided_polygons = utils::path_safety_checker::get_collided_polygons(
       lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info,
       selected_rss_param, hysteresis_factor, safety_check_max_vel,
-      collision_check_yaw_diff_threshold, current_debug_data.second);
+      common_data_ptr_->lc_param_ptr->safety.collision_check.th_yaw_diff,
+      current_debug_data.second);
 
     if (collided_polygons.empty()) {
       utils::path_safety_checker::updateCollisionCheckDebugMap(
