From e5e1d2dab83e6f3f5be9929ee7e987a20d1ac428 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Fri, 7 Jul 2023 12:37:03 +0900 Subject: [PATCH] feat(avoidance): extend lanelets for target filtering (#4189) (#649) Signed-off-by: satoshi-ota Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../src/utils/avoidance/utils.cpp | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 8d33faf82e645..53b166474cd73 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -791,6 +791,25 @@ void filterTargetObjects( ? calcSignedArcLength(path_points, ego_pos, rh->getGoalPose().position) : std::numeric_limits::max(); + // extend lanelets if the reference path is cut for lane change. + const auto & ego_pose = planner_data->self_odometry->pose.pose; + lanelet::ConstLanelets extend_lanelets = data.current_lanelets; + while (rclcpp::ok()) { + const double lane_length = lanelet::utils::getLaneletLength2d(extend_lanelets); + const auto arclength = lanelet::utils::getArcCoordinates(extend_lanelets, ego_pose); + const auto next_lanelets = rh->getNextLanelets(extend_lanelets.back()); + + if (next_lanelets.empty()) { + break; + } + + if (lane_length - arclength.length < planner_data->parameters.forward_path_length) { + extend_lanelets.push_back(next_lanelets.front()); + } else { + break; + } + } + for (auto & o : objects) { const auto & object_pose = o.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = findNearestIndex(path_points, object_pose.position); @@ -949,15 +968,14 @@ void filterTargetObjects( // check traffic light const auto to_traffic_light = - utils::getDistanceToNextTrafficLight(object_pose, data.current_lanelets); + utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); { not_parked_object = to_traffic_light < parameters->object_ignore_distance_traffic_light; } // check crosswalk - const auto & ego_pose = planner_data->self_odometry->pose.pose; const auto to_crosswalk = - utils::getDistanceToCrosswalk(ego_pose, data.current_lanelets, *rh->getOverallGraphPtr()) - + utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - o.longitudinal; { const auto stop_for_crosswalk =