fix(lane_change): modify lane change requested condition (autowarefou…

…ndation#8510)

* modify lane change requested condition

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp

Co-authored-by: mkquda <[email protected]>

* style(pre-commit): autofix

* fix docstring

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
Co-authored-by: mkquda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp

@@ -75,6 +75,44 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr);
 double calc_dist_to_last_fit_width(
   const lanelet::ConstLanelets lanelets, const Pose & src_pose,
   const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1);
+
+/**
+ * @brief Calculates the maximum preparation longitudinal distance for lane change.
+ *
+ * This function computes the maximum distance that the ego vehicle can travel during
+ * the preparation phase of a lane change. The distance is calculated as the product
+ * of the maximum lane change preparation duration and the maximum velocity of the ego vehicle.
+ *
+ * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
+ *  - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change
+ * preparation.
+ *  - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle.
+ *
+ * @return The maximum preparation longitudinal distance in meters.
+ */
+double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr);
+
+/**
+ * @brief Calculates the distance from the ego vehicle to the start of the target lanes.
+ *
+ * This function computes the shortest distance from the current position of the ego vehicle
+ * to the start of the target lanes by measuring the arc length to the front points of
+ * the left and right boundaries of the target lane. If the target lanes are empty or other
+ * required data is unavailable, the function returns numeric_limits<double>::max() preventing lane
+ * change being executed.
+ *
+ * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
+ *  - `route_handler_ptr`: Pointer to the route handler that manages the route.
+ *  - Other necessary parameters for ego vehicle positioning.
+ * @param current_lanes The set of lanelets representing the current lanes of the ego vehicle.
+ * @param target_lanes The set of lanelets representing the target lanes for lane changing.
+ *
+ * @return The distance from the ego vehicle to the start of the target lanes in meters,
+ * or numeric_limits<double>::max() if the target lanes are empty or data is unavailable.
+ */
+double calc_ego_dist_to_lanes_start(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & target_lanes);
 }  // namespace autoware::behavior_path_planner::utils::lane_change::calculation
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -152,6 +152,8 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
 
 bool NormalLaneChange::isLaneChangeRequired()
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto current_lanes =
     utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_);
 
@@ -161,7 +163,15 @@ bool NormalLaneChange::isLaneChangeRequired()
 
   const auto target_lanes = getLaneChangeLanes(current_lanes, direction_);
 
-  return !target_lanes.empty();
+  if (target_lanes.empty()) {
+    return false;
+  }
+
+  const auto ego_dist_to_target_start =
+    calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes);
+  const auto maximum_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_);
+
+  return ego_dist_to_target_start <= maximum_prepare_length;
 }
 
 bool NormalLaneChange::isStoppedAtRedTrafficLight() const

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp

@@ -101,4 +101,44 @@ double calc_dist_to_last_fit_width(
 
   return std::max(distance - (bpp_param.base_link2front + margin), 0.0);
 }
+
+double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr)
+{
+  const auto max_prepare_duration = common_data_ptr->lc_param_ptr->lane_change_prepare_duration;
+  const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel;
+
+  return max_prepare_duration * ego_max_speed;
+}
+
+double calc_ego_dist_to_lanes_start(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & target_lanes)
+{
+  const auto & route_handler_ptr = common_data_ptr->route_handler_ptr;
+
+  if (!route_handler_ptr || target_lanes.empty() || current_lanes.empty()) {
+    return std::numeric_limits<double>::max();
+  }
+
+  const auto & target_bound =
+    common_data_ptr->direction == autoware::route_handler::Direction::RIGHT
+      ? target_lanes.front().leftBound()
+      : target_lanes.front().rightBound();
+
+  if (target_bound.empty()) {
+    return std::numeric_limits<double>::max();
+  }
+
+  const auto path =
+    route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
+
+  if (path.points.empty()) {
+    return std::numeric_limits<double>::max();
+  }
+
+  const auto target_front_pt = lanelet::utils::conversion::toGeomMsgPt(target_bound.front());
+  const auto ego_position = common_data_ptr->get_ego_pose().position;
+
+  return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt);
+}
 }  // namespace autoware::behavior_path_planner::utils::lane_change::calculation