修改计算剩余轨迹导航逻辑

puzhibing

2026-03-20 67d4d5baacbeeea06cf3245ca50c2d14aa6cab36

修改计算剩余轨迹导航逻辑

 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>

 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());

 xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/GeodesyUtil.java

New file
@@ -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");
    }
}

 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