From 9425a123e3f32f53b70b7f0493208515f7eaa857 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 13 Feb 2024 13:11:22 +0900 Subject: [PATCH 1/2] fix(static_drivable_area_expansion): remove process to make drivable bound longitudinally monotonic (#6385) fix(static_drivable_area_expansion): remove makeBoundLongitudinallyMonotonic Signed-off-by: satoshi-ota --- .../static_drivable_area.hpp | 9 +- .../static_drivable_area.cpp | 376 +++--------------- 2 files changed, 53 insertions(+), 332 deletions(-) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 9afa2608e1db6..e47adbd9f6680 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -87,13 +87,8 @@ std::vector calcBound( std::vector postProcess( const std::vector & original_bound, const PathWithLaneId & path, const std::shared_ptr planner_data, - const std::vector & drivable_lanes, - const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left, const bool is_driving_forward = true); - -std::vector makeBoundLongitudinallyMonotonic( - const std::vector & original_bound, const PathWithLaneId & path, - const std::shared_ptr & planner_data, const bool is_left); + const std::vector & drivable_lanes, const bool is_left, + const bool is_driving_forward = true); DrivableAreaInfo combineDrivableAreaInfo( const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index abad804503476..8c1fae40712ef 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -31,6 +31,53 @@ namespace { +template +std::vector removeSharpPoints(const std::vector & points) +{ + if (points.size() < 2) { + return points; + } + + std::vector ret = points; + auto itr = std::next(ret.begin()); + while (std::next(itr) != ret.end()) { + if (itr == ret.begin()) { + itr++; + continue; + } + + const auto p1 = *std::prev(itr); + const auto p2 = *itr; + const auto p3 = *std::next(itr); + + const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; + const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; + const auto product = + std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); + + const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); + + constexpr double epsilon = 1e-3; + + // Remove overlapped point. + if (dist_1to2 < epsilon || dist_3to2 < epsilon) { + itr = std::prev(ret.erase(itr)); + continue; + } + + // If the angle between the points is sharper than 45 degrees, remove the middle point. + if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { + itr = std::prev(ret.erase(itr)); + continue; + } + + itr++; + } + + return ret; +} + template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) @@ -1426,9 +1473,8 @@ std::pair, bool> getBoundWithFreeSpaceAreas( std::vector postProcess( const std::vector & original_bound, const PathWithLaneId & path, const std::shared_ptr planner_data, - const std::vector & drivable_lanes, - const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left, const bool is_driving_forward) + const std::vector & drivable_lanes, const bool is_left, + const bool is_driving_forward) { const auto lanelets = utils::transformToLanelets(drivable_lanes); const auto & current_pose = planner_data->self_odometry->pose.pose; @@ -1554,13 +1600,7 @@ std::vector postProcess( processed_bound.push_back(goal_point); } - const bool skip_monotonic_process = - !is_driving_forward || - !(enable_expanding_hatched_road_markings || enable_expanding_intersection_areas); - - return skip_monotonic_process - ? processed_bound - : makeBoundLongitudinallyMonotonic(processed_bound, path, planner_data, is_left); + return removeSharpPoints(processed_bound); } // calculate bounds from drivable lanes and hatched road markings @@ -1614,9 +1654,7 @@ std::vector calcBound( const auto post_process = [&](const auto & bound, const auto skip) { return skip ? bound - : postProcess( - bound, path, planner_data, drivable_lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, is_left, is_driving_forward); + : postProcess(bound, path, planner_data, drivable_lanes, is_left, is_driving_forward); }; // Step2. if there is no drivable area defined by polygon, return original drivable bound. @@ -1642,318 +1680,6 @@ std::vector calcBound( return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } -std::vector makeBoundLongitudinallyMonotonic( - const std::vector & original_bound, const PathWithLaneId & path, - const std::shared_ptr & planner_data, const bool is_left) -{ - using motion_utils::findNearestSegmentIndex; - using tier4_autoware_utils::calcAzimuthAngle; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::calcOffsetPose; - using tier4_autoware_utils::createQuaternionFromRPY; - using tier4_autoware_utils::getPoint; - using tier4_autoware_utils::getPose; - using tier4_autoware_utils::intersect; - using tier4_autoware_utils::normalizeRadian; - - const auto set_orientation = []( - auto & bound_with_pose, const auto idx, const auto & orientation) { - bound_with_pose.at(idx).orientation = orientation; - }; - - const auto get_intersect_idx = []( - const auto & bound_with_pose, const auto start_idx, - const auto & p1, const auto & p2) -> std::optional { - std::vector> intersects; - for (size_t i = start_idx; i < bound_with_pose.size() - 1; i++) { - const auto opt_intersect = - intersect(p1, p2, bound_with_pose.at(i).position, bound_with_pose.at(i + 1).position); - - if (!opt_intersect) { - continue; - } - - intersects.emplace_back(i, *opt_intersect); - } - - if (intersects.empty()) { - return std::nullopt; - } - - std::sort(intersects.begin(), intersects.end(), [&](const auto & a, const auto & b) { - return calcDistance2d(p1, a.second) < calcDistance2d(p1, b.second); - }); - - return intersects.front().first; - }; - - const auto get_bound_with_pose = [&](const auto & bound_with_pose, const auto & path_points) { - auto ret = bound_with_pose; - - const double offset = is_left ? 100.0 : -100.0; - - size_t start_bound_idx = 0; - - const size_t start_path_idx = - findNearestSegmentIndex(path_points, bound_with_pose.front().position); - - // append bound point with point - for (size_t i = start_path_idx + 1; i < path_points.size(); i++) { - const auto p_path_offset = calcOffsetPose(getPose(path_points.at(i)), 0.0, offset, 0.0); - const auto intersect_idx = get_intersect_idx( - bound_with_pose, start_bound_idx, getPoint(path_points.at(i)), getPoint(p_path_offset)); - - if (!intersect_idx) { - continue; - } - - if (i + 1 == path_points.size()) { - for (size_t j = intersect_idx.value(); j < bound_with_pose.size(); j++) { - if (j + 1 == bound_with_pose.size()) { - const auto yaw = - calcAzimuthAngle(bound_with_pose.at(j - 1).position, bound_with_pose.at(j).position); - set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); - } else { - const auto yaw = - calcAzimuthAngle(bound_with_pose.at(j).position, bound_with_pose.at(j + 1).position); - set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); - } - } - } else { - for (size_t j = intersect_idx.value() + 1; j < bound_with_pose.size(); j++) { - set_orientation(ret, j, getPose(path_points.at(i)).orientation); - } - } - - constexpr size_t OVERLAP_CHECK_NUM = 3; - start_bound_idx = - intersect_idx.value() < OVERLAP_CHECK_NUM ? 0 : intersect_idx.value() - OVERLAP_CHECK_NUM; - } - - return ret; - }; - - const auto is_non_monotonic = [&]( - const auto & base_pose, const auto & point, - const auto is_points_left, const auto is_reverse) { - const auto p_transformed = tier4_autoware_utils::inverseTransformPoint(point, base_pose); - if (p_transformed.x < 0.0 && p_transformed.y > 0.0) { - return is_points_left && !is_reverse; - } - - if (p_transformed.x < 0.0 && p_transformed.y < 0.0) { - return !is_points_left && !is_reverse; - } - - if (p_transformed.x > 0.0 && p_transformed.y > 0.0) { - return is_points_left && is_reverse; - } - - if (p_transformed.x > 0.0 && p_transformed.y < 0.0) { - return !is_points_left && is_reverse; - } - - return false; - }; - - // define a function to remove non monotonic point on bound - const auto remove_non_monotonic_point = [&](const auto & bound_with_pose, const bool is_reverse) { - std::vector monotonic_bound; - - size_t bound_idx = 0; - while (true) { - monotonic_bound.push_back(bound_with_pose.at(bound_idx)); - - if (bound_idx + 1 == bound_with_pose.size()) { - break; - } - - // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is - // opposite. - const double lat_offset = is_left ? 100.0 : -100.0; - - const auto p_bound_1 = getPose(bound_with_pose.at(bound_idx)); - const auto p_bound_2 = getPose(bound_with_pose.at(bound_idx + 1)); - - const auto p_bound_offset = calcOffsetPose(p_bound_1, 0.0, lat_offset, 0.0); - - if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_left, is_reverse)) { - bound_idx++; - continue; - } - - // skip non monotonic points - for (size_t i = bound_idx + 1; i < bound_with_pose.size() - 1; ++i) { - const auto intersect_point = intersect( - p_bound_1.position, p_bound_offset.position, bound_with_pose.at(i).position, - bound_with_pose.at(i + 1).position); - - if (intersect_point) { - Pose pose; - pose.position = *intersect_point; - pose.position.z = bound_with_pose.at(i).position.z; - const auto yaw = calcAzimuthAngle(*intersect_point, bound_with_pose.at(i + 1).position); - pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw); - monotonic_bound.push_back(pose); - bound_idx = i; - break; - } - } - - bound_idx++; - } - - return monotonic_bound; - }; - - const auto remove_orientation = [](const auto & bound_with_pose) { - std::vector ret; - - ret.reserve(bound_with_pose.size()); - - std::for_each(bound_with_pose.begin(), bound_with_pose.end(), [&ret](const auto & p) { - ret.push_back(p.position); - }); - - return ret; - }; - - const auto remove_sharp_points = [](const auto & bound) { - if (bound.size() < 2) { - return bound; - } - - std::vector ret = bound; - auto itr = std::next(ret.begin()); - while (std::next(itr) != ret.end()) { - if (itr == ret.begin()) { - itr++; - continue; - } - - const auto p1 = *std::prev(itr); - const auto p2 = *itr; - const auto p3 = *std::next(itr); - - const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; - const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; - const auto product = - std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - - const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); - - constexpr double epsilon = 1e-3; - - // Remove overlapped point. - if (dist_1to2 < epsilon || dist_3to2 < epsilon) { - itr = std::prev(ret.erase(itr)); - continue; - } - - // If the angle between the points is sharper than 45 degrees, remove the middle point. - if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { - itr = std::prev(ret.erase(itr)); - continue; - } - - itr++; - } - - return ret; - }; - - if (path.points.empty()) { - return original_bound; - } - - if (original_bound.size() < 2) { - return original_bound; - } - - if (path.points.front().lane_ids.empty()) { - return original_bound; - } - - // step.1 create bound with pose vector. - std::vector original_bound_with_pose; - { - original_bound_with_pose.reserve(original_bound.size()); - - std::for_each(original_bound.begin(), original_bound.end(), [&](const auto & p) { - Pose pose; - pose.position = p; - original_bound_with_pose.push_back(pose); - }); - - for (size_t i = 0; i < original_bound_with_pose.size(); i++) { - if (i + 1 == original_bound_with_pose.size()) { - const auto yaw = calcAzimuthAngle( - original_bound_with_pose.at(i - 1).position, original_bound_with_pose.at(i).position); - set_orientation(original_bound_with_pose, i, createQuaternionFromRPY(0.0, 0.0, yaw)); - } else { - const auto yaw = calcAzimuthAngle( - original_bound_with_pose.at(i).position, original_bound_with_pose.at(i + 1).position); - set_orientation(original_bound_with_pose, i, createQuaternionFromRPY(0.0, 0.0, yaw)); - } - } - } - - // step.2 get base pose vector. - std::vector clipped_points; - { - const auto & route_handler = planner_data->route_handler; - const auto p = planner_data->parameters; - const auto start_id = path.points.front().lane_ids.front(); - const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id); - - const auto lanelet_sequence = - route_handler->getLaneletSequence(start_lane, p.backward_path_length, p.forward_path_length); - const auto centerline_path = getCenterLinePath( - *route_handler, lanelet_sequence, getPose(path.points.front()), p.backward_path_length, - p.forward_path_length, p); - - if (centerline_path.points.size() < 2) { - return original_bound; - } - - const auto ego_idx = planner_data->findEgoIndex(centerline_path.points); - const auto end_idx = findNearestSegmentIndex(centerline_path.points, original_bound.back()); - - if (ego_idx >= end_idx) { - return original_bound; - } - - clipped_points.insert( - clipped_points.end(), centerline_path.points.begin() + ego_idx, - centerline_path.points.begin() + end_idx + 1); - } - - if (clipped_points.empty()) { - return original_bound; - } - - // step.3 update bound pose by reference path pose. - const auto updated_bound_with_pose = - get_bound_with_pose(original_bound_with_pose, clipped_points); // for reverse - - // step.4 create remove monotonic points by forward direction. - auto half_monotonic_bound_with_pose = - remove_non_monotonic_point(updated_bound_with_pose, false); // for reverse - std::reverse(half_monotonic_bound_with_pose.begin(), half_monotonic_bound_with_pose.end()); - - // step.5 create remove monotonic points by backward direction. - auto full_monotonic_bound_with_pose = - remove_non_monotonic_point(half_monotonic_bound_with_pose, true); - std::reverse(full_monotonic_bound_with_pose.begin(), full_monotonic_bound_with_pose.end()); - - // step.6 remove orientation from bound with pose. - auto full_monotonic_bound = remove_orientation(full_monotonic_bound_with_pose); - - // step.7 remove sharp bound points. - return remove_sharp_points(full_monotonic_bound); -} - std::vector combineDrivableLanes( const std::vector & original_drivable_lanes_vec, const std::vector & new_drivable_lanes_vec) From 74f0bbecbdf23946c0814c235381bd9f450b4ee0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 13 Feb 2024 15:39:36 +0900 Subject: [PATCH 2/2] fix(avoidance): safety check target lanes (#6324) * fix(avoidance): improve safety check lanes Signed-off-by: satoshi-ota * fix(avoidance): check both side lane if need Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/utils.hpp | 5 +- .../src/scene.cpp | 19 +++++-- .../src/utils.cpp | 55 +++++++++++++------ 3 files changed, 56 insertions(+), 23 deletions(-) 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 afde14875030f..da3a224689149 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 @@ -78,6 +78,7 @@ std::vector generateObstaclePolygonsForDrivableArea( const double vehicle_width); lanelet::ConstLanelets getAdjacentLane( + const lanelet::ConstLanelet & current_lane, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); @@ -147,8 +148,8 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift, - DebugData & debug); + const std::shared_ptr & parameters, const bool has_left_shift, + const bool has_right_shift, DebugData & debug); std::pair separateObjectsByPath( const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 685739fee432e..b8253d9aabc0e 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -695,30 +695,39 @@ bool AvoidanceModule::isSafePath( } const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); - const auto is_right_shift = [&]() -> std::optional { + + const auto has_left_shift = [&]() { for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { const auto length = shifted_path.shift_length.at(i); if (parameters_->lateral_avoid_check_threshold < length) { - return false; + return true; } + } + + return false; + }(); + + const auto has_right_shift = [&]() { + for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { + const auto length = shifted_path.shift_length.at(i); if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { return true; } } - return std::nullopt; + return false; }(); - if (!is_right_shift.has_value()) { + if (!has_left_shift && !has_right_shift) { return true; } const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug); + avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug); if (safety_check_target_objects.empty()) { return true; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index d6d4b26608784..e8b7600684509 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1263,11 +1263,13 @@ lanelet::ConstLanelets getCurrentLanesFromPath( throw std::logic_error("empty path."); } - if (path.points.front().lane_ids.empty()) { + const auto idx = planner_data->findEgoIndex(path.points); + + if (path.points.at(idx).lane_ids.empty()) { throw std::logic_error("empty lane ids."); } - const auto start_id = path.points.front().lane_ids.front(); + const auto start_id = path.points.at(idx).lane_ids.front(); const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id); const auto & p = planner_data->parameters; @@ -1814,6 +1816,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( } lanelet::ConstLanelets getAdjacentLane( + const lanelet::ConstLanelet & current_lane, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift) { @@ -1822,18 +1825,17 @@ lanelet::ConstLanelets getAdjacentLane( const auto & backward_distance = parameters->safety_check_backward_distance; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(vehicle_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Satoshi Ota) - } - const auto ego_succeeding_lanes = rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance); lanelet::ConstLanelets lanes{}; + + const auto exist = [&lanes](const auto id) { + const auto itr = std::find_if( + lanes.begin(), lanes.end(), [&id](const auto & lane) { return lane.id() == id; }); + return itr != lanes.end(); + }; + for (const auto & lane : ego_succeeding_lanes) { const auto opt_left_lane = rh->getLeftLanelet(lane, true, false); if (!is_right_shift && opt_left_lane) { @@ -1848,6 +1850,20 @@ lanelet::ConstLanelets getAdjacentLane( const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); if (is_right_shift && !right_opposite_lanes.empty()) { lanes.push_back(right_opposite_lanes.front()); + + for (const auto & prev_lane : rh->getPreviousLanelets(right_opposite_lanes.front())) { + if (!exist(prev_lane.id())) { + lanes.push_back(prev_lane); + } + } + } + } + + for (const auto & lane : lanes) { + for (const auto & next_lane : rh->getNextLanelets(lane)) { + if (!exist(next_lane.id())) { + lanes.push_back(next_lane); + } } } @@ -1856,14 +1872,14 @@ lanelet::ConstLanelets getAdjacentLane( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift, - DebugData & debug) + const std::shared_ptr & parameters, const bool has_left_shift, + const bool has_right_shift, DebugData & debug) { const auto & p = parameters; const auto check_right_lanes = - (is_right_shift && p->check_shift_side_lane) || (!is_right_shift && p->check_other_side_lane); + (has_right_shift && p->check_shift_side_lane) || (has_left_shift && p->check_other_side_lane); const auto check_left_lanes = - (!is_right_shift && p->check_shift_side_lane) || (is_right_shift && p->check_other_side_lane); + (has_left_shift && p->check_shift_side_lane) || (has_right_shift && p->check_other_side_lane); std::vector target_objects; @@ -1901,9 +1917,16 @@ std::vector getSafetyCheckTargetObjects( return ret; }(); + lanelet::ConstLanelet closest_lanelet; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + if (!lanelet::utils::query::getClosestLanelet( + data.current_lanelets, ego_pose, &closest_lanelet)) { + return {}; + } + // check right lanes if (check_right_lanes) { - const auto check_lanes = getAdjacentLane(planner_data, p, true); + 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( @@ -1925,7 +1948,7 @@ std::vector getSafetyCheckTargetObjects( // check left lanes if (check_left_lanes) { - const auto check_lanes = getAdjacentLane(planner_data, p, false); + const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, false); if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(