feat(behavior_velocitiy_planner): predict front vehicle deceleration …

…in intersection and temporarily stop (autowarefoundation#1194)

* calculating stopping distance for frontcar from estimated velocity

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

* calc stopping_point_projected and stopping_point along centerline

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

* create stuck_vehicle_detect_area in modifyVelocity(TODO: use pose of frontcar at stopping_position

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

* use centerline on ego_lane_with_next_lane

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

* properly checking if stopping_point is in stuck_vehicle_detect_area

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

* also check if the point behind stopping_point is in detection_area(aka attention area)

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

* refactored

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

* will return collision_deteced if is_in_sturck_area && is_behind_point_in_detection_area

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

* look working

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

* refactored, rename parameter

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

* added flag

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

* fixed the order of isAheadOf, working in scenario test as well

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

* added description in stuck vehicle detection section

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

* reflected comments: (1) use vector of ids (2) changed intersection.param.yaml

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

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -141,21 +141,6 @@ lanelet::ConstLanelet generateOffsetLanelet(
 
 geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p);
 
-/**
- * @brief check if ego is over the target_idx. If the index is same, compare the exact pose
- * @param path path
- * @param closest_idx ego's closest index on the path
- * @param current_pose ego's exact pose
- * @return true if ego is over the target_idx
- */
-bool isOverTargetIndex(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-  const geometry_msgs::msg::Pose & current_pose, const int target_idx);
-
-bool isBeforeTargetIndex(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-  const geometry_msgs::msg::Pose & current_pose, const int target_idx);
-
 }  // namespace util
 }  // namespace behavior_velocity_planner