java/Robotaxi.git

			@@ -1,5 +1,6 @@
			package com.xiaopeng.pushMessage.utils;

			import com.xiaopeng.api.dto.operation.LonLatDto;
			import org.gavaghan.geodesy.Ellipsoid;
			import org.gavaghan.geodesy.GeodeticCalculator;
			import org.gavaghan.geodesy.GeodeticCurve;
			@@ -7,9 +8,104 @@

			import java.time.Instant;
			import java.util.ArrayList;
			import java.util.HashMap;
			import java.util.List;
			import java.util.Map;
			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;
			}
			CountDownLatch latch = new CountDownLatch(num);
			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;
			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()));
			}
			}
			if (!new_polyline.isEmpty()) {
			new_polyline.remove(new_polyline.size() - 1);
			}
			map.put(finalI, new_polyline);
			} finally {
			latch.countDown();
			}
			});
			}
			try {
			latch.await();
			} catch (InterruptedException e) {
			Thread.currentThread().interrupt();
			throw new RuntimeException(e);
			}
			List<LonLatDto> result = new ArrayList<>();
			for (int i = 0; i < num; i++) {
			List<LonLatDto> chunk = map.get(i);
			if (null != chunk) {
			result.addAll(chunk);
			}
			}
			//添加最后一个点
			result.add(originalPoints.get(originalPoints.size() - 1));
			return result;
			}



			/**
			@@ -18,7 +114,7 @@
			* @param distance 补密的间隔距离，例如每 5 米补一个点
			* @return 补密后的轨迹列表
			*/
			public static List<TrackPoint> densifyByTime(List<TrackPoint> originalPoints, int distance) {
			public static List<TrackPoint> densifyByDistance(List<TrackPoint> originalPoints, int distance) {
			List<TrackPoint> result = new ArrayList<>();
			if (originalPoints == null \|\| originalPoints.size() < 2) {
			return originalPoints;
			@@ -61,7 +157,7 @@
			}

			// 添加最后一个点
			result.add(originalPoints.get(originalPoints.size() - 1));
			// result.add(originalPoints.get(originalPoints.size() - 1));

			return result;
			}
			@@ -129,18 +225,29 @@

			// 主函数测试
			public static void main(String[] args) {
			List<TrackPoint> track = new ArrayList<>();
			// 模拟数据：两个点相差 20 秒
			track.add(new TrackPoint(39.9042, 116.4074, Instant.parse("2026-03-28T10:00:00Z"))); // 北京
			track.add(new TrackPoint(39.9050, 116.4080, Instant.parse("2026-03-28T10:00:20Z")));
			// List<TrackPoint> track = new ArrayList<>();
			// // 模拟数据：两个点相差 20 秒
			// track.add(new TrackPoint(39.9042, 116.4074, Instant.parse("2026-03-28T10:00:00Z"))); // 北京
			// track.add(new TrackPoint(39.9050, 116.4080, Instant.parse("2026-03-28T10:00:20Z")));
			//
			// // 每 5 秒补一个点
			// List<TrackPoint> densified = densifyByDistance(track, 10);
			//
			// System.out.println("原始点数: " + track.size());
			// System.out.println("补密后点数: " + densified.size());
			// System.out.println("轨迹详情:");
			// for (TrackPoint p : densified) {
			// System.out.println("[" + p.getLongitude() + ", " + p.getLatitude() + "]");
			// }

			// 每 5 秒补一个点
			List<TrackPoint> densified = densifyByTime(track, 10);
			List<LonLatDto> track = new ArrayList<>();
			track.add(new LonLatDto(113.386036, 23.15308));
			track.add(new LonLatDto(113.385671, 23.153252));
			track.add(new LonLatDto(113.385402, 23.153397));
			track.add(new LonLatDto(113.385211, 23.15352));

			System.out.println("原始点数: " + track.size());
			System.out.println("补密后点数: " + densified.size());
			System.out.println("轨迹详情:");
			for (TrackPoint p : densified) {
			List<LonLatDto> densify = densify(track, 10);
			for (LonLatDto p : densify) {
			System.out.println("[" + p.getLongitude() + ", " + p.getLatitude() + "]");
			}
			}