From 436c2f280c1c4d7a88704054ecdf79d9aa8c1472 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 11 Nov 2024 17:23:53 +0900 Subject: [PATCH] fix(lane_change): extending lane change path for multiple lane change (RT1-8427) (#9268) * RT1-8427 extending lc path for multiple lc Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../src/scene.cpp | 20 +++++-------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index a91aaba86bb1a..bb38079917d08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -584,13 +584,6 @@ std::optional NormalLaneChange::extendPath() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto path = status_.lane_change_path.path; - const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; - - const auto dist = calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); - - if (dist < 0.0) { - return std::nullopt; - } auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); @@ -601,22 +594,21 @@ std::optional NormalLaneChange::extendPath() if ((target_lane_length - dist_in_target.length) > forward_path_length) { return std::nullopt; } + const auto dist_to_end_of_path = + lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; - const auto is_goal_in_target = getRouteHandler()->isInGoalRouteSection(target_lanes.back()); - - if (is_goal_in_target) { + if (common_data_ptr_->lanes_ptr->target_lane_in_goal_section) { const auto goal_pose = getRouteHandler()->getGoalPose(); const auto dist_to_goal = lanelet::utils::getArcCoordinates(target_lanes, goal_pose).length; - const auto dist_to_end_of_path = - lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; return getRouteHandler()->getCenterLinePath(target_lanes, dist_to_end_of_path, dist_to_goal); } lanelet::ConstLanelet next_lane; if (!getRouteHandler()->getNextLaneletWithinRoute(target_lanes.back(), &next_lane)) { - return std::nullopt; + return getRouteHandler()->getCenterLinePath( + target_lanes, dist_to_end_of_path, transient_data.target_lane_length); } target_lanes.push_back(next_lane); @@ -640,8 +632,6 @@ std::optional NormalLaneChange::extendPath() const auto dist_to_target_pose = lanelet::utils::getArcCoordinates(target_lanes, target_pose).length; - const auto dist_to_end_of_path = - lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; return getRouteHandler()->getCenterLinePath( target_lanes, dist_to_end_of_path, dist_to_target_pose);