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 94a4cb9462a2c..f8d80d8883b38 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 @@ -85,6 +85,8 @@ class LaneChangeBase virtual bool hasFinishedAbort() const = 0; + virtual bool isLaneChangeRequired() const = 0; + virtual bool isAbortState() const = 0; virtual bool isAbleToReturnCurrentLane() const = 0; @@ -197,7 +199,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 6572d4f8221b0..f7c6cdc07a200 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 @@ -85,6 +85,8 @@ class NormalLaneChange : public LaneChangeBase bool isAbortState() const override; + bool isLaneChangeRequired() const override; + protected: lanelet::ConstLanelets getCurrentLanes() const override; @@ -100,7 +102,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 81df39cb8f789..26051c4d78f3e 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 548d23642581e..7f0d84fc94aed 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; @@ -472,7 +494,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()) { @@ -706,6 +728,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,