From 70f50f9f00a70397a336a75787ea4255f3b9ccf6 Mon Sep 17 00:00:00 2001
From: puzhibing <393733352@qq.com>
Date: 星期六, 28 三月 2026 22:10:02 +0800
Subject: [PATCH] 修改bug
---
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrackPoint.java | 26 ++++++
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/controller/PushOrderRouteController.java | 53 ++++++++-----
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java | 11 +-
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrajectoryDensifier.java | 148 +++++++++++++++++++++++++++++++++++++
4 files changed, 213 insertions(+), 25 deletions(-)
diff --git a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/controller/PushOrderRouteController.java b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/controller/PushOrderRouteController.java
index d77157d..b6631d2 100644
--- a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/controller/PushOrderRouteController.java
+++ b/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());
diff --git a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java
index 97c4693..422ed98 100644
--- a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java
+++ b/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;
diff --git a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrackPoint.java b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrackPoint.java
new file mode 100644
index 0000000..896d4db
--- /dev/null
+++ b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrackPoint.java
@@ -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);
+ }
+
+}
diff --git a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrajectoryDensifier.java b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrajectoryDensifier.java
new file mode 100644
index 0000000..a8d033d
--- /dev/null
+++ b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrajectoryDensifier.java
@@ -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() + "]");
+ }
+ }
+
+}
--
Gitblit v1.7.1