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

…n between pose and lane (autowarefoundation#7828)

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

Signed-off-by: satoshi-ota <[email protected]>

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

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>

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);
  }