fix(lane_change): regulate at the traffic light (autowarefoundation#5457

) * fix(lane_change): regulate at the traffic light Signed-off-by: Zulfaqar Azmi <[email protected]> * fix conflict Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]> Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
tier4 · Nov 22, 2023 · 4d27fa4 · 4d27fa4
1 parent 1e65d39
commit 4d27fa4
Show file tree

Hide file tree

Showing 6 changed files with 29 additions and 0 deletions.
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
@@ -81,6 +81,7 @@
       regulation:
         crosswalk: false
         intersection: false
+        traffic_light: true
 
       # ego vehicle stuck detection
       stuck_detection:

diff --git a/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/...g/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/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/..._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
@@ -45,6 +45,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 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
@@ -84,6 +84,8 @@ LaneChangeModuleManager::LaneChangeModuleManager(
   p.regulate_on_crosswalk = getOrDeclareParameter<bool>(*node, parameter("regulation.crosswalk"));
   p.regulate_on_intersection =
     getOrDeclareParameter<bool>(*node, parameter("regulation.intersection"));
+  p.regulate_on_traffic_light =
+    getOrDeclareParameter<bool>(*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
@@ -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 <lanelet2_extension/utility/message_conversion.hpp>
@@ -38,6 +39,7 @@
 namespace behavior_path_planner
 {
 using motion_utils::calcSignedArcLength;
+using utils::traffic_light::getDistanceToNextTrafficLight;
 
 NormalLaneChange::NormalLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
@@ -1086,6 +1088,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,
@@ -1345,6 +1360,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<ExtendedPredictedObject> filtered_objects =