feat(intersection): consider amber traffic signal for collision detec…

…tion level (autowarefoundation#5096)

* feat(intersection): consider amber traffic signal for collision detection level

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* protected -> prioritized

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -24,12 +24,15 @@
         state_transit_margin_time: 1.0
         min_predicted_path_confidence: 0.05
         minimum_ego_predicted_velocity: 1.388 # [m/s]
-        normal:
-          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
-          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
-        relaxed:
+        fully_prioritized:
           collision_start_margin_time: 2.0
           collision_end_margin_time: 0.0
+        partially_prioritized:
+          collision_start_margin_time: 2.0
+          collision_end_margin_time: 2.0
+        not_prioritized:
+          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
+          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
         keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
 
       occlusion:

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -84,14 +84,23 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".collision_detection.minimum_ego_predicted_velocity");
   ip.collision_detection.state_transit_margin_time =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.state_transit_margin_time");
-  ip.collision_detection.normal.collision_start_margin_time = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.normal.collision_start_margin_time");
-  ip.collision_detection.normal.collision_end_margin_time = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.normal.collision_end_margin_time");
-  ip.collision_detection.relaxed.collision_start_margin_time = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.relaxed.collision_start_margin_time");
-  ip.collision_detection.relaxed.collision_end_margin_time = getOrDeclareParameter<double>(
-    node, ns + ".collision_detection.relaxed.collision_end_margin_time");
+  ip.collision_detection.fully_prioritized.collision_start_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time");
+  ip.collision_detection.fully_prioritized.collision_end_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time");
+  ip.collision_detection.partially_prioritized.collision_start_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time");
+  ip.collision_detection.partially_prioritized.collision_end_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time");
+  ip.collision_detection.not_prioritized.collision_start_margin_time =
+    getOrDeclareParameter<double>(
+      node, ns + ".collision_detection.not_prioritized.collision_start_margin_time");
+  ip.collision_detection.not_prioritized.collision_end_margin_time = getOrDeclareParameter<double>(
+    node, ns + ".collision_detection.not_prioritized.collision_end_margin_time");
   ip.collision_detection.keep_detection_vel_thr =
     getOrDeclareParameter<double>(node, ns + ".collision_detection.keep_detection_vel_thr");
 

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -752,9 +752,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
       planner_param_.occlusion.occlusion_attention_area_length,
       planner_param_.common.consider_wrong_direction_vehicle);
   }
-  const bool tl_arrow_solid_on =
-    util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
-  intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info);
+  const auto traffic_prioritized_level =
+    util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map);
+  const bool is_prioritized =
+    traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED;
+  intersection_lanelets_.value().update(is_prioritized, interpolated_path_info);
 
   const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting();
   const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area();
@@ -885,22 +887,22 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   }
 
   // calculate dynamic collision around detection area
-  const double time_delay = (is_go_out_ || tl_arrow_solid_on)
+  const double time_delay = (is_go_out_ || is_prioritized)
                               ? 0.0
                               : (planner_param_.collision_detection.state_transit_margin_time -
                                  collision_state_machine_.getDuration());
   const auto target_objects =
     filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area);
 
   const bool has_collision = checkCollision(
-    *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on);
+    *path, target_objects, path_lanelets, closest_idx, time_delay, traffic_prioritized_level);
   collision_state_machine_.setStateWithMarginTime(
     has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("collision state_machine"), *clock_);
   const bool has_collision_with_margin =
     collision_state_machine_.getState() == StateMachine::State::STOP;
 
-  if (tl_arrow_solid_on) {
+  if (is_prioritized) {
     return TrafficLightArrowSolidOn{
       has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx};
   }
@@ -926,7 +928,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
                object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh;
     });
   const bool is_occlusion_cleared =
-    (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on)
+    (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized)
       ? isOcclusionCleared(
           *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets,
           first_attention_area, interpolated_path_info, occlusion_attention_divisions,
@@ -1053,7 +1055,7 @@ bool IntersectionModule::checkCollision(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const autoware_auto_perception_msgs::msg::PredictedObjects & objects,
   const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay,
-  const bool tl_arrow_solid_on)
+  const util::TrafficPrioritizedLevel & traffic_prioritized_level)
 {
   using lanelet::utils::getArcCoordinates;
   using lanelet::utils::getPolygonFromArcLength;
@@ -1076,12 +1078,21 @@ bool IntersectionModule::checkCollision(
 
   const auto ego_poly = ego_lane.polygon2d().basicPolygon();
   // check collision between predicted_path and ego_area
-  const double collision_start_margin_time =
-    tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_start_margin_time
-                      : planner_param_.collision_detection.normal.collision_start_margin_time;
-  const double collision_end_margin_time =
-    tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_end_margin_time
-                      : planner_param_.collision_detection.normal.collision_end_margin_time;
+  const auto [collision_start_margin_time, collision_end_margin_time] = [&]() {
+    if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) {
+      return std::make_pair(
+        planner_param_.collision_detection.fully_prioritized.collision_start_margin_time,
+        planner_param_.collision_detection.fully_prioritized.collision_end_margin_time);
+    }
+    if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) {
+      return std::make_pair(
+        planner_param_.collision_detection.partially_prioritized.collision_start_margin_time,
+        planner_param_.collision_detection.partially_prioritized.collision_end_margin_time);
+    }
+    return std::make_pair(
+      planner_param_.collision_detection.not_prioritized.collision_start_margin_time,
+      planner_param_.collision_detection.not_prioritized.collision_end_margin_time);
+  }();
   bool collision_detected = false;
   for (const auto & object : target_objects.objects) {
     for (const auto & predicted_path : object.kinematics.predicted_paths) {

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -79,16 +79,21 @@ class IntersectionModule : public SceneModuleInterface
       //! minimum confidence value of predicted path to use for collision detection
       double minimum_ego_predicted_velocity;  //! used to calculate ego's future velocity profile
       double state_transit_margin_time;
-      struct Normal
+      struct FullyPrioritized
       {
         double collision_start_margin_time;  //! start margin time to check collision
         double collision_end_margin_time;    //! end margin time to check collision
-      } normal;
-      struct Relaxed
+      } fully_prioritized;
+      struct PartiallyPrioritized
       {
-        double collision_start_margin_time;
-        double collision_end_margin_time;
-      } relaxed;
+        double collision_start_margin_time;  //! start margin time to check collision
+        double collision_end_margin_time;    //! end margin time to check collision
+      } partially_prioritized;
+      struct NotPrioritized
+      {
+        double collision_start_margin_time;  //! start margin time to check collision
+        double collision_end_margin_time;    //! end margin time to check collision
+      } not_prioritized;
       double keep_detection_vel_thr;  //! keep detection if ego is ego.vel < keep_detection_vel_thr
     } collision_detection;
     struct Occlusion
@@ -250,7 +255,7 @@ class IntersectionModule : public SceneModuleInterface
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects,
     const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay,
-    const bool tl_arrow_solid_on);
+    const util::TrafficPrioritizedLevel & traffic_prioritized_level);
 
   bool isOcclusionCleared(
     const nav_msgs::msg::OccupancyGrid & occ_grid,

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -763,37 +763,40 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane)
   return tl_id.has_value();
 }
 
-bool isTrafficLightArrowActivated(
+TrafficPrioritizedLevel getTrafficPrioritizedLevel(
   lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos)
 {
   using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
 
-  const auto & turn_direction = lane.attributeOr("turn_direction", "else");
   std::optional<int> tl_id = std::nullopt;
   for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
     tl_id = tl_reg_elem->id();
     break;
   }
   if (!tl_id) {
     // this lane has no traffic light
-    return false;
+    return TrafficPrioritizedLevel::NOT_PRIORITIZED;
   }
   const auto tl_info_it = tl_infos.find(tl_id.value());
   if (tl_info_it == tl_infos.end()) {
     // the info of this traffic light is not available
-    return false;
+    return TrafficPrioritizedLevel::NOT_PRIORITIZED;
   }
   const auto & tl_info = tl_info_it->second;
+  bool has_amber_signal{false};
   for (auto && tl_light : tl_info.signal.elements) {
-    if (tl_light.color != TrafficSignalElement::GREEN) continue;
-    if (tl_light.status != TrafficSignalElement::SOLID_ON) continue;
-    if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW)
-      return true;
-    if (
-      turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW)
-      return true;
+    if (tl_light.color == TrafficSignalElement::AMBER) {
+      has_amber_signal = true;
+    }
+    if (tl_light.color == TrafficSignalElement::RED) {
+      // NOTE: Return here since the red signal has the highest priority.
+      return TrafficPrioritizedLevel::FULLY_PRIORITIZED;
+    }
   }
-  return false;
+  if (has_amber_signal) {
+    return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
+  }
+  return TrafficPrioritizedLevel::NOT_PRIORITIZED;
 }
 
 std::vector<DiscretizedLane> generateDetectionLaneDivisions(
@@ -1176,9 +1179,9 @@ double calcDistanceUntilIntersectionLanelet(
 }
 
 void IntersectionLanelets::update(
-  const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info)
+  const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info)
 {
-  tl_arrow_solid_on_ = tl_arrow_solid_on;
+  is_prioritized_ = is_prioritized;
   // find the first conflicting/detection area polygon intersecting the path
   const auto & path = interpolated_path_info.path;
   const auto & lane_interval = interpolated_path_info.lane_id_interval.value();

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -111,7 +111,8 @@ std::optional<Polygon2d> getIntersectionArea(
   lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr);
 
 bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane);
-bool isTrafficLightArrowActivated(
+
+TrafficPrioritizedLevel getTrafficPrioritizedLevel(
   lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos);
 
 std::vector<DiscretizedLane> generateDetectionLaneDivisions(

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -64,20 +64,20 @@ struct InterpolatedPathInfo
 struct IntersectionLanelets
 {
 public:
-  void update(const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info);
+  void update(const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info);
   const lanelet::ConstLanelets & attention() const
   {
-    return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_;
+    return is_prioritized_ ? attention_non_preceding_ : attention_;
   }
   const lanelet::ConstLanelets & conflicting() const { return conflicting_; }
   const lanelet::ConstLanelets & adjacent() const { return adjacent_; }
   const lanelet::ConstLanelets & occlusion_attention() const
   {
-    return tl_arrow_solid_on_ ? attention_non_preceding_ : occlusion_attention_;
+    return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_;
   }
   const std::vector<lanelet::CompoundPolygon3d> & attention_area() const
   {
-    return tl_arrow_solid_on_ ? attention_non_preceding_area_ : attention_area_;
+    return is_prioritized_ ? attention_non_preceding_area_ : attention_area_;
   }
   const std::vector<lanelet::CompoundPolygon3d> & conflicting_area() const
   {
@@ -110,7 +110,7 @@ struct IntersectionLanelets
   // the first area intersecting with the path
   // even if lane change/re-routing happened on the intersection, these areas area are supposed to
   // be invariant under the 'associative' lanes.
-  bool tl_arrow_solid_on_ = false;
+  bool is_prioritized_ = false;
   std::optional<lanelet::CompoundPolygon3d> first_conflicting_area_ = std::nullopt;
   std::optional<lanelet::CompoundPolygon3d> first_attention_area_ = std::nullopt;
 };
@@ -151,6 +151,14 @@ struct PathLanelets
                                          // conflicting lanelets plus the next lane part of the path
 };
 
+enum class TrafficPrioritizedLevel {
+  // The target lane's traffic signal is red or the ego's traffic signal has an arrow.
+  FULLY_PRIORITIZED = 0,
+  // The target lane's traffic signal is amber
+  PARTIALLY_PRIORITIZED,
+  // The target lane's traffic signal is green
+  NOT_PRIORITIZED
+};
 }  // namespace behavior_velocity_planner::util
 
 #endif  // UTIL_TYPE_HPP_