add parked

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
tier4 · Jul 21, 2024 · b118ba6 · b118ba6
1 parent 829b18c
commit b118ba6
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 1 deletion.
diff --git a/...r_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/...r_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -151,6 +151,7 @@ struct Parameters
   bool allow_loose_check_for_cancel{true};
   double collision_check_yaw_diff_threshold{3.1416};
   utils::path_safety_checker::RSSparams rss_params{};
+  utils::path_safety_checker::RSSparams rss_params_for_parked{};
   utils::path_safety_checker::RSSparams rss_params_for_abort{};
   utils::path_safety_checker::RSSparams rss_params_for_stuck{};
 

diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp
@@ -121,6 +121,23 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.lateral_distance_max_threshold"));
 
+  p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.longitudinal_distance_min_threshold"));
+  p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.longitudinal_distance_min_threshold"));
+  p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.longitudinal_velocity_delta_time"));
+  p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.expected_front_deceleration"));
+  p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.expected_rear_deceleration"));
+  p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.rear_vehicle_reaction_time"));
+  p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin"));
+  p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.parked.lateral_distance_max_threshold"));
+
   p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold"));
   p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(

diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -2069,9 +2069,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       obj, lane_change_parameters_->use_all_predicted_path);
     auto is_safe = true;
+    const auto selected_rss_param =
+      (obj.initial_twist.twist.linear.x <=
+       lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh)
+        ? lane_change_parameters_->rss_params_for_parked
+        : rss_params;
     for (const auto & obj_path : obj_predicted_paths) {
       const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
-        path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
+        path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0,
         get_max_velocity_for_safety_check(), current_debug_data.second);
 
       if (collided_polygons.empty()) {