feat(avoidance): consider objects on shift side lane (autowarefoundation#6252)

* fix(avoidance): check safety for only moving objects

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

* feat(avoidance): consider ignore object in avoid margin calculation

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

* fix(avoidance): use nearest overhang point

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

---------

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

Author: satoshi-ota

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -37,16 +37,6 @@
 
 namespace behavior_path_planner
 {
-namespace
-{
-lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
-{
-  lanelet::BasicLineString3d ret{};
-  std::for_each(
-    bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); });
-  return ret;
-}
-}  // namespace
 AvoidanceByLaneChange::AvoidanceByLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters,
   std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
@@ -237,10 +227,10 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
   // calc drivable bound
   const auto shorten_lanes =
     utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes);
-  data.left_bound = toLineString3d(utils::calcBound(
-    data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true));
-  data.right_bound = toLineString3d(utils::calcBound(
-    data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false));
+  data.left_bound = utils::calcBound(
+    data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true);
+  data.right_bound = utils::calcBound(
+    data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false);
 
   // get related objects from dynamic_objects, and then separates them as target objects and non
   // target objects
@@ -340,8 +330,8 @@ ObjectData AvoidanceByLaneChange::createObjectData(
                             : Direction::RIGHT;
 
   // Find the footprint point closest to the path, set to object_data.overhang_distance.
-  object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance(
-    object_data, data.reference_path, object_data.overhang_pose.position);
+  object_data.overhang_points =
+    utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path);
 
   // Check whether the the ego should avoid the object.
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -336,12 +336,8 @@ struct ObjectData  // avoidance target
 {
   ObjectData() = default;
 
-  ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang)
-  : object(std::move(obj)),
-    to_centerline(lat),
-    longitudinal(lon),
-    length(len),
-    overhang_dist(overhang)
+  ObjectData(PredictedObject obj, double lat, double lon, double len)
+  : object(std::move(obj)), to_centerline(lat), longitudinal(lon), length(len)
   {
   }
 
@@ -365,9 +361,6 @@ struct ObjectData  // avoidance target
   // longitudinal length of vehicle, in Frenet coordinate
   double length{0.0};
 
-  // lateral distance to the closest footprint, in Frenet coordinate
-  double overhang_dist{0.0};
-
   // lateral shiftable ratio
   double shiftable_ratio{0.0};
 
@@ -392,9 +385,6 @@ struct ObjectData  // avoidance target
   // the position at the detected moment
   Pose init_pose;
 
-  // the position of the overhang
-  Pose overhang_pose;
-
   // envelope polygon
   Polygon2d envelope_poly{};
 
@@ -425,14 +415,17 @@ struct ObjectData  // avoidance target
   // object direction.
   Direction direction{Direction::NONE};
 
+  // overhang points (sort by distance)
+  std::vector<std::pair<double, Point>> overhang_points{};
+
   // unavoidable reason
   std::string reason{};
 
   // lateral avoid margin
   std::optional<double> avoid_margin{std::nullopt};
 
   // the nearest bound point (use in road shoulder distance calculation)
-  std::optional<Point> nearest_bound_point{std::nullopt};
+  std::optional<std::pair<Point, Point>> narrowest_place{std::nullopt};
 };
 using ObjectDataArray = std::vector<ObjectData>;
 
@@ -541,9 +534,9 @@ struct AvoidancePlanningData
 
   std::vector<DrivableLanes> drivable_lanes{};
 
-  lanelet::BasicLineString3d right_bound{};
+  std::vector<Point> right_bound{};
 
-  lanelet::BasicLineString3d left_bound{};
+  std::vector<Point> left_bound{};
 
   bool safe{false};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -176,7 +176,8 @@ class AvoidanceHelper
   {
     using utils::avoidance::calcShiftLength;
 
-    const auto shift_length = calcShiftLength(is_on_right, object.overhang_dist, margin);
+    const auto shift_length =
+      calcShiftLength(is_on_right, object.overhang_points.front().first, margin);
     return is_on_right ? std::min(shift_length, getLeftShiftBound())
                        : std::max(shift_length, getRightShiftBound());
   }

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -56,8 +56,8 @@ double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
 void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
   const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj);
 
-double calcEnvelopeOverhangDistance(
-  const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose);
+std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(
+  const ObjectData & object_data, const PathWithLaneId & path);
 
 void setEndData(
   AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,
@@ -127,6 +127,10 @@ void filterTargetObjects(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+void updateRoadShoulderDistance(
+  AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
+
 void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines);
 
 void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line);

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -113,7 +113,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
   MarkerArray msg;
 
   for (const auto & object : objects) {
-    if (!object.nearest_bound_point.has_value()) {
+    if (!object.narrowest_place.has_value()) {
       continue;
     }
 
@@ -122,8 +122,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
         "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
         createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999));
 
-      marker.points.push_back(object.overhang_pose.position);
-      marker.points.push_back(object.nearest_bound_point.value());
+      marker.points.push_back(object.narrowest_place.value().first);
+      marker.points.push_back(object.narrowest_place.value().second);
       marker.id = uuidToInt32(object.object.object_id);
       msg.markers.push_back(marker);
     }
@@ -133,7 +133,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
         "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
         createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));
 
-      marker.pose.position = object.nearest_bound_point.value();
+      marker.pose.position = object.narrowest_place.value().second;
       std::ostringstream string_stream;
       string_stream << object.to_road_shoulder_distance << "[m]";
       marker.text = string_stream.str();
@@ -469,7 +469,7 @@ MarkerArray createDrivableBounds(
       createMarkerColor(r, g, b, 0.999));
 
     for (const auto & p : data.right_bound) {
-      marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+      marker.points.push_back(p);
     }
 
     msg.markers.push_back(marker);
@@ -482,7 +482,7 @@ MarkerArray createDrivableBounds(
       createMarkerColor(r, g, b, 0.999));
 
     for (const auto & p : data.left_bound) {
-      marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+      marker.points.push_back(p);
     }
 
     msg.markers.push_back(marker);

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -234,14 +234,14 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
   // calc drivable bound
   auto tmp_path = getPreviousModuleOutput().path;
   const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
-  data.left_bound = toLineString3d(utils::calcBound(
+  data.left_bound = utils::calcBound(
     getPreviousModuleOutput().path, planner_data_, shorten_lanes,
     parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
-    parameters_->use_freespace_areas, true));
-  data.right_bound = toLineString3d(utils::calcBound(
+    parameters_->use_freespace_areas, true);
+  data.right_bound = utils::calcBound(
     getPreviousModuleOutput().path, planner_data_, shorten_lanes,
     parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
-    parameters_->use_freespace_areas, false));
+    parameters_->use_freespace_areas, false);
 
   // reference path
   if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
@@ -294,6 +294,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
   using utils::avoidance::filterTargetObjects;
   using utils::avoidance::getTargetLanelets;
   using utils::avoidance::separateObjectsByPath;
+  using utils::avoidance::updateRoadShoulderDistance;
 
   // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
   constexpr double MARGIN = 10.0;
@@ -315,6 +316,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
 
   // Filter out the objects to determine the ones to be avoided.
   filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_);
+  updateRoadShoulderDistance(data, planner_data_, parameters_);
 
   // Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
@@ -929,10 +931,6 @@ BehaviorModuleOutput AvoidanceModule::plan()
     DrivableAreaInfo current_drivable_area_info;
     // generate drivable lanes
     current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes;
-    // generate obstacle polygons
-    current_drivable_area_info.obstacles =
-      utils::avoidance::generateObstaclePolygonsForDrivableArea(
-        avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
     // expand hatched road markings
     current_drivable_area_info.enable_expanding_hatched_road_markings =
       parameters_->use_hatched_road_markings;
@@ -941,6 +939,21 @@ BehaviorModuleOutput AvoidanceModule::plan()
       parameters_->use_intersection_areas;
     // expand freespace areas
     current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
+    // generate obstacle polygons
+    if (parameters_->enable_bound_clipping) {
+      ObjectDataArray clip_objects;
+      // If avoidance is executed by both behavior and motion, only non-avoidable object will be
+      // extracted from the drivable area.
+      std::for_each(
+        data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) {
+          if (!object.is_avoidable) clip_objects.push_back(object);
+        });
+      current_drivable_area_info.obstacles =
+        utils::avoidance::generateObstaclePolygonsForDrivableArea(
+          clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
+    } else {
+      current_drivable_area_info.obstacles.clear();
+    }
 
     output.drivable_area_info = utils::combineDrivableAreaInfo(
       current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
@@ -1150,8 +1163,8 @@ bool AvoidanceModule::isValidShiftLine(
     const size_t end_idx = shift_lines.back().end_idx;
 
     const auto path = shifter_for_validate.getReferencePath();
-    const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound);
-    const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound);
+    const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound));
+    const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound));
     for (size_t i = start_idx; i <= end_idx; ++i) {
       const auto p = getPoint(path.points.at(i));
       lanelet::BasicPoint2d basic_point{p.x, p.y};

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -235,7 +235,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3;
 
     const auto infeasible =
-      std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER <
+      std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER <
       0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
     if (infeasible) {
       RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -16,6 +16,7 @@
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/utils.hpp"
+#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
@@ -818,8 +819,8 @@ bool isNoNeedAvoidanceBehavior(
     return false;
   }
 
-  const auto shift_length =
-    calcShiftLength(isOnRight(object), object.overhang_dist, object.avoid_margin.value());
+  const auto shift_length = calcShiftLength(
+    isOnRight(object), object.overhang_points.front().first, object.avoid_margin.value());
   if (!isShiftNecessary(isOnRight(object), shift_length)) {
     object.reason = "NotNeedAvoidance";
     return true;
@@ -880,45 +881,41 @@ double getRoadShoulderDistance(
     return 0.0;
   }
 
-  const auto centerline_pose =
-    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
-  const auto & p1_object = object.overhang_pose.position;
-  const auto p_tmp =
-    geometry_msgs::build<Pose>().position(p1_object).orientation(centerline_pose.orientation);
-  const auto p2_object =
-    calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position;
-
-  // TODO(Satoshi OTA): check if the basic point is on right or left of bound.
-  const auto bound = isOnRight(object) ? data.left_bound : data.right_bound;
-
-  std::vector<Point> intersects;
-  for (size_t i = 1; i < bound.size(); i++) {
-    const auto p1_bound =
-      geometry_msgs::build<Point>().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z());
-    const auto p2_bound =
-      geometry_msgs::build<Point>().x(bound[i].x()).y(bound[i].y()).z(bound[i].z());
-
-    const auto opt_intersect =
-      tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound);
-
-    if (!opt_intersect) {
-      continue;
-    }
+  std::vector<std::pair<Point, Point>> intersects;
+  for (const auto & p1 : object.overhang_points) {
+    const auto centerline_pose =
+      lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
+    const auto p_tmp =
+      geometry_msgs::build<Pose>().position(p1.second).orientation(centerline_pose.orientation);
+    const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position;
+
+    // TODO(Satoshi OTA): check if the basic point is on right or left of bound.
+    const auto bound = isOnRight(object) ? data.left_bound : data.right_bound;
 
-    intersects.push_back(opt_intersect.value());
+    for (size_t i = 1; i < bound.size(); i++) {
+      const auto opt_intersect =
+        tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));
+
+      if (!opt_intersect) {
+        continue;
+      }
+
+      intersects.emplace_back(p1.second, opt_intersect.value());
+    }
   }
 
+  std::sort(intersects.begin(), intersects.end(), [](const auto & a, const auto & b) {
+    return calcDistance2d(a.first, a.second) < calcDistance2d(b.first, b.second);
+  });
+
   if (intersects.empty()) {
     return 0.0;
   }
 
-  std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) {
-    return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b);
-  });
-
-  object.nearest_bound_point = intersects.front();
+  object.narrowest_place = intersects.front();
 
-  return calcDistance2d(p1_object, object.nearest_bound_point.value());
+  return calcDistance2d(
+    object.narrowest_place.value().first, object.narrowest_place.value().second);
 }
 }  // namespace filtering_utils
 
@@ -1074,34 +1071,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
   return;
 }
 
-double calcEnvelopeOverhangDistance(
-  const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose)
+std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(
+  const ObjectData & object_data, const PathWithLaneId & path)
 {
-  double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0;  // large number
+  std::vector<std::pair<double, Point>> overhang_points{};
 
   for (const auto & p : object_data.envelope_poly.outer()) {
     const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
     // TODO(someone): search around first position where the ego should avoid the object.
     const auto idx = findNearestIndex(path.points, point);
     const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point);
-
-    const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() {
-      if (lateral > largest_overhang) {
-        overhang_pose = point;
-      }
-      return std::max(largest_overhang, lateral);
-    };
-
-    const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() {
-      if (lateral < largest_overhang) {
-        overhang_pose = point;
-      }
-      return std::min(largest_overhang, lateral);
-    };
-
-    largest_overhang = isOnRight(object_data) ? overhang_pose_on_right() : overhang_pose_on_left();
+    overhang_points.emplace_back(lateral, point);
   }
-  return largest_overhang;
+  std::sort(overhang_points.begin(), overhang_points.end(), [&](const auto & a, const auto & b) {
+    return isOnRight(object_data) ? b.first < a.first : a.first < b.first;
+  });
+  return overhang_points;
 }
 
 void setEndData(
@@ -1184,24 +1169,21 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
 {
   std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
 
-  if (objects.empty() || !parameters->enable_bound_clipping) {
+  if (objects.empty()) {
     return obstacles_for_drivable_area;
   }
 
   for (const auto & object : objects) {
-    // If avoidance is executed by both behavior and motion, only non-avoidable object will be
-    // extracted from the drivable area.
-    if (!parameters->disable_path_update) {
-      if (object.is_avoidable) {
-        continue;
-      }
-    }
-
     // check if avoid marin is calculated
     if (!object.avoid_margin.has_value()) {
       continue;
     }
 
+    // check original polygon
+    if (object.envelope_poly.outer().empty()) {
+      continue;
+    }
+
     const auto object_type = utils::getHighestProbLabel(object.object.classification);
     const auto object_parameter = parameters->object_parameters.at(object_type);
 
@@ -1449,10 +1431,10 @@ void fillAvoidanceNecessity(
     0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor;
 
   const auto check_necessity = [&](const auto hysteresis_factor) {
-    return (isOnRight(object_data) &&
-            std::abs(object_data.overhang_dist) < safety_margin * hysteresis_factor) ||
+    return (isOnRight(object_data) && std::abs(object_data.overhang_points.front().first) <
+                                        safety_margin * hysteresis_factor) ||
            (!isOnRight(object_data) &&
-            object_data.overhang_dist < safety_margin * hysteresis_factor);
+            object_data.overhang_points.front().first < safety_margin * hysteresis_factor);
   };
 
   const auto id = object_data.object.object_id;
@@ -1615,6 +1597,40 @@ void compensateDetectionLost(
   }
 }
 
+void updateRoadShoulderDistance(
+  AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  ObjectDataArray clip_objects;
+  std::for_each(data.other_objects.begin(), data.other_objects.end(), [&](const auto & object) {
+    if (!filtering_utils::isMovingObject(object, parameters)) {
+      clip_objects.push_back(object);
+    }
+  });
+  for (auto & o : clip_objects) {
+    const auto & vehicle_width = planner_data->parameters.vehicle_width;
+    const auto object_type = utils::getHighestProbLabel(o.object.classification);
+    const auto object_parameter = parameters->object_parameters.at(object_type);
+
+    o.avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
+  }
+  const auto extract_obstacles = generateObstaclePolygonsForDrivableArea(
+    clip_objects, parameters, planner_data->parameters.vehicle_width / 2.0);
+
+  auto tmp_path = data.reference_path;
+  tmp_path.left_bound = data.left_bound;
+  tmp_path.right_bound = data.right_bound;
+  utils::extractObstaclesFromDrivableArea(tmp_path, extract_obstacles);
+
+  data.left_bound = tmp_path.left_bound;
+  data.right_bound = tmp_path.right_bound;
+
+  for (auto & o : data.target_objects) {
+    o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data);
+    o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);
+  }
+}
+
 void filterTargetObjects(
   ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
   const std::shared_ptr<const PlannerData> & planner_data,
@@ -1638,8 +1654,7 @@ void filterTargetObjects(
     }
 
     // Find the footprint point closest to the path, set to object_data.overhang_distance.
-    o.overhang_dist =
-      calcEnvelopeOverhangDistance(o, data.reference_path, o.overhang_pose.position);
+    o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance(o, data.reference_path);
     o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data);
     o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);
 
@@ -1846,7 +1861,10 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
     PredictedObjects ret{};
     std::for_each(objects.begin(), objects.end(), [&p, &ret, &parameters](const auto & object) {
       if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) {
-        ret.objects.push_back(object.object);
+        // check only moving objects
+        if (filtering_utils::isMovingObject(object, parameters)) {
+          ret.objects.push_back(object.object);
+        }
       }
     });
     return ret;