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 <[email protected]> * update config Signed-off-by: Takayuki Murooka <[email protected]> * protected -> prioritized Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
soblin · Nov 17, 2023 · 50183b6 · 50183b6
1 parent ef8799d
commit 50183b6
Show file tree

Hide file tree

Showing 7 changed files with 101 additions and 59 deletions.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/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:

diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -81,14 +81,23 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter<double>(ns + ".collision_detection.minimum_ego_predicted_velocity");
   ip.collision_detection.state_transit_margin_time =
     node.declare_parameter<double>(ns + ".collision_detection.state_transit_margin_time");
-  ip.collision_detection.normal.collision_start_margin_time =
-    node.declare_parameter<double>(ns + ".collision_detection.normal.collision_start_margin_time");
-  ip.collision_detection.normal.collision_end_margin_time =
-    node.declare_parameter<double>(ns + ".collision_detection.normal.collision_end_margin_time");
-  ip.collision_detection.relaxed.collision_start_margin_time =
-    node.declare_parameter<double>(ns + ".collision_detection.relaxed.collision_start_margin_time");
-  ip.collision_detection.relaxed.collision_end_margin_time =
-    node.declare_parameter<double>(ns + ".collision_detection.relaxed.collision_end_margin_time");
+  ip.collision_detection.fully_prioritized.collision_start_margin_time =
+    node.declare_parameter<double>(
+      ns + ".collision_detection.fully_prioritized.collision_start_margin_time");
+  ip.collision_detection.fully_prioritized.collision_end_margin_time =
+    node.declare_parameter<double>(
+      ns + ".collision_detection.fully_prioritized.collision_end_margin_time");
+  ip.collision_detection.partially_prioritized.collision_start_margin_time =
+    node.declare_parameter<double>(
+      ns + ".collision_detection.partially_prioritized.collision_start_margin_time");
+  ip.collision_detection.partially_prioritized.collision_end_margin_time =
+    node.declare_parameter<double>(
+      ns + ".collision_detection.partially_prioritized.collision_end_margin_time");
+  ip.collision_detection.not_prioritized.collision_start_margin_time =
+    node.declare_parameter<double>(
+      ns + ".collision_detection.not_prioritized.collision_start_margin_time");
+  ip.collision_detection.not_prioritized.collision_end_margin_time = node.declare_parameter<double>(
+    ns + ".collision_detection.not_prioritized.collision_end_margin_time");
   ip.collision_detection.keep_detection_vel_thr =
     node.declare_parameter<double>(ns + ".collision_detection.keep_detection_vel_thr");
 

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -853,10 +853,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 
   // at the very first time of registration of this module, the path may not be conflicting with the
   // attention area, so update() is called to update the internal data as well as traffic light info
-  const bool tl_arrow_solid_on =
-    util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
+  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;
   const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
-  intersection_lanelets.update(tl_arrow_solid_on, interpolated_path_info, footprint);
+  intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint);
 
   // this is abnormal
   const auto & conflicting_lanelets = intersection_lanelets.conflicting();
@@ -1010,23 +1012,22 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   // get intersection area
   const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
 
+  // calculate dynamic collision around detection area
+  const double time_delay = (is_go_out_ || is_prioritized)
+                              ? 0.0
+                              : (planner_param_.collision_detection.state_transit_margin_time -
+                                 collision_state_machine_.getDuration());
   auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area);
 
-  // calculate dynamic collision around attention area
-  const double time_to_restart = (is_go_out_ || tl_arrow_solid_on)
-                                   ? 0.0
-                                   : (planner_param_.collision_detection.state_transit_margin_time -
-                                      collision_state_machine_.getDuration());
-
   const bool has_collision = checkCollision(
-    *path, &target_objects, path_lanelets, closest_idx, time_to_restart, 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};
   }
@@ -1045,7 +1046,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) /
     (2 * planner_param_.occlusion.min_vehicle_brake_for_rss));
   auto occlusion_status =
-    (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on)
+    (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized)
       ? getOcclusionStatus(
           *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets,
           first_attention_area, interpolated_path_info, occlusion_attention_divisions,
@@ -1256,7 +1257,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects(
 bool IntersectionModule::checkCollision(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
-  const int closest_idx, const double time_delay, const bool tl_arrow_solid_on)
+  const int closest_idx, const double time_delay,
+  const util::TrafficPrioritizedLevel & traffic_prioritized_level)
 {
   using lanelet::utils::getArcCoordinates;
   using lanelet::utils::getPolygonFromArcLength;
@@ -1277,12 +1279,21 @@ bool IntersectionModule::checkCollision(
   debug_data_.ego_lane = ego_lane.polygon3d();
   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 & target_object : target_objects->all_attention_objects) {
     const auto & object = target_object.object;

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -78,16 +78,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
@@ -295,7 +300,8 @@ class IntersectionModule : public SceneModuleInterface
   bool checkCollision(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
-    const int closest_idx, const double time_delay, const bool tl_arrow_solid_on);
+    const int closest_idx, const double time_delay,
+    const util::TrafficPrioritizedLevel & traffic_prioritized_level);
 
   OcclusionType getOcclusionStatus(
     const nav_msgs::msg::OccupancyGrid & occ_grid,

diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -975,37 +975,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;
 }
 
 double getHighestCurvature(const lanelet::ConstLineString3d & centerline)
@@ -1335,10 +1338,10 @@ 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,
   const tier4_autoware_utils::LinearRing2d & footprint)
 {
-  tl_arrow_solid_on_ = tl_arrow_solid_on;
+  is_prioritized_ = is_prioritized;
   // find the first conflicting/detection area polygon intersecting the path
   if (!first_conflicting_area_) {
     auto first =

diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp
@@ -103,7 +103,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<lanelet::ConstLineString3d> generateDetectionLaneDivisions(

diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp
@@ -71,25 +71,25 @@ struct IntersectionLanelets
 {
 public:
   void update(
-    const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info,
+    const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info,
     const tier4_autoware_utils::LinearRing2d & footprint);
   const lanelet::ConstLanelets & attention() const
   {
-    return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_;
+    return is_prioritized_ ? attention_non_preceding_ : attention_;
   }
   const std::vector<std::optional<lanelet::ConstLineString3d>> & attention_stop_lines() const
   {
-    return tl_arrow_solid_on_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_;
+    return is_prioritized_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_;
   }
   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
   {
@@ -141,7 +141,7 @@ struct IntersectionLanelets
   std::optional<lanelet::CompoundPolygon3d> first_conflicting_area_{std::nullopt};
   std::optional<lanelet::ConstLanelet> first_attention_lane_{std::nullopt};
   std::optional<lanelet::CompoundPolygon3d> first_attention_area_{std::nullopt};
-  bool tl_arrow_solid_on_ = false;
+  bool is_prioritized_ = false;
 };
 
 struct IntersectionStopLines
@@ -192,6 +192,15 @@ struct TargetObjects
   std::vector<TargetObject> intersection_area_objects;
   std::vector<TargetObject> all_attention_objects;  // TODO(Mamoru Sobue): avoid copy
 };
+
+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_