diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 4a5541508d65f..82ac5a483dfec 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -874,7 +874,6 @@ bool IntersectionModule::checkCollision( using lanelet::utils::getPolygonFromArcLength; const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects autoware_auto_perception_msgs::msg::PredictedObjects target_objects; target_objects.header = objects_ptr->header; @@ -990,8 +989,26 @@ bool IntersectionModule::checkCollision( const double end_arc_length = std::max( 0.0, closest_arc_coords.length + (*end_time_distance_itr).second + base_link2front - distance_until_intersection); + + long double lanes_length = 0.0; + std::vector ego_lane_with_next_lanes; + + const auto lanelets_on_path = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->current_odometry->pose); + for (const auto & lane : lanelets_on_path) { + lanes_length += bg::length(lane.centerline()); + ego_lane_with_next_lanes.push_back(lane); + if (lanes_length > start_arc_length && lanes_length < end_arc_length) { + break; + } + } const auto trimmed_ego_polygon = - getPolygonFromArcLength(ego_lane_with_next_lane, start_arc_length, end_arc_length); + getPolygonFromArcLength(ego_lane_with_next_lanes, start_arc_length, end_arc_length); + + if (trimmed_ego_polygon.empty()) { + continue; + } Polygon2d polygon{}; for (const auto & p : trimmed_ego_polygon) {