diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index f98eac5315006..0cb7f3f1a7a92 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -81,6 +81,7 @@ regulation: crosswalk: false intersection: false + traffic_light: true # ego vehicle stuck detection stuck_detection: 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 dc9f44fbd53da..241ac515d415b 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 @@ -135,6 +135,9 @@ class NormalLaneChange : public LaneChangeBase bool hasEnoughLengthToIntersection( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool hasEnoughLengthToTrafficLight( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index c664ae3e15aef..3f53f4b9c51e8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -65,6 +65,7 @@ struct LaneChangeParameters // regulatory elements bool regulate_on_crosswalk{false}; bool regulate_on_intersection{false}; + bool regulate_on_traffic_light{false}; // ego vehicle stuck detection double stop_velocity_threshold{0.1}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 3f9591f6c2143..4d34c548e640e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index ee7b45de09e93..e772c21331ae8 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -84,6 +84,8 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = getOrDeclareParameter(*node, parameter("regulation.intersection")); + p.regulate_on_traffic_light = + getOrDeclareParameter(*node, parameter("regulation.traffic_light")); // ego vehicle stuck detection p.stop_velocity_threshold = 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 f2c6782b3ef6e..7c6f1b3df08f1 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 @@ -19,6 +19,7 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/traffic_light_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -38,6 +39,7 @@ namespace behavior_path_planner { using motion_utils::calcSignedArcLength; +using utils::traffic_light::getDistanceToNextTrafficLight; NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, @@ -1081,6 +1083,19 @@ bool NormalLaneChange::hasEnoughLengthToIntersection( return true; } +bool NormalLaneChange::hasEnoughLengthToTrafficLight( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto dist_to_next_traffic_light = + getDistanceToNextTrafficLight(current_pose, current_lanes); + const auto dist_to_next_traffic_light_from_lc_start_pose = + dist_to_next_traffic_light - path.info.length.prepare; + + return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 || + dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; +} + bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, @@ -1340,6 +1355,12 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "Stop time is over threshold. Allow lane change in intersection."); } + if ( + lane_change_parameters_->regulate_on_traffic_light && + !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { + continue; + } + candidate_paths->push_back(*candidate_path); std::vector filtered_objects =