feat(intersection): ensure temporal stop before peeking into oncoming…

… lane (autowarefoundation#5047)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -45,9 +45,14 @@
         min_vehicle_brake_for_rss: -2.5 # [m/s^2]
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
-        possible_object_bbox: [1.0, 2.0] # [m x m]
-        ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h
-        stop_release_margin_time: 1.0 # [s]
+        possible_object_bbox: [1.5, 2.5] # [m x m]
+        ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h
+        stop_release_margin_time: 1.5 # [s]
+        temporal_stop_before_attention_area: false
+
+      enable_rtc:
+        intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
+        intersection_to_occlusion: true
 
       enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
         intersection: true

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -115,6 +115,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter<double>(ns + ".occlusion.ignore_parked_vehicle_speed_threshold");
   ip.occlusion.stop_release_margin_time =
     node.declare_parameter<double>(ns + ".occlusion.stop_release_margin_time");
+  ip.occlusion.temporal_stop_before_attention_area =
+    node.declare_parameter<bool>(ns + ".occlusion.temporal_stop_before_attention_area");
 }
 
 void IntersectionModuleManager::launchNewModules(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -94,6 +94,11 @@
     occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time);
     occlusion_stop_state_machine_.setState(StateMachine::State::GO);
   }
+  {
+    temporal_stop_before_attention_state_machine_.setMarginTime(
+      planner_param_.occlusion.before_creep_stop_time);
+    temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP);
+  }
 
   decision_state_pub_ =
     node_.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
@@ -476,7 +481,10 @@
   // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved
 
   if (!rtc_occlusion_approved && planner_param.occlusion.enable) {
-    const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx;
+    const size_t occlusion_peeking_stop_line =
+      decision_result.temporal_stop_before_attention_required
+        ? decision_result.first_attention_stop_line_idx
+        : decision_result.occlusion_stop_line_idx;
     if (planner_param.occlusion.enable_creeping) {
       const size_t closest_idx = decision_result.closest_idx;
       for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) {
@@ -540,7 +548,9 @@
     }
   }
   if (!rtc_occlusion_approved && planner_param.occlusion.enable) {
-    const auto stop_line_idx = decision_result.occlusion_stop_line_idx;
+    const auto stop_line_idx = decision_result.temporal_stop_before_attention_required
+                                 ? decision_result.first_attention_stop_line_idx
+                                 : decision_result.occlusion_stop_line_idx;
     planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path);
     debug_data->occlusion_stop_wall_pose =
       planning_utils::getAheadPose(stop_line_idx, baselink2front, *path);
@@ -778,7 +788,8 @@
   const auto & intersection_stop_lines = intersection_stop_lines_opt.value();
   const auto
     [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt,
-     occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines;
+     first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] =
+      intersection_stop_lines;
 
   const auto & conflicting_area = intersection_lanelets_.value().conflicting_area();
   const auto path_lanelets_opt = util::generatePathLanelets(
@@ -857,12 +868,13 @@
     return IntersectionModule::Indecisive{};
   }
 
-  if (!occlusion_peeking_stop_line_idx_opt) {
+  if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) {
     RCLCPP_DEBUG(logger_, "occlusion stop line is null");
     return IntersectionModule::Indecisive{};
   }
   const auto collision_stop_line_idx =
     is_over_default_stop_line ? closest_idx : default_stop_line_idx;
+  const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value();
   const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value();
 
   const auto & attention_lanelets = intersection_lanelets_.value().attention();
@@ -938,35 +950,63 @@
   if (
     occlusion_stop_state_machine_.getState() == StateMachine::State::STOP ||
     ext_occlusion_requested) {
-    const double dist_stopline = motion_utils::calcSignedArcLength(
+    const double dist_default_stopline = motion_utils::calcSignedArcLength(
       path->points, path->points.at(closest_idx).point.pose.position,
       path->points.at(default_stop_line_idx).point.pose.position);
-    const bool approached_stop_line =
-      (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin);
-    const bool over_stop_line = (dist_stopline < 0.0);
-    const bool is_stopped = planner_data_->isVehicleStopped();
-    if (over_stop_line) {
+    const bool approached_default_stop_line =
+      (std::fabs(dist_default_stopline) < planner_param_.common.stop_overshoot_margin);
+    const bool over_default_stop_line = (dist_default_stopline < 0.0);
+    const bool is_stopped_at_default =
+      planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time);
+    if (over_default_stop_line) {
       before_creep_state_machine_.setState(StateMachine::State::GO);
     }
     if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
+      const double dist_first_attention_stopline = motion_utils::calcSignedArcLength(
+        path->points, path->points.at(closest_idx).point.pose.position,
+        path->points.at(first_attention_stop_line_idx).point.pose.position);
+      const bool approached_first_attention_stop_line =
+        (std::fabs(dist_first_attention_stopline) < planner_param_.common.stop_overshoot_margin);
+      const bool over_first_attention_stop_line = (dist_first_attention_stopline < 0.0);
+      const bool is_stopped_at_first_attention =
+        planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time);
+      if (planner_param_.occlusion.temporal_stop_before_attention_area) {
+        if (over_first_attention_stop_line) {
+          temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO);
+        }
+        if (is_stopped_at_first_attention && approached_first_attention_stop_line) {
+          temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO);
+        }
+      }
+      const bool temporal_stop_before_attention_required =
+        planner_param_.occlusion.temporal_stop_before_attention_area &&
+        temporal_stop_before_attention_state_machine_.getState() == StateMachine::State::STOP;
       if (has_collision_with_margin) {
-        return IntersectionModule::OccludedCollisionStop{
-          is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx,
-          occlusion_stop_line_idx};
+        return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin,
+                                                         temporal_stop_before_attention_required,
+                                                         closest_idx,
+                                                         collision_stop_line_idx,
+                                                         first_attention_stop_line_idx,
+                                                         occlusion_stop_line_idx};
       } else {
-        return IntersectionModule::PeekingTowardOcclusion{
-          is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx,
-          occlusion_stop_line_idx};
+        return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin,
+                                                          temporal_stop_before_attention_required,
+                                                          closest_idx,
+                                                          collision_stop_line_idx,
+                                                          first_attention_stop_line_idx,
+                                                          occlusion_stop_line_idx};
       }
     } else {
-      if (is_stopped && approached_stop_line) {
+      if (is_stopped_at_default && approached_default_stop_line) {
         // start waiting at the first stop line
         before_creep_state_machine_.setStateWithMarginTime(
           StateMachine::State::GO, logger_.get_child("occlusion state_machine"), *clock_);
       }
+      const auto occlusion_stop_line = planner_param_.occlusion.temporal_stop_before_attention_area
+                                         ? first_attention_stop_line_idx
+                                         : occlusion_stop_line_idx;
       return IntersectionModule::FirstWaitBeforeOcclusion{
-        is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx,
-        occlusion_stop_line_idx};
+        is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line};
     }
   } else if (has_collision_with_margin) {
     return IntersectionModule::NonOccludedCollisionStop{
@@ -1175,7 +1215,7 @@
   const lanelet::ConstLanelets & adjacent_lanelets,
   const lanelet::CompoundPolygon3d & first_attention_area,
   const util::InterpolatedPathInfo & interpolated_path_info,
   const std::vector<util::DescritizedLane> & lane_divisions,
   [[maybe_unused]] const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
     blocking_attention_objects,
   const double occlusion_dist_thr)

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -108,6 +108,7 @@
       std::vector<double> possible_object_bbox;
       double ignore_parked_vehicle_speed_threshold;
       double stop_release_margin_time;
+      bool temporal_stop_before_attention_area;
     } occlusion;
   };
 
@@ -136,15 +137,19 @@
     // NOTE: if intersection_occlusion is disapproved externally through RTC,
     // it indicates "is_forcefully_occluded"
     bool is_actually_occlusion_cleared{false};
+    bool temporal_stop_before_attention_required{false};
     size_t closest_idx{0};
     size_t collision_stop_line_idx{0};
+    size_t first_attention_stop_line_idx{0};
     size_t occlusion_stop_line_idx{0};
   };
   struct OccludedCollisionStop
   {
     bool is_actually_occlusion_cleared{false};
+    bool temporal_stop_before_attention_required{false};
     size_t closest_idx{0};
     size_t collision_stop_line_idx{0};
+    size_t first_attention_stop_line_idx{0};
     size_t occlusion_stop_line_idx{0};
   };
   struct Safe
@@ -212,11 +217,12 @@
 
   // for occlusion detection
   const bool enable_occlusion_detection_;
   std::optional<std::vector<util::DescritizedLane>> occlusion_attention_divisions_;
   // OcclusionState prev_occlusion_state_ = OcclusionState::NONE;
   StateMachine collision_state_machine_;     //! for stable collision checking
   StateMachine before_creep_state_machine_;  //! for two phase stop
   StateMachine occlusion_stop_state_machine_;
+  StateMachine temporal_stop_before_attention_state_machine_;
   // NOTE: uuid_ is base member
 
   // for stuck vehicle detection
@@ -258,7 +264,7 @@
     const lanelet::ConstLanelets & adjacent_lanelets,
     const lanelet::CompoundPolygon3d & first_attention_area,
     const util::InterpolatedPathInfo & interpolated_path_info,
     const std::vector<util::DescritizedLane> & lane_divisions,
     const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
       parked_attention_objects,
     const double occlusion_dist_thr);

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -277,6 +277,8 @@
       }
     }
   }
+  const auto first_attention_stop_line_ip = static_cast<size_t>(occlusion_peeking_line_ip_int);
+  const bool first_attention_stop_line_valid = occlusion_peeking_line_valid;
   occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds);
   const auto occlusion_peeking_line_ip = static_cast<size_t>(
     std::clamp<int>(occlusion_peeking_line_ip_int, 0, static_cast<int>(path_ip.points.size()) - 1));
@@ -322,6 +324,7 @@
     size_t closest_idx{0};
     size_t stuck_stop_line{0};
     size_t default_stop_line{0};
+    size_t first_attention_stop_line{0};
     size_t occlusion_peeking_stop_line{0};
     size_t pass_judge_line{0};
   };
@@ -331,6 +334,7 @@
     {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx},
     {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line},
     {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line},
+    {&first_attention_stop_line_ip, &intersection_stop_lines_temp.first_attention_stop_line},
     {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line},
     {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line},
   };
@@ -365,6 +369,10 @@
   if (default_stop_line_valid) {
     intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line;
   }
+  if (first_attention_stop_line_valid) {
+    intersection_stop_lines.first_attention_stop_line =
+      intersection_stop_lines_temp.first_attention_stop_line;
+  }
   if (occlusion_peeking_line_valid) {
     intersection_stop_lines.occlusion_peeking_stop_line =
       intersection_stop_lines_temp.occlusion_peeking_stop_line;
@@ -796,7 +804,7 @@
   return false;
 }
 
 std::vector<DescritizedLane> generateDetectionLaneDivisions(
   lanelet::ConstLanelets detection_lanelets_all,
   [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const double resolution)
@@ -865,10 +873,10 @@
     auto & branch = branches[(ind2id[src])];
     int node_iter = ind2id[src];
     while (true) {
       const auto & dsts = adjacency[(id2ind[node_iter])];
       // NOTE: assuming detection lanelets have only one previous lanelet
       const auto next = std::find(dsts.begin(), dsts.end(), true);
       if (next == dsts.end()) {
         branch.push_back(node_iter);
         break;
       }

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -115,7 +115,7 @@
   std::optional<lanelet::CompoundPolygon3d> first_attention_area_ = std::nullopt;
 };
 
 struct DescritizedLane
 {
   int lane_id{0};
   // discrete fine lines from left to right
@@ -128,8 +128,10 @@
   size_t closest_idx{0};
   // NOTE: null if path does not conflict with first_conflicting_area
   std::optional<size_t> stuck_stop_line{std::nullopt};
-  // NOTE: null if path is over map stop_line OR its value is calculated negative area
+  // NOTE: null if path is over map stop_line OR its value is calculated negative
   std::optional<size_t> default_stop_line{std::nullopt};
+  // NOTE: null if the index is calculated negative
+  std::optional<size_t> first_attention_stop_line{std::nullopt};
   // NOTE: null if footprints do not change from outside to inside of detection area
   std::optional<size_t> occlusion_peeking_stop_line{std::nullopt};
   // if the value is calculated negative, its value is 0
@@ -139,7 +141,7 @@
 struct PathLanelets
 {
   lanelet::ConstLanelets prev;
   // lanelet::Constlanelet entry2ego; this is included in `all` if exists
   lanelet::ConstLanelet
     ego_or_entry2exit;  // this is `assigned lane` part of the path(not from
                         // ego) if ego is before the intersection, otherwise from ego to exit