diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/helper.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/helper.hpp index ae13117c2f2ca..bde577fa40da3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/helper.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/helper.hpp @@ -57,8 +57,10 @@ class Helper BehaviorPathPlannerParameters common_params_; std::shared_ptr lc_params_; - [[nodiscard]] Pose get_ego_pose() const; - double get_signed_distance_from_current_pose(const lanelet::ConstLanelets & lanes) const; + Pose get_ego_pose() const; + double calc_signed_distance_from_current_pose(const lanelet::ConstLanelets & lanes) const; + + [[nodiscard]] bool is_in_goal_route_section(const lanelet::ConstLanelets & lanes) const; lanelet::ConstLanelets current_lanes_; lanelet::ConstLanelets target_lanes_; diff --git a/planning/behavior_path_lane_change_module/src/utils/helper.cpp b/planning/behavior_path_lane_change_module/src/utils/helper.cpp index 32e835df80798..350fe8d97d9b0 100644 --- a/planning/behavior_path_lane_change_module/src/utils/helper.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/helper.cpp @@ -93,7 +93,7 @@ bool Helper::is_near_end_of_current_lanes() const const auto min_lc_buffer = calc_min_lane_change_length(current_lanes_); - const auto remaining_distance = get_signed_distance_from_current_pose(current_lanes_); + const auto remaining_distance = calc_signed_distance_from_current_pose(current_lanes_); const auto threshold = common_params_.base_link2front + lc_params_->min_length_for_turn_signal_activation; @@ -120,7 +120,7 @@ bool Helper::has_enough_length_to_retry(const double abort_return_dist, Directio const auto required_length = calc_min_lane_change_length(current_lanes_, direction) + abort_return_dist; - const auto remaining_dist = get_signed_distance_from_current_pose(current_lanes_); + const auto remaining_dist = calc_signed_distance_from_current_pose(current_lanes_); return required_length <= remaining_dist; } @@ -130,18 +130,23 @@ Pose Helper::get_ego_pose() const return self_odometry_->pose.pose; } -double Helper::get_signed_distance_from_current_pose(const lanelet::ConstLanelets & lanes) const +double Helper::calc_signed_distance_from_current_pose(const lanelet::ConstLanelets & lanes) const { if (lanes.empty()) { return std::numeric_limits::infinity(); } - const auto is_in_goal_route_section = route_handler_->isInGoalRouteSection(lanes.back()); - - if (is_in_goal_route_section) { + if (is_in_goal_route_section(lanes)) { return utils::getSignedDistance(get_ego_pose(), route_handler_->getGoalPose(), current_lanes_); } return utils::getDistanceToEndOfLane(get_ego_pose(), current_lanes_); } + +bool Helper::is_in_goal_route_section(const lanelet::ConstLanelets & lanes) const +{ + return std::any_of(lanes.rbegin(), lanes.rend(), [&](const auto & lane) { + return route_handler_->isInGoalRouteSection(lane); + }); +} } // namespace behavior_path_planner::helper::lane_change