fix(behavior_velocity_planner): fix blind spot over-detection (autowa…

…refoundation#2570)

* chore: sync files (#629)

* chore: sync files

Signed-off-by: GitHub <noreply@github.com>

* ci(pre-commit): autofix

Co-authored-by: kenji-miyake <kenji-miyake@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* feat(map_based_prediction): set orientation to predicted path of pedestrian

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
Co-authored-by: kenji-miyake <kenji-miyake@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/map_based_prediction/src/path_generator.cpp

@@ -112,6 +112,17 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser(
     }
   }
 
+  // calculate orientation of each point
+  if (predicted_path.path.size() >= 2) {
+    for (size_t i = 0; i < predicted_path.path.size() - 1; i++) {
+      const auto yaw = tier4_autoware_utils::calcAzimuthAngle(
+        predicted_path.path.at(i).position, predicted_path.path.at(i + 1).position);
+      predicted_path.path.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);
+    }
+    predicted_path.path.back().orientation =
+      predicted_path.path.at(predicted_path.path.size() - 2).orientation;
+  }
+
   predicted_path.confidence = 1.0;
   predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_);
 

planning/behavior_velocity_planner/config/blind_spot.param.yaml

@@ -6,3 +6,4 @@
       backward_length: 15.0 # [m]
       ignore_width_from_center_line: 0.7 # [m]
       max_future_movement_time: 10.0 # [second]
+      threshold_yaw_diff: 0.523 # [rad]

planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp

@@ -65,6 +65,7 @@ class BlindSpotModule : public SceneModuleInterface
     double ignore_width_from_center_line;  //! ignore width from center line from detection_area
     double
       max_future_movement_time;  //! maximum time[second] for considering future movement of object
+    double threshold_yaw_diff;   //! threshold of yaw difference between ego and target object
   };
 
   BlindSpotModule(
@@ -153,7 +154,7 @@ class BlindSpotModule : public SceneModuleInterface
    */
   bool isPredictedPathInArea(
     const autoware_auto_perception_msgs::msg::PredictedObject & object,
-    const lanelet::CompoundPolygon3d & area) const;
+    const lanelet::CompoundPolygon3d & area, geometry_msgs::msg::Pose ego_pose) const;
 
   /**
    * @brief Generate a stop line and insert it into the path.

planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp

@@ -36,6 +36,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
     node.declare_parameter(ns + ".ignore_width_from_center_line", 1.0);
   planner_param_.max_future_movement_time =
     node.declare_parameter(ns + ".max_future_movement_time", 10.0);
+  planner_param_.threshold_yaw_diff =
+    node.declare_parameter(ns + ".threshold_yaw_diff", M_PI / 6.0);
 }
 
 void BlindSpotModuleManager::launchNewModules(

planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp

@@ -389,7 +389,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
       bool exist_in_detection_area = bg::within(
         to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position),
         lanelet::utils::to2D(areas_opt.get().detection_area));
-      bool exist_in_conflict_area = isPredictedPathInArea(object, areas_opt.get().conflict_area);
+      bool exist_in_conflict_area = isPredictedPathInArea(
+        object, areas_opt.get().conflict_area, planner_data_->current_pose.pose);
       if (exist_in_detection_area || exist_in_conflict_area) {
         obstacle_detected = true;
         debug_data_.conflicting_targets.objects.push_back(object);
@@ -403,16 +404,23 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
 
 bool BlindSpotModule::isPredictedPathInArea(
   const autoware_auto_perception_msgs::msg::PredictedObject & object,
-  const lanelet::CompoundPolygon3d & area) const
+  const lanelet::CompoundPolygon3d & area, geometry_msgs::msg::Pose ego_pose) const
 {
   const auto area_2d = lanelet::utils::to2D(area);
+  const auto ego_yaw = tf2::getYaw(ego_pose.orientation);
+  const auto threshold_yaw_diff = planner_param_.threshold_yaw_diff;
   // NOTE: iterating all paths including those of low confidence
   return std::any_of(
     object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(),
-    [&area_2d](const auto & path) {
-      return std::any_of(path.path.begin(), path.path.end(), [&area_2d](const auto & point) {
-        return bg::within(to_bg2d(point.position), area_2d);
-      });
+    [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & path) {
+      return std::any_of(
+        path.path.begin(), path.path.end(),
+        [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & point) {
+          const auto is_in_area = bg::within(to_bg2d(point.position), area_2d);
+          const auto match_yaw =
+            std::fabs(ego_yaw - tf2::getYaw(point.orientation)) < threshold_yaw_diff;
+          return is_in_area && match_yaw;
+        });
     });
 }