Merge pull request autowarefoundation#1118 from tier4/cherry-pick/intersection

feat(intersection): cherry pick intersection (autowarefoundation#6047 + autowarefoundation#6134+ autowarefoundation#6143 + autowarefoundation#6201 + autowarefoundation#6207 + autowarefoundation#6272 + autowarefoundation#6315)

Author: soblin

planning/behavior_velocity_blind_spot_module/src/scene.cpp

@@ -348,7 +348,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-  const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const
+  const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose)
 {
   /* get detection area */
   if (turn_direction_ == TurnDirection::INVALID) {
@@ -409,6 +409,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
         exist_in_right_conflict_area || exist_in_opposite_conflict_area;
       if (exist_in_detection_area && exist_in_conflict_area) {
         debug_data_.conflicting_targets.objects.push_back(object);
+        setObjectsOfInterestData(
+          object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED);
         return true;
       }
     }

planning/behavior_velocity_blind_spot_module/src/scene.hpp

@@ -113,7 +113,7 @@ class BlindSpotModule : public SceneModuleInterface
     lanelet::routing::RoutingGraphPtr routing_graph_ptr,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-    const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const;
+    const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose);
 
   /**
    * @brief Create half lanelet

planning/behavior_velocity_intersection_module/CMakeLists.txt

@@ -8,11 +8,18 @@ pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 find_package(OpenCV REQUIRED)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/debug.cpp
   src/manager.cpp
+  src/util.cpp
   src/scene_intersection.cpp
+  src/intersection_lanelets.cpp
+  src/object_manager.cpp
+  src/decision_result.cpp
+  src/scene_intersection_prepare_data.cpp
+  src/scene_intersection_stuck.cpp
+  src/scene_intersection_occlusion.cpp
+  src/scene_intersection_collision.cpp
   src/scene_merge_from_private_road.cpp
-  src/util.cpp
+  src/debug.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

planning/behavior_velocity_intersection_module/README.md

@@ -12,6 +12,7 @@
         max_accel: -2.8
         max_jerk: -5.0
         delay_response_time: 0.5
+        enable_pass_judge_before_default_stopline: false
 
       stuck_vehicle:
         turn_direction:
@@ -36,7 +37,6 @@
         consider_wrong_direction_vehicle: false
         collision_detection_hold_time: 0.5
         min_predicted_path_confidence: 0.05
-        keep_detection_velocity_threshold: 0.833
         velocity_profile:
           use_upstream: true
           minimum_upstream_velocity: 0.01
@@ -59,6 +59,8 @@
           object_expected_deceleration: 2.0
         ignore_on_red_traffic_light:
           object_margin_to_path: 2.0
+        avoid_collision_by_acceleration:
+          object_time_margin_to_collision_point: 4.0
 
       occlusion:
         enable: false

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -22,18 +22,19 @@
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>behavior_velocity_planner_common</depend>
+  <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>libopencv-dev</depend>
+  <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>route_handler</depend>
   <depend>rtc_interface</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>tier4_api_msgs</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>vehicle_info_util</depend>