#拐点加大惩罚

Junjie

2024-11-22 b1aa89d5cc6058a04a848f5d6973f581757eed3f

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

@@ -1,6 +1,14 @@
package com.zy.common.utils;

import com.zy.asrs.utils.Utils;
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.*;

@@ -9,10 +17,19 @@
 */
public class NavigateUtils {

    public static List<NavigateNode> calc(String startPoint, String endPoint, String mapType) {
    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);

        //初始化开始节点
        NavigateNode start = new NavigateNode(startArr[0], startArr[1]);
@@ -20,10 +37,12 @@
        start.setFather(null);

        NavigateNode end = new NavigateNode(endArr[0], endArr[1]);
        NavigateSolution solution = new NavigateSolution(mapType);
        NavigateSolution solution = new NavigateSolution(mapType, lev, whiteList, shuttlePoints);
        //开始节点，不纳入禁用节点内计算

        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<>();
@@ -33,6 +52,7 @@
            while (res_node != null) {
                res_node.setDirection(null);
                res_node.setIsInflectionPoint(false);
                res_node.setZ(lev);//设置层高

                //寻找拐点
                HashMap<String, Object> result = searchInflectionPoint(res_node, fatherNode, res_node.getFather());//分别传入当前节点、父节点、下一节点
@@ -126,15 +146,25 @@
    public static ArrayList<ArrayList<NavigateNode>> getSectionPath(List<NavigateNode> mapList) {
        ArrayList<ArrayList<NavigateNode>> list = new ArrayList<>();
        ArrayList<NavigateNode> data = new ArrayList<>();
        for (NavigateNode mapNode : mapList) {
        String direction = mapList.get(0).getDirection();//行走方向

        for (int i = 0; i < mapList.size(); i++) {
            NavigateNode mapNode = mapList.get(i);
            boolean isInflectionPoint = mapNode.getIsInflectionPoint();
            data.add(mapNode);
            if (isInflectionPoint) {
                //拐点
                //分割数据
                list.add(data);//添加某一段数据
                direction = mapNode.getDirection();//更新行走方向
                data = new ArrayList<>();
                data.add(mapNode);//将拐点的终点，更新成下一段命令的起点坐标
            }else {
                //直行线路
                mapNode.setDirection(direction);//设置行走方向
            }
            Integer distance = getXToNextDistance(mapNode);//获取当前点到下一点的行走距离
            mapNode.setMoveDistance(distance);
        }

        //将最后一段数据添加进入
@@ -143,15 +173,125 @@
        return list;
    }

    //获取从x点到下一点的行走距离
    public static Integer getXToNextDistance(NavigateNode xNode) {
        NavigateMapData mapData = new NavigateMapData();
        List<List<MapNode>> lists = mapData.getJsonData(NavigationMapType.NONE.id, null, null);
        if (lists != null) {
            MapNode mapNode = lists.get(xNode.getX()).get(xNode.getY());
            if (mapNode != null) {
                switch (xNode.getDirection()) {
                    case "top":
                        return mapNode.getTop();
                    case "bottom":
                        return mapNode.getBottom();
                    case "left":
                        return mapNode.getLeft();
                    case "right":
                        return mapNode.getRight();
                }
            }
            return 0;
        }
        return 0;
    }

    /**
     * 根据原始节点结果，计算总行走距离
     */
    public static Integer getOriginPathAllDistance(List<NavigateNode> path) {
        ArrayList<ArrayList<NavigateNode>> sectionPath = NavigateUtils.getSectionPath(path);
        Integer allDistance = 0;
        for (ArrayList<NavigateNode> navigateNodes : sectionPath) {
            Integer distance = NavigateUtils.getCurrentPathAllDistance(navigateNodes);
            allDistance += distance;
        }
        return allDistance;
    }

    /**
     * 获取当前路径总行走距离
     */
    public static Integer getCurrentPathAllDistance(List<NavigateNode> path) {
        if (path.size() == 1) {
            //路径只有一条数据，则直接返回行走距离
            return path.get(0).getMoveDistance();
        }

        //总距离
        int allDistance = 0;
        for (int i = 0; i < path.size() - 1; i++) {//路径中最后一个节点不统计到总距离，最后一个节点并不会再行走
            allDistance += path.get(i).getMoveDistance();
        }
        return allDistance;
    }

    /**
     * 获取中间点到目标点行走距离
     */
    public static Integer getMiddleToDistDistance(List<NavigateNode> path, NavigateNode middlePath) {
        //最后一条节点不计算进行走距离
        NavigateNode lastPath = path.get(path.size() - 1);
        //总距离
        int allDistance = 0;
        boolean flag = false;
        for (NavigateNode navigateNode : path) {
            if (!flag && navigateNode.equals(middlePath)) {
                flag = true;
            }

            if (navigateNode.equals(lastPath)) {
                continue;//最后一条节点不计算进行走距离
            }

            if (flag) {
                allDistance += navigateNode.getMoveDistance();
            }
        }
        return allDistance;
    }

    /**
     * 检测路径是否可用(可走)
     */
    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 && value != 5) {//母轨道3、子轨道0、充电桩5
                return false;
            }
        }
        return true;
    }

    public static void main(String[] args) {
        //计算路径
        List<NavigateNode> calc = calc("1000901", "0201801", "out");
        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");
        //将路径分割成多段
        ArrayList<ArrayList<NavigateNode>> data = getSectionPath(calc);
        for (ArrayList<NavigateNode> list : data) {
            Integer currentPathAllDistance = getCurrentPathAllDistance(list);//计算当前路径总距离
            System.out.println(currentPathAllDistance);
            System.out.println(list);
        }