Merge branch '1.0' of http://120.76.84.145:10101/gitblit/r/java/Robotaxi in...

liujie

2026-05-12 0bcfca6b2561d096a2ce26e2e7200cd0768b2b6d

 xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrajectoryDensifier.java

@@ -11,62 +11,95 @@
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.concurrent.ThreadPoolExecutor;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.*;
import java.util.concurrent.atomic.AtomicInteger;

public class TrajectoryDensifier {

    private static final int THREAD_NUM = 5;

    /**
     * 轨迹补全计算线程池（类级别单例复用）
     * 修复：原代码每次调用 densify() 都 new ThreadPoolExecutor 并在方法结束时 shutdown，
     * 频繁创建/销毁线程开销大，且异常路径（latch.await 被中断）会跳过 shutdown 导致线程泄漏。
     * 改为单例线程池复用，避免上述问题。
     */
    private static final ThreadPoolExecutor DENSIFY_EXECUTOR = new ThreadPoolExecutor(
            5, 5, 60L, TimeUnit.SECONDS,
            new LinkedBlockingQueue<>(256),
            new ThreadFactory() {
                private final AtomicInteger counter = new AtomicInteger(0);
                @Override
                public Thread newThread(Runnable r) {
                    Thread t = new Thread(r, "trajectory-densify-" + counter.incrementAndGet());
                    t.setDaemon(true);
                    return t;
                }
            },
            new ThreadPoolExecutor.CallerRunsPolicy()
    );

    static {
        // 允许核心线程超时回收，空闲时不占用资源
        DENSIFY_EXECUTOR.allowCoreThreadTimeOut(true);
    }


    public static List<LonLatDto> densify(List<LonLatDto> originalPoints, int distance) {
        if (originalPoints == null || originalPoints.size() <= 1) {
            return new ArrayList<>(originalPoints);
        }
        int num = originalPoints.size() / THREAD_NUM;
        if(num == 0){
            num = 1;
        }
        ThreadPoolExecutor poolExecutor = new ThreadPoolExecutor(num, num, 0L, TimeUnit.MILLISECONDS, new LinkedBlockingQueue<>());
        CountDownLatch latch = new CountDownLatch(num);
        Map<Integer, List<LonLatDto>> map = new HashMap<>();
        Map<Integer, List<LonLatDto>> map = new ConcurrentHashMap<>();
        for (int i = 0; i < num; i++) {
            List<LonLatDto> trackPoints = originalPoints.subList(i * THREAD_NUM, i == num - 1 ? originalPoints.size() : (i + 1) * THREAD_NUM);
            int finalI = i;
            poolExecutor.execute(()->{
                List<LonLatDto> new_polyline = new ArrayList<>();
                for (int j = 0; j < trackPoints.size(); j++) {
                    LonLatDto trackPoint = trackPoints.get(j);
                    if(j == trackPoints.size() - 1){
                        new_polyline.add(new LonLatDto(trackPoint.getLongitude(), trackPoint.getLatitude()));
                        break;
                    }
                    LonLatDto trackPoint1 = trackPoints.get(j+1);
            DENSIFY_EXECUTOR.execute(()->{
                try {
                    List<LonLatDto> new_polyline = new ArrayList<>();
                    for (int j = 0; j < trackPoints.size(); j++) {
                        LonLatDto trackPoint = trackPoints.get(j);
                        if(j == trackPoints.size() - 1){
                            new_polyline.add(new LonLatDto(trackPoint.getLongitude(), trackPoint.getLatitude()));
                            break;
                        }
                        LonLatDto trackPoint1 = trackPoints.get(j+1);

                    //对两个点位进行坐标补全
                    List<TrackPoint> track = new ArrayList<>();
                    track.add(new TrackPoint(trackPoint.getLatitude(), trackPoint.getLongitude(), null));
                    track.add(new TrackPoint(trackPoint1.getLatitude(), trackPoint1.getLongitude(), null));
                    //10米补全一个点位
                    List<TrackPoint> densified = densifyByDistance(track, distance);
                    for (TrackPoint point : densified) {
                        new_polyline.add(new LonLatDto(point.getLongitude(), point.getLatitude()));
                        //对两个点位进行坐标补全
                        List<TrackPoint> track = new ArrayList<>();
                        track.add(new TrackPoint(trackPoint.getLatitude(), trackPoint.getLongitude(), null));
                        track.add(new TrackPoint(trackPoint1.getLatitude(), trackPoint1.getLongitude(), null));
                        //10米补全一个点位
                        List<TrackPoint> densified = densifyByDistance(track, distance);
                        for (TrackPoint point : densified) {
                            new_polyline.add(new LonLatDto(point.getLongitude(), point.getLatitude()));
                        }
                    }
                    if (!new_polyline.isEmpty()) {
                        new_polyline.remove(new_polyline.size() - 1);
                    }
                    map.put(finalI, new_polyline);
                } finally {
                    latch.countDown();
                }
                new_polyline.remove(new_polyline.size() - 1);
                map.put(finalI, new_polyline);
                latch.countDown();
            });
        }
        try {
            latch.await();
            poolExecutor.shutdown();
        } catch (InterruptedException e) {
            e.printStackTrace();
            Thread.currentThread().interrupt();
            throw new RuntimeException(e);
        }
        List<LonLatDto> result = new ArrayList<>();
        for (int i = 0; i < num; i++) {
            result.addAll(map.get(i));
            List<LonLatDto> chunk = map.get(i);
            if (null != chunk) {
                result.addAll(chunk);
            }
        }
        //添加最后一个点
        result.add(originalPoints.get(originalPoints.size() - 1));