consider stop line ahead of occlusion stop line

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -34,7 +34,7 @@
           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
-        use_upstream_velocity: false # flag to use the planned velocity profile from the upstream module
+        use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
 
       occlusion:
         enable: false

planning/behavior_velocity_intersection_module/package.xml

@@ -9,6 +9,7 @@
   <maintainer email="[email protected]">Takayuki Murooka</maintainer>
   <maintainer email="[email protected]">Tomoya Kimura</maintainer>
   <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
+  <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
 
   <license>Apache License 2.0</license>
 

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -911,7 +911,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area);
 
   const bool has_collision = checkCollision(
-    *path, target_objects, path_lanelets, closest_idx, time_delay, traffic_prioritized_level);
+    *path, target_objects, path_lanelets, closest_idx,
+    std::min<size_t>(occlusion_stop_line_idx, path->points.size() - 1), time_delay,
+    traffic_prioritized_level);
   collision_state_machine_.setStateWithMarginTime(
     has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("collision state_machine"), *clock_);
@@ -1097,7 +1099,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT
 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 util::PathLanelets & path_lanelets, const size_t closest_idx,
+  const size_t last_intersection_stop_line_candidate_idx, const double time_delay,
   const util::TrafficPrioritizedLevel & traffic_prioritized_level)
 {
   using lanelet::utils::getArcCoordinates;
@@ -1106,8 +1109,8 @@ bool IntersectionModule::checkCollision(
   // check collision between target_objects predicted path and ego lane
   // cut the predicted path at passing_time
   const auto time_distance_array = util::calcIntersectionPassingTime(
-    path, planner_data_, associative_ids_, closest_idx, time_delay,
-    planner_param_.common.intersection_velocity,
+    path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx,
+    time_delay, planner_param_.common.intersection_velocity,
     planner_param_.collision_detection.minimum_ego_predicted_velocity,
     planner_param_.collision_detection.use_upstream_velocity);
   const double passing_time = time_distance_array.back().first;

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -261,7 +261,8 @@ class IntersectionModule : public SceneModuleInterface
   bool checkCollision(
     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 util::PathLanelets & path_lanelets, const size_t closest_idx,
+    const size_t last_intersection_stop_line_candidate_idx, const double time_delay,
     const util::TrafficPrioritizedLevel & traffic_prioritized_level);
 
   bool isOcclusionCleared(

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -1108,8 +1108,9 @@ void cutPredictPathWithDuration(
 TimeDistanceArray calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data, const std::set<int> & associative_ids,
-  const int closest_idx, const double time_delay, const double intersection_velocity,
-  const double minimum_ego_velocity, const bool use_upstream_velocity)
+  const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
+  const double time_delay, const double intersection_velocity, const double minimum_ego_velocity,
+  const bool use_upstream_velocity)
 {
   double dist_sum = 0.0;
   int assigned_lane_found = false;
@@ -1152,10 +1153,13 @@ TimeDistanceArray calcIntersectionPassingTime(
     // use average velocity between p1 and p2
     const double average_velocity =
       (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0;
-    passing_time +=
-      (dist / std::max<double>(
-                minimum_ego_velocity,
-                average_velocity));  // to avoid zero-division
+    const double minimum_ego_velocity_division =
+      (use_upstream_velocity && i > last_intersection_stop_line_candidate_idx)
+        ? 0.001 /* to avoid null division */
+        : minimum_ego_velocity;
+    const double passing_velocity =
+      std::max<double>(minimum_ego_velocity_division, average_velocity);
+    passing_time += (dist / passing_velocity);
 
     time_distance_array.emplace_back(passing_time, dist_sum);
   }

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -147,8 +147,9 @@ void cutPredictPathWithDuration(
 TimeDistanceArray calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data, const std::set<int> & associative_ids,
-  const int closest_idx, const double time_delay, const double intersection_velocity,
-  const double minimum_ego_velocity, const bool use_upstream_velocity);
+  const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
+  const double time_delay, const double intersection_velocity, const double minimum_ego_velocity,
+  const bool use_upstream_velocity);
 
 double calcDistanceUntilIntersectionLanelet(
   const lanelet::ConstLanelet & assigned_lanelet,