feat(lane_change): fix delay logic that caused timing to be late (autowarefoundation#8549)

* RT1-5067 fix delay logic that caused timing to be late

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

* remove autoware namespace

Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

Author: 2 people

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

@@ -14,6 +14,7 @@
 
 #include "autoware/behavior_path_lane_change_module/utils/utils.hpp"
 
+#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp"
 #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp"
 #include "autoware/behavior_path_lane_change_module/utils/path.hpp"
 #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp"
@@ -988,30 +989,41 @@ bool passed_parked_objects(
   }
 
   const auto & current_path_end = current_lane_path.points.back().point.pose.position;
-  double min_dist_to_end_of_current_lane = std::numeric_limits<double>::max();
-  const auto & target_lanes = common_data_ptr->lanes_ptr->target;
-  const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back());
-  for (const auto & point : leading_obj_poly.outer()) {
-    const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0);
-    const double dist = autoware::motion_utils::calcSignedArcLength(
-      current_lane_path.points, obj_p, current_path_end);
-    min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane);
-    if (is_goal_in_route) {
+  const auto dist_to_path_end = [&](const auto & src_point) {
+    if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) {
       const auto goal_pose = route_handler.getGoalPose();
-      const double dist_to_goal = autoware::motion_utils::calcSignedArcLength(
-        current_lane_path.points, obj_p, goal_pose.position);
-      min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal);
+      return motion_utils::calcSignedArcLength(
+        current_lane_path.points, src_point, goal_pose.position);
     }
-  }
+    return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end);
+  };
+
+  const auto min_dist_to_end_of_current_lane = std::invoke([&]() {
+    auto min_dist_to_end_of_current_lane = std::numeric_limits<double>::max();
+    for (const auto & point : leading_obj_poly.outer()) {
+      const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0);
+      const auto dist = dist_to_path_end(obj_p);
+      min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane);
+    }
+    return min_dist_to_end_of_current_lane;
+  });
 
   // If there are still enough length after the target object, we delay the lane change
-  if (min_dist_to_end_of_current_lane > minimum_lane_change_length) {
-    debug.second.unsafe_reason = "delay lane change";
-    utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false);
-    return false;
+  if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) {
+    return true;
   }
 
-  return true;
+  const auto current_pose = common_data_ptr->get_ego_pose();
+  const auto dist_ego_to_obj = motion_utils::calcSignedArcLength(
+    current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position);
+
+  if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) {
+    return true;
+  }
+
+  debug.second.unsafe_reason = "delay lane change";
+  utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false);
+  return false;
 }
 
 std::optional<size_t> getLeadingStaticObjectIdx(