From 7cafb35dc21adc1e5c13ccb6a225dedc0b8d9deb Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 8 Feb 2024 09:06:17 +0900 Subject: [PATCH] feat(avoidance): consider objects on shift side lane (#6252) * fix(avoidance): check safety for only moving objects Signed-off-by: satoshi-ota * feat(avoidance): consider ignore object in avoid margin calculation Signed-off-by: satoshi-ota * fix(avoidance): use nearest overhang point Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 22 +-- .../data_structs.hpp | 23 +-- .../behavior_path_avoidance_module/helper.hpp | 3 +- .../behavior_path_avoidance_module/utils.hpp | 8 +- .../src/debug.cpp | 12 +- .../src/scene.cpp | 33 ++-- .../src/shift_line_generator.cpp | 2 +- .../src/utils.cpp | 154 ++++++++++-------- 8 files changed, 138 insertions(+), 119 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 32c92b4f4b88b..501d1c157d4df 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -30,16 +30,6 @@ namespace behavior_path_planner { -namespace -{ -lanelet::BasicLineString3d toLineString3d(const std::vector & 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 & parameters, std::shared_ptr avoidance_parameters) @@ -172,10 +162,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 @@ -275,8 +265,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; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 5e162a17a5064..a536fa1b568cb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/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,6 +415,9 @@ struct ObjectData // avoidance target // object direction. Direction direction{Direction::NONE}; + // overhang points (sort by distance) + std::vector> overhang_points{}; + // unavoidable reason std::string reason{}; @@ -432,7 +425,7 @@ struct ObjectData // avoidance target std::optional avoid_margin{std::nullopt}; // the nearest bound point (use in road shoulder distance calculation) - std::optional nearest_bound_point{std::nullopt}; + std::optional> narrowest_place{std::nullopt}; }; using ObjectDataArray = std::vector; @@ -541,9 +534,9 @@ struct AvoidancePlanningData std::vector drivable_lanes{}; - lanelet::BasicLineString3d right_bound{}; + std::vector right_bound{}; - lanelet::BasicLineString3d left_bound{}; + std::vector left_bound{}; bool safe{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 0f72c1aefc33b..cc00bfbb978f5 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/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()); } diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index cbf68ada44abb..afde14875030f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/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> 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 & planner_data, const std::shared_ptr & parameters); +void updateRoadShoulderDistance( + AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line); diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index fcca9b1a18f49..cc7870375d0db 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/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); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 7cea678fb8b03..a87c2d062262a 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/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; @@ -316,6 +317,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; @@ -930,10 +932,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; @@ -942,6 +940,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); @@ -1151,8 +1164,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}; diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 96e22152a2f93..cd2a08489fd29 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/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. "); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index edc76f09f1763..6cdc6e755a452 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/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" @@ -816,8 +817,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; @@ -878,45 +879,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().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 intersects; - for (size_t i = 1; i < bound.size(); i++) { - const auto p1_bound = - geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); - const auto p2_bound = - geometry_msgs::build().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> 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().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; + + 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)); - intersects.push_back(opt_intersect.value()); + 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 @@ -1072,34 +1069,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( return; } -double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose) +std::vector> calcEnvelopeOverhangDistance( + const ObjectData & object_data, const PathWithLaneId & path) { - double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number + std::vector> 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( @@ -1182,24 +1167,21 @@ std::vector generateObstaclePolygonsForDrivableArea( { std::vector 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); @@ -1447,10 +1429,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; @@ -1613,6 +1595,40 @@ void compensateDetectionLost( } } +void updateRoadShoulderDistance( + AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & 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 & planner_data, @@ -1636,8 +1652,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); @@ -1844,7 +1859,10 @@ std::vector getSafetyCheckTargetObjects( PredictedObjects ret{}; std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](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;