From 6852936fbe2d75e1e2a1e71b86cd8ed9180e0e5b Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Wed, 1 Nov 2023 18:10:42 +0900 Subject: [PATCH 1/2] fix(lane_change): regulate at the traffic light Signed-off-by: Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 1 + .../scene_module/lane_change/normal.hpp | 4 ++++ .../lane_change/lane_change_module_data.hpp | 1 + .../behavior_path_planner/utils/utils.hpp | 1 + .../src/scene_module/lane_change/manager.cpp | 2 ++ .../src/scene_module/lane_change/normal.cpp | 22 +++++++++++++++++++ 6 files changed, 31 insertions(+) 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..c1f2fda4b82dc 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,10 @@ 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, + lanelet::Id & traffic_light_id) 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..8d80b415ae3b8 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 @@ -1081,6 +1081,21 @@ bool NormalLaneChange::hasEnoughLengthToIntersection( return true; } +bool NormalLaneChange::hasEnoughLengthToTrafficLight( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, + lanelet::Id & traffic_light_id) const +{ + const auto current_pose = getEgoPose(); + const auto [next_traffic_light_id, dist_to_next_traffic_light] = + utils::getDistanceToNextTrafficLight(current_pose, current_lanes); + traffic_light_id = next_traffic_light_id; + 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,13 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "Stop time is over threshold. Allow lane change in intersection."); } + auto traffic_light_id = lanelet::InvalId; + if ( + lane_change_parameters_->regulate_on_traffic_light && + !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes, traffic_light_id)) { + continue; + } + candidate_paths->push_back(*candidate_path); std::vector filtered_objects = From 64a80d83b0189812c6392d4b29afc6140ba2732c Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 7 Nov 2023 15:40:53 +0900 Subject: [PATCH 2/2] fix conflict Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/normal.hpp | 3 +-- .../src/scene_module/lane_change/normal.cpp | 13 ++++++------- 2 files changed, 7 insertions(+), 9 deletions(-) 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 c1f2fda4b82dc..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 @@ -136,8 +136,7 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; bool hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - lanelet::Id & traffic_light_id) const; + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, 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 8d80b415ae3b8..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, @@ -1082,13 +1084,11 @@ bool NormalLaneChange::hasEnoughLengthToIntersection( } bool NormalLaneChange::hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - lanelet::Id & traffic_light_id) const + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const { const auto current_pose = getEgoPose(); - const auto [next_traffic_light_id, dist_to_next_traffic_light] = - utils::getDistanceToNextTrafficLight(current_pose, current_lanes); - traffic_light_id = next_traffic_light_id; + 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; @@ -1355,10 +1355,9 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "Stop time is over threshold. Allow lane change in intersection."); } - auto traffic_light_id = lanelet::InvalId; if ( lane_change_parameters_->regulate_on_traffic_light && - !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes, traffic_light_id)) { + !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { continue; }