From 67d4d5baacbeeea06cf3245ca50c2d14aa6cab36 Mon Sep 17 00:00:00 2001
From: puzhibing <393733352@qq.com>
Date: 星期五, 20 三月 2026 01:32:04 +0800
Subject: [PATCH] 修改计算剩余轨迹导航逻辑
---
xp-robotaxi-push-message-boot/pom.xml | 7 ++
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/GeodesyUtil.java | 75 +++++++++++++++++++++++++
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/controller/PushOrderRouteController.java | 2
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java | 85 ++++++++++++++++++++++++++++
4 files changed, 167 insertions(+), 2 deletions(-)
diff --git a/xp-robotaxi-push-message-boot/pom.xml b/xp-robotaxi-push-message-boot/pom.xml
index 66d7cab..c0dff74 100644
--- a/xp-robotaxi-push-message-boot/pom.xml
+++ b/xp-robotaxi-push-message-boot/pom.xml
@@ -130,7 +130,12 @@
<groupId>org.springframework.boot</groupId>
<artifactId>spring-boot-starter-websocket</artifactId>
</dependency>
-
+ <!-- 计算两坐标间的直线距离 -->
+ <dependency>
+ <groupId>org.gavaghan</groupId>
+ <artifactId>geodesy</artifactId>
+ <version>1.1.3</version>
+ </dependency>
</dependencies>
<build>
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 31622d6..f105860 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
@@ -69,7 +69,7 @@
if(Objects.nonNull(routeVo)){
// 计算实时点位到线段的最优截取
- List<LonLatDto> lonLatDtos = MapMatchingUtil.clipBySegmentDistance(routeVo.getPolyline(), new LonLatDto(dto.getLongitude(), dto.getLatitude()));
+ List<LonLatDto> lonLatDtos = MapMatchingUtil.getRemainingNavigation(routeVo.getPolyline(), dto.getLongitude(), dto.getLatitude());
routeVo.setDistance(dto.getDistance());
routeVo.setDuration(dto.getDuration());
diff --git a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/GeodesyUtil.java b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/GeodesyUtil.java
new file mode 100644
index 0000000..c6df264
--- /dev/null
+++ b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/GeodesyUtil.java
@@ -0,0 +1,75 @@
+package com.xiaopeng.pushMessage.utils;
+
+import com.stylefeng.guns.core.util.ToolUtil;
+import org.gavaghan.geodesy.Ellipsoid;
+import org.gavaghan.geodesy.GeodeticCalculator;
+import org.gavaghan.geodesy.GeodeticCurve;
+import org.gavaghan.geodesy.GlobalCoordinates;
+import org.springframework.stereotype.Component;
+
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * 计算两个金纬度坐标之间的直线距离
+ */
+@Component
+public class GeodesyUtil {
+
+
+ /**
+ * 获取直线距离
+ * @param fromLonLat
+ * @param toLonLat
+ * @return
+ */
+ public Map<String, Double> getDistance(String fromLonLat, String toLonLat){
+ Map<String, Double> map = null;
+ if(ToolUtil.isNotEmpty(fromLonLat) && ToolUtil.isNotEmpty(toLonLat)){
+ map = new HashMap<>();
+ String[] from = fromLonLat.split(",");
+ String[] to = toLonLat.split(",");
+ GlobalCoordinates source = new GlobalCoordinates(Double.valueOf(from[1]), Double.valueOf(from[0]));
+ GlobalCoordinates target = new GlobalCoordinates(Double.valueOf(to[1]), Double.valueOf(to[0]));
+ double Sphere = getDistanceMeter(source, target, Ellipsoid.Sphere);
+ double WGS84 = getDistanceMeter(source, target, Ellipsoid.WGS84);
+ double GRS80 = getDistanceMeter(source, target, Ellipsoid.GRS80);
+ double GRS67 = getDistanceMeter(source, target, Ellipsoid.GRS67);
+ double ANS = getDistanceMeter(source, target, Ellipsoid.ANS);
+ double WGS72 = getDistanceMeter(source, target, Ellipsoid.WGS72);
+ double Clarke1858 = getDistanceMeter(source, target, Ellipsoid.Clarke1858);
+ double Clarke1880 = getDistanceMeter(source, target, Ellipsoid.Clarke1880);
+// System.out.println("Sphere坐标系计算结果："+Sphere + "米");
+// System.out.println("WGS84坐标系计算结果："+WGS84 + "米");
+// System.out.println("GRS80坐标系计算结果："+GRS80 + "米");
+// System.out.println("GRS67坐标系计算结果："+GRS67 + "米");
+// System.out.println("ANS坐标系计算结果："+ANS + "米");
+// System.out.println("WGS72坐标系计算结果："+WGS72 + "米");
+// System.out.println("Clarke1858坐标系计算结果："+Clarke1858 + "米");
+// System.out.println("Clarke1880坐标系计算结果："+Clarke1880 + "米");
+ map.put("Sphere", Sphere);
+ map.put("WGS84", WGS84);
+ map.put("GRS80", GRS80);
+ map.put("GRS67", GRS67);
+ map.put("ANS", ANS);
+ map.put("WGS72", WGS72);
+ map.put("Clarke1858", Clarke1858);
+ map.put("Clarke1880", Clarke1880);
+ }
+ return map;
+ }
+
+
+ private double getDistanceMeter(GlobalCoordinates gpsFrom, GlobalCoordinates gpsTo, Ellipsoid ellipsoid){
+ //创建GeodeticCalculator，调用计算方法，传入坐标系、经纬度用于计算距离
+ GeodeticCurve geoCurve = new GeodeticCalculator().calculateGeodeticCurve(ellipsoid, gpsFrom, gpsTo);
+ return geoCurve.getEllipsoidalDistance();
+ }
+
+
+
+ public static void main(String[] ages){
+ GeodesyUtil geodesyUtil = new GeodesyUtil();
+ geodesyUtil.getDistance("115.481028,39.989643", "114.465302,40.004717");
+ }
+}
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 5784d42..f621649 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
@@ -2,14 +2,99 @@
import com.xiaopeng.api.dto.operation.LonLatDto;
import lombok.extern.slf4j.Slf4j;
+import org.gavaghan.geodesy.Ellipsoid;
+import org.gavaghan.geodesy.GeodeticCalculator;
+import org.gavaghan.geodesy.GeodeticCurve;
+import org.gavaghan.geodesy.GlobalCoordinates;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
+import java.util.concurrent.*;
+
@Slf4j
public class MapMatchingUtil {
+
+
+
+ /**
+ * 获取剩余导航数据
+ * @param polyline
+ * @param longitude
+ * @param latitude
+ * @return
+ */
+ public static List<LonLatDto> getRemainingNavigation(List<LonLatDto> polyline, Double longitude, Double latitude){
+ //导航数据只有一个点的情况
+ if(polyline.size() == 1){
+ LonLatDto lonLatDto = polyline.get(0);
+ GlobalCoordinates source1 = new GlobalCoordinates(lonLatDto.getLatitude(), lonLatDto.getLongitude());
+ GlobalCoordinates target1 = new GlobalCoordinates(latitude, longitude);
+ GeodeticCurve geoCurve1 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source1, target1);
+ double distance =geoCurve1.getEllipsoidalDistance();
+ //判断距离小于5米，则可以返回空数据
+ if(5 >= distance){
+ return Collections.emptyList();
+ }
+ //大于5米的情况，将车子当前位置添加进结果中
+ lonLatDto = new LonLatDto(longitude, latitude);
+ polyline.add(0, lonLatDto);
+ return polyline;
+ }
+ //多个点位的情况，需要找出当前点位处于那两个点位之间，然后截取掉超出的部分轨迹
+ return getRecentlyPoint(polyline, longitude, latitude);
+ }
+
+
+ /**
+ * 判断点位在轨迹线路中的点位之间
+ * 返回截取后的剩余轨迹
+ * @param polyline
+ * @param longitude
+ * @param latitude
+ * @return
+ */
+ public static List<LonLatDto> getRecentlyPoint(List<LonLatDto> polyline, Double longitude, Double latitude){
+ for (int i = 0; i < polyline.size(); i++) {
+ LonLatDto lonLatDto1 = polyline.get(i);
+
+ GlobalCoordinates source1 = new GlobalCoordinates(lonLatDto1.getLatitude(), lonLatDto1.getLongitude());
+ GlobalCoordinates target1 = new GlobalCoordinates(latitude, longitude);
+ GeodeticCurve geoCurve1 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source1, target1);
+ double distance1 =geoCurve1.getEllipsoidalDistance();
+
+ LonLatDto lonLatDto2 = polyline.get(i + 1);
+ GlobalCoordinates source2 = new GlobalCoordinates(lonLatDto2.getLatitude(), lonLatDto2.getLongitude());
+ GlobalCoordinates target2 = new GlobalCoordinates(latitude, longitude);
+ GeodeticCurve geoCurve2 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source2, target2);
+ double distance2 =geoCurve2.getEllipsoidalDistance();
+
+
+ GlobalCoordinates source3 = new GlobalCoordinates(lonLatDto1.getLatitude(), lonLatDto1.getLongitude());
+ GlobalCoordinates target3 = new GlobalCoordinates(lonLatDto2.getLatitude(), lonLatDto2.getLongitude());
+ GeodeticCurve geoCurve3 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source3, target3);
+ double distance3 =geoCurve3.getEllipsoidalDistance();
+
+ //判断当前位置是否处于两个点位之间
+ if((distance1 + distance2) == distance3 || (distance1 + distance2) <= distance3 + 5){
+ //取距离最近的点位数据
+ if(distance1 < distance2){
+ return polyline.subList(i, polyline.size() - 1);
+ }
+ return polyline.subList(i + 1, polyline.size() - 1);
+ }
+ }
+ return Collections.emptyList();
+ }
+
+
+
+
+
+
+
/**
* 轨迹裁剪（线段版本）
* @param polyline
--
Gitblit v1.7.1