fix(behavior_path_planner): make lane change safety check adaptive (#…

…2704)

* fix(behavior_path_planner): make lane change safety check adaptive

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Temporarily hard code use all predicted path

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Revert "Temporarily hard code use all predicted path"

This reverts commit 8f92e45.

* fix external lane change request

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* use prediction resolution as rounding multiplier

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -2,7 +2,6 @@
   ros__parameters:
     lane_change:
       lane_change_prepare_duration: 4.0         # [s]
-      lane_changing_safety_check_duration: 8.0  # [s]
 
       minimum_lane_change_prepare_distance: 2.0 # [m]
       minimum_lane_change_length: 16.5          # [m]

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp

@@ -14,7 +14,6 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
 
-#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "lanelet2_core/geometry/Lanelet.h"
 
 #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp"
@@ -59,19 +58,6 @@ struct LaneChangeParameters
   bool publish_debug_marker{false};
 };
 
-struct LaneChangeStatus
-{
-  PathWithLaneId lane_follow_path;
-  LaneChangePath lane_change_path;
-  lanelet::ConstLanelets current_lanes;
-  lanelet::ConstLanelets lane_change_lanes;
-  std::vector<uint64_t> lane_follow_lane_ids;
-  std::vector<uint64_t> lane_change_lane_ids;
-  bool is_safe;
-  bool is_valid_path = true;
-  double start_distance;
-};
-
 enum class LaneChangeStates {
   Normal = 0,
   Cancel,

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp

@@ -15,6 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
 
+#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
 
@@ -34,12 +35,25 @@ struct LaneChangePath
   ShiftedPath shifted_path;
   ShiftLine shift_line;
   double acceleration{0.0};
-  double preparation_length{0.0};
-  double lane_change_length{0.0};
+  LaneChangePhaseInfo length{};
+  LaneChangePhaseInfo duration{};
   TurnSignalInfo turn_signal_info;
   PathWithLaneId prev_path;
 };
 using LaneChangePaths = std::vector<LaneChangePath>;
 
+struct LaneChangeStatus
+{
+  PathWithLaneId lane_follow_path;
+  LaneChangePath lane_change_path;
+  lanelet::ConstLanelets current_lanes;
+  lanelet::ConstLanelets lane_change_lanes;
+  std::vector<uint64_t> lane_follow_lane_ids;
+  std::vector<uint64_t> lane_change_lane_ids;
+  bool is_safe;
+  bool is_valid_path = true;
+  double start_distance;
+};
+
 }  // namespace behavior_path_planner
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_

planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp

@@ -382,8 +382,7 @@ std::pair<bool, bool> ExternalRequestLaneChangeModule::getSafePath(
     if (!lane_change_paths.empty()) {
       const auto & longest_path = lane_change_paths.front();
       // we want to see check_distance [m] behind vehicle so add lane changing length
-      const double check_distance_with_path =
-        check_distance + longest_path.preparation_length + longest_path.lane_change_length;
+      const double check_distance_with_path = check_distance + longest_path.length.sum();
       check_lanes = route_handler->getCheckTargetLanesFromPath(
         longest_path.path, lane_change_lanes, check_distance_with_path);
     }
@@ -557,9 +556,8 @@ bool ExternalRequestLaneChangeModule::hasFinishedLaneChange() const
   const auto arclength_current =
     lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose);
   const double travel_distance = arclength_current.length - status_.start_distance;
-  const double finish_distance = status_.lane_change_path.preparation_length +
-                                 status_.lane_change_path.lane_change_length +
-                                 parameters_->lane_change_finish_judge_buffer;
+  const double finish_distance =
+    status_.lane_change_path.length.sum() + parameters_->lane_change_finish_judge_buffer;
   return travel_distance > finish_distance;
 }
 
@@ -681,8 +679,7 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_
 
   constexpr double check_distance = 100.0;
   // get lanes used for detection
-  const double check_distance_with_path =
-    check_distance + path.preparation_length + path.lane_change_length;
+  const double check_distance_with_path = check_distance + path.length.sum();
   const auto check_lanes = route_handler->getCheckTargetLanesFromPath(
     path.path, status_.lane_change_lanes, check_distance_with_path);
 
@@ -745,7 +742,7 @@ void ExternalRequestLaneChangeModule::calcTurnSignalInfo()
   turn_signal_info.desired_end_point = path.shift_line.end;
 
   turn_signal_info.required_start_point = path.shift_line.start;
-  const auto mid_lane_change_length = path.lane_change_length / 2;
+  const auto mid_lane_change_length = path.length.lane_changing / 2;
   const auto & shifted_path = path.shifted_path.path;
   turn_signal_info.required_end_point =
     get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length);

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -382,8 +382,7 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(
     if (!lane_change_paths.empty()) {
       const auto & longest_path = lane_change_paths.front();
       // we want to see check_distance [m] behind vehicle so add lane changing length
-      const double check_distance_with_path =
-        check_distance + longest_path.preparation_length + longest_path.lane_change_length;
+      const double check_distance_with_path = check_distance + longest_path.length.sum();
       check_lanes = route_handler->getCheckTargetLanesFromPath(
         longest_path.path, lane_change_lanes, check_distance_with_path);
     }
@@ -557,9 +556,8 @@ bool LaneChangeModule::hasFinishedLaneChange() const
   const auto arclength_current =
     lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose);
   const double travel_distance = arclength_current.length - status_.start_distance;
-  const double finish_distance = status_.lane_change_path.preparation_length +
-                                 status_.lane_change_path.lane_change_length +
-                                 parameters_->lane_change_finish_judge_buffer;
+  const double finish_distance =
+    status_.lane_change_path.length.sum() + parameters_->lane_change_finish_judge_buffer;
   return travel_distance > finish_distance;
 }
 
@@ -674,12 +672,11 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons
   const auto & current_lanes = status_.current_lanes;
   const auto & common_parameters = planner_data_->parameters;
   const auto & route_handler = planner_data_->route_handler;
-  const auto path = status_.lane_change_path;
+  const auto & path = status_.lane_change_path;
 
   constexpr double check_distance = 100.0;
   // get lanes used for detection
-  const double check_distance_with_path =
-    check_distance + path.preparation_length + path.lane_change_length;
+  const double check_distance_with_path = check_distance + path.length.sum();
   const auto check_lanes = route_handler->getCheckTargetLanesFromPath(
     path.path, status_.lane_change_lanes, check_distance_with_path);
 
@@ -742,7 +739,7 @@ void LaneChangeModule::calcTurnSignalInfo()
   turn_signal_info.desired_end_point = path.shift_line.end;
 
   turn_signal_info.required_start_point = path.shift_line.start;
-  const auto mid_lane_change_length = path.lane_change_length / 2;
+  const auto mid_lane_change_length = path.length.prepare / 2;
   const auto & shifted_path = path.shifted_path.path;
   turn_signal_info.required_end_point =
     get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length);

planning/behavior_path_planner/src/scene_module/lane_change/util.cpp

@@ -208,8 +208,18 @@ std::optional<LaneChangePath> constructCandidatePath(
 
   LaneChangePath candidate_path;
   candidate_path.acceleration = acceleration;
-  candidate_path.preparation_length = prepare_distance;
-  candidate_path.lane_change_length = lane_change_distance;
+  candidate_path.length.prepare = prepare_distance;
+  candidate_path.length.lane_changing = lane_change_distance;
+  candidate_path.duration.prepare = std::invoke([&]() {
+    const auto duration =
+      prepare_distance / std::max(lane_change_param.minimum_lane_change_velocity, speed.prepare);
+    return std::min(duration, lane_change_param.lane_change_prepare_duration);
+  });
+  candidate_path.duration.lane_changing = std::invoke([&]() {
+    const auto rounding_multiplier = 1.0 / lane_change_param.prediction_time_resolution;
+    return std::ceil((lane_change_distance / lane_changing_speed) * rounding_multiplier) /
+           rounding_multiplier;
+  });
   candidate_path.shift_line = shift_line;
   candidate_path.reference_lanelets = original_lanelets;
   candidate_path.target_lanelets = target_lanelets;
@@ -440,9 +450,7 @@ bool hasEnoughDistance(
   const Pose & goal_pose, const RouteHandler & route_handler,
   const double minimum_lane_change_length)
 {
-  const double & lane_change_prepare_distance = path.preparation_length;
-  const double & lane_changing_distance = path.lane_change_length;
-  const double lane_change_total_distance = lane_change_prepare_distance + lane_changing_distance;
+  const double lane_change_total_distance = path.length.sum();
   const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back()));
   const auto overall_graphs = route_handler.getOverallGraphPtr();
 
@@ -495,15 +503,27 @@ bool isLaneChangePathSafe(
   }
 
   const double time_resolution = lane_change_parameters.prediction_time_resolution;
-  const auto & lane_change_prepare_duration = lane_change_parameters.lane_change_prepare_duration;
+  const auto & lane_change_prepare_duration = lane_change_path.duration.prepare;
   const auto & enable_collision_check_at_prepare_phase =
     lane_change_parameters.enable_collision_check_at_prepare_phase;
-  const auto & lane_changing_safety_check_duration =
-    lane_change_parameters.lane_changing_safety_check_duration;
+  const auto & lane_changing_safety_check_duration = lane_change_path.duration.lane_changing;
   const double check_end_time = lane_change_prepare_duration + lane_changing_safety_check_duration;
   const double min_lc_speed{lane_change_parameters.minimum_lane_change_velocity};
+
+  const auto get_pose = std::invoke([&]() {
+    Pose p;
+    double dist{0.0};
+    for (size_t i = 1; i < path.points.size(); ++i) {
+      dist += motion_utils::calcSignedArcLength(path.points, i - 1, i);
+      if (dist >= common_parameters.backward_path_length) {
+        return path.points.at(i).point.pose;
+      }
+    }
+    return path.points.front().point.pose;
+  });
+
   const auto vehicle_predicted_path = util::convertToPredictedPath(
-    path, current_twist, current_pose, static_cast<double>(current_seg_idx), check_end_time,
+    path, current_twist, get_pose, static_cast<double>(current_seg_idx), check_end_time,
     time_resolution, acceleration, min_lc_speed);
   const auto prepare_phase_ignore_target_speed_thresh =
     lane_change_parameters.prepare_phase_ignore_target_speed_thresh;