feat(avoidance): check traffic light info in order to limit drivable …

…area (#6016)

* feat(avoidance): don't use opposite lane before intersection

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(avoidance): check traffic light info in order to limit drivable area

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -166,7 +166,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
   std::for_each(
     data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
       data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
-        lanelet, planner_data_, avoidance_parameters_));
+        lanelet, planner_data_, avoidance_parameters_, false));
     });
 
   // calc drivable bound

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -163,7 +163,7 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
 
 DrivableLanes generateExpandDrivableLanes(
   const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters);
+  const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver);
 
 double calcDistanceToReturnDeadLine(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -223,10 +223,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
     utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_);
 
   // expand drivable lanes
+  const auto has_shift_point = !path_shifter_.getShiftLines().empty();
+  const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted();
   std::for_each(
     data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
-      data.drivable_lanes.push_back(
-        utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_));
+      data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
+        lanelet, planner_data_, parameters_, in_avoidance_maneuver));
     });
 
   // calc drivable bound

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -1953,7 +1953,7 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
 
 DrivableLanes generateExpandDrivableLanes(
   const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters)
+  const std::shared_ptr<AvoidanceParameters> & parameters, const bool in_avoidance_maneuver)
 {
   const auto & route_handler = planner_data->route_handler;
 
@@ -1967,6 +1967,11 @@ DrivableLanes generateExpandDrivableLanes(
 
   // 1. get left/right side lanes
   const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
+    const auto next_lanes = route_handler->getNextLanelets(target_lane);
+    const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
+    if (is_stop_signal && !in_avoidance_maneuver) {
+      return;
+    }
     const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets(
       target_lane, parameters->use_opposite_lane, true);
     if (!all_left_lanelets.empty()) {
@@ -1977,6 +1982,11 @@ DrivableLanes generateExpandDrivableLanes(
     }
   };
   const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
+    const auto next_lanes = route_handler->getNextLanelets(target_lane);
+    const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data);
+    if (is_stop_signal && !in_avoidance_maneuver) {
+      return;
+    }
     const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets(
       target_lane, parameters->use_opposite_lane, true);
     if (!all_right_lanelets.empty()) {

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp

@@ -92,6 +92,21 @@ bool isStoppedAtRedTrafficLightWithinDistance(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data,
   const double distance_threshold = std::numeric_limits<double>::infinity());
+
+/**
+ * @brief Determines if a traffic signal indicates a stop for the given lanelet.
+ *
+ * Evaluates the current state of the traffic light, considering if it's green or unknown,
+ * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet
+ * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can
+ * proceed based on allowed turn directions.
+ *
+ * @param lanelet The lanelet to check for a stop signal at its traffic light.
+ * @param planner_data Shared pointer to the planner data containing vehicle state information.
+ * @return True if the traffic signal indicates a stop is required, false otherwise.
+ */
+bool isTrafficSignalStop(
+  const lanelet::ConstLanelets & lanelets, const std::shared_ptr<const PlannerData> & planner_data);
 }  // namespace behavior_path_planner::utils::traffic_light
 
 #endif  // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_

planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp

@@ -127,4 +127,24 @@ bool isStoppedAtRedTrafficLightWithinDistance(
   return (distance_to_red_traffic_light < distance_threshold);
 }
 
+bool isTrafficSignalStop(
+  const lanelet::ConstLanelets & lanelets, const std::shared_ptr<const PlannerData> & planner_data)
+{
+  for (const auto & lanelet : lanelets) {
+    for (const auto & element : lanelet.regulatoryElementsAs<TrafficLight>()) {
+      const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id());
+      if (!traffic_signal_stamped.has_value()) {
+        continue;
+      }
+
+      if (traffic_light_utils::isTrafficSignalStop(
+            lanelet, traffic_signal_stamped.value().signal)) {
+        return true;
+      }
+    }
+  }
+
+  return false;
+}
+
 }  // namespace behavior_path_planner::utils::traffic_light