Skip computation if prepare length exceed distance to terminal start

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

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt

@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/interface.cpp
   src/manager.cpp
   src/scene.cpp
+  src/utils/calculation.cpp
   src/utils/markers.cpp
   src/utils/utils.cpp
 )

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

@@ -0,0 +1,45 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
+#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
+
+#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp"
+
+namespace autoware::behavior_path_planner::utils::lane_change::calculation
+{
+using behavior_path_planner::lane_change::CommonDataPtr;
+
+/**
+ * @brief Calculates the distance from the ego vehicle to the terminal point.
+ *
+ * This function computes the distance from the current position of the ego vehicle
+ * to either the goal pose or the end of the current lane, depending on whether
+ * the vehicle's current lane is within the goal section.
+ *
+ * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
+ *  - Non null `lanes_ptr` that points to the current lanes data.
+ *  - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle.
+ *  - Non null `route_handler_ptr` that contains the goal pose of the route.
+ *
+ * @return The distance to the terminal point (either the goal pose or the end of the current lane)
+ * in meters.
+ */
+double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr);
+
+double calc_dist_from_pose_to_terminal_end(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
+  const Pose & src_pose);
+}  // 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

@@ -14,6 +14,7 @@
 
 #include "autoware/behavior_path_lane_change_module/scene.hpp"
 
+#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp"
 #include "autoware/behavior_path_lane_change_module/utils/utils.hpp"
 #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
@@ -45,6 +46,7 @@ using utils::lane_change::calcMinimumLaneChangeLength;
 using utils::lane_change::create_lanes_polygon;
 using utils::path_safety_checker::isPolygonOverlapLanelet;
 using utils::traffic_light::getDistanceToNextTrafficLight;
+namespace calculation = utils::lane_change::calculation;
 
 NormalLaneChange::NormalLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
@@ -1361,7 +1363,7 @@ bool NormalLaneChange::getLaneChangePaths(
     route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()));
 
   const auto dist_to_end_of_current_lanes =
-    utils::getDistanceToEndOfLane(getEgoPose(), current_lanes);
+    calculation::calc_ego_dist_to_terminal_end(common_data_ptr_);
 
   const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes);
 
@@ -1402,6 +1404,16 @@ bool NormalLaneChange::getLaneChangePaths(
       const auto prepare_length = utils::lane_change::calcPhaseLength(
         current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration);
 
+      const auto ego_dist_to_terminal_start = dist_to_end_of_current_lanes - lane_change_buffer;
+      if (prepare_length > ego_dist_to_terminal_start) {
+        RCLCPP_DEBUG(
+          logger_,
+          "Reject: Prepare length exceed distance to terminal start. prep_len: %.5f,  ego dist to "
+          "terminal start: %.5f",
+          prepare_length, ego_dist_to_terminal_start);
+        continue;
+      }
+
       auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
 
       const auto debug_print = [&](const auto & s) {

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

@@ -0,0 +1,45 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <autoware/behavior_path_lane_change_module/utils/calculation.hpp>
+#include <autoware/behavior_path_planner_common/utils/utils.hpp>
+
+namespace autoware::behavior_path_planner::utils::lane_change::calculation
+{
+double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr)
+{
+  const auto & lanes_ptr = common_data_ptr->lanes_ptr;
+  const auto & current_lanes = lanes_ptr->current;
+  const auto & current_pose = common_data_ptr->get_ego_pose();
+
+  return calc_dist_from_pose_to_terminal_end(common_data_ptr, current_lanes, current_pose);
+}
+
+double calc_dist_from_pose_to_terminal_end(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
+  const Pose & src_pose)
+{
+  if (lanes.empty()) {
+    return 0.0;
+  }
+
+  const auto & lanes_ptr = common_data_ptr->lanes_ptr;
+  const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose();
+
+  if (lanes_ptr->current_lane_in_goal_section) {
+    return utils::getSignedDistance(src_pose, goal_pose, lanes);
+  }
+  return utils::getDistanceToEndOfLane(src_pose, lanes);
+}
+}  // namespace autoware::behavior_path_planner::utils::lane_change::calculation