#

Junjie

昨天 af31b83bf19862a6eca99b2ed610052c468d655a

#

 src/main/java/com/zy/core/ServerBootstrap.java

@@ -135,6 +135,8 @@
                    thread = new ZyStationThread(deviceConfig, redisUtil);
                } else if (deviceConfig.getThreadImpl().equals("ZyStationV3Thread")) {
                    thread = new ZyStationV3Thread(deviceConfig, redisUtil);
                } else if (deviceConfig.getThreadImpl().equals("ZyStationV4Thread")) {
                    thread = new ZyStationV4Thread(deviceConfig, redisUtil);
                } else {
                    throw new CoolException("未知的线程实现");
                }

 src/main/java/com/zy/core/model/command/StationCommand.java

@@ -19,6 +19,9 @@

    private List<Integer> navigatePath;

    // 路径中的顶升移栽点（按路径顺序）
    private List<Integer> liftTransferPath;

    private List<Integer> originalNavigatePath;

    private StationCommandType commandType;

 src/main/java/com/zy/core/network/ZyStationConnectDriver.java

@@ -11,8 +11,10 @@
import java.util.List;
import com.zy.core.network.fake.ZyStationFakeConnect;
import com.zy.core.network.fake.ZyStationFakeSegConnect;
import com.zy.core.network.fake.ZyStationV4FakeSegConnect;
import com.zy.core.network.real.ZyStationRealConnect;
import com.zy.core.network.real.ZyStationV3RealConnect;
import com.zy.core.network.real.ZyStationV4RealConnect;
import lombok.extern.slf4j.Slf4j;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
@@ -27,6 +29,7 @@

    private static final ZyStationFakeConnect zyStationFakeConnect = new ZyStationFakeConnect();
    private static final ZyStationFakeSegConnect zyStationFakeSegConnect = new ZyStationFakeSegConnect();
    private static final ZyStationV4FakeSegConnect zyStationV4FakeSegConnect = new ZyStationV4FakeSegConnect();

    private boolean connected = false;
    private DeviceConfig deviceConfig;
@@ -50,6 +53,8 @@
        if (deviceConfig.getFake() == 0) {
            if ("ZyStationV3Thread".equals(deviceConfig.getThreadImpl())) {
                zyStationConnectApi = new ZyStationV3RealConnect(deviceConfig, redisUtil);
            } else if ("ZyStationV4Thread".equals(deviceConfig.getThreadImpl())) {
                zyStationConnectApi = new ZyStationV4RealConnect(deviceConfig, redisUtil);
            } else {
                zyStationConnectApi = new ZyStationRealConnect(deviceConfig, redisUtil);
            }
@@ -57,6 +62,9 @@
            if ("ZyStationV3Thread".equals(deviceConfig.getThreadImpl())) {
                zyStationFakeSegConnect.addFakeConnect(deviceConfig, redisUtil);
                zyStationConnectApi = zyStationFakeSegConnect;
            } else if ("ZyStationV4Thread".equals(deviceConfig.getThreadImpl())) {
                zyStationV4FakeSegConnect.addFakeConnect(deviceConfig, redisUtil);
                zyStationConnectApi = zyStationV4FakeSegConnect;
            } else {
                zyStationFakeConnect.addFakeConnect(deviceConfig, redisUtil);
                zyStationConnectApi = zyStationFakeConnect;

 src/main/java/com/zy/core/network/entity/ZyStationStatusEntity.java

@@ -50,6 +50,9 @@
    //重量
    private Double weight;

    //任务可写区
    private Integer taskWriteIdx;

    //运行堵塞
    private boolean runBlock = false;


 src/main/java/com/zy/core/network/fake/ZyStationV4FakeSegConnect.java

New file
@@ -0,0 +1,8 @@
package com.zy.core.network.fake;

/**
 * 输送站 V4 仿真分段连接实现。
 * 当前复用 V3 分段仿真逻辑，保留独立类用于后续 V4 仿真策略演进。
 */
public class ZyStationV4FakeSegConnect extends ZyStationFakeSegConnect {
}

 src/main/java/com/zy/core/network/real/ZyStationV4RealConnect.java

New file
@@ -0,0 +1,335 @@
package com.zy.core.network.real;

import HslCommunication.Core.Types.OperateResult;
import HslCommunication.Core.Types.OperateResultExOne;
import HslCommunication.Profinet.Siemens.SiemensPLCS;
import HslCommunication.Profinet.Siemens.SiemensS7Net;
import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.mapper.EntityWrapper;
import com.core.common.DateUtils;
import com.core.common.SpringUtils;
import com.zy.asrs.entity.BasDevp;
import com.zy.asrs.entity.DeviceConfig;
import com.zy.asrs.service.BasDevpService;
import com.zy.common.utils.RedisUtil;
import com.zy.core.News;
import com.zy.core.cache.OutputQueue;
import com.zy.core.model.CommandResponse;
import com.zy.core.model.StationObjModel;
import com.zy.core.model.command.StationCommand;
import com.zy.core.network.api.ZyStationConnectApi;
import com.zy.core.network.entity.ZyStationStatusEntity;
import lombok.extern.slf4j.Slf4j;

import java.text.MessageFormat;
import java.util.Collections;
import java.util.Comparator;
import java.util.Date;
import java.util.List;

/**
 * 输送站真实连接（PLC）
 */
@Slf4j
public class ZyStationV4RealConnect implements ZyStationConnectApi {

    private List<ZyStationStatusEntity> statusList;
    private List<StationObjModel> barcodeOriginList;
    private SiemensS7Net siemensNet;
    private DeviceConfig deviceConfig;
    private RedisUtil redisUtil;

    public ZyStationV4RealConnect(DeviceConfig deviceConfig, RedisUtil redisUtil) {
        this.deviceConfig = deviceConfig;
        this.redisUtil = redisUtil;
    }

    @Override
    public boolean connect() {
        boolean connected = false;
        siemensNet = new SiemensS7Net(SiemensPLCS.S1200, deviceConfig.getIp());
        OperateResult connect = siemensNet.ConnectServer();
        if (connect.IsSuccess) {
            connected = true;
            OutputQueue.DEVP.offer(MessageFormat.format("【{0}】输送站plc连接成功 ===>> [id:{1}] [ip:{2}] [port:{3}]",
                    DateUtils.convert(new Date()), deviceConfig.getDeviceNo(), deviceConfig.getIp(),
                    deviceConfig.getPort()));
            News.info("输送站plc连接成功 ===>> [id:{}] [ip:{}] [port:{}]",
                    deviceConfig.getDeviceNo(), deviceConfig.getIp(), deviceConfig.getPort());
        } else {
            OutputQueue.DEVP.offer(MessageFormat.format("【{0}】输送站plc连接失败！！！ ===>> [id:{1}] [ip:{2}] [port:{3}]",
                    DateUtils.convert(new Date()), deviceConfig.getDeviceNo(), deviceConfig.getIp(),
                    deviceConfig.getPort()));
            News.error("输送站plc连接失败！！！ ===>> [id:{}] [ip:{}] [port:{}]",
                    deviceConfig.getDeviceNo(), deviceConfig.getIp(), deviceConfig.getPort());
        }
//        siemensNet.ConnectClose();
        return connected;
    }

    @Override
    public boolean disconnect() {
        siemensNet.ConnectClose();
        return true;
    }

    @Override
    public List<ZyStationStatusEntity> getStatus(Integer deviceNo) {
        if (statusList == null) {
            BasDevpService basDevpService = SpringUtils.getBean(BasDevpService.class);
            if (basDevpService == null) {
                return Collections.emptyList();
            }

            BasDevp basDevp = basDevpService
                    .selectOne(new EntityWrapper<BasDevp>().eq("devp_no", deviceConfig.getDeviceNo()));
            if (basDevp == null) {
                return Collections.emptyList();
            }

            statusList = JSONObject.parseArray(basDevp.getStationList(), ZyStationStatusEntity.class);
            if (statusList != null) {
                statusList.sort(Comparator.comparing(ZyStationStatusEntity::getStationId));
            }
            barcodeOriginList = basDevp.getBarcodeStationList$();
        }

        if (siemensNet == null) {
            return statusList;
        }

        OperateResultExOne<byte[]> result = siemensNet.Read("DB100.0", (short) (statusList.size() * 10));
        if (result.IsSuccess) {
            for (int i = 0; i < statusList.size(); i++) {
                ZyStationStatusEntity statusEntity = statusList.get(i); // 站点编号
                statusEntity.setTaskNo(siemensNet.getByteTransform().TransInt32(result.Content, i * 10)); // 工作号
                statusEntity.setTargetStaNo((int) siemensNet.getByteTransform().TransInt16(result.Content, i * 10 + 4)); // 目标站

                boolean[] status = siemensNet.getByteTransform().TransBool(result.Content, i * 10 + 6, 1);
                statusEntity.setAutoing(status[0]); // 自动
                statusEntity.setLoading(status[1]); // 有物
                statusEntity.setInEnable(status[2]); // 可入
                statusEntity.setOutEnable(status[3]);// 可出
                statusEntity.setEmptyMk(status[4]); // 空托盘
                statusEntity.setFullPlt(status[5]); // 满托盘

                boolean[] status2 = siemensNet.getByteTransform().TransBool(result.Content, i * 10 + 7, 1);
                statusEntity.setEnableIn(status2[1]);//启动入库

                Integer palletHeight = null;
                if (status[7]) {
                    palletHeight = 1;//低
                }
                if (status2[0]) {
                    palletHeight = 2;//中
                }
                if (status[6]) {
                    palletHeight = 3;//高
                }
                statusEntity.setPalletHeight(palletHeight);//高低信号

                statusEntity.setError(0);//默认无报警

                statusEntity.setTaskWriteIdx((int) siemensNet.getByteTransform().TransInt16(result.Content, i * 10 + 8));//任务可写区
            }
        }

        // 条码扫描器
        OperateResultExOne<byte[]> result2 = siemensNet.Read("DB101.16", (short) (barcodeOriginList.size() * 16));
        if (result2.IsSuccess) {
            for (int i = 0; i < barcodeOriginList.size(); i++) {
                ZyStationStatusEntity barcodeEntity = findStatusEntityByBarcodeIdx(i + 1);
                if (barcodeEntity == null) {
                    continue;
                }
                String barcode = siemensNet.getByteTransform().TransString(result2.Content, i * 16 + 2, 14, "UTF-8");
                barcode = barcode.trim();
                barcodeEntity.setBarcode(barcode);
            }
        }

        // 称重
        OperateResultExOne<byte[]> result3 = siemensNet.Read("DB102.4", (short) (barcodeOriginList.size() * 4));
        if (result3.IsSuccess) {
            for (int i = 0; i < barcodeOriginList.size(); i++) {
                ZyStationStatusEntity barcodeEntity = findStatusEntityByBarcodeIdx(i + 1);
                if (barcodeEntity == null) {
                    continue;
                }
                double weight = (double) siemensNet.getByteTransform().TransSingle(result3.Content, i * 4);
                barcodeEntity.setWeight(weight);
            }
        }

        // 报警信息
        OperateResultExOne<byte[]> result4 = siemensNet.Read("DB103.2", (short) (barcodeOriginList.size() * 2));
        if (result4.IsSuccess) {
            for (int i = 0; i < barcodeOriginList.size(); i++) {
                ZyStationStatusEntity barcodeEntity = findStatusEntityByBarcodeIdx(i + 1);
                if (barcodeEntity == null) {
                    continue;
                }

                StringBuilder sb = new StringBuilder();
                boolean[] status1 = siemensNet.getByteTransform().TransBool(result4.Content, i * 2, 1);
                boolean[] status2 = siemensNet.getByteTransform().TransBool(result4.Content, i * 2 + 1, 1);

                if(status1[0]){
                    sb.append("左超宽报警;");
                }
                if(status1[1]) {
                    sb.append("右超宽报警;");
                }
                if(status1[2]) {
                    sb.append("前超长报警;");
                }
                if(status1[3]) {
                    sb.append("后超长报警;");
                }
                if(status1[4]) {
                    sb.append("超高报警;");
                }
                if(status1[5]) {
                    sb.append("有货报警，空托入库时检测托盘上有无货物;");
                }
                if(status1[6]) {
                    sb.append("重量异常报警;");
                }
                if(status1[7]) {
                    sb.append("扫码异常;");
                }

                if(sb.length() > 0) {
                    barcodeEntity.setError(1);
                }else {
                    barcodeEntity.setError(0);
                }
                barcodeEntity.setErrorMsg(sb.toString());
            }
        }

        return statusList;
    }

    @Override
    public CommandResponse sendCommand(Integer deviceNo, StationCommand command) {
        CommandResponse commandResponse = new CommandResponse(false);
        if (null == command) {
            commandResponse.setMessage("命令为空");
            return commandResponse;
        }

        int taskWriteIdx = getTaskWriteIdx(command.getStationId());
        if (taskWriteIdx == -1) {
            commandResponse.setMessage("命令下发超时，无法找到可用下发区域");
            return commandResponse;
        }

        int stationIdx = findIndex(command.getStationId());

        short[] data = new short[2];
        data[0] = command.getStationId().shortValue();
        data[1] = command.getTargetStaNo().shortValue();

        OperateResult writeTaskNo = siemensNet.Write("DB13." + (stationIdx * 48 + (taskWriteIdx * 12)), command.getTaskNo());
        if (!writeTaskNo.IsSuccess) {
            log.error("写入输送线命令失败。站点编号={}，站点数据={}", command.getTaskNo(), JSON.toJSON(command));
            commandResponse.setResult(false);
            commandResponse.setMessage("命令下发失败，写入工作号失败");
            return commandResponse;
        }

        OperateResult writeData = siemensNet.Write("DB13." + (stationIdx * 48 + (taskWriteIdx * 12 + 4)), data);
        if (!writeData.IsSuccess) {
            log.error("写入输送线命令失败。站点编号={}，站点数据={}", command.getTaskNo(), JSON.toJSON(command));
            commandResponse.setResult(false);
            commandResponse.setMessage("命令下发失败，写入数据区域失败");
            return commandResponse;
        }

        log.info("写入输送线命令成功。任务号={}，站点数据={}", command.getTaskNo(), JSON.toJSON(command));
        commandResponse.setResult(true);
        return commandResponse;
    }

    @Override
    public synchronized CommandResponse sendOriginCommand(String address, short[] data) {
        CommandResponse commandResponse = new CommandResponse(false);
        if (null == data || data.length == 0) {
            commandResponse.setMessage("数据为空");
            return commandResponse;
        }

        OperateResult write = siemensNet.Write(address, data);
        if (write.IsSuccess) {
            log.info("写入原始命令成功。地址={}，数据={}", address, JSON.toJSON(data));
            commandResponse.setResult(true);
        } else {
            log.error("写入原始命令失败。地址={}，数据={}", address, JSON.toJSON(data));
            commandResponse.setResult(false);
        }
        return commandResponse;
    }

    @Override
    public byte[] readOriginCommand(String address, int length) {
        OperateResultExOne<byte[]> result = siemensNet.Read(address, (short) length);
        if (result.IsSuccess) {
            return result.Content;
        }
        return new byte[0];
    }

    private ZyStationStatusEntity findStatusEntityByBarcodeIdx(Integer barcodeIdx) {
        Integer stationId = null;
        for (StationObjModel stationObjModel : barcodeOriginList) {
            if (stationObjModel.getBarcodeIdx().equals(barcodeIdx)) {
                stationId = stationObjModel.getStationId();
                break;
            }
        }

        for (ZyStationStatusEntity zyStationStatusEntity : statusList) {
            if(zyStationStatusEntity.getStationId().equals(stationId)) {
                return zyStationStatusEntity;
            }
        }
        return null;
    }

    private int getTaskWriteIdx(int stationId) {
        int useIdx = -1;

        int stationIdx = findIndex(stationId);
        if (stationIdx != -1) {
            ZyStationStatusEntity statusEntity = statusList.get(stationIdx);

            Integer taskWriteIdx = statusEntity.getTaskWriteIdx();
            if (taskWriteIdx > 0) {
                OperateResultExOne<byte[]> resultTask = siemensNet.Read("DB13." + (stationId * 48), (short) 48);
                if (resultTask.IsSuccess) {
                    int taskNo = siemensNet.getByteTransform().TransInt32(resultTask.Content, taskWriteIdx * 12);
                    int startPoint = siemensNet.getByteTransform().TransInt16(resultTask.Content, taskWriteIdx * 12 + 4);
                    int targetPoint = siemensNet.getByteTransform().TransInt16(resultTask.Content, taskWriteIdx * 12 + 6);
                    if (taskNo == 0 && startPoint == 0 && targetPoint == 0) {
                        useIdx = taskWriteIdx;
                    }
                }
            }
        }

        return useIdx;
    }

    private int findIndex(Integer stationId) {
        for (int i = 0; i < statusList.size(); i++) {
            ZyStationStatusEntity statusEntity = statusList.get(i);
            if (statusEntity.getStationId().equals(stationId)) {
                return i;
            }
        }
        return -1;
    }

}

 src/main/java/com/zy/core/thread/impl/ZyStationV4Thread.java

@@ -213,8 +213,25 @@

        if (commandType == StationCommandType.MOVE) {
            if (!stationId.equals(targetStationId)) {
                List<Integer> path = calcPathStationIds(stationId, targetStationId);
                List<NavigateNode> nodes = calcPathNavigateNodes(stationId, targetStationId);
                List<Integer> path = new ArrayList<>();
                List<Integer> liftTransferPath = new ArrayList<>();
                for (NavigateNode n : nodes) {
                    JSONObject v = JSONObject.parseObject(n.getNodeValue());
                    if (v == null) {
                        continue;
                    }
                    Integer stationNo = v.getInteger("stationId");
                    if (stationNo == null) {
                        continue;
                    }
                    path.add(stationNo);
                    if (Boolean.TRUE.equals(n.getIsLiftTransferPoint())) {
                        liftTransferPath.add(stationNo);
                    }
                }
                stationCommand.setNavigatePath(path);
                stationCommand.setLiftTransferPath(liftTransferPath);
            }
        }
        return stationCommand;
@@ -271,65 +288,88 @@
        return zyStationConnectDriver.readOriginCommand(address, length);
    }

    private List<Integer> calcPathStationIds(Integer startStationId, Integer targetStationId) {
    private List<NavigateNode> calcPathNavigateNodes(Integer startStationId, Integer targetStationId) {
        NavigateUtils navigateUtils = SpringUtils.getBean(NavigateUtils.class);
        if (navigateUtils == null) {
            return new ArrayList<>();
        }
        List<NavigateNode> nodes = navigateUtils.calcByStationId(startStationId, targetStationId);
        List<Integer> ids = new ArrayList<>();
        for (NavigateNode n : nodes) {
            JSONObject v = JSONObject.parseObject(n.getNodeValue());
            if (v != null) {
                ids.add(v.getInteger("stationId"));
            }
        }
        return ids;
        return navigateUtils.calcByStationId(startStationId, targetStationId);
    }

    private void executeMoveWithSeg(StationCommand original) {
        int stationCommandSendLength = 20;
        Object systemConfigMapObj = redisUtil.get(RedisKeyType.SYSTEM_CONFIG_MAP.key);
        if (systemConfigMapObj != null) {
            try {
                HashMap<String, String> systemConfigMap = (HashMap<String, String>) systemConfigMapObj;
                String stationCommandSendLengthStr = systemConfigMap.get("stationCommandSendLength");
                if(stationCommandSendLengthStr != null){
                    stationCommandSendLength = Integer.parseInt(stationCommandSendLengthStr);
                }
            } catch (Exception ignore) {}
        }

        if(original.getCommandType() == StationCommandType.MOVE){
            List<Integer> path = JSON.parseArray(JSON.toJSONString(original.getNavigatePath(), SerializerFeature.DisableCircularReferenceDetect), Integer.class);
            List<Integer> liftTransferPath = JSON.parseArray(JSON.toJSONString(original.getLiftTransferPath(), SerializerFeature.DisableCircularReferenceDetect), Integer.class);
            if (path == null || path.isEmpty()) {
                return;
            }

            int total = path.size();
            List<Integer> segmentTargets = new ArrayList<>();
            List<Integer> segmentEndIndices = new ArrayList<>();
            int idx = 0;
            while (idx < total) {
                int end = Math.min(idx + stationCommandSendLength, total) - 1;
                segmentTargets.add(path.get(end));
                segmentEndIndices.add(end);
                idx = end + 1;
            if (liftTransferPath != null) {
                for (Integer liftTransferStationId : liftTransferPath) {
                    int endIndex = path.indexOf(liftTransferStationId);
                    // 避免以起点作为切点导致空分段
                    if (endIndex <= 0) {
                        continue;
                    }
                    if (segmentEndIndices.isEmpty() || endIndex > segmentEndIndices.get(segmentEndIndices.size() - 1)) {
                        segmentEndIndices.add(endIndex);
                    }
                }
            }
            if (segmentEndIndices.isEmpty() || segmentEndIndices.get(segmentEndIndices.size() - 1) != total - 1) {
                segmentEndIndices.add(total - 1);
            }

            List<StationCommand> segmentCommands = new ArrayList<>();
            int buildStartIdx = 0;
            for (Integer endIdx : segmentEndIndices) {
                if (endIdx == null || endIdx < buildStartIdx) {
                    continue;
                }
                List<Integer> segmentPath = new ArrayList<>(path.subList(buildStartIdx, endIdx + 1));
                if (segmentPath.isEmpty()) {
                    buildStartIdx = endIdx + 1;
                    continue;
                }

                StationCommand segmentCommand = new StationCommand();
                segmentCommand.setTaskNo(original.getTaskNo());
                segmentCommand.setCommandType(original.getCommandType());
                segmentCommand.setPalletSize(original.getPalletSize());
                segmentCommand.setBarcode(original.getBarcode());
                segmentCommand.setOriginalNavigatePath(path);
                segmentCommand.setNavigatePath(segmentPath);
                // 每段命令：起点=当前段首站点，终点=当前段末站点
                segmentCommand.setStationId(segmentPath.get(0));
                segmentCommand.setTargetStaNo(segmentPath.get(segmentPath.size() - 1));
                segmentCommands.add(segmentCommand);

                // 分段边界点需要同时作为下一段的起点（例如 [221,220,219] + [219,213,212]）
                buildStartIdx = endIdx;
            }

            if (segmentCommands.isEmpty()) {
                return;
            }

            int segCursor = 0;
            Integer currentTarget = segmentTargets.get(segCursor);
            Integer currentEndIdx = segmentEndIndices.get(segCursor);
            Integer currentStartIdx = 0;

            StationCommand segCmd = new StationCommand();
            segCmd.setTaskNo(original.getTaskNo());
            segCmd.setStationId(original.getStationId());
            segCmd.setTargetStaNo(original.getTargetStaNo());
            segCmd.setCommandType(original.getCommandType());
            segCmd.setPalletSize(original.getPalletSize());
            segCmd.setNavigatePath(new ArrayList<>(path.subList(0, currentEndIdx + 1)));
            sendCommand(segCmd);
            while (true) {
                CommandResponse commandResponse = sendCommand(segmentCommands.get(segCursor));
                if (commandResponse == null) {
                    try {
                        Thread.sleep(200);
                    } catch (Exception ignore) {}
                    continue;
                }
                if (commandResponse.getResult()) {
                    break;
                }
                try {
                    Thread.sleep(200);
                } catch (Exception ignore) {}
            }

            long runTime = System.currentTimeMillis();
            boolean firstRun = true;
@@ -362,26 +402,15 @@
                    if (remaining <= 0) {
                        break;
                    }
                    int currentSegEndIndex = path.indexOf(segmentTargets.get(segCursor));
                    int currentSegStartIndex = segCursor == 0 ? 0 : path.indexOf(segmentTargets.get(segCursor - 1)) + 1;
                    int currentSegEndIndex = segmentEndIndices.get(segCursor);
                    int currentSegStartIndex = segCursor == 0 ? 0 : segmentEndIndices.get(segCursor - 1);
                    int segLen = currentSegEndIndex - currentSegStartIndex + 1;
                    int remainingSegment = Math.max(0, currentSegEndIndex - currentIndex);
                    int thresholdSegment = (int) Math.ceil(segLen * 0.3);
                    if (remainingSegment <= thresholdSegment && segCursor < segmentTargets.size() - 1) {
                    if (remainingSegment <= thresholdSegment && segCursor < segmentCommands.size() - 1) {
                        segCursor++;
                        currentEndIdx = segmentEndIndices.get(segCursor);
                        currentStartIdx = segmentEndIndices.get(segCursor - 1) + 1;

                        StationCommand nextCmd = new StationCommand();
                        nextCmd.setTaskNo(original.getTaskNo());
                        nextCmd.setStationId(original.getStationId());
                        nextCmd.setTargetStaNo(original.getTargetStaNo());
                        nextCmd.setCommandType(original.getCommandType());
                        nextCmd.setPalletSize(original.getPalletSize());
                        nextCmd.setNavigatePath(new ArrayList<>(path.subList(currentStartIdx, currentEndIdx + 1)));
                        nextCmd.setOriginalNavigatePath(path);
                        while (true) {
                            CommandResponse commandResponse = sendCommand(nextCmd);
                            CommandResponse commandResponse = sendCommand(segmentCommands.get(segCursor));
                            if (commandResponse == null) {
                                Thread.sleep(200);
                                continue;