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 02d8d68cf7a56..14d3bd1800dad 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 @@ -85,10 +85,6 @@ lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); -lanelet::ConstLanelets getTargetLanelets( - const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, - const double left_offset, const double right_offset); - lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 814c39083151d..d8ab5c37b8c31 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -318,7 +318,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::fillAvoidanceNecessity; using utils::avoidance::fillObjectStoppableJudge; using utils::avoidance::filterTargetObjects; - using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; using utils::avoidance::updateRoadShoulderDistance; using utils::traffic_light::calcDistanceToRedTrafficLight; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 6fc5e42f12a9c..4cac3a501334c 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -667,9 +667,11 @@ bool isNeverAvoidanceTarget( } if (object.is_on_ego_lane) { - if ( - planner_data->route_handler->getRightLanelet(object.overhang_lanelet).has_value() && - planner_data->route_handler->getLeftLanelet(object.overhang_lanelet).has_value()) { + const auto right_lane = + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false); + const auto left_lane = + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); + if (right_lane.has_value() && left_lane.has_value()) { object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); return true; @@ -1311,43 +1313,6 @@ std::vector generateObstaclePolygonsForDrivableArea( return obstacles_for_drivable_area; } -lanelet::ConstLanelets getTargetLanelets( - const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, - const double left_offset, const double right_offset) -{ - const auto & rh = planner_data->route_handler; - - lanelet::ConstLanelets target_lanelets{}; - for (const auto & lane : route_lanelets) { - auto l_offset = 0.0; - auto r_offset = 0.0; - - const auto opt_left_lane = rh->getLeftLanelet(lane); - if (opt_left_lane) { - target_lanelets.push_back(opt_left_lane.value()); - } else { - l_offset = left_offset; - } - - const auto opt_right_lane = rh->getRightLanelet(lane); - if (opt_right_lane) { - target_lanelets.push_back(opt_right_lane.value()); - } else { - r_offset = right_offset; - } - - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); - if (!right_opposite_lanes.empty()) { - target_lanelets.push_back(right_opposite_lanes.front()); - } - - const auto expand_lane = lanelet::utils::getExpandedLanelet(lane, l_offset, r_offset); - target_lanelets.push_back(expand_lane); - } - - return target_lanelets; -} - lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data) {