From 0a88b27aba698073dbb786f9ea3c4b0c5eaf202e Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 16 Mar 2023 18:44:54 +0900 Subject: [PATCH 1/6] fix(behavior_path_planner): fix lane change speed Signed-off-by: yutaka --- .../util/lane_change/util.hpp | 7 +++--- .../util/path_shifter/path_shifter.hpp | 3 +-- .../src/util/lane_change/util.cpp | 24 +++++++++---------- .../src/util/path_shifter/path_shifter.cpp | 3 +-- 4 files changed, 16 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp index 679f1ae230f1c..7132a14b7ce62 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp @@ -54,10 +54,9 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); -std::pair calcLaneChangingSpeedAndDistance( - const double velocity, const double shift_length, const double deceleration, - const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, - const LaneChangeParameters & lc_param); +double calcLaneChangingDistance( + const double lane_changing_velocity, const double shift_length, const double min_total_lc_len, + const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param); std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp index ee41a91cd500a..a6c8ae67cecf0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp @@ -134,8 +134,7 @@ class PathShifter static double calcLongitudinalDistFromJerk( const double lateral, const double jerk, const double velocity); - static double calcShiftTimeFromJerkAndJerk( - const double lateral, const double jerk, const double acc); + static double calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc); static double calcJerkFromLatLonDistance( const double lateral, const double longitudinal, const double velocity); diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index 9dcf4b11dcfdb..75870ca8404ac 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -359,9 +359,10 @@ std::pair getLaneChangePaths( const auto estimated_shift_length = util::calcLateralDistanceToLanelet( target_lanelets, prepare_segment.points.front().point.pose); - const auto [lane_changing_speed, lane_changing_distance] = calcLaneChangingSpeedAndDistance( - prepare_speed, estimated_shift_length, acceleration, end_of_lane_dist, common_parameter, - parameter); + // we assume constant speed during lane change + const auto lane_changing_speed = std::max(prepare_speed, minimum_lane_change_velocity); + const auto lane_changing_distance = calcLaneChangingDistance( + lane_changing_speed, estimated_shift_length, end_of_lane_dist, common_parameter, parameter); const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance}; @@ -685,17 +686,14 @@ PathWithLaneId getPrepareSegment( return prepare_segment; } -std::pair calcLaneChangingSpeedAndDistance( - const double velocity, const double shift_length, const double deceleration, - const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, - const LaneChangeParameters & lc_param) +double calcLaneChangingDistance( + const double lane_changing_speed, const double shift_length, const double min_total_lc_len, + const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param) { - const auto required_time = PathShifter::calcShiftTimeFromJerkAndJerk( + const auto required_time = PathShifter::calcShiftTimeFromJerk( shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc); - const auto lane_changing_average_speed = - std::max(velocity + deceleration * 0.5 * required_time, lc_param.minimum_lane_change_velocity); - const auto expected_dist = lane_changing_average_speed * required_time; + const auto expected_dist = lane_changing_speed * required_time; const auto lane_changing_distance = (expected_dist < min_total_lc_len) ? expected_dist : com_param.minimum_lane_change_length; @@ -705,9 +703,9 @@ std::pair calcLaneChangingSpeedAndDistance( .get_child("util") .get_child("calcLaneChangingSpeedAndDistance"), "required_time: %f [s] average_speed: %f [m/s], lane_changing_distance : %f [m]", required_time, - lane_changing_average_speed, lane_changing_distance); + lane_changing_speed, lane_changing_distance); - return {lane_changing_average_speed, lane_changing_distance}; + return lane_changing_distance; } PathWithLaneId getLaneChangingSegment( diff --git a/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp index aa90563f16a33..4e0ec714031bd 100644 --- a/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp @@ -473,8 +473,7 @@ void PathShifter::shiftBaseLength(ShiftedPath * path, double offset) const } } -double PathShifter::calcShiftTimeFromJerkAndJerk( - const double lateral, const double jerk, const double acc) +double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc) { const double j = std::abs(jerk); const double a = std::abs(acc); From f5b4cbf51673f0f52b813cd9c97a0e74e288b9f8 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 17 Mar 2023 13:32:18 +0900 Subject: [PATCH 2/6] update Signed-off-by: yutaka --- .../src/scene_module/lane_change/lane_change_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 99c0ff5ddd467..9aa7769e4238c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -827,7 +827,7 @@ void LaneChangeModule::calcTurnSignalInfo() return get_blinker_pose(path.path, path.reference_lanelets, prepare_to_blinker_start_diff); }); - turn_signal_info.desired_end_point = path.shift_line.end; + turn_signal_info.desired_end_point = path.shifted_path.path.points.back().point.pose; turn_signal_info.required_start_point = path.shift_line.start; const auto mid_lane_change_length = path.length.prepare / 2; From 9d939eb2c7f171371f1e31b603997966cd489b4e Mon Sep 17 00:00:00 2001 From: yutaka Date: Sat, 18 Mar 2023 02:20:53 +0900 Subject: [PATCH 3/6] update Signed-off-by: yutaka --- .../util/lane_change/util.hpp | 2 +- .../lane_change/lane_change_module.cpp | 14 ++--- .../src/util/lane_change/util.cpp | 54 +++++++++++++------ 3 files changed, 46 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp index 7132a14b7ce62..013374f769f32 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp @@ -119,7 +119,7 @@ PathWithLaneId getPrepareSegment( PathWithLaneId getLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const double forward_path_length, const double arc_length_from_target, + const double forward_path_length, const Pose & lane_changing_start_pose, const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, const double lane_changing_speed, const double total_required_min_dist); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 9aa7769e4238c..1442d8f537331 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -629,12 +630,11 @@ bool LaneChangeModule::isAbortState() const bool LaneChangeModule::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); - 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.length.sum() + parameters_->lane_change_finish_judge_buffer; - return travel_distance > finish_distance; + const auto & lane_change_path = status_.lane_change_path.path; + const auto & lane_change_end = status_.lane_change_path.shift_line.end; + const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( + lane_change_path.points, current_pose.position, lane_change_end.position); + return dist_to_lane_change_end + parameters_->lane_change_finish_judge_buffer < 0.0; } void LaneChangeModule::setObjectDebugVisualization() const @@ -827,7 +827,7 @@ void LaneChangeModule::calcTurnSignalInfo() return get_blinker_pose(path.path, path.reference_lanelets, prepare_to_blinker_start_diff); }); - turn_signal_info.desired_end_point = path.shifted_path.path.points.back().point.pose; + 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.length.prepare / 2; diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index 203f1f2a341f1..fbc78403172c6 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -357,6 +358,13 @@ std::pair getLaneChangePaths( std::max(prepare_speed, minimum_lane_change_velocity)); #endif + if (prepare_segment.points.empty()) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "prepare segment is empty!! something wrong..."); + continue; + } + const auto estimated_shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( target_lanelets, prepare_segment.points.front().point.pose); @@ -365,21 +373,32 @@ std::pair getLaneChangePaths( const auto lane_changing_distance = calcLaneChangingDistance( lane_changing_speed, estimated_shift_length, end_of_lane_dist, common_parameter, parameter); + // lane changing start pose is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + + if (is_goal_in_route) { + const double s_start = + lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose).length; + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()).length; + if (s_start + lane_changing_distance + parameter.lane_change_finish_judge_buffer > s_goal) { + continue; + } + } + const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance}; const auto lane_changing_segment = getLaneChangingSegment( - route_handler, target_lanelets, forward_path_length, arc_position_from_target.length, + route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, target_lane_length, lc_dist, lane_changing_speed, required_total_min_distance); - if (prepare_segment.points.empty() || lane_changing_segment.points.empty()) { + if (lane_changing_segment.points.empty()) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), - "reference path is empty!! something wrong..."); + "lane changing segment is empty!! something wrong..."); continue; } - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto resample_interval = calcLaneChangeResampleInterval(lane_changing_distance, lane_changing_speed); @@ -711,16 +730,20 @@ double calcLaneChangingDistance( PathWithLaneId getLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const double forward_path_length, const double arc_length_from_target, + const double forward_path_length, const Pose & lane_changing_start_pose, const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, const double lane_changing_speed, const double total_required_min_dist) { - const double s_start = std::invoke([&arc_length_from_target, &dist_prepare_to_lc_end, - &target_lane_length, &total_required_min_dist]() { - const double dist_from_current_pose = arc_length_from_target + dist_prepare_to_lc_end.sum(); - const double end_of_lane_dist_without_buffer = target_lane_length - total_required_min_dist; - return std::min(dist_from_current_pose, end_of_lane_dist_without_buffer); - }); + const double s_start = + std::invoke([&lane_changing_start_pose, &target_lanelets, &dist_prepare_to_lc_end, + &target_lane_length, &total_required_min_dist]() { + const auto arc_to_start_pose = + lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose); + const double dist_from_front_target_lanelet = + arc_to_start_pose.length + dist_prepare_to_lc_end.lane_changing; + const double end_of_lane_dist_without_buffer = target_lane_length - total_required_min_dist; + return std::min(dist_from_front_target_lanelet, end_of_lane_dist_without_buffer); + }); const double s_end = std::invoke([&s_start, &forward_path_length, &target_lane_length, &total_required_min_dist]() { @@ -760,10 +783,9 @@ PathWithLaneId getReferencePathFromTargetLane( const double s_end = std::invoke([&]() { const auto dist_from_lc_start = s_start + lane_changing_distance + forward_path_length; if (is_goal_in_route) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); - const auto dist_to_goal = goal_arc_coordinates.length - min_total_lane_changing_distance; - return std::min(dist_from_lc_start, dist_to_goal); + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; + return std::min(dist_from_lc_start, s_goal); } const auto dist_from_end = target_lane_length - min_total_lane_changing_distance; return std::min(dist_from_lc_start, dist_from_end); From 5a38b2ef1c53fa3def73bbf0dfb9411019012ebd Mon Sep 17 00:00:00 2001 From: yutaka Date: Wed, 22 Mar 2023 00:07:41 +0900 Subject: [PATCH 4/6] update Signed-off-by: yutaka --- .../util/lane_change/util.hpp | 6 +-- .../src/util/lane_change/util.cpp | 46 +++++++++---------- 2 files changed, 24 insertions(+), 28 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp index 013374f769f32..cd328d9f80d57 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp @@ -104,8 +104,8 @@ ShiftLine getLaneChangingShiftLine( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_distance, const double min_total_lane_changing_distance, - const double forward_path_length, const double resample_interval, const bool is_goal_in_route); + const double lane_changing_distance, const double forward_path_length, + const double resample_interval, const bool is_goal_in_route); PathWithLaneId getPrepareSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, @@ -120,7 +120,7 @@ PathWithLaneId getPrepareSegment( PathWithLaneId getLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const Pose & lane_changing_start_pose, - const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, + const double target_lane_length, const double lane_changing_distance, const double lane_changing_speed, const double total_required_min_dist); bool isEgoWithinOriginalLane( diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index fbc78403172c6..3216dada7cd2f 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -365,17 +365,17 @@ std::pair getLaneChangePaths( continue; } - const auto estimated_shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( - target_lanelets, prepare_segment.points.front().point.pose); + // lane changing start pose is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + + const auto estimated_shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); // we assume constant speed during lane change const auto lane_changing_speed = std::max(prepare_speed, minimum_lane_change_velocity); const auto lane_changing_distance = calcLaneChangingDistance( lane_changing_speed, estimated_shift_length, end_of_lane_dist, common_parameter, parameter); - // lane changing start pose is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - if (is_goal_in_route) { const double s_start = lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose).length; @@ -386,11 +386,9 @@ std::pair getLaneChangePaths( } } - const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance}; - const auto lane_changing_segment = getLaneChangingSegment( route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, - target_lane_length, lc_dist, lane_changing_speed, required_total_min_distance); + target_lane_length, lane_changing_distance, lane_changing_speed, required_total_min_distance); if (lane_changing_segment.points.empty()) { RCLCPP_ERROR_STREAM( @@ -402,10 +400,10 @@ std::pair getLaneChangePaths( const auto resample_interval = calcLaneChangeResampleInterval(lane_changing_distance, lane_changing_speed); + const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance}; const auto target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, - lc_dist.lane_changing, required_total_min_distance, forward_path_length, resample_interval, - is_goal_in_route); + lc_dist.lane_changing, forward_path_length, resample_interval, is_goal_in_route); if (target_lane_reference_path.points.empty()) { continue; @@ -731,19 +729,18 @@ double calcLaneChangingDistance( PathWithLaneId getLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const Pose & lane_changing_start_pose, - const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, + const double target_lane_length, const double lane_changing_distance, const double lane_changing_speed, const double total_required_min_dist) { - const double s_start = - std::invoke([&lane_changing_start_pose, &target_lanelets, &dist_prepare_to_lc_end, - &target_lane_length, &total_required_min_dist]() { - const auto arc_to_start_pose = - lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose); - const double dist_from_front_target_lanelet = - arc_to_start_pose.length + dist_prepare_to_lc_end.lane_changing; - const double end_of_lane_dist_without_buffer = target_lane_length - total_required_min_dist; - return std::min(dist_from_front_target_lanelet, end_of_lane_dist_without_buffer); - }); + const double s_start = std::invoke([&lane_changing_start_pose, &target_lanelets, + &lane_changing_distance, &target_lane_length, + &total_required_min_dist]() { + const auto arc_to_start_pose = + lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose); + const double dist_from_front_target_lanelet = arc_to_start_pose.length + lane_changing_distance; + const double end_of_lane_dist_without_buffer = target_lane_length - total_required_min_dist; + return std::min(dist_from_front_target_lanelet, end_of_lane_dist_without_buffer); + }); const double s_end = std::invoke([&s_start, &forward_path_length, &target_lane_length, &total_required_min_dist]() { @@ -773,8 +770,8 @@ PathWithLaneId getLaneChangingSegment( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_distance, const double min_total_lane_changing_distance, - const double forward_path_length, const double resample_interval, const bool is_goal_in_route) + const double lane_changing_distance, const double forward_path_length, + const double resample_interval, const bool is_goal_in_route) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -787,8 +784,7 @@ PathWithLaneId getReferencePathFromTargetLane( lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; return std::min(dist_from_lc_start, s_goal); } - const auto dist_from_end = target_lane_length - min_total_lane_changing_distance; - return std::min(dist_from_lc_start, dist_from_end); + return std::min(dist_from_lc_start, target_lane_length); }); RCLCPP_DEBUG( From ed3cd53f5c16c210235326596eaeca13b75ab118 Mon Sep 17 00:00:00 2001 From: yutaka Date: Tue, 28 Mar 2023 13:37:18 +0900 Subject: [PATCH 5/6] enable collision check with unknown objects Signed-off-by: yutaka --- planning/behavior_path_planner/src/util/lane_change/util.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index 3216dada7cd2f..b86a734b135eb 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -324,7 +324,7 @@ std::pair getLaneChangePaths( const auto sorted_lane_ids = getSortedLaneIds( route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); - constexpr auto ignore_unknown{true}; + constexpr auto ignore_unknown{false}; const auto lateral_buffer = calcLateralBufferForFiltering(common_parameter.vehicle_width, 0.5); LaneChangeTargetObjectIndices dynamic_object_indices; From d7e9ba3ae368084fb69e3d738b2e4eeca48c2d4d Mon Sep 17 00:00:00 2001 From: yutaka Date: Wed, 29 Mar 2023 16:54:46 +0900 Subject: [PATCH 6/6] update Signed-off-by: yutaka --- planning/behavior_path_planner/src/util/lane_change/util.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index c4b33d8bbf058..c3b74ed68730d 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -500,9 +500,7 @@ bool hasEnoughDistance( const double lane_change_required_distance = static_cast(num) * minimum_lane_change_length; - if ( - lane_change_total_distance + lane_change_required_distance > - util::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; }