diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 39de619c4e3eb..4381932343206 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -169,6 +169,7 @@ class LaneChangeModule : public SceneModuleInterface void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; bool isSafe() const; + bool isValidPath() const; bool isValidPath(const PathWithLaneId & path) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index be14bb120aa42..9ce30b6f42733 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -55,6 +55,7 @@ struct LaneChangeStatus std::vector lane_follow_lane_ids; std::vector lane_change_lane_ids; bool is_safe; + bool is_valid_path = true; double start_distance; }; } // namespace behavior_path_planner 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 135c26d3dfe3a..d6add69549067 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 @@ -59,7 +59,7 @@ BehaviorModuleOutput LaneChangeModule::run() is_activated_ = isActivated(); auto output = plan(); - if (!isSafe()) { + if (!isValidPath()) { current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop return output; } @@ -118,7 +118,7 @@ bool LaneChangeModule::isExecutionReady() const BT::NodeStatus LaneChangeModule::updateState() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState"); - if (!isSafe()) { + if (!isValidPath()) { current_state_ = BT::NodeStatus::SUCCESS; return current_state_; } @@ -148,8 +148,10 @@ BehaviorModuleOutput LaneChangeModule::plan() auto path = status_.lane_change_path.path; if (!isValidPath(path)) { - status_.is_safe = false; + status_.is_valid_path = false; return BehaviorModuleOutput{}; + } else { + status_.is_valid_path = true; } generateExtendedDrivableArea(path); @@ -374,6 +376,8 @@ std::pair LaneChangeModule::getSafePath( bool LaneChangeModule::isSafe() const { return status_.is_safe; } +bool LaneChangeModule::isValidPath() const { return status_.is_valid_path; } + bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const { const auto & route_handler = planner_data_->route_handler;