#拐点加大惩罚

Junjie

2024-11-22 b1aa89d5cc6058a04a848f5d6973f581757eed3f

 src/main/java/com/zy/common/utils/NavigateUtils.java

@@ -4,7 +4,11 @@
import com.zy.common.model.MapNode;
import com.zy.common.model.NavigateNode;
import com.zy.common.model.enums.NavigationMapType;
import com.zy.core.cache.SlaveConnection;
import com.zy.core.enums.ShuttleTaskModeType;
import com.zy.core.enums.SlaveType;
import com.zy.core.model.protocol.NyShuttleProtocol;
import com.zy.core.thread.NyShuttleThread;

import java.util.*;

@@ -13,13 +17,16 @@
 */
public class NavigateUtils {

    public static List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints) {
    public static List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints, List<int[]> whites) {
        //通过开始编号和结束编号获取对应的xy轴坐标
        int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点
        int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点

        ArrayList<int[]> whiteList = new ArrayList<>();//设置计算节点的白名单
        whiteList.add(startArr);//将开始节点设置为白名单，以防被过滤
        if (whites != null && !whites.isEmpty()) {
            whiteList.addAll(whites);//批量添加白名单节点
        }

        //获取当前节点计算的层高，并赋值到每一个节点中
        int lev = Utils.getLev(startPoint);
@@ -35,7 +42,7 @@

        NavigateNode res_node = solution.astarSearch(start, end);
        if (res_node == null) {
            System.out.println("未找到路径");
            System.out.println(start + "," + end + "：未找到路径");
            return null;
        } else {
            ArrayList<NavigateNode> list = new ArrayList<>();
@@ -247,12 +254,27 @@
    /**
     * 检测路径是否可用(可走)
     */
    public static boolean checkPathIsAvailable(List<NavigateNode> path, Integer shuttleNo, Integer lev) {
        NavigateSolution solution = new NavigateSolution(NavigationMapType.DFX.id, lev, null, Utils.getShuttlePoints(shuttleNo, lev));//获取无白名单地图(该地图包含小车坐标)
    public static boolean checkPathIsAvailable(List<NavigateNode> path, Integer shuttleNo, Integer lev, List<int[]> whitePoints) {
        NyShuttleThread shuttleThread = (NyShuttleThread) SlaveConnection.get(SlaveType.Shuttle, shuttleNo);
        if (shuttleThread == null) {
            return false;
        }
        NyShuttleProtocol shuttleProtocol = shuttleThread.getShuttleProtocol();
        if (shuttleProtocol == null) {
            return false;
        }

        Integer mapType = NavigationMapType.DFX.id;
        if (shuttleProtocol.getLiftPosition() == 1) {
            //下降位置
            mapType = NavigationMapType.NORMAL.id;
        }

        NavigateSolution solution = new NavigateSolution(mapType, lev, whitePoints, Utils.getShuttlePoints(shuttleNo, lev));//获取无白名单地图(该地图包含小车坐标)
        int[][] map = solution.map;
        for (NavigateNode node : path) {
            int value = map[node.getX()][node.getY()];
            if (value != 0 && value != 3) {
            if (value != 0 && value != 3 && value != 5) {//母轨道3、子轨道0、充电桩5
                return false;
            }
        }
@@ -261,7 +283,7 @@

    public static void main(String[] args) {
        //计算路径
        List<NavigateNode> calc = calc("1000901", "1800201", NavigationMapType.NONE.id, null);
        List<NavigateNode> calc = calc("1000901", "1800201", NavigationMapType.NONE.id, null, null);
        System.out.println(calc);
        System.out.println("------------------------");
//        List<NavigateNode> calc = calc("0501401", "0201801", "out");