*

L

2026-04-23 702549f5d846ae76cc005c56d03fd774a3caa341

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

@@ -5,7 +5,7 @@
import HslCommunication.Profinet.Siemens.SiemensPLCS;
import HslCommunication.Profinet.Siemens.SiemensS7Net;
import com.alibaba.fastjson.JSON;
import com.baomidou.mybatisplus.mapper.EntityWrapper;
import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
import com.core.common.DateUtils;
import com.core.common.SpringUtils;
import com.zy.asrs.entity.BasCircularShuttle;
@@ -13,6 +13,7 @@
import com.zy.asrs.entity.BasRgvErrLog;
import com.zy.asrs.entity.BasRgvOpt;
import com.zy.asrs.service.BasCircularShuttleService;
import com.zy.asrs.service.BasDevpPositionService;
import com.zy.asrs.service.BasRgvErrLogService;
import com.zy.asrs.service.BasRgvOptService;
import com.zy.asrs.service.BasRgvService;
@@ -20,6 +21,7 @@
import com.zy.core.ThreadHandler;
import com.zy.core.cache.MessageQueue;
import com.zy.core.cache.OutputQueue;
import com.zy.core.cache.SlaveConnection;
import com.zy.core.enums.RgvModeType;
import com.zy.core.enums.RgvTaskModeType;
import com.zy.core.enums.SlaveType;
@@ -31,10 +33,8 @@
import lombok.extern.slf4j.Slf4j;

import java.text.MessageFormat;
import java.util.ArrayList;
import java.util.Date;
import java.util.HashSet;
import java.util.List;
import java.util.*;
import java.util.concurrent.ConcurrentHashMap;

/**
 * RGV线程
@@ -57,6 +57,34 @@
    private boolean resetFlag2 = false;
    private boolean connectRgv = false;
    private boolean alarmChangeSign = false;
    private boolean csSign = true;//测试标记

    /** 模拟移动速度：PLC轨道单位/tick（200ms），5000单位≈2.5m/s */
    private static final long SIM_CS_MOVE_STEP = 5000;
    /** 环形轨道总长（PLC轨道单位） */
    private static final long PERIMETER = 1737000;
    /** 避让安全距离 */
    private static final long AVOIDANCE_DISTANCE = 50000;

    /** 模拟任务阶段 */
    private enum SimPhase { IDLE, MOVING, WAITING_AT_SOURCE }

    /** 模拟任务数据 */
    @Data
    static class SimTask {
        int taskNo;
        short sourceSta;
        short destSta;
        short mode;          // 任务模式：1=取货 2=放货 3=取放货 5=漫游
        SimPhase phase;
        long targetPos;      // 目标轨道位置
        long startPos;       // 起始轨道位置
        long destPos;        // 放货站位置（仅取放货任务使用）
        long waitStartTime;  // 到达取货站后的等待开始时间
    }

    /** 全局模拟任务表：rgvNo → SimTask，线程安全 */
    static final Map<Integer, SimTask> simTasks = new ConcurrentHashMap<>();

    public RgvThread(RgvSlave slave) {
        this.slave = slave;
@@ -65,7 +93,11 @@
    @Override
    @SuppressWarnings("InfiniteLoopStatement")
    public void run() {
        connectRgv = this.connect();
        if (csSign) {
            connectRgv = true; // 模拟模式跳过PLC连接
        } else {
            connectRgv = this.connect();
        }

        // 启动线程自动重连
        new Thread(this::rgvConnect).start();
@@ -75,6 +107,11 @@

        // 启动任务下发线程
        new Thread(this::taskIssued).start();

        // 模拟模式：启动模拟运行线程
        if (csSign) {
            new Thread(this::simThreadRun).start();
        }
    }


@@ -97,47 +134,89 @@
                if (task != null) {
                    step = task.getStep();
                }
                switch (step) {
                    //漫游任务完成信号
                    case 1:
                if (csSign) {
                    // ===== 模拟模式：不写PLC，用模拟方法替代 =====
                    switch (step) {
                        case 1:
                            taskCompleteCS();
                            break;
                        case 2:
                            RgvCommand command2 = (RgvCommand) task.getData();
                            rgvOpt(command2);
                            writeCS(command2);
                            rgvProtocol.setTaskNo1(command2.getTaskNo1());
                            break;
                        case 3:
                            RgvCommand command = (RgvCommand) task.getData();
                            if (null == command) {
                                command = new RgvCommand();
                            }
                            command.setRgvNo(slave.getId());
                            command.setTaskNo1((short) 0);
                            command.setAckFinish1((short) 1);
                            command.setTaskMode1(RgvTaskModeType.NONE);
                            command.setSourceStaNo1((short)0);
                            command.setDestinationStaNo1((short)0);
                            rgvOpt(command);
                            write3CS(command);
                            break;
                        case 4:
                            RgvCommand command4 = (RgvCommand) task.getData();
                            rgvOpt(command4);
                            write4CS(command4);
                            break;
                        case 5:
                            Long aLong = (Long) task.getData();
                            rgvOpt(aLong);
                            write5CS(aLong);
                            break;
                        default:
                            break;
                    }
                } else {
                    // ===== 真实模式：写PLC =====
                    switch (step) {
                        //漫游任务完成信号
                        case 1:
//                        readStatus();
                        taskComplete();
                        break;
                    //工位1写入数据
                    case 2:
                        RgvCommand command2 = (RgvCommand) task.getData();
                        rgvOpt(command2);
                        write(command2);
                        break;
                    // 复位
                    case 3:
                        RgvCommand command = (RgvCommand) task.getData();
                        if (null == command) {
                            command = new RgvCommand();
                        }
                        command.setRgvNo(slave.getId()); // RGV编号
                        command.setTaskNo1((short) 0); // 工作号
                        command.setAckFinish1((short) 1);  // 任务完成确认位
                        command.setTaskMode1(RgvTaskModeType.NONE); // 任务模式
                        command.setSourceStaNo1((short)0);     // 源站
                        command.setDestinationStaNo1((short)0);     // 目标站
                        rgvOpt(command);
                        write3(command);
                        break;
                    //工位1写入取消数据
                    case 4:
                        RgvCommand command4 = (RgvCommand) task.getData();
                        rgvOpt(command4);
                        write4(command4);
                        break;
                    // 漫游
                    case 5:
                        Long aLong = (Long) task.getData();
                        rgvOpt(aLong);
                        write5(aLong);
                        break;
                    default:
                        break;
                            taskComplete();
                            break;
                        //工位1写入数据
                        case 2:
                            RgvCommand command2 = (RgvCommand) task.getData();
                            rgvOpt(command2);
                            write(command2);
                            break;
                        // 复位
                        case 3:
                            RgvCommand command = (RgvCommand) task.getData();
                            if (null == command) {
                                command = new RgvCommand();
                            }
                            command.setRgvNo(slave.getId()); // RGV编号
                            command.setTaskNo1((short) 0); // 工作号
                            command.setAckFinish1((short) 1);  // 任务完成确认位
                            command.setTaskMode1(RgvTaskModeType.NONE); // 任务模式
                            command.setSourceStaNo1((short)0);     // 源站
                            command.setDestinationStaNo1((short)0);     // 目标站
                            rgvOpt(command);
                            write3(command);
                            break;
                        //工位1写入取消数据
                        case 4:
                            RgvCommand command4 = (RgvCommand) task.getData();
                            rgvOpt(command4);
                            write4(command4);
                            break;
                        // 漫游
                        case 5:
                            Long aLong = (Long) task.getData();
                            rgvOpt(aLong);
                            write5(aLong);
                            break;
                        default:
                            break;
                    }
                }
                Thread.sleep(50);
            } catch (Exception e) {
@@ -156,7 +235,7 @@
                    try {
                        // 根据实时信息更新数据库
                        BasCircularShuttleService basCircularShuttleService = SpringUtils.getBean(BasCircularShuttleService.class);
                        BasCircularShuttle basCircularShuttle = basCircularShuttleService.selectOne(new EntityWrapper<BasCircularShuttle>().eq("rgv_no",  slave.getId()));
                        BasCircularShuttle basCircularShuttle = basCircularShuttleService.getOne(new QueryWrapper<BasCircularShuttle>().eq("rgv_no",  slave.getId()));
                        if (basCircularShuttle.getStatus() != 0){
                            continue;
                        }
@@ -210,34 +289,28 @@
     * 初始化RGV状态
     */
    private void initRgv() {
       if (csSign){
           initRgvCS();
       } else {
           initRgvT();
       }
    }

    /**
     * 初始化RGV状态
     */
    private void initRgvT() {
        if (null == rgvProtocol) {
            rgvProtocol = new RgvProtocol();
        }
        rgvProtocol.setRgvNo(slave.getId());
//        rgvProtocol.setMode((short) -1);
//        rgvProtocol.setStatus((short)-1);
//        rgvProtocol.setTaskNo1((short)0);
//        rgvProtocol.setStatus1((short)-1);
//        rgvProtocol.setLoaded1((short)0);
//        rgvProtocol.setWalkPos((short)0);
//        rgvProtocol.setRgvPos(0L);
//        rgvProtocol.setTaskNo2((short)0);
//        rgvProtocol.setStatus2((short)-1);
//        rgvProtocol.setLoaded2((short)0);
//        rgvProtocol.setAlarm((short)0);
//        rgvProtocol.setxSpeed((short) 0);
//        rgvProtocol.setxDistance((short) 0);
//        rgvProtocol.setxDuration((short) 0);
        rgvProtocol.setMode((short) 3);
        rgvProtocol.setStatus((short)0);
        rgvProtocol.setMode((short) -1);
        rgvProtocol.setStatus((short)-1);
        rgvProtocol.setTaskNo1((short)0);
        rgvProtocol.setStatus1((short)0);
        rgvProtocol.setStatus1((short)-1);
        rgvProtocol.setLoaded1((short)0);
        rgvProtocol.setWalkPos((short)0);
        if (rgvProtocol.getRgvPos()==null ||  rgvProtocol.getRgvPos()==0){
            rgvProtocol.setRgvPos(1L+rgvProtocol.getRgvNo()*10000);
        }
        rgvProtocol.setRgvPos(rgvProtocol.getRgvPos()+1000);
        rgvProtocol.setRgvPos(0L);
        rgvProtocol.setTaskNo2((short)0);
        rgvProtocol.setStatus2((short)-1);
        rgvProtocol.setLoaded2((short)0);
@@ -245,6 +318,51 @@
        rgvProtocol.setxSpeed((short) 0);
        rgvProtocol.setxDistance((short) 0);
        rgvProtocol.setxDuration((short) 0);
        log.error("连接中断：RGV号："+slave.getId());
        try {
            // 根据实时信息更新数据库
            BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class);
            BasRgv basRgv = new BasRgv();
            basRgv.setRgvNo(slave.getId());
            basRgv.setRgvSts((int)rgvProtocol.getMode());
            basRgv.setLoaded2(rgvProtocol.getLoaded2().intValue());
            if (!basRgvService.updateById(rgvProtocol.toSqlModel(basRgv))){
                log.error("RGV plc数据库更新失败 ===>> [id:{}] [ip:{}] [port:{}] [rack:{}] [slot:{}]", slave.getId(), slave.getIp(), slave.getPort(), slave.getRack(), slave.getSlot());
            }
        } catch (Exception ignore) {
        }
    }

    /**
     * 初始化RGV状态
     */
    private void initRgvCS() {
        if (null == rgvProtocol) {
            rgvProtocol = new RgvProtocol();
            rgvProtocol.setRgvNo(slave.getId());
            rgvProtocol.setMode((short) 3);
            rgvProtocol.setStatus((short)0);
            rgvProtocol.setTaskNo1((short)0);
            rgvProtocol.setStatus1((short)0);
            rgvProtocol.setLoaded1((short)0);
            rgvProtocol.setWalkPos((short)0);
            if (rgvProtocol.getRgvPos()==null ||  rgvProtocol.getRgvPos()==0){
                rgvProtocol.setRgvPos(1L+rgvProtocol.getRgvNo()*100000);
            }
//        rgvProtocol.setRgvPos(rgvProtocol.getRgvPos()+1000);
            rgvProtocol.setRgvPos(rgvProtocol.getRgvPos());
            if (rgvProtocol.getRgvPos()>0){
                rgvProtocol.setRgvPos((long)rgvProtocol.getRgvPos());
                rgvProtocol.setRgvPosInt(rgvProtocol.getRgvPos().intValue());
            }
            rgvProtocol.setTaskNo2((short)0);
            rgvProtocol.setStatus2((short)-1);
            rgvProtocol.setLoaded2((short)0);
            rgvProtocol.setAlarm((short)0);
            rgvProtocol.setxSpeed((short) 0);
            rgvProtocol.setxDistance((short) 0);
            rgvProtocol.setxDuration((short) 0);
        }
//        log.error("连接中断：RGV号："+slave.getId());
        try {
            // 根据实时信息更新数据库
@@ -340,7 +458,7 @@
                        if (!alarmChangeSign && !alarmList.isEmpty()){
                            BasRgvErrLogService basRgvErrLogService = SpringUtils.getBean(BasRgvErrLogService.class);
                            BasRgvErrLog basRgvErrLog = new BasRgvErrLog(rgvProtocol.getAlarmList(), rgvProtocol.getTaskNo1(), rgvProtocol.getRgvNo());
                            basRgvErrLogService.insert(basRgvErrLog);
                            basRgvErrLogService.save(basRgvErrLog);
                        }
                    } catch (Exception e){
                        log.error("RGV异常信息保存失败！！");
@@ -371,7 +489,7 @@
            BasRgvOptService basRgvOptService = SpringUtils.getBean(BasRgvOptService.class);
            BasRgvOpt basRgvOpt = new BasRgvOpt(rgvProtocol.getTaskNo1().intValue(), rgvProtocol.getRgvNo(), rgvProtocol.getRgvPosInt(), command);
            log.info(rgvProtocol.getRgvNo()+"号小车写入命令定位值："+rgvProtocol.getRgvPosInt());
            basRgvOptService.insert(basRgvOpt);
            basRgvOptService.save(basRgvOpt);
        }catch (Exception e){
            log.error("RGV写入命令保存失败！！");
        }
@@ -380,7 +498,7 @@
        try{
            BasRgvOptService basRgvOptService = SpringUtils.getBean(BasRgvOptService.class);
            BasRgvOpt basRgvOpt = new BasRgvOpt(rgvProtocol.getTaskNo1().intValue(), rgvProtocol.getRgvNo(), rgvProtocol.getRgvPosI(), command);
            basRgvOptService.insert(basRgvOpt);
            basRgvOptService.save(basRgvOpt);
        }catch (Exception e){
            log.error("RGV写入命令保存失败！！");
        }
@@ -432,7 +550,7 @@
                    new Date(),
                    null
            );
            bean.insert(basRgvOpt);
            bean.save(basRgvOpt);
        } catch (Exception ignore) {}

        if (result != null && result.IsSuccess) {
@@ -494,7 +612,7 @@
                    new Date(),
                    null
            );
            bean.insert(basRgvOpt);
            bean.save(basRgvOpt);
        } catch (Exception ignore) {}

        if (result != null && result.IsSuccess) {
@@ -579,7 +697,7 @@
                    new Date(),
                    null
            );
            bean.insert(basRgvOpt);
            bean.save(basRgvOpt);
        } catch (Exception ignore) {}

        if (result != null && result.IsSuccess) {
@@ -597,7 +715,367 @@

    @Override
    public void close() {
        siemensNet.ConnectClose();
        if (siemensNet != null) {
            siemensNet.ConnectClose();
        }
    }

    // ==================== 模拟模式方法 ====================

    /**
     * 模拟完成确认（step=1）：模拟模式无需读PLC
     */
    private void taskCompleteCS() {
        // 模拟模式下无PLC，跳过
    }

    /**
     * 模拟取放货下发（step=2）：记录到simTask，启动模拟移动
     * 取放货(mode=3): 第一阶段去取货站，到达后等待5秒，再去放货站
     */
    private void writeCS(RgvCommand command) {
        if (command == null) return;
        SimTask st = new SimTask();
        st.setTaskNo(command.getTaskNo1());
        st.setSourceSta(command.getSourceStaNo1());
        st.setDestSta(command.getDestinationStaNo1());
        st.setMode(command.getTaskMode1());
        st.setPhase(SimPhase.MOVING);
        st.setStartPos(rgvProtocol.getRgvPos() != null ? rgvProtocol.getRgvPos() : 0L);

        if (command.getTaskMode1().intValue() == RgvTaskModeType.FETCH_PUT.id) {
            // 取放货：先去取货站，记下放货站位置
            st.setTargetPos(getStationPosition(command.getSourceStaNo1()));
            st.setDestPos(getStationPosition(command.getDestinationStaNo1()));
        } else {
            // 其他模式：直接去目标站
            st.setTargetPos(getStationPosition(command.getDestinationStaNo1()));
            st.setDestPos(0L);
        }

        simTasks.put(slave.getId(), st);

        log.info("[模拟] RGV{} 取放货任务接收: taskNo={} source={} dest={} 目标位置={}",
                slave.getId(), command.getTaskNo1(), command.getSourceStaNo1(),
                command.getDestinationStaNo1(), st.getTargetPos());
        OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 取放货任务接收: source={2} dest={3} target={4}",
                DateUtils.convert(new Date()), slave.getId(),
                command.getSourceStaNo1(), command.getDestinationStaNo1(), st.getTargetPos()));
    }

    /**
     * 模拟复位（step=3）：清除模拟任务
     */
    private void write3CS(RgvCommand command) {
        simTasks.remove(slave.getId());
        rgvProtocol.setTaskNo1((short) 0);
        rgvProtocol.setStatus((short) 0);
        log.info("[模拟] RGV{} 复位完成", slave.getId());
        OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 复位完成",
                DateUtils.convert(new Date()), slave.getId()));
    }

    /**
     * 模拟取消（step=4）：清除模拟任务
     */
    private void write4CS(RgvCommand command) {
        simTasks.remove(slave.getId());
        log.info("[模拟] RGV{} 任务取消: taskNo={}", slave.getId(), command.getTaskNo1());
        OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 任务取消: taskNo={2}",
                DateUtils.convert(new Date()), slave.getId(), command.getTaskNo1()));
    }

    /**
     * 模拟漫游下发（step=5）：记录到simTask，启动模拟移动
     */
    private void write5CS(Long destination) {
        SimTask st = new SimTask();
        st.setMode((short) 5);
        st.setPhase(SimPhase.MOVING);
        st.setTargetPos(destination);
        st.setStartPos(rgvProtocol.getRgvPos() != null ? rgvProtocol.getRgvPos() : 0L);
        simTasks.put(slave.getId(), st);

        log.info("[模拟] RGV{} 漫游任务接收: 目标位置={}", slave.getId(), destination);
        OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 漫游任务接收: 目标={2}",
                DateUtils.convert(new Date()), slave.getId(), destination));
    }

    // ==================== 模拟运行线程 ====================

    /**
     * 模拟运行线程：每200ms更新本车位置 + 检测避让（各线程只处理自己的simTask）
     */
    private void simThreadRun() {
        log.info("[模拟] RGV{} 模拟运行线程启动", slave.getId());
        int tick = 0;
        int myId = slave.getId();
        while (true) {
            try {
                Thread.sleep(200);
                tick++;

                SimTask st = simTasks.get(myId);
                if (st == null || st.getPhase() == SimPhase.IDLE) continue;

                if (rgvProtocol == null || rgvProtocol.getRgvPos() == null) continue;
                long current = rgvProtocol.getRgvPos();
                long target = st.getTargetPos();
                double dist = forwardDistance(current, target);

                // 到达取货站：等待5秒后前往放货站
                if (st.getPhase() == SimPhase.WAITING_AT_SOURCE) {
                    long waited = System.currentTimeMillis() - st.getWaitStartTime();
                    if (waited >= 5000) {
                        st.setTargetPos(st.getDestPos());
                        st.setStartPos(current);
                        st.setPhase(SimPhase.MOVING);
                        log.info("[模拟] RGV{} 等待{}ms结束，开始前往放货站: pos={}", myId, waited, st.getDestPos());
                        OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 等待结束，前往放货站: {2}",
                                DateUtils.convert(new Date()), myId, st.getDestPos()));
                    }
                    continue;
                }

                if (dist <= SIM_CS_MOVE_STEP) {
                    // 到达目标
                    rgvProtocol.setRgvPos(target);
                    rgvProtocol.setRgvPosInt((int) target);

                    if (st.getMode() == RgvTaskModeType.FETCH_PUT.id.shortValue() && st.getDestPos() != 0L && target != st.getDestPos()) {
                        // 阶段1：到达取货站，进入等待
                        rgvProtocol.setStatus((short) 2); // 进站运行中
                        st.setPhase(SimPhase.WAITING_AT_SOURCE);
                        st.setWaitStartTime(System.currentTimeMillis());

                        log.info("[模拟] RGV{} 到达取货站: pos={}，等待5秒后前往放货站", myId, target);
                        OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 到达取货站: {2}，等待5秒",
                                DateUtils.convert(new Date()), myId, target));
                    } else {
                        // 阶段2到达放货站 / 漫游到达 → 申请完成任务
                        rgvProtocol.setStatus((short) 100); // WAITING
                        simTasks.remove(myId);

                        log.info("[模拟] RGV{} 到达放货站: pos={}，状态变为申请完成(WAITING)", myId, target);
                        OutputQueue.RGV.offer(MessageFormat.format("【{0}】[模拟][id:{1}] 到达放货站: {2}，申请完成任务",
                                DateUtils.convert(new Date()), myId, target));
                    }

                    try {
                        BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class);
                        basRgvService.updateById(rgvProtocol.toSqlModel(new BasRgv()));
                    } catch (Exception ignore) {}
                } else {
                    // 前进（减速逻辑：前方有车则降低步长，不重合不超过）
                    long step = calcAdjustedStep(myId, current);
                    long newPos = (current + step) % PERIMETER;
                    rgvProtocol.setRgvPos(newPos);
                    rgvProtocol.setRgvPosInt((int) newPos);
                    rgvProtocol.setStatus((short) 11); // ROAM

                    if (tick % 5 == 0) {
                        log.info("[模拟] RGV{} 移动中: {} → {} (距目标{}, 步长={})",
                                myId, current, newPos, (long) dist, step);
                    }

                    try {
                        BasRgvService basRgvService = SpringUtils.getBean(BasRgvService.class);
                        basRgvService.updateById(rgvProtocol.toSqlModel(new BasRgv()));
                    } catch (Exception ignore) {}

                    // 检测避让：只检查本车前方是否有阻挡
                    checkAndAvoidBlocker(myId, target);
                }

            } catch (Exception e) {
                log.error("[模拟] RGV{} 模拟线程异常: {}", slave.getId(), e.getMessage());
            }
        }
    }

    /**
     * 检测前方阻挡并递推避让
     * @param myRgvNo   当前小车编号
     * @param myTarget  当前小车的目标位置
     */
    private void checkAndAvoidBlocker(int myRgvNo, long myTarget) {
        RgvThread myThread = getRgvThread(myRgvNo);
        if (myThread == null || myThread.getRgvProtocol() == null) return;
        long myPos = myThread.getRgvProtocol().getRgvPos();
        if (myPos == 0) return;

        double distToTarget = forwardDistance(myPos, myTarget);
        if (distToTarget <= 0) return;

        try {
            BasCircularShuttleService shuttleService = SpringUtils.getBean(BasCircularShuttleService.class);
            List<BasCircularShuttle> shuttles = shuttleService.list(
                    new QueryWrapper<BasCircularShuttle>().orderByAsc("rgv_id"));
            if (shuttles == null) return;

            // 只找离自己最近的前车（在自己与目标之间）
            int nearestRgvNo = -1;
            double nearestDist = Double.MAX_VALUE;

            for (BasCircularShuttle shuttle : shuttles) {
                int otherNo = shuttle.getRgvNo();
                if (otherNo == myRgvNo) continue;
                if (!isIdleForAvoidance(otherNo)) continue;

                RgvThread otherThread = getRgvThread(otherNo);
                if (otherThread == null || otherThread.getRgvProtocol() == null) continue;
                long otherPos = otherThread.getRgvProtocol().getRgvPos();
                if (otherPos == 0) continue;

                double fwdDist = forwardDistance(myPos, otherPos);
                // 对方在前方 且 在目标点之前 → 阻挡
                if (fwdDist > 0 && fwdDist < distToTarget) {
                    if (fwdDist < nearestDist) {
                        nearestDist = fwdDist;
                        nearestRgvNo = otherNo;
                    }
                }
            }

            if (nearestRgvNo > 0) {
                long avoidDest = forwardAvoidDest(myTarget);
                issueAvoidanceRecursive(nearestRgvNo, avoidDest);
            }
        } catch (Exception ignore) {}
    }

    /**
     * 判断RGV是否空闲可用于避让
     */
    private boolean isIdleForAvoidance(int rgvNo) {
        SimTask otherSim = simTasks.get(rgvNo);
        if (otherSim != null && otherSim.getPhase() == SimPhase.MOVING) return false;

        RgvThread otherThread = getRgvThread(rgvNo);
        if (otherThread == null) return false;
        RgvProtocol protocol = otherThread.getRgvProtocol();
        if (protocol == null) return false;

        int st = protocol.getStatus() != null ? protocol.getStatus() : -1;
        return (st == 0 || st == 11) && protocol.getTaskNo1() == 0;
    }

    /**
     * 下发避让：给rgvNo写入避让simTask至avoidDest
     * 该RGV的线程会在下一tick自行检测避让路径上的阻挡
     */
    private void issueAvoidanceRecursive(int rgvNo, long avoidDest) {
        MessageQueue.offer(SlaveType.Rgv, rgvNo, new Task(5, avoidDest));
//        RgvThread otherThread = getRgvThread(rgvNo);
//        if (otherThread == null || otherThread.getRgvProtocol() == null) return;
//        long currentPos = otherThread.getRgvProtocol().getRgvPos();
//
//        // 写入simTask（覆盖已有的WAITING_AT_SOURCE或MOVING任务）
//        SimTask st = new SimTask();
//        st.setMode((short) 5);
//        st.setPhase(SimPhase.MOVING);
//        st.setTargetPos(avoidDest);
//        st.setStartPos(currentPos);
//        simTasks.put(rgvNo, st);
//
//        log.info("[模拟避让] RGV{} 前方RGV{}阻挡，避让至: {}",
//                slave.getId(), rgvNo, avoidDest);
//        OutputQueue.RGV.offer(MessageFormat.format(
//                "【{0}】[模拟避让] RGV{1} 前方RGV{2}阻挡, 避让至: {3}",
//                DateUtils.convert(new Date()), slave.getId(), rgvNo, avoidDest));
    }

    /**
     * 计算目标点前方10000处的避让位置
     */
    private long forwardAvoidDest(long target) {
        long dest = target + 100000;
        if (dest > PERIMETER) dest -= PERIMETER;
        return dest;
    }

    /** 前车跟随距离：进入此范围开始减速 */
    private static final long FOLLOW_DISTANCE = 20000;
    /** 最小安全间距：不能比这更近 */
    private static final long MIN_GAP = 3000;

    /**
     * 根据前方车辆距离计算本步实际步长
     * 前方无车 → 全速；有车 → 线性减速；贴紧 → 停止
     */
    private long calcAdjustedStep(int myId, long myPos) {
        long aheadDist = distanceToNearestAhead(myId, myPos);
        if (aheadDist < 0) return SIM_CS_MOVE_STEP; // 前方无车，全速

        long gap = aheadDist - MIN_GAP;
        if (gap <= 0) return 0; // 已到最小间距，停止
        return Math.min(SIM_CS_MOVE_STEP, gap);
    }

    /**
     * 查找正前方最近一辆车的正向距离，无车返回-1
     * 只关注有任务或正在移动的车，空闲静止的车由避让逻辑处理
     */
    private long distanceToNearestAhead(int myId, long myPos) {
        long best = -1;
        try {
            BasCircularShuttleService shuttleService = SpringUtils.getBean(BasCircularShuttleService.class);
            List<BasCircularShuttle> shuttles = shuttleService.list(
                    new QueryWrapper<BasCircularShuttle>().orderByAsc("rgv_id"));
            if (shuttles == null) return -1;

            for (BasCircularShuttle shuttle : shuttles) {
                int otherId = shuttle.getRgvNo();
                if (otherId == myId) continue;

                SimTask otherTask = simTasks.get(otherId);
                // 只关注有任务的车（MOVING / WAITING_AT_SOURCE）
                if (otherTask == null || otherTask.getPhase() == SimPhase.IDLE) continue;

                RgvThread otherThread = getRgvThread(otherId);
                if (otherThread == null || otherThread.getRgvProtocol() == null) continue;
                Long otherPos = otherThread.getRgvProtocol().getRgvPos();
                if (otherPos == null || otherPos == 0) continue;

                double fwd = forwardDistance(myPos, otherPos);
                if (fwd > 0 && fwd <= FOLLOW_DISTANCE && (best < 0 || fwd < best)) {
                    best = (long) fwd;
                }
            }
        } catch (Exception ignore) {}
        return best;
    }

    /**
     * 计算环形轨道上从current到target的正向距离
     */
    private double forwardDistance(long current, long target) {
        if (target >= current) return target - current;
        return PERIMETER - current + target;
    }

    /**
     * 获取站点在轨道上的位置
     */
    private long getStationPosition(short staNo) {
        try {
            BasDevpPositionService positionService = SpringUtils.getBean(BasDevpPositionService.class);
            com.zy.asrs.entity.BasDevpPosition pos = positionService.getOne(
                    new QueryWrapper<com.zy.asrs.entity.BasDevpPosition>().eq("dev_no", (int) staNo));
            if (pos != null && pos.getPlcPosition() != null) return pos.getPlcPosition();
        } catch (Exception ignore) {}
        return 0L;
    }

    /**
     * 获取指定RGV的RgvThread实例
     */
    private RgvThread getRgvThread(int rgvNo) {
        try {
            return (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvNo);
        } catch (Exception e) {
            return null;
        }
    }

    /******************************************************************************************/