From 50a775f2964dbc728cbb043b93714d82fc2b6d37 Mon Sep 17 00:00:00 2001
From: puzhibing <393733352@qq.com>
Date: 星期五, 20 三月 2026 04:27:19 +0800
Subject: [PATCH] 修改计算剩余轨迹导航逻辑
---
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java | 117 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
1 files changed, 113 insertions(+), 4 deletions(-)
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 f621649..4450ddb 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
@@ -1,5 +1,8 @@
package com.xiaopeng.pushMessage.utils;
+import com.alibaba.fastjson2.JSON;
+import com.alibaba.fastjson2.JSONArray;
+import com.alibaba.fastjson2.JSONObject;
import com.xiaopeng.api.dto.operation.LonLatDto;
import lombok.extern.slf4j.Slf4j;
import org.gavaghan.geodesy.Ellipsoid;
@@ -81,18 +84,124 @@
if((distance1 + distance2) == distance3 || (distance1 + distance2) <= distance3 + 5){
//取距离最近的点位数据
if(distance1 < distance2){
- return polyline.subList(i, polyline.size() - 1);
+ return polyline.subList(i, polyline.size());
}
- return polyline.subList(i + 1, polyline.size() - 1);
+ return polyline.subList(i + 1, polyline.size());
}
}
- return Collections.emptyList();
+ polyline.add(0, new LonLatDto(longitude, latitude));
+ return polyline;
}
+ public static void main(String[] args) {
+ JSONArray objects = JSON.parseArray("[{\n" +
+ "\t\t\"latitude\": 30.58549,\n" +
+ "\t\t\"longitude\": 104.054501\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.585938,\n" +
+ "\t\t\"longitude\": 104.054501\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.586522,\n" +
+ "\t\t\"longitude\": 104.054501\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.587026,\n" +
+ "\t\t\"longitude\": 104.054507\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.587922,\n" +
+ "\t\t\"longitude\": 104.054518\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.588062,\n" +
+ "\t\t\"longitude\": 104.054518\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.588292,\n" +
+ "\t\t\"longitude\": 104.054528\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.588292,\n" +
+ "\t\t\"longitude\": 104.0544\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.588292,\n" +
+ "\t\t\"longitude\": 104.0544\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.587949,\n" +
+ "\t\t\"longitude\": 104.054384\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.587311,\n" +
+ "\t\t\"longitude\": 104.054389\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.587118,\n" +
+ "\t\t\"longitude\": 104.054394\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.586704,\n" +
+ "\t\t\"longitude\": 104.054394\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.586125,\n" +
+ "\t\t\"longitude\": 104.0544\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.585938,\n" +
+ "\t\t\"longitude\": 104.054405\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.585476,\n" +
+ "\t\t\"longitude\": 104.054405\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.584693,\n" +
+ "\t\t\"longitude\": 104.054405\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.584403,\n" +
+ "\t\t\"longitude\": 104.054416\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.584312,\n" +
+ "\t\t\"longitude\": 104.054416\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.583593,\n" +
+ "\t\t\"longitude\": 104.054416\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.583486,\n" +
+ "\t\t\"longitude\": 104.054416\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.583116,\n" +
+ "\t\t\"longitude\": 104.054421\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.582391,\n" +
+ "\t\t\"longitude\": 104.054426\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.582397,\n" +
+ "\t\t\"longitude\": 104.054577\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.582397,\n" +
+ "\t\t\"longitude\": 104.054577\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.582815,\n" +
+ "\t\t\"longitude\": 104.054561\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.58311,\n" +
+ "\t\t\"longitude\": 104.05455\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.58311,\n" +
+ "\t\t\"longitude\": 104.05455\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.58311,\n" +
+ "\t\t\"longitude\": 104.054899\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.583143,\n" +
+ "\t\t\"longitude\": 104.054931\n" +
+ "\t}, {\n" +
+ "\t\t\"latitude\": 30.583148,\n" +
+ "\t\t\"longitude\": 104.054973\n" +
+ "\t}]");
+ List<LonLatDto> lonLatDtoList = new ArrayList<>();
+ for (int i = 0; i < objects.size(); i++) {
+ JSONObject object = objects.getJSONObject(i);
+ LonLatDto lonLatDto = new LonLatDto(object.getDouble("longitude"), object.getDouble("latitude"));
+ lonLatDtoList.add(lonLatDto);
+ }
+ for (int i = 0; i < 20; i++) {
+ List<LonLatDto> remainingNavigation = getRemainingNavigation(lonLatDtoList, 104.054503, 30.58549);
+ lonLatDtoList = remainingNavigation;
+ }
-
+ }
/**
--
Gitblit v1.7.1