From 2f41fe068fece3fcbe721c935ca33e898f3c0803 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 18 Apr 2024 17:30:46 +0900 Subject: [PATCH] refactor(lane_change): refactor min and max lane change length Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene.cpp | 6 +- .../utils/utils.hpp | 9 +- .../src/scene.cpp | 61 +++++------- .../src/utils/utils.cpp | 95 +++++++++++-------- 4 files changed, 89 insertions(+), 82 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 27c6713adfead..525d9e247e308 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -273,12 +273,8 @@ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const return std::numeric_limits::infinity(); } - // get minimum lane change distance - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); return utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, shift_intervals, - lane_change_parameters_->backward_length_buffer_for_end_of_lane); + getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); } double AvoidanceByLaneChange::calcLateralOffset() const diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 76331bd98eb9c..cd4f55599d964 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -58,8 +58,11 @@ double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); double calcMinimumLaneChangeLength( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, - const double length_to_intersection = 0.0); + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); + +double calcMinimumLaneChangeLength( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, + const LaneChangeParameters & lane_change_parameters, Direction direction); double calcMaximumLaneChangeLength( const double current_velocity, const LaneChangeParameters & lane_change_parameters, @@ -139,7 +142,7 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path); bool hasEnoughLengthToLaneChangeAfterAbort( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & curent_pose, const double abort_return_dist, const LaneChangeParameters & lane_change_parameters, const Direction direction); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index db96f7c83d72a..57ad52b602e39 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -38,6 +38,7 @@ namespace behavior_path_planner { using motion_utils::calcSignedArcLength; +using utils::lane_change::calcMinimumLaneChangeLength; using utils::traffic_light::getDistanceToNextTrafficLight; NormalLaneChange::NormalLaneChange( @@ -274,8 +275,8 @@ void NormalLaneChange::insertStopPoint( } const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const double lane_change_buffer = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); + const auto lane_change_buffer = + calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -562,10 +563,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & route_handler = getRouteHandler(); const auto & current_pose = getEgoPose(); - const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const auto lane_change_buffer = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + const auto lane_change_buffer = calcMinimumLaneChangeLength( + route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); const auto distance_to_lane_change_end = std::invoke([&]() { auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -922,12 +921,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const lanelet::ConstLanelets & target_backward_lanes) const { const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); + const auto & route_handler = getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto shift_intervals = - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); - const double minimum_lane_change_length = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + const auto minimum_lane_change_length = calcMinimumLaneChangeLength( + route_handler, current_lanes.back(), *lane_change_parameters_, direction_); // Guard if (objects.objects.empty()) { @@ -936,9 +933,9 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // Get path const auto path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); const auto target_path = - route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); const auto current_polygon = utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); @@ -1125,32 +1122,30 @@ bool NormalLaneChange::hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction) const { + if (target_lanes.empty()) { + return false; + } + const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - const double lane_change_length = path.info.length.sum(); - const auto shift_intervals = - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back(), direction); - double minimum_lane_change_length_to_preferred_lane = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + const auto & route_handler = getRouteHandler(); + const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); + const auto minimum_lane_change_length_to_preferred_lane = calcMinimumLaneChangeLength( + route_handler, target_lanes.back(), *lane_change_parameters_, direction); + const double lane_change_length = path.info.length.sum(); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; } - const auto goal_pose = route_handler.getGoalPose(); + const auto goal_pose = route_handler->getGoalPose(); if ( - route_handler.isInGoalRouteSection(current_lanes.back()) && + route_handler->isInGoalRouteSection(current_lanes.back()) && lane_change_length + minimum_lane_change_length_to_preferred_lane > utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { return false; } // return if there are no target lanes - if (target_lanes.empty()) { - return true; - } - if ( lane_change_length + minimum_lane_change_length_to_preferred_lane > utils::getDistanceToEndOfLane(current_pose, target_lanes)) { @@ -1787,10 +1782,8 @@ bool NormalLaneChange::calcAbortPath() const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto direction = getDirection(); - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane( - selected_path.info.current_lanes.back(), direction); - const double minimum_lane_change_length = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( + route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { @@ -1837,7 +1830,7 @@ bool NormalLaneChange::calcAbortPath() } if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - *route_handler, reference_lanelets, current_pose, abort_return_dist, + route_handler, reference_lanelets, current_pose, abort_return_dist, *lane_change_parameters_, direction)) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; @@ -2044,10 +2037,8 @@ bool NormalLaneChange::isVehicleStuck( route_handler->isInGoalRouteSection(current_lanes.back()) ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = - utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); + const auto lane_change_buffer = calcMinimumLaneChangeLength( + route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 97fcc841a546e..97a5ae732ec1b 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -49,6 +49,8 @@ #include #include #include +#include +#include #include #include #include @@ -77,29 +79,40 @@ double calcLaneChangeResampleInterval( } double calcMinimumLaneChangeLength( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, - const double length_to_intersection) + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals) { if (shift_intervals.empty()) { return 0.0; } - const double & vel = lane_change_parameters.minimum_lane_changing_velocity; - const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel); - const double & max_lateral_acc = lat_acc.second; - const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - const double & backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - - double accumulated_length = length_to_intersection; - for (const auto & shift_interval : shift_intervals) { - const double t = - PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); - accumulated_length += vel * t + finish_judge_buffer; + const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; + const auto [min_lat_acc, max_lat_acc] = + lane_change_parameters.lane_change_lat_acc_map.find(min_vel); + const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + + const auto calc_sum = [&](double sum, double shift_interval) { + const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + return sum + (min_vel * t + finish_judge_buffer); + }; + + const auto total_length = + std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); + + const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; + return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); +} + +double calcMinimumLaneChangeLength( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, + const LaneChangeParameters & lane_change_parameters, Direction direction) +{ + if (!route_handler) { + return std::numeric_limits::max(); } - accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); - return accumulated_length; + const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lane, direction); + return utils::lane_change::calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals); } double calcMaximumLaneChangeLength( @@ -110,31 +123,34 @@ double calcMaximumLaneChangeLength( return 0.0; } - const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const double & t_prepare = lane_change_parameters.lane_change_prepare_duration; + const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const auto t_prepare = lane_change_parameters.lane_change_prepare_duration; - double vel = current_velocity; - double accumulated_length = 0.0; - for (const auto & shift_interval : shift_intervals) { + auto vel = current_velocity; + + const auto calc_sum = [&](double sum, double shift_interval) { // prepare section - const double prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; vel = vel + max_acc * t_prepare; // lane changing section - const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel); - const double t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, lat_acc.second); - const double lane_changing_length = + const auto [min_lat_acc, max_lat_acc] = + lane_change_parameters.lane_change_lat_acc_map.find(vel); + const auto t_lane_changing = + PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; - accumulated_length += prepare_length + lane_changing_length + finish_judge_buffer; vel = vel + max_acc * t_lane_changing; - } - accumulated_length += - lane_change_parameters.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); + return sum + (prepare_length + lane_changing_length + finish_judge_buffer); + }; + + const auto total_length = + std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); - return accumulated_length; + const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; + return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); } double calcMinimumAcceleration( @@ -658,23 +674,24 @@ double getLateralShift(const LaneChangePath & path) } bool hasEnoughLengthToLaneChangeAfterAbort( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const double abort_return_dist, const LaneChangeParameters & lane_change_parameters, const Direction direction) { - const auto shift_intervals = - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction); - const double minimum_lane_change_length = - calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals); + if (current_lanes.empty()) { + return false; + } + + const auto minimum_lane_change_length = calcMinimumLaneChangeLength( + route_handler, current_lanes.back(), lane_change_parameters, direction); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; } if ( - route_handler.isInGoalRouteSection(current_lanes.back()) && abort_plus_lane_change_length > - utils::getSignedDistance(current_pose, route_handler.getGoalPose(), current_lanes)) { + utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) { return false; }