Merge pull request #1091 from tier4/cherry-pick/intersection

feat(intersection): cherry pick awf autowarefoundation#6036, autowarefoundation#6042, autowarefoundation#6050

Author: soblin

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1055,19 +1055,20 @@ bool CrosswalkModule::isRedSignalForPedestrians() const
     crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>();
 
   for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) {
-    const auto traffic_signal_stamped =
+    const auto traffic_signal_stamped_opt =
       planner_data_->getTrafficSignal(traffic_lights_reg_elem->id());
-    if (!traffic_signal_stamped) {
+    if (!traffic_signal_stamped_opt) {
       continue;
     }
+    const auto traffic_signal_stamped = traffic_signal_stamped_opt.value();
 
     if (
       planner_param_.traffic_light_state_timeout <
-      (clock_->now() - traffic_signal_stamped->stamp).seconds()) {
+      (clock_->now() - traffic_signal_stamped.stamp).seconds()) {
       continue;
     }
 
-    const auto & lights = traffic_signal_stamped->signal.elements;
+    const auto & lights = traffic_signal_stamped.signal.elements;
     if (lights.empty()) {
       continue;
     }

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -72,7 +72,7 @@
           enable: false
           creep_velocity: 0.8333
         peeking_offset: -0.5
-        occlusion_required_clearance_distance: 55
+        occlusion_required_clearance_distance: 55.0
         possible_object_bbox: [1.5, 2.5]
         ignore_parked_vehicle_speed_threshold: 0.8333
         occlusion_detection_hold_time: 1.5

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -188,6 +188,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       &debug_marker_array);
   }
 
+  if (debug_data_.first_attention_area) {
+    appendMarkerArray(
+      createLaneletPolygonsMarkerArray(
+        {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647,
+        0.0),
+      &debug_marker_array, now);
+  }
+
+  if (debug_data_.second_attention_area) {
+    appendMarkerArray(
+      createLaneletPolygonsMarkerArray(
+        {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647,
+        0.0),
+      &debug_marker_array, now);
+  }
+
   if (debug_data_.stuck_vehicle_detect_area) {
     appendMarkerArray(
       debug::createPolygonMarkerArray(

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -152,6 +152,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".occlusion.creep_during_peeking.creep_velocity");
   ip.occlusion.peeking_offset =
     getOrDeclareParameter<double>(node, ns + ".occlusion.peeking_offset");
+  ip.occlusion.occlusion_required_clearance_distance =
+    getOrDeclareParameter<double>(node, ns + ".occlusion.occlusion_required_clearance_distance");
   ip.occlusion.possible_object_bbox =
     getOrDeclareParameter<std::vector<double>>(node, ns + ".occlusion.possible_object_bbox");
   ip.occlusion.ignore_parked_vehicle_speed_threshold =

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -53,6 +53,8 @@ struct DebugData
   std::optional<std::vector<lanelet::CompoundPolygon3d>> occlusion_attention_area{std::nullopt};
   std::optional<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> adjacent_area{std::nullopt};
+  std::optional<lanelet::CompoundPolygon3d> first_attention_area{std::nullopt};
+  std::optional<lanelet::CompoundPolygon3d> second_attention_area{std::nullopt};
   std::optional<geometry_msgs::msg::Polygon> stuck_vehicle_detect_area{std::nullopt};
   std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_stuck_detect_area{std::nullopt};
   std::optional<geometry_msgs::msg::Polygon> candidate_collision_ego_lane_polygon{std::nullopt};
@@ -82,7 +84,8 @@ struct IntersectionLanelets
    */
   void update(
     const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info,
-    const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length);
+    const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length,
+    lanelet::routing::RoutingGraphPtr routing_graph_ptr);
 
   const lanelet::ConstLanelets & attention() const
   {
@@ -131,6 +134,14 @@ struct IntersectionLanelets
   {
     return first_attention_area_;
   }
+  const std::optional<lanelet::ConstLanelet> & second_attention_lane() const
+  {
+    return second_attention_lane_;
+  }
+  const std::optional<lanelet::CompoundPolygon3d> & second_attention_area() const
+  {
+    return second_attention_area_;
+  }
 
   /**
    * the set of attention lanelets which is topologically merged
@@ -178,17 +189,25 @@ struct IntersectionLanelets
   std::vector<double> occlusion_attention_size_;
 
   /**
-   * the attention lanelet which ego path points intersect for the first time
+   * the first conflicting lanelet which ego path points intersect for the first time
    */
   std::optional<lanelet::ConstLanelet> first_conflicting_lane_{std::nullopt};
   std::optional<lanelet::CompoundPolygon3d> first_conflicting_area_{std::nullopt};
 
   /**
-   * the attention lanelet which ego path points intersect for the first time
+   * the first attention lanelet which ego path points intersect for the first time
    */
   std::optional<lanelet::ConstLanelet> first_attention_lane_{std::nullopt};
   std::optional<lanelet::CompoundPolygon3d> first_attention_area_{std::nullopt};
 
+  /**
+   * the second attention lanelet which ego path points intersect next to the
+   * first_attention_lanelet
+   */
+  bool second_attention_lane_empty_{false};
+  std::optional<lanelet::ConstLanelet> second_attention_lane_{std::nullopt};
+  std::optional<lanelet::CompoundPolygon3d> second_attention_area_{std::nullopt};
+
   /**
    * flag if the intersection is prioritized by the traffic light
    */
@@ -219,16 +238,28 @@ struct IntersectionStopLines
    */
   std::optional<size_t> first_attention_stopline{std::nullopt};
 
+  /**
+   * second_attention_stopline is null if ego footprint along the path does not intersect with
+   * second_attention_lane. if path[0] satisfies the condition, it is 0
+   */
+  std::optional<size_t> second_attention_stopline{std::nullopt};
+
   /**
    * occlusion_peeking_stopline is null if path[0] is already inside the attention area
    */
   std::optional<size_t> occlusion_peeking_stopline{std::nullopt};
 
   /**
-   * pass_judge_line is before first_attention_stop_line by the braking distance. if its value is
-   * calculated negative, it is 0
+   * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value
+   * is calculated negative, it is 0
+   */
+  size_t first_pass_judge_line{0};
+
+  /**
+   * second_pass_judge_line is before second_attention_stopline by the braking distance. if its
+   * value is calculated negative, it is 0
    */
-  size_t pass_judge_line{0};
+  size_t second_pass_judge_line{0};
 
   /**
    * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with
@@ -631,8 +662,7 @@ class IntersectionModule : public SceneModuleInterface
    * @fn
    * @brief find TrafficPrioritizedLevel
    */
-  TrafficPrioritizedLevel getTrafficPrioritizedLevel(
-    lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos);
+  TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane);
 
   /**
    * @fn
@@ -651,6 +681,7 @@ class IntersectionModule : public SceneModuleInterface
     lanelet::ConstLanelet assigned_lanelet,
     const lanelet::CompoundPolygon3d & first_conflicting_area,
     const lanelet::ConstLanelet & first_attention_lane,
+    const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
     const util::InterpolatedPathInfo & interpolated_path_info,
     autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
 
@@ -738,6 +769,12 @@ class IntersectionModule : public SceneModuleInterface
     const std::vector<lanelet::ConstLineString3d> & lane_divisions,
     const TargetObjects & target_objects);
 
+  /*
+   * @fn
+   * @brief check if associated traffic light is green
+   */
+  bool isGreenSolidOn(lanelet::ConstLanelet lane);
+
   /*
   bool IntersectionModule::checkFrontVehicleDeceleration(
     lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet,

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -327,7 +327,21 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
     TrafficSignalStamped traffic_signal;
     traffic_signal.stamp = msg->stamp;
     traffic_signal.signal = signal;
-    planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal;
+    planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal;
+    const bool is_unknown_observation =
+      std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) {
+        return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN;
+      });
+    // if the observation is UNKNOWN and past observation is available, only update the timestamp
+    // and keep the body of the info
+    if (
+      is_unknown_observation &&
+      planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) {
+      planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp =
+        msg->stamp;
+    } else {
+      planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal;
+    }
   }
 }
 

planning/behavior_velocity_planner/src/node.cpp

@@ -76,7 +76,10 @@ struct PlannerData
   double ego_nearest_yaw_threshold;
 
   // other internal data
-  std::map<int, TrafficSignalStamped> traffic_light_id_map;
+  // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the
+  // last observed infomation for UNKNOWN
+  std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
+  std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
   std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
   tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
@@ -125,12 +128,20 @@ struct PlannerData
     return true;
   }
 
-  std::shared_ptr<TrafficSignalStamped> getTrafficSignal(const int id) const
+  /**
+   *@fn
+   *@brief queries the traffic signal information of given Id. if keep_last_observation = true,
+   *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation
+   */
+  std::optional<TrafficSignalStamped> getTrafficSignal(
+    const lanelet::Id id, const bool keep_last_observation = false) const
   {
+    const auto & traffic_light_id_map =
+      keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_;
     if (traffic_light_id_map.count(id) == 0) {
-      return {};
+      return std::nullopt;
     }
-    return std::make_shared<TrafficSignalStamped>(traffic_light_id_map.at(id));
+    return std::make_optional<TrafficSignalStamped>(traffic_light_id_map.at(id));
   }
 };
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -353,15 +353,15 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
 bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const
 {
   // get traffic signal associated with the regulatory element id
-  const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id());
-  if (!traffic_signal_stamped) {
+  const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal(
+    traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */);
+  if (!traffic_signal_stamped_opt) {
     RCLCPP_WARN_THROTTLE(
       logger_, *clock_, 5000 /* ms */,
       "the traffic signal data associated with regulatory element id is not received");
     return false;
   }
-
-  valid_traffic_signal = *traffic_signal_stamped;
+  valid_traffic_signal = traffic_signal_stamped_opt.value();
   return true;
 }