xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/controller/PushOrderRouteController.java
@@ -17,6 +17,8 @@ import com.xiaopeng.common.core.enums.PushMessageTypeEnum; import com.xiaopeng.common.redis.service.RedisService; import com.xiaopeng.pushMessage.utils.MapMatchingUtil; import com.xiaopeng.pushMessage.utils.TrackPoint; import com.xiaopeng.pushMessage.utils.TrajectoryDensifier; import io.swagger.annotations.Api; import lombok.extern.slf4j.Slf4j; import org.powermock.core.ListMap; @@ -27,6 +29,7 @@ import org.springframework.web.bind.annotation.RequestMapping; import org.springframework.web.bind.annotation.RestController; import java.time.Instant; import java.util.*; import java.util.concurrent.LinkedBlockingQueue; import java.util.concurrent.ThreadPoolExecutor; @@ -66,8 +69,33 @@ } Object cacheObject = redisService.getCacheObject(key); RouteVo routeVo = JSON.parseObject(cacheObject.toString(), RouteVo.class); log.info("缓存获取的轨迹 {}", JSON.toJSONString(routeVo)); if(Objects.nonNull(routeVo)){ //开始对轨迹进行点位补全，然后更新缓存中的轨迹数据 if(xpOrder.getStatus() == 2 || xpOrder.getStatus() == 5){ List<LonLatDto> polyline = routeVo.getPolyline(); List<LonLatDto> new_polyline = new ArrayList<>(); for (int i = 0; i < polyline.size(); i++) { if(i == polyline.size() - 1){ new_polyline.add(polyline.get(i)); break; } LonLatDto lonLatDto = polyline.get(i); LonLatDto lonLatDto1 = polyline.get(i + 1); //对两个点位进行坐标补全 List<TrackPoint> track = new ArrayList<>(); // 模拟数据：两个点相差 20 秒 track.add(new TrackPoint(lonLatDto.getLatitude(), lonLatDto.getLongitude(), null)); track.add(new TrackPoint(lonLatDto1.getLatitude(), lonLatDto1.getLongitude(), null)); //10米补全一个点位 List<TrackPoint> densified = TrajectoryDensifier.densifyByTime(track, 10); for (TrackPoint trackPoint : densified) { new_polyline.add(new LonLatDto(trackPoint.getLongitude(), trackPoint.getLatitude())); } } routeVo.setPolyline(new_polyline); redisService.setCacheObject(key, JSON.toJSONString(routeVo), 24L, TimeUnit.HOURS); } Integer distance = dto.getDistance(); Integer duration = dto.getDuration(); // 计算实时点位到线段的最优截取 @@ -86,33 +114,16 @@ List<LonLatDto> lonLatDtos1 = remainingNavigation.get("served"); routeVo.setPolyline(lonLatDtos); routeVo.setServed(lonLatDtos1); log.info("线路截取后存储的轨迹 {}", JSON.toJSONString(routeVo)); // 存储最新路线偏离缓存，后续进行轨迹截取 redisService.setCacheObject(key, JSON.toJSONString(routeVo), 24L, TimeUnit.HOURS); //将车辆的当前位置加入到第一个轨迹点位集合中 lonLatDtos.add(0, new LonLatDto(dto.getLongitude(), dto.getLatitude())); // 封装轨迹点位集合 List<Map<String, Object>> polyline = new ArrayList<>(); for (LonLatDto lonLatDto : lonLatDtos) { Map<String, Object> polylineMap = new HashMap<>(); polylineMap.put("longitude",lonLatDto.getLongitude()); polylineMap.put("latitude",lonLatDto.getLatitude()); polyline.add(polylineMap); } List<Map<String, Object>> served = new ArrayList<>(); for (LonLatDto lonLatDto : lonLatDtos1) { Map<String, Object> polylineMap = new HashMap<>(); polylineMap.put("longitude",lonLatDto.getLongitude()); polylineMap.put("latitude",lonLatDto.getLatitude()); served.add(polylineMap); } // 封装路由详情 Map<String, Object> route = new HashMap<>(); route.put("distance",distance); route.put("duration",duration); route.put("polyline",JSONObject.toJSONString(polyline)); route.put("served",JSONObject.toJSONString(served)); route.put("polyline",JSONObject.toJSONString(lonLatDtos)); route.put("served",JSONObject.toJSONString(lonLatDtos1)); Map<String, Object> content = new HashMap<>(); content.put("orderId",dto.getOrderId().toString()); xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java
@@ -63,6 +63,9 @@ public static Map<String, List<LonLatDto>> getRecentlyPoint(List<LonLatDto> polyline, Double longitude, Double latitude){ Map<String, List<LonLatDto>> map = new HashMap<>(); for (int i = 0; i < polyline.size(); i++) { if(i == polyline.size() - 1){ break; } LonLatDto lonLatDto1 = polyline.get(i); GlobalCoordinates source1 = new GlobalCoordinates(lonLatDto1.getLatitude(), lonLatDto1.getLongitude()); @@ -84,10 +87,10 @@ //判断当前位置是否处于两个点位之间 if((distance1 + distance2) == distance3 || (distance1 + distance2) <= distance3 + 5){ //剩余未服务的点位 List<LonLatDto> lonLatDtos = polyline.subList(i + 1, polyline.size()); //已经服务的点位，给到前端处理 List<LonLatDto> lonLatDtos1 = polyline.subList(0, i + 1); //剩余未服务的点位（创建新副本避免修改原列表） List<LonLatDto> lonLatDtos = new ArrayList<>(polyline.subList(i + 1, polyline.size())); //已经服务的点位，给到前端处理（创建新副本避免修改原列表） List<LonLatDto> lonLatDtos1 = new ArrayList<>(polyline.subList(0, i + 1)); map.put("unserved", lonLatDtos); map.put("served", lonLatDtos1); return map; xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrackPoint.java
New file @@ -0,0 +1,26 @@ package com.xiaopeng.pushMessage.utils; import java.time.Instant; public class TrackPoint { private double latitude; // 纬度 private double longitude; // 经度 private Instant timestamp; // 时间戳 public TrackPoint(double latitude, double longitude, Instant timestamp) { this.latitude = latitude; this.longitude = longitude; this.timestamp = timestamp; } public double getLatitude() { return latitude; } public double getLongitude() { return longitude; } public Instant getTimestamp() { return timestamp; } @Override public String toString() { return String.format("Point[%.6f, %.6f, %s]", latitude, longitude, timestamp); } } xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrajectoryDensifier.java
New file @@ -0,0 +1,148 @@ package com.xiaopeng.pushMessage.utils; import org.gavaghan.geodesy.Ellipsoid; import org.gavaghan.geodesy.GeodeticCalculator; import org.gavaghan.geodesy.GeodeticCurve; import org.gavaghan.geodesy.GlobalCoordinates; import java.time.Instant; import java.util.ArrayList; import java.util.List; public class TrajectoryDensifier { /** * 轨迹补密主方法 * @param originalPoints 原始轨迹点列表（必须按时间排序） * @param distance 补密的间隔距离，例如每 5 米补一个点 * @return 补密后的轨迹列表 */ public static List<TrackPoint> densifyByTime(List<TrackPoint> originalPoints, int distance) { List<TrackPoint> result = new ArrayList<>(); if (originalPoints == null || originalPoints.size() < 2) { return originalPoints; } // 添加第一个点 result.add(originalPoints.get(0)); for (int i = 0; i < originalPoints.size() - 1; i++) { TrackPoint start = originalPoints.get(i); TrackPoint end = originalPoints.get(i + 1); GlobalCoordinates source1 = new GlobalCoordinates(start.getLatitude(), start.getLongitude()); GlobalCoordinates target1 = new GlobalCoordinates(end.getLatitude(), end.getLongitude()); GeodeticCurve geoCurve1 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source1, target1); double distance1 =geoCurve1.getEllipsoidalDistance(); // 如果两点间时间差小于间隔，说明不需要补密，直接添加终点（在循环结束后添加） if (distance1 <= distance) { // 这里不做处理，等待下一轮或直接添加终点 } else { // 计算需要插入多少个点 long steps = (long) (distance1 / distance); for (long j = 1; j <= steps; j++) { // 计算插值比例 t (0.0 ~ 1.0) double fraction = (double) (j * distance) / distance1; // 线性插值计算经纬度 double newLat = start.getLatitude() + fraction * (end.getLatitude() - start.getLatitude()); double newLon = start.getLongitude() + fraction * (end.getLongitude() - start.getLongitude()); result.add(new TrackPoint(newLat, newLon, null)); } } // 确保每一段的终点被添加（除了最后一次循环，因为循环结束后会添加最后一个点） // 为了避免重复添加，我们只在循环最后添加最后一个点，或者在这里判断 } // 添加最后一个点 result.add(originalPoints.get(originalPoints.size() - 1)); return result; } /** * 轨迹补密主方法 * @param originalPoints 原始轨迹点列表（必须按时间排序） * @param intervalSeconds 补密的时间间隔（秒），例如每 5 秒补一个点 * @return 补密后的轨迹列表 */ public static List<TrackPoint> densifyByTime(List<TrackPoint> originalPoints, long intervalSeconds) { List<TrackPoint> result = new ArrayList<>(); if (originalPoints == null || originalPoints.size() < 2) { return originalPoints; } // 添加第一个点 result.add(originalPoints.get(0)); for (int i = 0; i < originalPoints.size() - 1; i++) { TrackPoint start = originalPoints.get(i); TrackPoint end = originalPoints.get(i + 1); long timeDiffMillis = end.getTimestamp().toEpochMilli() - start.getTimestamp().toEpochMilli(); long intervalMillis = intervalSeconds * 1000; // 如果两点间时间差小于间隔，说明不需要补密，直接添加终点（在循环结束后添加） if (timeDiffMillis <= intervalMillis) { // 这里不做处理，等待下一轮或直接添加终点 } else { // 计算需要插入多少个点 long steps = timeDiffMillis / intervalMillis; for (long j = 1; j <= steps; j++) { // 计算当前插入点的时间 Instant newTime = start.getTimestamp().plusMillis(j * intervalMillis); // 如果计算出的时间超过了终点时间，则停止（防止浮点数误差或边界问题） if (newTime.isAfter(end.getTimestamp())) { break; } // 计算插值比例 t (0.0 ~ 1.0) double fraction = (double) (j * intervalMillis) / timeDiffMillis; // 线性插值计算经纬度 double newLat = start.getLatitude() + fraction * (end.getLatitude() - start.getLatitude()); double newLon = start.getLongitude() + fraction * (end.getLongitude() - start.getLongitude()); result.add(new TrackPoint(newLat, newLon, newTime)); } } // 确保每一段的终点被添加（除了最后一次循环，因为循环结束后会添加最后一个点） // 为了避免重复添加，我们只在循环最后添加最后一个点，或者在这里判断 } // 添加最后一个点 result.add(originalPoints.get(originalPoints.size() - 1)); return result; } // 主函数测试 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"))); // 每 5 秒补一个点 List<TrackPoint> densified = densifyByTime(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() + "]"); } } }
