四向车下命令时计算移动距离

Junjie

2023-03-28 6bad808389ec9d4c7331bb471c1410b50893fc1c

四向车下命令时计算移动距离

 src/main/java/com/zy/common/model/NavigateNode.java

@@ -16,6 +16,7 @@
    private NavigateNode Father;//父节点
    private Boolean isInflectionPoint;//是否为拐点
    private String direction;//行走方向
    private Integer moveDistance;//行走距离

    public NavigateNode(int x, int y) {
        this.x = x;

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

@@ -66,4 +66,53 @@
        return null;
    }

    //获取JSON格式数据
    public ArrayList<ArrayList<JSONObject>> getJsonData(Integer mapType) {
        try {
            String mapFilename = "";
            if (mapType == ShuttleTaskModeType.PAK_IN.id) {
                //入库地图
                mapFilename = "mapIn.json";
            }else {
                //出库地图
                mapFilename = "mapOut.json";
            }

            String fileName = this.getClass().getClassLoader().getResource(mapFilename).getPath();//获取文件路径
            File file = new File(fileName);
            StringBuffer stringBuffer = new StringBuffer();
            if (file.isFile() && file.exists()) {
                InputStreamReader isr = new InputStreamReader(new FileInputStream(file), "GBK");
                BufferedReader br = new BufferedReader(isr);
                String lineTxt = null;
                while ((lineTxt = br.readLine()) != null) {
                    stringBuffer.append(lineTxt);
                }
                br.close();

                //返回的结果集
                ArrayList<ArrayList<JSONObject>> returnLists = new ArrayList<>();

                //解析json地图数据
                ArrayList arrayList = JSON.parseObject(stringBuffer.toString(), ArrayList.class);
                for (Object obj : arrayList) {
                    ArrayList list = JSON.parseObject(obj.toString(), ArrayList.class);
                    ArrayList<JSONObject> maps = new ArrayList<>();
                    for (Object o : list) {
                        JSONObject jsonObject = JSON.parseObject(o.toString());
                        maps.add(jsonObject);
                    }
                    returnLists.add(maps);
                }

                return returnLists;
            } else {
                System.out.println("文件不存在!");
            }
        } catch (IOException ioException) {
            ioException.printStackTrace();
        }
        return null;
    }

}

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

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

import com.alibaba.fastjson.JSONObject;
import com.zy.common.model.NavigateNode;
import com.zy.core.enums.ShuttleTaskModeType;

@@ -141,6 +142,8 @@
                //直行线路
                mapNode.setDirection(direction);//设置行走方向
            }
            Integer distance = getXToNextDistance(mapNode);//获取当前点到下一点的行走距离
            mapNode.setMoveDistance(distance);
        }

        //将最后一段数据添加进入
@@ -149,15 +152,48 @@
        return list;
    }

    //获取从x点到下一点的行走距离
    public static Integer getXToNextDistance(NavigateNode xNode) {
        NavigateMapData mapData = new NavigateMapData();
        ArrayList<ArrayList<JSONObject>> lists = mapData.getJsonData(1);
        if (lists != null) {
            JSONObject jsonObject = lists.get(xNode.getX()).get(xNode.getY());
            if (jsonObject != null) {
                return Integer.parseInt(jsonObject.getOrDefault(xNode.getDirection(), 0).toString());
            }
            return 0;
        }
        return 0;
    }

    /**
     * 获取当前路径总行走距离
     */
    public static Integer getCurrentPathAllDistance(ArrayList<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 void main(String[] args) {
        //计算路径
        List<NavigateNode> calc = calc("1000901", "0201801", ShuttleTaskModeType.PAK_OUT.id);
        List<NavigateNode> calc = calc("1000901", "1800201", ShuttleTaskModeType.PAK_OUT.id);
        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);
        }


 src/main/java/com/zy/core/thread/ShuttleThread.java

@@ -341,21 +341,27 @@
                ArrayList pathList = JSON.parseObject(data.toString(), ArrayList.class);
                //取第一条路径
                Object o1 = pathList.get(commandStep);
                ArrayList path = JSON.parseObject(o1.toString(), ArrayList.class);
                ArrayList objectPath = JSON.parseObject(o1.toString(), ArrayList.class);
                ArrayList<NavigateNode> path = new ArrayList<>();
                for (Object o2 : objectPath) {
                    NavigateNode navigateNode = JSON.parseObject(o2.toString(), NavigateNode.class);
                    path.add(navigateNode);
                }
                Integer allDistance = NavigateUtils.getCurrentPathAllDistance(path);//计算当前路径行走总距离

                size = Integer.parseInt(map.get("pathSize").toString());
                //开始路径
                JSONObject startPath = JSON.parseObject(path.get(0).toString());
                NavigateNode startPath = path.get(0);
                //目标路径
                JSONObject endPath = JSON.parseObject(path.get(path.size() - 1).toString());
                NavigateNode endPath = path.get(path.size() - 1);

                command.setCommandWord((short) 1);
                command.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getIntValue("x"), startPath.getIntValue("y")));
                command.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY()));
                command.setMiddleCodeNum((short) 0);
                command.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getIntValue("x"), endPath.getIntValue("y")));
                command.setStartToDistDistance(1000);
                command.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY()));
                command.setStartToDistDistance(allDistance);
                command.setMiddleToDistDistance(1000);
                command.setRunDirection(ShuttleRunDirection.get(startPath.get("direction").toString()).id);
                command.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
                command.setPalletLift((short) 1);
                command.setForceMoveDistance(1000);
                command.setChargeSwitch((short) 2);
@@ -431,6 +437,10 @@

                //对主线程抛出等待确认状态waiting
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WAITING);
                //设置四向穿梭车为空闲状态
                shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
                //任务号清零
                shuttleProtocol.setTaskNo((short) 0);

                News.info("四向穿梭车任务执行完成等待确认中，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
            }