feat(safety_check): filter safety check targe objects by yaw deviatio…

…n between pose and lane (#7828)

* fix(safety_check): filter by yaw deviation to check object belongs to lane

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(static_obstacle_avoidance): check yaw only when the object is moving

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -151,8 +151,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
   const auto [object_within_target_lane, object_outside_target_lane] =
     utils::path_safety_checker::separateObjectsByLanelets(
       *planner_data_->dynamic_object, data.current_lanelets,
-      [](const auto & obj, const auto & lane) {
-        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+      [](const auto & obj, const auto & lane, const auto yaw_threshold) {
+        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold);
       });
 
   // Assume that the maximum allocation for data.other object is the sum of

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

@@ -145,8 +145,8 @@ PredictedObjects extractObjectsInExpandedPullOverLanes(
     route_handler, left_side, backward_distance, forward_distance, bound_offset);
 
   const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets(
-    objects, lanes, [](const auto & obj, const auto & lanelet) {
-      return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet);
+    objects, lanes, [](const auto & obj, const auto & lanelet, const auto yaw_threshold) {
+      return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet, yaw_threshold);
     });
 
   return objects_in_lanes;

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp

@@ -56,18 +56,24 @@ using tier4_planning_msgs::msg::PathPointWithLaneId;
  *
  * @param objects The predicted objects to filter.
  * @param lanelet
+ * @param yaw_threshold
  * @return result.
  */
-bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet);
+bool isCentroidWithinLanelet(
+  const PredictedObject & object, const lanelet::ConstLanelet & lanelet,
+  const double yaw_threshold);
 
 /**
  * @brief Filters objects based on object polygon overlapping with lanelet.
  *
  * @param objects The predicted objects to filter.
  * @param lanelet
+ * @param yaw_threshold
  * @return result.
  */
-bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet);
+bool isPolygonOverlapLanelet(
+  const PredictedObject & object, const lanelet::ConstLanelet & lanelet,
+  const double yaw_threshold);
 
 bool isPolygonOverlapLanelet(
   const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon);
@@ -168,15 +174,19 @@ void filterObjectsByClass(
  */
 std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,
-  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet)> & condition);
+  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet, const double)> &
+    condition,
+  const double yaw_threshold = M_PI);
 
 /**
  * @brief Separate the objects into two part based on whether the object is within lanelet.
  * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet.
  */
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,
-  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet)> & condition);
+  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet, const double)> &
+    condition,
+  const double yaw_threshold = M_PI);
 
 /**
  * @brief Get the predicted path from an object.

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp

@@ -57,15 +57,34 @@ bool is_within_circle(
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker
 {
-bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet)
+bool isCentroidWithinLanelet(
+  const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold)
 {
-  const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position;
-  lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y);
-  return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon());
+  const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
+  const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position);
+  if (
+    std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) >
+    yaw_threshold) {
+    return false;
+  }
+
+  return boost::geometry::within(
+    lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pose.position))
+      .basicPoint(),
+    lanelet.polygon2d().basicPolygon());
 }
 
-bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet)
+bool isPolygonOverlapLanelet(
+  const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold)
 {
+  const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
+  const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position);
+  if (
+    std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) >
+    yaw_threshold) {
+    return false;
+  }
+
   const auto lanelet_polygon = utils::toPolygon2d(lanelet);
   return isPolygonOverlapLanelet(object, lanelet_polygon);
 }
@@ -174,7 +193,9 @@ void filterObjectsByClass(
 
 std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,
-  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet)> & condition)
+  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet, const double)> &
+    condition,
+  const double yaw_threshold)
 {
   if (target_lanelets.empty()) {
     return {};
@@ -184,7 +205,9 @@ std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanel
   std::vector<size_t> other_indices;
 
   for (size_t i = 0; i < objects.objects.size(); i++) {
-    const auto filter = [&](const auto & llt) { return condition(objects.objects.at(i), llt); };
+    const auto filter = [&](const auto & llt) {
+      return condition(objects.objects.at(i), llt, yaw_threshold);
+    };
     const auto found = std::find_if(target_lanelets.begin(), target_lanelets.end(), filter);
     if (found != target_lanelets.end()) {
       target_indices.push_back(i);
@@ -198,13 +221,15 @@ std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanel
 
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,
-  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet)> & condition)
+  const std::function<bool(const PredictedObject, const lanelet::ConstLanelet, const double)> &
+    condition,
+  const double yaw_threshold)
 {
   PredictedObjects target_objects;
   PredictedObjects other_objects;
 
   const auto [target_indices, other_indices] =
-    separateObjectIndicesByLanelets(objects, target_lanelets, condition);
+    separateObjectIndicesByLanelets(objects, target_lanelets, condition, yaw_threshold);
 
   target_objects.objects.reserve(target_indices.size());
   other_objects.objects.reserve(other_indices.size());

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp

@@ -74,8 +74,9 @@ class PullOutPlannerBase
       *dynamic_objects, parameters_.th_moving_object_velocity);
     auto [pull_out_lane_stop_objects, others] =
       utils::path_safety_checker::separateObjectsByLanelets(
-        stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) {
-          return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+        stop_objects, pull_out_lanes,
+        [](const auto & obj, const auto & lane, const auto yaw_threshold) {
+          return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold);
         });
     utils::path_safety_checker::filterObjectsByClass(
       pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation);

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -1217,8 +1217,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
   // filter for objects located in pull out lanes and moving at a speed below the threshold
   auto [stop_objects_in_pull_out_lanes, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) {
-        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+      stop_objects, pull_out_lanes,
+      [](const auto & obj, const auto & lane, const auto yaw_threshold) {
+        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold);
       });
 
   const auto path = planner_data_->route_handler->getCenterLinePath(

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

@@ -2064,21 +2064,35 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
     return {};
   }
 
+  const auto is_moving = [&parameters](const auto & object) {
+    const auto & object_twist = object.kinematics.initial_twist_with_covariance.twist;
+    const auto object_vel_norm = std::hypot(object_twist.linear.x, object_twist.linear.y);
+    const auto object_parameter =
+      parameters->object_parameters.at(utils::getHighestProbLabel(object.classification));
+    return object_vel_norm > object_parameter.moving_speed_threshold;
+  };
+
+  const auto filter =
+    [&is_moving](const auto & object, const auto & lanelet, [[maybe_unused]] const auto unused) {
+      // filter by yaw deviation only when the object is moving because the head direction is not
+      // reliable while object is stopping.
+      const auto yaw_threshold = is_moving(object) ? M_PI_2 : M_PI;
+      return utils::path_safety_checker::isCentroidWithinLanelet(object, lanelet, yaw_threshold);
+    };
+
   // check right lanes
   if (check_right_lanes) {
     const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, true);
 
     if (p->check_other_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(data.other_objects), check_lanes,
-        utils::path_safety_checker::isCentroidWithinLanelet);
+        to_predicted_objects(data.other_objects), check_lanes, filter);
       append(targets);
     }
 
     if (p->check_unavoidable_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(unavoidable_objects), check_lanes,
-        utils::path_safety_checker::isCentroidWithinLanelet);
+        to_predicted_objects(unavoidable_objects), check_lanes, filter);
       append(targets);
     }
 
@@ -2092,15 +2106,13 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 
     if (p->check_other_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(data.other_objects), check_lanes,
-        utils::path_safety_checker::isCentroidWithinLanelet);
+        to_predicted_objects(data.other_objects), check_lanes, filter);
       append(targets);
     }
 
     if (p->check_unavoidable_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(unavoidable_objects), check_lanes,
-        utils::path_safety_checker::isCentroidWithinLanelet);
+        to_predicted_objects(unavoidable_objects), check_lanes, filter);
       append(targets);
     }
 
@@ -2114,15 +2126,13 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 
     if (p->check_other_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(data.other_objects), check_lanes,
-        utils::path_safety_checker::isCentroidWithinLanelet);
+        to_predicted_objects(data.other_objects), check_lanes, filter);
       append(targets);
     }
 
     if (p->check_unavoidable_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(unavoidable_objects), check_lanes,
-        utils::path_safety_checker::isCentroidWithinLanelet);
+        to_predicted_objects(unavoidable_objects), check_lanes, filter);
       append(targets);
     }