diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 61584ed06cd8..2f6d2d482da0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -83,6 +83,8 @@ class LaneChangeBase virtual bool hasFinishedAbort() const = 0; + virtual bool isLaneChangeRequired() const = 0; + virtual bool isAbortState() const = 0; virtual bool isAbleToReturnCurrentLane() const = 0; @@ -193,7 +195,7 @@ class LaneChangeBase virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths) const = 0; + LaneChangePaths * candidate_paths, const bool check_safety) const = 0; virtual std::vector getDrivableLanes() const = 0; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 6869ad22cae5..8658ad5d619c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -81,6 +81,8 @@ class NormalLaneChange : public LaneChangeBase bool isAbortState() const override; + bool isLaneChangeRequired() const override; + protected: lanelet::ConstLanelets getCurrentLanes() const override; @@ -96,7 +98,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths) const override; + LaneChangePaths * candidate_paths, const bool check_safety = true) const override; std::vector getDrivableLanes() const override; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 5253a9f22eae..9c9a295b4c83 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -67,22 +67,12 @@ bool LaneChangeInterface::isExecutionRequested() const return true; } - LaneChangePath selected_path; - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path); - - return found_valid_path; + return module_type_->isLaneChangeRequired(); } bool LaneChangeInterface::isExecutionReady() const { - LaneChangePath selected_path; - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path); - - return found_safe_path; + return module_type_->isSafe(); } ModuleStatus LaneChangeInterface::updateState() diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 6a86008213ba..577aeaad5ff1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -89,6 +89,28 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } +bool NormalLaneChange::isLaneChangeRequired() const +{ + const auto current_lanes = getCurrentLanes(); + + if (current_lanes.empty()) { + return false; + } + + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + + if (target_lanes.empty()) { + return false; + } + + // find candidate paths + LaneChangePaths valid_paths{}; + [[maybe_unused]] const auto found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths, false); + + return !valid_paths.empty(); +} + LaneChangePath NormalLaneChange::getLaneChangePath() const { return isAbortState() ? *abort_path_ : status_.lane_change_path; @@ -397,7 +419,7 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - Direction direction, LaneChangePaths * candidate_paths) const + Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const { object_debug_.clear(); if (original_lanelets.empty() || target_lanelets.empty()) { @@ -648,6 +670,10 @@ bool NormalLaneChange::getLaneChangePaths( } candidate_paths->push_back(*candidate_path); + if (!check_safety) { + return false; + } + const auto [is_safe, is_object_coming_from_rear] = utils::lane_change::isLaneChangePathSafe( *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(), common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration,