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

@@ -51,6 +51,7 @@
         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

planning/behavior_velocity_intersection_module/src/manager.cpp

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

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -98,6 +98,11 @@ IntersectionModule::IntersectionModule(
     stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area);
     stuck_private_area_timeout_.setState(StateMachine::State::STOP);
   }
+  {
+    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);
@@ -480,7 +485,10 @@ void reactRTCApprovalByDecisionResult(
   // 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++) {
@@ -544,7 +552,9 @@ void reactRTCApprovalByDecisionResult(
     }
   }
   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);
@@ -784,7 +794,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   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(
@@ -866,12 +877,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     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();
@@ -949,35 +961,62 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   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 =
+    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_stop_line) {
+    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_.setState(StateMachine::State::GO);
       }
+      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{

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -113,6 +113,7 @@ class IntersectionModule : public SceneModuleInterface
       std::vector<double> possible_object_bbox;
       double ignore_parked_vehicle_speed_threshold;
       double stop_release_margin_time;
+      bool temporal_stop_before_attention_area;
     } occlusion;
   };
 
@@ -141,15 +142,19 @@ class IntersectionModule : public SceneModuleInterface
     // 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
@@ -222,6 +227,7 @@ class IntersectionModule : public SceneModuleInterface
   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

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -279,6 +279,8 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
       }
     }
   }
+  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));
@@ -324,6 +326,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
     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};
   };
@@ -333,6 +336,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
     {&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},
   };
@@ -367,6 +371,10 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
   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;

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -128,8 +128,10 @@ struct IntersectionStopLines
   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