Merge remote-tracking branch 'origin/jx-test' into jx-test

servo/src/main/java/com/galaxis/rcs/RCSService.java

@@ -3,7 +3,7 @@ package com.galaxis.rcs;
 import com.galaxis.rcs.common.entity.AddTaskRequest;
 import com.galaxis.rcs.common.entity.AddTaskResult;
 import com.galaxis.rcs.common.enums.LCCDirection;
-import com.galaxis.rcs.connector.cl2.Cl2Item;
+import com.galaxis.rcs.connector.amr.cl2.Cl2Item;
 import com.galaxis.rcs.plan.path.PathUtils;
 import com.google.common.base.Strings;
 import com.yvan.logisticsModel.LogisticsRuntime;

servo/src/main/java/com/galaxis/rcs/amr/AmrAgvItem.java

@@ -0,0 +1,884 @@
+package com.galaxis.rcs.amr;
+
+import com.galaxis.rcs.common.entity.RcsTaskPlan;
+import com.galaxis.rcs.common.enums.*;
+import com.galaxis.rcs.connector.amr.AmrDeviceConnector;
+import com.galaxis.rcs.plan.PlanTaskSequence;
+import com.galaxis.rcs.plan.path.PathUtils;
+import com.galaxis.rcs.amr.receiveEntity.AmrHeartbeatMessage;
+import com.galaxis.rcs.amr.receiveEntity.base.CurBatteryData;
+import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage;
+import com.galaxis.rcs.amr.sendEntity.RcsSRMessage;
+import com.galaxis.rcs.amr.sendEntity.RcsSetLocationMessage;
+import com.google.common.collect.Queues;
+import com.yvan.entity.AgvStatusVo;
+import com.yvan.logisticsModel.ExecutorItem;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import com.yvan.logisticsModel.StaticItem;
+import lombok.Getter;
+import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.clever.core.json.JsonWrapper;
+import org.clever.data.redis.Redis;
+import org.clever.data.redis.RedisAdmin;
+
+import java.util.*;
+import java.util.concurrent.BlockingQueue;
+import java.util.concurrent.locks.LockSupport;
+
+@Slf4j
+public abstract class AmrAgvItem  extends ExecutorItem {
+    private static final int BLOCKING_QUEUE_CAPACITY = 100;
+    private static final Redis redis = RedisAdmin.getRedis();
+
+    // ip
+    public volatile String ip;
+    // agv名称
+    public volatile String agvName;
+    // agv类型
+    public volatile String agvType;
+    // agv型号
+    public volatile String agvModel;
+    // AMR功能型号
+    public volatile String agvFnModel;
+    // 电池信息
+    public volatile CurBatteryData battery;
+    // agv当前x坐标
+    public volatile double x;
+    // agv当前y坐标
+    public volatile double y;
+    // agv当前z坐标
+    public volatile double z;
+    // 当前所在站点的逻辑X坐标 Int32
+    public volatile int logicX;
+    // 当前所在站点的逻辑Y坐标 Int32
+    public volatile int logicY;
+    // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public volatile short direction;
+    // agv当前转动角度值
+    public volatile double orientation;
+
+    private volatile boolean isPaused = false;
+
+    private volatile PosDirection lastPausedPosition;
+
+    // 任务模式
+    @Getter
+    private volatile AmrTaskMode __taskMode;
+
+
+    // 执行中的任务
+    public List<AmrDeviceTask> runningDeviceTaskList = new ArrayList<>();
+
+    /**
+     * 当前执行的任务规划列表
+     */
+    public volatile PlanTaskSequence planTaskSequence;
+
+    /**
+     * 当前执行的设备任务列表
+     */
+    final BlockingQueue<AmrDeviceTask> deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);
+
+    final AmrDeviceConnector amrDeviceConnector = new AmrDeviceConnector(this.runtime);
+
+    public final AmrMessageHandler amrMessageHandler;
+
+    /**
+     * 连接器线程
+     */
+    public final AmrConnectorThread connectorThread;
+
+    public AmrAgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
+        super(logisticsRuntime, raw);
+        this.connectorThread = new AmrConnectorThread(this, this.amrDeviceConnector, logisticsRuntime);
+        this.amrMessageHandler = logisticsRuntime.amrMessageHandler;
+    }
+
+    @Override
+    public boolean isRunning() {
+        return connectorThread.isRunning();
+    }
+
+    public abstract RcsConfigMessage getConfig();
+
+
+    @Override
+    public void start() {
+        this.amrMessageHandler.registeHeartBeatSet(this);
+
+        // 查询当前状态
+        requestCurrentStatus();
+        this.isRunning = true;
+
+        this.startConnector();
+    }
+
+    @Override
+    public void stop() {
+        // 停止连接器线程
+        stopConnector();
+
+        this.amrMessageHandler.unregisteHeartBeatSet(this);
+
+        // 清理任务序列
+        if (planTaskSequence != null) {
+            planTaskSequence = null;
+        }
+        // 清理设备任务队列
+        deviceTaskQueue.clear();
+        // 清理运行中的设备任务列表
+        runningDeviceTaskList.clear();
+        // 更新Redis状态
+        updateRedisStatus();
+    }
+
+    public synchronized void dispatchTask(PlanTaskSequence taskSequence) {
+        if (!isFree()) {
+            if (!this.runtime.isRunning()) {
+                throw new IllegalStateException("runtime is not running!");
+            }
+            if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) {
+                throw new IllegalStateException("AGV is busy with an existing task sequence");
+            }
+            if (!deviceTaskQueue.isEmpty()) {
+                throw new IllegalStateException("AGV has pending device tasks in the queue");
+            }
+            if (!isOnline) {
+                throw new IllegalStateException("AGV is not online and cannot accept new tasks");
+            }
+
+            throw new IllegalStateException("AGV is not free to accept new tasks");
+        }
+
+        this.planTaskSequence = taskSequence;
+        buildPlanToDeviceTask();
+        this.runtime.eventManager.firePlanTaskSequenceAcceptEvent(this, taskSequence);
+        connectorThread.resumeProcessing();
+    }
+
+    public synchronized void pauseTask() {
+        if (planTaskSequence == null) {
+            throw new IllegalStateException("No active task to pause");
+        }
+
+        isPaused = true;
+        lastPausedPosition = new PosDirection(logicX, logicY, PathUtils.getDirectionByArmDirection(direction));
+
+        // 发送停止指令
+        RcsSRMessage stopMsg = new RcsSRMessage(this.runtime);
+        stopMsg.SeqNo = amrMessageHandler.getNewSeqNo();
+        stopMsg.OperationCode = 0; // 停止
+        stopMsg.StopX = logicX;
+        stopMsg.StopY = logicY;
+
+        try {
+            amrMessageHandler.sendCmdSR(this.getId(), stopMsg);
+        } catch (Exception e) {
+            log.error("Failed to send stop command to AGV {}", this.getId(), e);
+        }
+
+        this.runtime.eventManager.firePlanTaskSequencePauseEvent(this, planTaskSequence);
+    }
+
+    public synchronized void resumeTask() {
+        if (!isPaused) {
+            throw new IllegalStateException("Task is not paused");
+        }
+
+        // 检查当前位置是否与暂停位置一致
+        if (Math.abs(logicX - lastPausedPosition.logicX()) > 1 ||
+            Math.abs(logicY - lastPausedPosition.logicY()) > 1 ||
+            PathUtils.getDirectionByArmDirection(direction) != lastPausedPosition.direction()) {
+            // 需要返回暂停位置
+            throw new RuntimeException("AGV position has changed since pause, cannot resume task safely");
+        }
+
+        isPaused = false;
+        connectorThread.resumeProcessing();
+        this.runtime.eventManager.firePlanTaskSequenceResumeEvent(this, planTaskSequence);
+    }
+
+    @SneakyThrows
+    public synchronized void cancelTask() {
+        // 发送取消指令
+        amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo());
+
+        if (planTaskSequence != null) {
+            planTaskSequence = null;
+        }
+        if (!deviceTaskQueue.isEmpty()) {
+            deviceTaskQueue.clear();
+        }
+        // todo 取消运行中的设备任务
+        runningDeviceTaskList.clear();
+        // 唤醒连接器线程
+        LockSupport.unpark(connectorThread);
+        this.runtime.eventManager.firePlanTaskSequenceCancelEvent(this, planTaskSequence);
+    }
+
+    @SneakyThrows
+    public void setPositionAndDirection(int x, int y, short direction) {
+        RcsSetLocationMessage setLoc = new RcsSetLocationMessage(amrMessageHandler.getNewSeqNo());
+        setLoc.X = (short) x;
+        setLoc.Y = (short) y;
+        setLoc.Direction = direction;
+
+        amrMessageHandler.sendCmdSetLocation(this.getId(), setLoc);
+    }
+
+    @SneakyThrows
+    public void requestCurrentStatus() {
+        amrMessageHandler.sendCmdQueryStatus(this.getId());
+    }
+
+    public boolean isFree() {
+        // return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning());
+        if (!this.runtime.isRunning()) {
+            return false;
+        }
+        if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) {
+            return false;
+        }
+        if (!deviceTaskQueue.isEmpty()) {
+            return false;
+        }
+        if (this.isPaused) {
+            return false;
+        }
+        // if (this.taskMode != AmrTaskMode.AMR_FREE_MODE) {
+        //     return false;
+        // }
+        return this.isOnline;
+    }
+
+    public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) {
+
+        updatePosition(logicX, logicY, direction);
+        // 查找当前分组任务
+        for (AmrDeviceTask task : runningDeviceTaskList) {
+            task.taskGroupStatus = taskStatus;
+            if (taskStatus == 4) {
+                if (task.taskStatus != 4) {
+                    this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task);
+                    task.taskStatus = 4;
+                }
+
+                // 更新计划任务
+                List<RcsTaskPlan> planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList();
+                for (RcsTaskPlan planTask : planTaskList) {
+                    if (PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) {
+                        continue;
+                    }
+
+                    planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
+                    this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask);
+                }
+            }
+        }
+
+        if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
+            this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence);
+            this.runningDeviceTaskList.clear();
+            planTaskSequence = null;
+        }
+        LockSupport.unpark(connectorThread);
+    }
+
+    public void updatePosition(int logicX, int logicY, short direction) {
+        int oldX = this.logicX;
+        int oldY = this.logicY;
+        short oldDirection = this.direction;
+
+        this.logicX = logicX;
+        this.logicY = logicY;
+        this.direction = direction;
+
+        LCCDirection oldLccDirection = PathUtils.getDirectionByArmDirection(oldDirection);
+        LCCDirection newLccDirection = PathUtils.getDirectionByArmDirection(direction);
+
+        // 更新Redis
+        updateRedisStatus();
+
+        // 触发位置变化事件
+        if (oldX != logicX || oldY != logicY) {
+            this.runtime.eventManager.firePosChangedEvent(this,
+                new PosDirection(logicX, logicY, newLccDirection),
+                new PosDirection(oldX, oldY, oldLccDirection));
+        }
+
+        if (oldDirection != direction) {
+            this.runtime.eventManager.fireDirectionChangedEvent(this, newLccDirection, oldLccDirection);
+        }
+
+        boolean needCompute = false;
+
+        // 从 runningDeviceTaskList 里面，找到完成到什么阶段
+        // 比如 (1,2) -> (2,2) -> (3,2) ,  如果 updatePosition=3,2 ，那么前2个任务都要完成
+        int finishTargetIndex = -1;
+        if (this.runningDeviceTaskList != null && !this.runningDeviceTaskList.isEmpty() &&
+            this.planTaskSequence != null && !this.planTaskSequence.isEmpty()) {
+
+            for (int i = 0; i < runningDeviceTaskList.size(); i++) {
+                AmrDeviceTask task = runningDeviceTaskList.get(i);
+                if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.direction == this.direction) {
+                    if (task.taskStatus < 4) {
+                        finishTargetIndex = i;
+                    }
+                    break;
+                }
+            }
+
+            if (finishTargetIndex >= 0) {
+                needCompute = true;
+                // 标记前面的任务都完成了
+                for (int i = 0; i <= finishTargetIndex; i++) {
+                    AmrDeviceTask task = runningDeviceTaskList.get(i);
+                    task.taskStatus = 4; // 标记为完成
+                    this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task);
+
+                    // 更新计划任务
+                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
+                    if (planTask != null && !PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) {
+                        planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
+                        this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask);
+                    }
+                }
+
+                if (planTaskSequence.isAllCompleted()) {
+                    this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence);
+                    this.runningDeviceTaskList.clear();
+                    planTaskSequence = null;
+                }
+            }
+        }
+
+        // BannerUtils.printConfig(log, "updatePosition", new String[]{
+        //     "logicX: " + logicX,
+        //     "logicY: " + logicY,
+        //     "direction: " + direction,
+        //     "finishTargetIndex: " + finishTargetIndex,
+        //     "runningDeviceSize:" + (this.runningDeviceTaskList == null ? "null" : this.runningDeviceTaskList.size()),
+        //     "planTask:" + (this.planTaskSequence == null ? "null" :
+        //         ("\n" + Joiner.on("\n").join((List) this.planTaskSequence.toPrettyMap().get("items")))
+        //     )
+        // });
+
+        if (needCompute && this.runningDeviceTaskList.size() > 0) {
+            int index = this.runningDeviceTaskList.size() - 1;
+            AmrDeviceTask task = this.runningDeviceTaskList.get(index);
+            if (task.groupEndPoint != task.endPoint) {
+                LockSupport.unpark(connectorThread);
+            }
+        }
+    }
+
+    /**
+     * 更新设备任务状态 暂时没有处理任务取消相关的状态
+     *
+     * @param seqNo
+     * @param x
+     * @param y
+     * @param messageStatus
+     */
+    public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) {
+        // 更新任务状态逻辑
+        if (messageStatus < 2) {
+            return;
+        }
+
+        // 任务完成逻辑，在地标检查里
+    }
+
+    public void updateTaskMode(int taskMode) {
+        this.setTaskMode(AmrTaskMode.fromValue(taskMode));
+    }
+
+    private void setTaskMode(AmrTaskMode taskMode) {
+        var originalMode = this.__taskMode;
+        this.__taskMode = taskMode;
+        this.runtime.eventManager.fireModeChangeEvent(this, taskMode, originalMode);
+        updateRedisStatus();
+    }
+
+    @SneakyThrows
+    public void updateRedisStatus() {
+        String statusKey = getRedisKey("status");
+        var state = this.getState();
+        var statusMap = new JsonWrapper(state).getInnerMap();
+        redis.hPutAll(statusKey, statusMap);
+    }
+
+    public void handleHeartbeat(AmrHeartbeatMessage heartbeat) {
+        // 更新在线状态
+        String aliveKey = getRedisKey("alive");
+        redis.vSet(aliveKey, this.runtime.serverId);
+        redis.kExpire(aliveKey, 5); // 5秒过期
+
+        // 更新状态信息
+        if (this.battery == null) {
+            this.battery = new CurBatteryData();
+        }
+        this.battery.SOC = heartbeat.Battery;
+        this.battery.setTemperature(heartbeat.Temperature.Battery);
+
+        // 检查低电量
+        if (this.battery.SOC < 20) {
+            this.runtime.eventManager.fireLowBatteryEvent(this);
+        }
+
+        updateRedisStatus();
+    }
+
+    public void handleOnlineEvent() {
+        isOnline = true;
+        this.runtime.eventManager.fireOnlineEvent(this);
+        requestCurrentStatus();
+    }
+
+    public void handleOfflineEvent() {
+        isOnline = false;
+        this.runtime.eventManager.fireOfflineEvent(this);
+    }
+
+    public String getTaskStatus() {
+        if (planTaskSequence == null) return "IDLE";
+        if (isPaused) return "PAUSED";
+        return "EXECUTING";
+    }
+
+
+    /**
+     * 启动连接器线程
+     */
+    public void startConnector() {
+        if (!connectorThread.isRunning()) {
+            connectorThread.start();
+        }
+    }
+
+    /**
+     * 停止连接器线程
+     */
+    public void stopConnector() {
+        connectorThread.stop();
+    }
+
+    private static final int speed = 1000;
+
+    /**
+     * 添加任务序列到当前执行器
+     */
+    protected void buildPlanToDeviceTask() {
+        PlanTaskSequence sequence = this.planTaskSequence;
+        LogisticsRuntime runtime = sequence.logisticsRuntime;
+
+        short direction = this.direction;
+
+        // 获取当前设备点位(逻辑点位)
+        StaticItem startPoint = runtime.getStaticItemByLogicXY(this.logicX, this.logicY);
+        if (startPoint == null) {
+            log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId());
+        }
+        StaticItem groupStartPoint = startPoint;
+
+        Set<Long> rotationPlanTaskIdSet = new HashSet<>();
+
+        // 生成移动报文
+        List<AmrDeviceTask> deviceTaskList = new ArrayList<>();
+        List<Map<String, Object>> linkStore = null;
+        // 检查 planList 是不是全都是我的任务
+        for (int i = 0; i < sequence.taskList.size(); i++) {
+            RcsTaskPlan plan = sequence.taskList.get(i);
+            String endPointId = plan.getTargetId();
+
+            if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) {
+                // 获取目标点信息
+                StaticItem pointItem = runtime.getStaticItemById(endPointId);
+                linkStore = (List<Map<String, Object>>) pointItem.dt.get("linkStore");
+                int d = -1;
+                if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) {
+                    d = pointItem.logicY >= startPoint.logicY ? AmrAgvItem.CDirection.db : AmrAgvItem.CDirection.dt;
+                    if ((d > direction && d - AmrAgvItem.CDirection.dl != direction) || (d < direction && d + AmrAgvItem.CDirection.dl != direction)) {
+                        throw new RuntimeException("方向错误");
+                    }
+
+                } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) {
+                    d = pointItem.logicX >= startPoint.logicX ? AmrAgvItem.CDirection.dr : AmrAgvItem.CDirection.dl;
+                    if ((d > direction && d - AmrAgvItem.CDirection.dl != direction) || (d < direction && d + AmrAgvItem.CDirection.dl != direction)) {
+                        throw new RuntimeException("方向错误");
+                    }
+                    // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
+
+                } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) {
+                    d = direction;
+                    // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
+
+                } else {
+                    throw new RuntimeException("无法识别的点位关系");
+                }
+                AmrDeviceTask deviceTask = new AmrDeviceTask();
+                deviceTask.x = pointItem.logicX;
+                deviceTask.y = pointItem.logicY;
+                deviceTask.speed = d == direction ? (speed) : (-speed);
+                deviceTask.direction = direction;
+                deviceTask.pickMode = 0;
+                deviceTask.startPoint = startPoint;
+                deviceTask.endPoint = pointItem;
+                deviceTask.bizTaskId = plan.getBizTaskId();
+                deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                rotationPlanTaskIdSet.clear();
+                // 行走任务完成后，检查用的字段
+                deviceTask.checkLogicX = pointItem.logicX;
+                deviceTask.checkLogicY = pointItem.logicY;
+                deviceTaskList.add(deviceTask);
+                // 设置新的起点
+                startPoint = pointItem;
+
+            } else if (plan.getPlanType().equals(PlanTaskType.ROTATION.toString())) {
+
+                float r = plan.getTargetRotation().floatValue();
+                while (r > 360) {
+                    r -= 360;
+                }
+                while (r < 0) {
+                    r += 360;
+                }
+
+                if (r >= 315 || r < 45) {
+                    direction = AmrAgvItem.CDirection.dr;
+
+                } else if (r >= 45 && r < 135) {
+                    direction = AmrAgvItem.CDirection.dt;
+
+                } else if (r >= 135 && r < 225) {
+                    direction = AmrAgvItem.CDirection.dl;
+
+                } else if (r >= 225 && r < 315) {
+                    direction = AmrAgvItem.CDirection.db;
+                }
+                rotationPlanTaskIdSet.add(plan.getPlanTaskId());
+
+            } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) {
+
+                if (deviceTaskList.isEmpty()) {
+                    AmrDeviceTask deviceTask = new AmrDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
+                }
+                AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
+                deviceTask.operationType = AmrAgvItem.COperationType.transplantLoadAndUnload;
+                deviceTask.pickMode = AmrAgvItem.CPickMode.load;
+                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
+                //处理取货高度
+                StaticItem storeItem = runtime.getStaticItemById(endPointId);
+                Map<String, Object> storeItemRaw = storeItem.dt;
+                if (storeItemRaw.containsKey("bays")) {
+                    List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
+                    Map<String, Object> bay = bays.get(plan.getTargetBay());
+                    List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
+                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
+                } else {
+                    deviceTask.goodsSlotHeight = 1;
+                }
+                if (linkStore != null) {
+                    for (Map<String, Object> store : linkStore) {
+                        if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) {
+                            short d = 0;
+                            switch (store.get("direction").toString()) {
+                                case "up":
+                                    d = 1;
+                                    break;
+                                case "right":
+                                    d = 2;
+                                    break;
+                                case "down":
+                                    d = 3;
+                                    break;
+                                case "left":
+                                    d = 0;
+                                    break;
+                            }
+                            deviceTask.goodsSlotDirection = d;
+                        }
+                    }
+                }
+
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
+
+            } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
+                if (deviceTaskList.isEmpty()) {
+                    AmrDeviceTask deviceTask = new AmrDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
+                }
+                AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
+                deviceTask.operationType = AmrAgvItem.COperationType.transplantLoadAndUnload;
+                deviceTask.pickMode = AmrAgvItem.CPickMode.unload;
+                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
+                // 处理卸货高度
+                StaticItem storeItem = runtime.getStaticItemById(endPointId);
+                Map<String, Object> storeItemRaw = storeItem.dt;
+                if (storeItemRaw.containsKey("bays")) {
+                    List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
+                    Map<String, Object> bay = bays.get(plan.getTargetBay());
+                    List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
+                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
+                } else {
+                    deviceTask.goodsSlotHeight = 1;
+                }
+                if (linkStore != null) {
+                    for (Map<String, Object> store : linkStore) {
+                        if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) {
+                            short d = 0;
+                            switch (store.get("direction").toString()) {
+                                case "up":
+                                    d = 3;
+                                    break;
+                                case "right":
+                                    d = 0;
+                                    break;
+                                case "down":
+                                    d = 1;
+                                    break;
+                                case "left":
+                                    d = 2;
+                                    break;
+                            }
+                            deviceTask.goodsSlotDirection = d;
+                        }
+                    }
+                }
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
+
+            } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
+                if (deviceTaskList.isEmpty()) {
+                    AmrDeviceTask deviceTask = new AmrDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                }
+                AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
+                deviceTask.operationType = AmrAgvItem.COperationType.charge;
+                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
+                // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
+                deviceTask.chargeDirection = 2;
+                deviceTask.chargeLocation = 200;
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
+            }
+
+            if (!plan.getExecutorId().equals(this.getId())) {
+                throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId());
+            }
+
+        }
+
+        if (deviceTaskList.isEmpty()) {
+            AmrDeviceTask deviceTask = new AmrDeviceTask();
+            deviceTask.x = startPoint.logicX;
+            deviceTask.y = startPoint.logicY;
+            deviceTask.speed = speed;
+            deviceTask.direction = direction;
+            deviceTask.pickMode = 0;
+            deviceTask.startPoint = startPoint;
+            deviceTask.endPoint = startPoint;
+            // 行走任务完成后，检查用的字段
+            deviceTask.checkLogicX = startPoint.logicX;
+            deviceTask.checkLogicY = startPoint.logicY;
+            deviceTaskList.add(deviceTask);
+        }
+
+        // 标记任务分组结束
+        AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
+        deviceTask.groupEndPoint = deviceTask.endPoint;
+        deviceTask.groupStartPoint = groupStartPoint;
+        deviceTask.isGroupEnd = true;
+        // 最后一个规划任务为旋转时需要添加一个endDirection
+        if (rotationPlanTaskIdSet.size() > 0) {
+            deviceTask.operationType = AmrAgvItem.COperationType.move;
+            deviceTask.pickMode = AmrAgvItem.CPickMode.normal;
+            deviceTask.endDirection = direction;
+            deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+            if (deviceTask.movePlanTaskId == null) {
+                deviceTask.movePlanTaskId = 0L;
+            }
+            rotationPlanTaskIdSet.clear();
+        }
+
+        // 反向标记任务组
+        int lastIndex = deviceTaskList.size() - 1;
+        for (int i = deviceTaskList.size() - 1; i >= 0; i--) {
+            AmrDeviceTask d = deviceTaskList.get(i);
+            if (d.isGroupEnd) {
+                lastIndex = i;
+            } else {
+                d.operationType = deviceTaskList.get(lastIndex).operationType;
+                d.pickMode = deviceTaskList.get(lastIndex).pickMode;
+                d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint;
+                d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint;
+                d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight;
+                d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection;
+                d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection;
+                d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation;
+            }
+        }
+
+        deviceTaskQueue.addAll(deviceTaskList);
+        String json = JsonWrapper.toJson(deviceTaskList);
+        log.info("deviceTaskList: {}", json);
+    }
+
+    public boolean isSamePosition(PosDirection startPos) {
+        return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() &&
+            PathUtils.getDirectionByArmDirection(this.direction) == startPos.direction();
+    }
+
+    private static class CDirection {
+        private static final short dr = 0;
+        private static final short db = 1;
+        private static final short dl = 2;
+        private static final short dt = 3;
+    }
+
+
+    private static class COperationType {
+        public static final short move = 0;
+        public static final short load = 1;
+        public static final short unpick = 2;
+        public static final short charge = 3;
+        public static final short transplantLoadAndUnload = 4;
+        public static final short rollerLoadAndUnload = 5;
+    }
+
+    private static class CPickMode {
+        public static final short normal = 0;
+        public static final short load = 1;
+        public static final short unload = 2;
+        public static final short adjustHeight = 3;
+        public static final short adjustHeightToLoad = 5;
+        public static final short adjustHeightToUnload = 6;
+    }
+
+
+    /**
+     * 从 AMR 方向转换为 LCC 方向枚举
+     *
+     * @return
+     */
+    public LCCDirection getLCCDirection() {
+        return switch (direction) {
+            case 0 -> LCCDirection.RIGHT;
+            case 1 -> LCCDirection.DOWN;
+            case 2 -> LCCDirection.LEFT;
+            case 3 -> LCCDirection.UP;
+            default -> null;
+        };
+    }
+
+    public short getAmrDirection(LCCDirection lccDirection) {
+        return switch (lccDirection) {
+            case RIGHT -> 0;
+            case DOWN -> 1;
+            case LEFT -> 2;
+            case UP -> 3;
+            default -> -1; // 未知方向
+        };
+    }
+
+    private String getRedisKey(String type) {
+        return String.format("lcc:%s:%s:device:%s:%s",
+            runtime.projectUuid, runtime.envId, this.getId(), type);
+    }
+
+    public AgvStatusVo getState() {
+        var ptr = this;
+
+        var state = new AgvStatusVo();
+        state.setId(ptr.id);
+        state.setType(ptr.getT());
+        state.setIsOnline(ptr.isOnline);
+        state.setIsSystemManaged(ptr.isSystemManaged);
+
+        state.setX(ptr.x);
+        state.setY(ptr.y);
+        state.setZ(ptr.z);
+        state.setLogicX(ptr.logicX);
+        state.setLogicY(ptr.logicY);
+
+        state.setDirection(PathUtils.getDirectionByArmDirection(ptr.direction));
+        state.setOrientation(ptr.orientation);
+        state.setSoc(ptr.battery == null ? -1 : ptr.battery.SOC);
+        state.setMode(ptr.get__taskMode());
+        state.setTaskStatus(ptr.getTaskStatus());
+
+        state.setIsBlocked(ptr.isBlocked);
+        if (ptr.planTaskSequence != null) {
+            state.setTaskCompleted(ptr.planTaskSequence.completedCount());
+            state.setTaskTotalCount(ptr.planTaskSequence.taskTotalCount());
+            state.setBizTaskId(ptr.planTaskSequence.bizTask.getBizTaskId());
+            state.setBizTaskType(BizTaskType.fromString(ptr.planTaskSequence.bizTask.getBizType()));
+            state.setBizTaskStatus(BizTaskStatus.fromString(ptr.planTaskSequence.bizTask.getBizTaskStatus()));
+            state.setBizTaskFrom(ptr.planTaskSequence.bizTask.getTaskFrom());
+            state.setBizTaskTo(ptr.planTaskSequence.bizTask.getTaskTo());
+            state.setBizLpn(ptr.planTaskSequence.bizTask.getLpn());
+
+            state.setLoadBasLocationVo(ptr.planTaskSequence.loadBasLocationVo);
+            state.setUnloadBasLocationVo(ptr.planTaskSequence.unloadBasLocationVo);
+        }
+
+        return state;
+    }
+}

servo/src/main/java/com/galaxis/rcs/amr/PtrAgvConnectorThread.java → servo/src/main/java/com/galaxis/rcs/amr/AmrConnectorThread.java

@@ -1,8 +1,8 @@
 package com.galaxis.rcs.amr;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
-import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
 import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage;
+import com.galaxis.rcs.connector.amr.AmrDeviceConnector;
 import com.yvan.logisticsModel.LogisticsRuntime;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.mqttv5.common.MqttException;
@@ -13,21 +13,21 @@ import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.concurrent.locks.LockSupport;
 
 @Slf4j
-public class PtrAgvConnectorThread extends Thread {
+public class AmrConnectorThread extends Thread {
     private final AtomicBoolean running = new AtomicBoolean(false);
     private final AtomicBoolean paused = new AtomicBoolean(false);
     private final Object pauseLock = new Object();
 
-    private final Cl2DeviceConnector cl2DeviceConnector;
-    private final PtrAgvItem ptrAgvItem;
+    public final AmrDeviceConnector amrDeviceConnector;
+    private final AmrAgvItem ptrAgvItem;
     private final LogisticsRuntime logisticsRuntime;
     private volatile int __currentTaskSeqNo = 0;
-    volatile PtrAgvDeviceTask __currentTask;
+    volatile AmrDeviceTask __currentTask;
 
-    public PtrAgvConnectorThread(PtrAgvItem ptrAgvItem, Cl2DeviceConnector cl2DeviceConnector, LogisticsRuntime logisticsRuntime) {
-        super("ExecutorConnector-" + ptrAgvItem.getId());
-        this.cl2DeviceConnector = cl2DeviceConnector;
-        this.ptrAgvItem = ptrAgvItem;
+    public AmrConnectorThread(AmrAgvItem amrAgvItem, AmrDeviceConnector amrDeviceConnector, LogisticsRuntime logisticsRuntime) {
+        super("ExecutorConnector-" + amrAgvItem.getId());
+        this.amrDeviceConnector = amrDeviceConnector;
+        this.ptrAgvItem = amrAgvItem;
         this.logisticsRuntime = logisticsRuntime;
     }
 
@@ -40,11 +40,11 @@ public class PtrAgvConnectorThread extends Thread {
             float distance = 0;
             int taskCount = 0;
 
-            PtrAgvDeviceTask startTask = null;
+            AmrDeviceTask startTask = null;
             RcsTaskMessage taskMessage = null;
 
             // 计算中的任务
-            List<PtrAgvDeviceTask> computingTaskList = new ArrayList<>();
+            List<AmrDeviceTask> computingTaskList = new ArrayList<>();
             while (running.get()) {
                 if (paused.get()) {
                     synchronized (pauseLock) {
@@ -54,7 +54,7 @@ public class PtrAgvConnectorThread extends Thread {
                     }
                 }
 
-                PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take();
+                AmrDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take();
                 if (startTask == null) {
                     startTask = currentTask;
                     taskMessage = new RcsTaskMessage(this.logisticsRuntime);
@@ -73,7 +73,7 @@ public class PtrAgvConnectorThread extends Thread {
                 computingTaskList.add(currentTask);
 
 
-                PtrAgvDeviceTask nextTask = this.ptrAgvItem.deviceTaskQueue.peek();
+                AmrDeviceTask nextTask = this.ptrAgvItem.deviceTaskQueue.peek();
                 if (currentTask.isGroupEnd
                     || (taskMessage.Link.size() > 0 && nextTask != null && ((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction)) // 下一个任务和开始任务方向不一致
                     // 单向移动距离大于2m时并且点位数量大于1 如果碰到当前点位与任务结束点位相同且当前任务还有后续，则继续添加步骤（link）错开，避免任务发生歧义
@@ -92,9 +92,9 @@ public class PtrAgvConnectorThread extends Thread {
                         // 发送任务
                         this.__currentTaskSeqNo = taskMessage.SeqNo;
                         this.__currentTask = currentTask;
-                        cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage);
+                        amrDeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage);
                         this.ptrAgvItem.runningDeviceTaskList.addAll(computingTaskList);
-                        for (PtrAgvDeviceTask task : computingTaskList) {
+                        for (AmrDeviceTask task : computingTaskList) {
                             task.taskStatus = 1;
                             task.taskGroupStatus = 1;
                         }
@@ -111,7 +111,7 @@ public class PtrAgvConnectorThread extends Thread {
                         // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒
                         LockSupport.park(); // 阻塞当前线程
                     } else {
-                        for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
+                        for (AmrDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
                             if (task.taskGroupStatus < 4) {
                                 LockSupport.park(); // 阻塞当前线程
                                 break;

servo/src/main/java/com/galaxis/rcs/amr/PtrAgvDeviceTask.java → servo/src/main/java/com/galaxis/rcs/amr/AmrDeviceTask.java

@@ -5,7 +5,7 @@ import com.yvan.logisticsModel.StaticItem;
 import java.util.HashSet;
 import java.util.Set;
 
-public class PtrAgvDeviceTask {
+public class AmrDeviceTask {
     //该段目标点X坐标	UInt16 逻辑单位，乘以一定系数才是物理距离
     public int x;
     //该段目标点Y坐标	UInt16 逻辑单位，乘以一定系数才是物理距离

servo/src/main/java/com/galaxis/rcs/amr/AmrMessageHandler.java

@@ -517,14 +517,14 @@ public class AmrMessageHandler {
     /**
      * 注册心跳单元
      */
-    public void registeHeartBeatSet(PtrAgvItem ptrAgvItem) {
+    public void registeHeartBeatSet(AmrAgvItem ptrAgvItem) {
         this.heartBeatSet.add(ptrAgvItem.getId());
     }
 
     /**
      * 注销心跳单元
      */
-    public void unregisteHeartBeatSet(PtrAgvItem ptrAgvItem) {
+    public void unregisteHeartBeatSet(AmrAgvItem ptrAgvItem) {
         this.heartBeatSet.remove(ptrAgvItem.getId());
     }
 

servo/src/main/java/com/galaxis/rcs/amr/FM600AgvItem.java

@@ -0,0 +1,18 @@
+package com.galaxis.rcs.amr;
+
+import com.yvan.logisticsModel.LogisticsRuntime;
+import lombok.extern.slf4j.Slf4j;
+
+import java.util.*;
+
+/**
+ * 侧叉式AGV执行器
+ */
+@Slf4j
+public abstract class FM600AgvItem extends AmrAgvItem {
+
+    public FM600AgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
+        super(logisticsRuntime, raw);
+    }
+
+}

servo/src/main/java/com/galaxis/rcs/amr/PtrAgvConnector.java

@@ -1,5 +0,0 @@
-package com.galaxis.rcs.amr;
-
-public abstract class PtrAgvConnector {
-
-}

servo/src/main/java/com/galaxis/rcs/amr/PtrAgvItem.java

@@ -1,889 +1,30 @@
 package com.galaxis.rcs.amr;
 
-import com.galaxis.rcs.common.entity.RcsTaskPlan;
-import com.galaxis.rcs.common.enums.*;
-import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
-import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.plan.path.PathUtils;
-import com.galaxis.rcs.amr.receiveEntity.AmrHeartbeatMessage;
-import com.galaxis.rcs.amr.receiveEntity.base.CurBatteryData;
 import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage;
-import com.galaxis.rcs.amr.sendEntity.RcsSRMessage;
-import com.galaxis.rcs.amr.sendEntity.RcsSetLocationMessage;
-import com.google.common.collect.Queues;
-import com.yvan.entity.AgvStatusVo;
-import com.yvan.entity.BasLocationVo;
-import com.yvan.logisticsModel.ExecutorItem;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.StaticItem;
-import lombok.Getter;
-import lombok.SneakyThrows;
 import lombok.extern.slf4j.Slf4j;
-import org.clever.core.Conv;
-import org.clever.core.json.JsonWrapper;
-import org.clever.data.redis.Redis;
-import org.clever.data.redis.RedisAdmin;
 
 import java.util.*;
-import java.util.concurrent.BlockingQueue;
-import java.util.concurrent.locks.LockSupport;
 
 /**
  * 侧叉式AGV执行器
  */
 @Slf4j
-public abstract class PtrAgvItem extends ExecutorItem {
-    private static final int BLOCKING_QUEUE_CAPACITY = 100;
-    private static final Redis redis = RedisAdmin.getRedis();
-
-    // ip
-    public volatile String ip;
-    // agv名称
-    public volatile String agvName;
-    // agv类型
-    public volatile String agvType;
-    // agv型号
-    public volatile String agvModel;
-    // AMR功能型号
-    public volatile String agvFnModel;
-    // 电池信息
-    public volatile CurBatteryData battery;
-    // agv当前x坐标
-    public volatile double x;
-    // agv当前y坐标
-    public volatile double y;
-    // agv当前z坐标
-    public volatile double z;
-    // 当前所在站点的逻辑X坐标 Int32
-    public volatile int logicX;
-    // 当前所在站点的逻辑Y坐标 Int32
-    public volatile int logicY;
-    // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-    public volatile short direction;
-    // agv当前转动角度值
-    public volatile double orientation;
-
-    private volatile boolean isPaused = false;
-
-    private volatile PosDirection lastPausedPosition;
-
-    // 任务模式
-    @Getter
-    private volatile AmrTaskMode __taskMode;
-
-
-    // 执行中的任务
-    public List<PtrAgvDeviceTask> runningDeviceTaskList = new ArrayList<>();
-
-    /**
-     * 当前执行的任务规划列表
-     */
-    public volatile PlanTaskSequence planTaskSequence;
-
-    /**
-     * 当前执行的设备任务列表
-     */
-    final BlockingQueue<PtrAgvDeviceTask> deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);
-
-    final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(this.runtime);
-
-    public final AmrMessageHandler amrMessageHandler;
-
-    /**
-     * 连接器线程
-     */
-    private final PtrAgvConnectorThread connectorThread;
+public class PtrAgvItem extends AmrAgvItem {
 
     public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
         super(logisticsRuntime, raw);
-        this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime);
-        this.amrMessageHandler = logisticsRuntime.amrMessageHandler;
     }
 
     @Override
-    public boolean isRunning() {
-        return connectorThread.isRunning();
-    }
-
-    public abstract RcsConfigMessage getConfig();
-
-
-    @Override
-    public void start() {
-        this.amrMessageHandler.registeHeartBeatSet(this);
-
-        // 查询当前状态
-        requestCurrentStatus();
-        this.isRunning = true;
-
-        this.startConnector();
-    }
-
-    @Override
-    public void stop() {
-        // 停止连接器线程
-        stopConnector();
-
-        this.amrMessageHandler.unregisteHeartBeatSet(this);
-
-        // 清理任务序列
-        if (planTaskSequence != null) {
-            planTaskSequence = null;
-        }
-        // 清理设备任务队列
-        deviceTaskQueue.clear();
-        // 清理运行中的设备任务列表
-        runningDeviceTaskList.clear();
-        // 更新Redis状态
-        updateRedisStatus();
-    }
-
-    public synchronized void dispatchTask(PlanTaskSequence taskSequence) {
-        if (!isFree()) {
-            if (!this.runtime.isRunning()) {
-                throw new IllegalStateException("runtime is not running!");
-            }
-            if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) {
-                throw new IllegalStateException("AGV is busy with an existing task sequence");
-            }
-            if (!deviceTaskQueue.isEmpty()) {
-                throw new IllegalStateException("AGV has pending device tasks in the queue");
-            }
-            if (!isOnline) {
-                throw new IllegalStateException("AGV is not online and cannot accept new tasks");
-            }
-
-            throw new IllegalStateException("AGV is not free to accept new tasks");
-        }
-
-        this.planTaskSequence = taskSequence;
-        buildPlanToDeviceTask();
-        this.runtime.eventManager.firePlanTaskSequenceAcceptEvent(this, taskSequence);
-        connectorThread.resumeProcessing();
-    }
-
-    public synchronized void pauseTask() {
-        if (planTaskSequence == null) {
-            throw new IllegalStateException("No active task to pause");
-        }
-
-        isPaused = true;
-        lastPausedPosition = new PosDirection(logicX, logicY, PathUtils.getDirectionByArmDirection(direction));
-
-        // 发送停止指令
-        RcsSRMessage stopMsg = new RcsSRMessage(this.runtime);
-        stopMsg.SeqNo = amrMessageHandler.getNewSeqNo();
-        stopMsg.OperationCode = 0; // 停止
-        stopMsg.StopX = logicX;
-        stopMsg.StopY = logicY;
-
-        try {
-            amrMessageHandler.sendCmdSR(this.getId(), stopMsg);
-        } catch (Exception e) {
-            log.error("Failed to send stop command to AGV {}", this.getId(), e);
-        }
-
-        this.runtime.eventManager.firePlanTaskSequencePauseEvent(this, planTaskSequence);
-    }
-
-    public synchronized void resumeTask() {
-        if (!isPaused) {
-            throw new IllegalStateException("Task is not paused");
-        }
-
-        // 检查当前位置是否与暂停位置一致
-        if (Math.abs(logicX - lastPausedPosition.logicX()) > 1 ||
-            Math.abs(logicY - lastPausedPosition.logicY()) > 1 ||
-            PathUtils.getDirectionByArmDirection(direction) != lastPausedPosition.direction()) {
-            // 需要返回暂停位置
-            throw new RuntimeException("AGV position has changed since pause, cannot resume task safely");
-        }
-
-        isPaused = false;
-        connectorThread.resumeProcessing();
-        this.runtime.eventManager.firePlanTaskSequenceResumeEvent(this, planTaskSequence);
-    }
-
-    @SneakyThrows
-    public synchronized void cancelTask() {
-        // 发送取消指令
-        amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo());
-
-        if (planTaskSequence != null) {
-            planTaskSequence = null;
-        }
-        if (!deviceTaskQueue.isEmpty()) {
-            deviceTaskQueue.clear();
-        }
-        // todo 取消运行中的设备任务
-        runningDeviceTaskList.clear();
-        // 唤醒连接器线程
-        LockSupport.unpark(connectorThread);
-        this.runtime.eventManager.firePlanTaskSequenceCancelEvent(this, planTaskSequence);
-    }
-
-    @SneakyThrows
-    public void setPositionAndDirection(int x, int y, short direction) {
-        RcsSetLocationMessage setLoc = new RcsSetLocationMessage(amrMessageHandler.getNewSeqNo());
-        setLoc.X = (short) x;
-        setLoc.Y = (short) y;
-        setLoc.Direction = direction;
-
-        amrMessageHandler.sendCmdSetLocation(this.getId(), setLoc);
-    }
-
-    @SneakyThrows
-    public void requestCurrentStatus() {
-        amrMessageHandler.sendCmdQueryStatus(this.getId());
-    }
-
-    public boolean isFree() {
-        // return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning());
-        if (!this.runtime.isRunning()) {
-            return false;
-        }
-        if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) {
-            return false;
-        }
-        if (!deviceTaskQueue.isEmpty()) {
-            return false;
-        }
-        if (this.isPaused) {
-            return false;
-        }
-        // if (this.taskMode != AmrTaskMode.AMR_FREE_MODE) {
-        //     return false;
-        // }
-        return this.isOnline;
-    }
-
-    public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) {
-
-        updatePosition(logicX, logicY, direction);
-        // 查找当前分组任务
-        for (PtrAgvDeviceTask task : runningDeviceTaskList) {
-            task.taskGroupStatus = taskStatus;
-            if (taskStatus == 4) {
-                if (task.taskStatus != 4) {
-                    this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task);
-                    task.taskStatus = 4;
-                }
-
-                // 更新计划任务
-                List<RcsTaskPlan> planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList();
-                for (RcsTaskPlan planTask : planTaskList) {
-                    if (PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) {
-                        continue;
-                    }
-
-                    planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
-                    this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask);
-                }
-            }
-        }
-
-        if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
-            this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence);
-            this.runningDeviceTaskList.clear();
-            planTaskSequence = null;
-        }
-        LockSupport.unpark(connectorThread);
-    }
-
-    public void updatePosition(int logicX, int logicY, short direction) {
-        int oldX = this.logicX;
-        int oldY = this.logicY;
-        short oldDirection = this.direction;
-
-        this.logicX = logicX;
-        this.logicY = logicY;
-        this.direction = direction;
-
-        LCCDirection oldLccDirection = PathUtils.getDirectionByArmDirection(oldDirection);
-        LCCDirection newLccDirection = PathUtils.getDirectionByArmDirection(direction);
-
-        // 更新Redis
-        updateRedisStatus();
-
-        // 触发位置变化事件
-        if (oldX != logicX || oldY != logicY) {
-            this.runtime.eventManager.firePosChangedEvent(this,
-                new PosDirection(logicX, logicY, newLccDirection),
-                new PosDirection(oldX, oldY, oldLccDirection));
-        }
-
-        if (oldDirection != direction) {
-            this.runtime.eventManager.fireDirectionChangedEvent(this, newLccDirection, oldLccDirection);
-        }
-
-        boolean needCompute = false;
-
-        // 从 runningDeviceTaskList 里面，找到完成到什么阶段
-        // 比如 (1,2) -> (2,2) -> (3,2) ,  如果 updatePosition=3,2 ，那么前2个任务都要完成
-        int finishTargetIndex = -1;
-        if (this.runningDeviceTaskList != null && !this.runningDeviceTaskList.isEmpty() &&
-            this.planTaskSequence != null && !this.planTaskSequence.isEmpty()) {
-
-            for (int i = 0; i < runningDeviceTaskList.size(); i++) {
-                PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
-                if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.direction == this.direction) {
-                    if (task.taskStatus < 4) {
-                        finishTargetIndex = i;
-                    }
-                    break;
-                }
-            }
-
-            if (finishTargetIndex >= 0) {
-                needCompute = true;
-                // 标记前面的任务都完成了
-                for (int i = 0; i <= finishTargetIndex; i++) {
-                    PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
-                    task.taskStatus = 4; // 标记为完成
-                    this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task);
-
-                    // 更新计划任务
-                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
-                    if (planTask != null && !PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) {
-                        planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
-                        this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask);
-                    }
-                }
-
-                if (planTaskSequence.isAllCompleted()) {
-                    this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence);
-                    this.runningDeviceTaskList.clear();
-                    planTaskSequence = null;
-                }
-            }
-        }
-
-        // BannerUtils.printConfig(log, "updatePosition", new String[]{
-        //     "logicX: " + logicX,
-        //     "logicY: " + logicY,
-        //     "direction: " + direction,
-        //     "finishTargetIndex: " + finishTargetIndex,
-        //     "runningDeviceSize:" + (this.runningDeviceTaskList == null ? "null" : this.runningDeviceTaskList.size()),
-        //     "planTask:" + (this.planTaskSequence == null ? "null" :
-        //         ("\n" + Joiner.on("\n").join((List) this.planTaskSequence.toPrettyMap().get("items")))
-        //     )
-        // });
-
-        if (needCompute && this.runningDeviceTaskList.size() > 0) {
-            int index = this.runningDeviceTaskList.size() - 1;
-            PtrAgvDeviceTask task = this.runningDeviceTaskList.get(index);
-            if (task.groupEndPoint != task.endPoint) {
-                LockSupport.unpark(connectorThread);
-            }
-        }
-    }
-
-    /**
-     * 更新设备任务状态 暂时没有处理任务取消相关的状态
-     *
-     * @param seqNo
-     * @param x
-     * @param y
-     * @param messageStatus
-     */
-    public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) {
-        // 更新任务状态逻辑
-        if (messageStatus < 2) {
-            return;
-        }
-
-        // 任务完成逻辑，在地标检查里
-    }
-
-    public void updateTaskMode(int taskMode) {
-        this.setTaskMode(AmrTaskMode.fromValue(taskMode));
-    }
-
-    private void setTaskMode(AmrTaskMode taskMode) {
-        var originalMode = this.__taskMode;
-        this.__taskMode = taskMode;
-        this.runtime.eventManager.fireModeChangeEvent(this, taskMode, originalMode);
-        updateRedisStatus();
-    }
-
-    @SneakyThrows
-    public void updateRedisStatus() {
-        String statusKey = getRedisKey("status");
-        var state = this.getState();
-        var statusMap = new JsonWrapper(state).getInnerMap();
-        redis.hPutAll(statusKey, statusMap);
-    }
-
-    public void handleHeartbeat(AmrHeartbeatMessage heartbeat) {
-        // 更新在线状态
-        String aliveKey = getRedisKey("alive");
-        redis.vSet(aliveKey, this.runtime.serverId);
-        redis.kExpire(aliveKey, 5); // 5秒过期
-
-        // 更新状态信息
-        if (this.battery == null) {
-            this.battery = new CurBatteryData();
-        }
-        this.battery.SOC = heartbeat.Battery;
-        this.battery.setTemperature(heartbeat.Temperature.Battery);
-
-        // 检查低电量
-        if (this.battery.SOC < 20) {
-            this.runtime.eventManager.fireLowBatteryEvent(this);
-        }
-
-        updateRedisStatus();
-    }
-
-    public void handleOnlineEvent() {
-        isOnline = true;
-        this.runtime.eventManager.fireOnlineEvent(this);
-        requestCurrentStatus();
-    }
-
-    public void handleOfflineEvent() {
-        isOnline = false;
-        this.runtime.eventManager.fireOfflineEvent(this);
-    }
-
-    public String getTaskStatus() {
-        if (planTaskSequence == null) return "IDLE";
-        if (isPaused) return "PAUSED";
-        return "EXECUTING";
-    }
-
-
-    /**
-     * 启动连接器线程
-     */
-    public void startConnector() {
-        if (!connectorThread.isRunning()) {
-            connectorThread.start();
-        }
-    }
-
-    /**
-     * 停止连接器线程
-     */
-    public void stopConnector() {
-        connectorThread.stop();
-    }
-
-    private static final int speed = 1000;
-
-    /**
-     * 添加任务序列到当前执行器
-     */
-    protected void buildPlanToDeviceTask() {
-        PlanTaskSequence sequence = this.planTaskSequence;
-        LogisticsRuntime runtime = sequence.logisticsRuntime;
-
-        short direction = this.direction;
-
-        // 获取当前设备点位(逻辑点位)
-        StaticItem startPoint = runtime.getStaticItemByLogicXY(this.logicX, this.logicY);
-        if (startPoint == null) {
-            log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId());
-        }
-        StaticItem groupStartPoint = startPoint;
-
-        Set<Long> rotationPlanTaskIdSet = new HashSet<>();
-
-        // 生成移动报文
-        List<PtrAgvDeviceTask> deviceTaskList = new ArrayList<>();
-        List<Map<String, Object>> linkStore = null;
-        // 检查 planList 是不是全都是我的任务
-        for (int i = 0; i < sequence.taskList.size(); i++) {
-            RcsTaskPlan plan = sequence.taskList.get(i);
-            String endPointId = plan.getTargetId();
-
-            if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) {
-                // 获取目标点信息
-                StaticItem pointItem = runtime.getStaticItemById(endPointId);
-                linkStore = (List<Map<String, Object>>) pointItem.dt.get("linkStore");
-                int d = -1;
-                if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) {
-                    d = pointItem.logicY >= startPoint.logicY ? CDirection.db : CDirection.dt;
-                    if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) {
-                        throw new RuntimeException("方向错误");
-                    }
-
-                } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) {
-                    d = pointItem.logicX >= startPoint.logicX ? CDirection.dr : CDirection.dl;
-                    if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) {
-                        throw new RuntimeException("方向错误");
-                    }
-                    // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
-
-                } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) {
-                    d = direction;
-                    // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
-
-                } else {
-                    throw new RuntimeException("无法识别的点位关系");
-                }
-                PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
-                deviceTask.x = pointItem.logicX;
-                deviceTask.y = pointItem.logicY;
-                deviceTask.speed = d == direction ? (speed) : (-speed);
-                deviceTask.direction = direction;
-                deviceTask.pickMode = 0;
-                deviceTask.startPoint = startPoint;
-                deviceTask.endPoint = pointItem;
-                deviceTask.bizTaskId = plan.getBizTaskId();
-                deviceTask.movePlanTaskId = plan.getPlanTaskId();
-                deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
-                rotationPlanTaskIdSet.clear();
-                // 行走任务完成后，检查用的字段
-                deviceTask.checkLogicX = pointItem.logicX;
-                deviceTask.checkLogicY = pointItem.logicY;
-                deviceTaskList.add(deviceTask);
-                // 设置新的起点
-                startPoint = pointItem;
-
-            } else if (plan.getPlanType().equals(PlanTaskType.ROTATION.toString())) {
-
-                float r = plan.getTargetRotation().floatValue();
-                while (r > 360) {
-                    r -= 360;
-                }
-                while (r < 0) {
-                    r += 360;
-                }
-
-                if (r >= 315 || r < 45) {
-                    direction = CDirection.dr;
-
-                } else if (r >= 45 && r < 135) {
-                    direction = CDirection.dt;
-
-                } else if (r >= 135 && r < 225) {
-                    direction = CDirection.dl;
-
-                } else if (r >= 225 && r < 315) {
-                    direction = CDirection.db;
-                }
-                rotationPlanTaskIdSet.add(plan.getPlanTaskId());
-
-            } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) {
-
-                if (deviceTaskList.isEmpty()) {
-                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
-                    deviceTask.x = startPoint.logicX;
-                    deviceTask.y = startPoint.logicY;
-                    deviceTask.speed = speed;
-                    deviceTask.direction = direction;
-                    deviceTask.pickMode = 0;
-                    deviceTask.startPoint = startPoint;
-                    deviceTask.endPoint = startPoint;
-                    deviceTask.bizTaskId = plan.getBizTaskId();
-                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
-                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
-                    rotationPlanTaskIdSet.clear();
-                    // 行走任务完成后，检查用的字段
-                    deviceTask.checkLogicX = startPoint.logicX;
-                    deviceTask.checkLogicY = startPoint.logicY;
-                    deviceTaskList.add(deviceTask);
-                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
-                }
-                PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
-                deviceTask.operationType = COperationType.transplantLoadAndUnload;
-                deviceTask.pickMode = CPickMode.load;
-                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
-                //处理取货高度
-                StaticItem storeItem = runtime.getStaticItemById(endPointId);
-                Map<String, Object> storeItemRaw = storeItem.dt;
-                if (storeItemRaw.containsKey("bays")) {
-                    List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
-                    Map<String, Object> bay = bays.get(plan.getTargetBay());
-                    List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
-                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
-                } else {
-                    deviceTask.goodsSlotHeight = 1;
-                }
-                if (linkStore != null) {
-                    for (Map<String, Object> store : linkStore) {
-                        if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) {
-                            short d = 0;
-                            switch (store.get("direction").toString()) {
-                                case "up":
-                                    d = 1;
-                                    break;
-                                case "right":
-                                    d = 2;
-                                    break;
-                                case "down":
-                                    d = 3;
-                                    break;
-                                case "left":
-                                    d = 0;
-                                    break;
-                            }
-                            deviceTask.goodsSlotDirection = d;
-                        }
-                    }
-                }
-
-                // 标记任务分组结束
-                deviceTask.isGroupEnd = true;
-                deviceTask.groupEndPoint = deviceTask.endPoint;
-                deviceTask.groupStartPoint = groupStartPoint;
-                groupStartPoint = deviceTask.endPoint;
-
-            } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
-                if (deviceTaskList.isEmpty()) {
-                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
-                    deviceTask.x = startPoint.logicX;
-                    deviceTask.y = startPoint.logicY;
-                    deviceTask.speed = speed;
-                    deviceTask.direction = direction;
-                    deviceTask.pickMode = 0;
-                    deviceTask.startPoint = startPoint;
-                    deviceTask.endPoint = startPoint;
-                    deviceTask.bizTaskId = plan.getBizTaskId();
-                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
-                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
-                    rotationPlanTaskIdSet.clear();
-                    // 行走任务完成后，检查用的字段
-                    deviceTask.checkLogicX = startPoint.logicX;
-                    deviceTask.checkLogicY = startPoint.logicY;
-                    deviceTaskList.add(deviceTask);
-                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
-                }
-                PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
-                deviceTask.operationType = COperationType.transplantLoadAndUnload;
-                deviceTask.pickMode = CPickMode.unload;
-                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
-                // 处理卸货高度
-                StaticItem storeItem = runtime.getStaticItemById(endPointId);
-                Map<String, Object> storeItemRaw = storeItem.dt;
-                if (storeItemRaw.containsKey("bays")) {
-                    List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
-                    Map<String, Object> bay = bays.get(plan.getTargetBay());
-                    List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
-                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
-                } else {
-                    deviceTask.goodsSlotHeight = 1;
-                }
-                if (linkStore != null) {
-                    for (Map<String, Object> store : linkStore) {
-                        if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) {
-                            short d = 0;
-                            switch (store.get("direction").toString()) {
-                                case "up":
-                                    d = 3;
-                                    break;
-                                case "right":
-                                    d = 0;
-                                    break;
-                                case "down":
-                                    d = 1;
-                                    break;
-                                case "left":
-                                    d = 2;
-                                    break;
-                            }
-                            deviceTask.goodsSlotDirection = d;
-                        }
-                    }
-                }
-                // 标记任务分组结束
-                deviceTask.isGroupEnd = true;
-                deviceTask.groupEndPoint = deviceTask.endPoint;
-                deviceTask.groupStartPoint = groupStartPoint;
-                groupStartPoint = deviceTask.endPoint;
-
-            } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
-                if (deviceTaskList.isEmpty()) {
-                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
-                    deviceTask.x = startPoint.logicX;
-                    deviceTask.y = startPoint.logicY;
-                    deviceTask.speed = speed;
-                    deviceTask.direction = direction;
-                    deviceTask.pickMode = 0;
-                    deviceTask.startPoint = startPoint;
-                    deviceTask.endPoint = startPoint;
-                    deviceTask.bizTaskId = plan.getBizTaskId();
-                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
-                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
-                    rotationPlanTaskIdSet.clear();
-                    // 行走任务完成后，检查用的字段
-                    deviceTask.checkLogicX = startPoint.logicX;
-                    deviceTask.checkLogicY = startPoint.logicY;
-                    deviceTaskList.add(deviceTask);
-                }
-                PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
-                deviceTask.operationType = COperationType.charge;
-                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
-                // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
-                deviceTask.chargeDirection = 2;
-                deviceTask.chargeLocation = 200;
-                // 标记任务分组结束
-                deviceTask.isGroupEnd = true;
-                deviceTask.groupEndPoint = deviceTask.endPoint;
-                deviceTask.groupStartPoint = groupStartPoint;
-                groupStartPoint = deviceTask.endPoint;
-            }
-
-            if (!plan.getExecutorId().equals(this.getId())) {
-                throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId());
-            }
-
-        }
-
-        if (deviceTaskList.isEmpty()) {
-            PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
-            deviceTask.x = startPoint.logicX;
-            deviceTask.y = startPoint.logicY;
-            deviceTask.speed = speed;
-            deviceTask.direction = direction;
-            deviceTask.pickMode = 0;
-            deviceTask.startPoint = startPoint;
-            deviceTask.endPoint = startPoint;
-            // 行走任务完成后，检查用的字段
-            deviceTask.checkLogicX = startPoint.logicX;
-            deviceTask.checkLogicY = startPoint.logicY;
-            deviceTaskList.add(deviceTask);
-        }
-
-        // 标记任务分组结束
-        PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
-        deviceTask.groupEndPoint = deviceTask.endPoint;
-        deviceTask.groupStartPoint = groupStartPoint;
-        deviceTask.isGroupEnd = true;
-        // 最后一个规划任务为旋转时需要添加一个endDirection
-        if (rotationPlanTaskIdSet.size() > 0) {
-            deviceTask.operationType = COperationType.move;
-            deviceTask.pickMode = CPickMode.normal;
-            deviceTask.endDirection = direction;
-            deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
-            if (deviceTask.movePlanTaskId == null) {
-                deviceTask.movePlanTaskId = 0L;
-            }
-            rotationPlanTaskIdSet.clear();
-        }
-
-        // 反向标记任务组
-        int lastIndex = deviceTaskList.size() - 1;
-        for (int i = deviceTaskList.size() - 1; i >= 0; i--) {
-            PtrAgvDeviceTask d = deviceTaskList.get(i);
-            if (d.isGroupEnd) {
-                lastIndex = i;
-            } else {
-                d.operationType = deviceTaskList.get(lastIndex).operationType;
-                d.pickMode = deviceTaskList.get(lastIndex).pickMode;
-                d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint;
-                d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint;
-                d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight;
-                d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection;
-                d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection;
-                d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation;
-            }
-        }
-
-        deviceTaskQueue.addAll(deviceTaskList);
-        String json = JsonWrapper.toJson(deviceTaskList);
-        log.info("deviceTaskList: {}", json);
-    }
-
-    public boolean isSamePosition(PosDirection startPos) {
-        return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() &&
-            PathUtils.getDirectionByArmDirection(this.direction) == startPos.direction();
-    }
-
-    private static class CDirection {
-        private static final short dr = 0;
-        private static final short db = 1;
-        private static final short dl = 2;
-        private static final short dt = 3;
-    }
-
-
-    private static class COperationType {
-        public static final short move = 0;
-        public static final short load = 1;
-        public static final short unpick = 2;
-        public static final short charge = 3;
-        public static final short transplantLoadAndUnload = 4;
-        public static final short rollerLoadAndUnload = 5;
-    }
-
-    private static class CPickMode {
-        public static final short normal = 0;
-        public static final short load = 1;
-        public static final short unload = 2;
-        public static final short adjustHeight = 3;
-        public static final short adjustHeightToLoad = 5;
-        public static final short adjustHeightToUnload = 6;
-    }
-
-
-    /**
-     * 从 AMR 方向转换为 LCC 方向枚举
-     *
-     * @return
-     */
-    public LCCDirection getLCCDirection() {
-        return switch (direction) {
-            case 0 -> LCCDirection.RIGHT;
-            case 1 -> LCCDirection.DOWN;
-            case 2 -> LCCDirection.LEFT;
-            case 3 -> LCCDirection.UP;
-            default -> null;
-        };
-    }
-
-    public short getAmrDirection(LCCDirection lccDirection) {
-        return switch (lccDirection) {
-            case RIGHT -> 0;
-            case DOWN -> 1;
-            case LEFT -> 2;
-            case UP -> 3;
-            default -> -1; // 未知方向
-        };
-    }
-
-    private String getRedisKey(String type) {
-        return String.format("lcc:%s:%s:device:%s:%s",
-            runtime.projectUuid, runtime.envId, this.getId(), type);
-    }
-
-    public AgvStatusVo getState() {
-        var ptr = this;
-
-        var state = new AgvStatusVo();
-        state.setId(ptr.id);
-        state.setType(ptr.getT());
-        state.setIsOnline(ptr.isOnline);
-        state.setIsSystemManaged(ptr.isSystemManaged);
-
-        state.setX(ptr.x);
-        state.setY(ptr.y);
-        state.setZ(ptr.z);
-        state.setLogicX(ptr.logicX);
-        state.setLogicY(ptr.logicY);
-
-        state.setDirection(PathUtils.getDirectionByArmDirection(ptr.direction));
-        state.setOrientation(ptr.orientation);
-        state.setSoc(ptr.battery == null ? -1 : ptr.battery.SOC);
-        state.setMode(ptr.get__taskMode());
-        state.setTaskStatus(ptr.getTaskStatus());
-
-        state.setIsBlocked(ptr.isBlocked);
-        if (ptr.planTaskSequence != null) {
-            state.setTaskCompleted(ptr.planTaskSequence.completedCount());
-            state.setTaskTotalCount(ptr.planTaskSequence.taskTotalCount());
-            state.setBizTaskId(ptr.planTaskSequence.bizTask.getBizTaskId());
-            state.setBizTaskType(BizTaskType.fromString(ptr.planTaskSequence.bizTask.getBizType()));
-            state.setBizTaskStatus(BizTaskStatus.fromString(ptr.planTaskSequence.bizTask.getBizTaskStatus()));
-            state.setBizTaskFrom(ptr.planTaskSequence.bizTask.getTaskFrom());
-            state.setBizTaskTo(ptr.planTaskSequence.bizTask.getTaskTo());
-            state.setBizLpn(ptr.planTaskSequence.bizTask.getLpn());
-
-            state.setLoadBasLocationVo(ptr.planTaskSequence.loadBasLocationVo);
-            state.setUnloadBasLocationVo(ptr.planTaskSequence.unloadBasLocationVo);
-        }
-
-        return state;
+    public RcsConfigMessage getConfig() {
+        var content = new RcsConfigMessage(this.runtime);
+        content.SeqNo = this.amrMessageHandler.getNewSeqNo();
+        content.XLength = 1000;
+        content.YLength = 1000;
+        content.Gap = 1000;
+        content.HeartBeat = 60;
+        content.MqRetryTime = 3;
+        return content;
     }
 }

servo/src/main/java/com/galaxis/rcs/amr/PtrConnectorThread.java

@@ -0,0 +1,14 @@
+package com.galaxis.rcs.amr;
+
+import com.galaxis.rcs.connector.amr.PtrDeviceConnector;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import lombok.extern.slf4j.Slf4j;
+
+@Slf4j
+public class PtrConnectorThread extends AmrConnectorThread {
+
+    public PtrConnectorThread(PtrAgvItem ptrAgvItem, PtrDeviceConnector ptrDeviceConnector, LogisticsRuntime logisticsRuntime) {
+        super(ptrAgvItem, ptrDeviceConnector, logisticsRuntime);
+
+    }
+}

servo/src/main/java/com/galaxis/rcs/connector/amr/AmrDeviceConnector.java

@@ -0,0 +1,23 @@
+package com.galaxis.rcs.connector.amr;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.galaxis.rcs.amr.AmrMessageHandler;
+import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import lombok.extern.slf4j.Slf4j;
+import org.eclipse.paho.mqttv5.common.MqttException;
+
+@Slf4j
+public class AmrDeviceConnector {
+    public final AmrMessageHandler amrMessageHandler;
+    public AmrDeviceConnector(LogisticsRuntime runtime) {
+        this.amrMessageHandler = runtime.amrMessageHandler;
+    }
+    public void sendTask(String vehicleId, RcsTaskMessage rcsTaskMessage) throws MqttException, JsonProcessingException {
+        // var list = Splitter.on("\n").splitToList(JsonWrapper.toJsonPretty(rcsTaskMessage));
+        // String[] ar = new String[list.size()];
+        // list.toArray(ar);
+        // BannerUtils.printConfig(log, "Amr 发送报文", ar);
+        amrMessageHandler.sendCmdTask(vehicleId, rcsTaskMessage);
+    }
+}

servo/src/main/java/com/galaxis/rcs/connector/amr/PtrDeviceConnector.java

@@ -0,0 +1,22 @@
+package com.galaxis.rcs.connector.amr;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.galaxis.rcs.amr.AmrMessageHandler;
+import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import lombok.extern.slf4j.Slf4j;
+import org.eclipse.paho.mqttv5.common.MqttException;
+
+@Slf4j
+public class PtrDeviceConnector extends AmrDeviceConnector {
+    public PtrDeviceConnector(LogisticsRuntime runtime) {
+        super(runtime);
+    }
+    public void sendTask(String vehicleId, RcsTaskMessage rcsTaskMessage) throws MqttException, JsonProcessingException {
+        // var list = Splitter.on("\n").splitToList(JsonWrapper.toJsonPretty(rcsTaskMessage));
+        // String[] ar = new String[list.size()];
+        // list.toArray(ar);
+        // BannerUtils.printConfig(log, "Amr 发送报文", ar);
+        amrMessageHandler.sendCmdTask(vehicleId, rcsTaskMessage);
+    }
+}

servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2DeviceConnector.java

@@ -0,0 +1,19 @@
+package com.galaxis.rcs.connector.amr.cl2;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage;
+import com.galaxis.rcs.amr.AmrMessageHandler;
+import com.galaxis.rcs.connector.amr.AmrDeviceConnector;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import lombok.extern.slf4j.Slf4j;
+import org.eclipse.paho.mqttv5.common.MqttException;
+
+/**
+ * CL2 车型报文推送
+ */
+@Slf4j
+public class Cl2DeviceConnector extends AmrDeviceConnector {
+    public Cl2DeviceConnector(LogisticsRuntime runtime) {
+        super(runtime);
+    }
+}

servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java → servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2Item.java

@@ -1,4 +1,4 @@
-package com.galaxis.rcs.connector.cl2;
+package com.galaxis.rcs.connector.amr.cl2;
 
 import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage;
 import com.yvan.logisticsModel.LogisticsRuntime;

servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/Cl2TaskManger.java

@@ -0,0 +1,4 @@
+package com.galaxis.rcs.connector.amr.cl2;
+
+public class Cl2TaskManger {
+}

servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java → servo/src/main/java/com/galaxis/rcs/connector/amr/cl2/VirtualCl2Connector.java

@@ -1,4 +1,4 @@
-package com.galaxis.rcs.connector.cl2;
+package com.galaxis.rcs.connector.amr.cl2;
 
 import lombok.extern.slf4j.Slf4j;
 

servo/src/main/java/com/galaxis/rcs/connector/amr/clx/ClxConnector.java

@@ -0,0 +1,4 @@
+package com.galaxis.rcs.connector.amr.clx;
+
+public interface ClxConnector {
+}

servo/src/main/java/com/galaxis/rcs/connector/amr/clx/ClxConnectorImp.java

@@ -0,0 +1,4 @@
+package com.galaxis.rcs.connector.amr.clx;
+
+public class ClxConnectorImp {
+}

servo/src/main/java/com/galaxis/rcs/connector/clx/VirtualClxConnector.java → servo/src/main/java/com/galaxis/rcs/connector/amr/clx/VirtualClxConnector.java

@@ -1,4 +1,4 @@
-package com.galaxis.rcs.connector.clx;
+package com.galaxis.rcs.connector.amr.clx;
 
 /**
  * @author cwj

servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java → servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600DeviceConnector.java

@@ -1,4 +1,4 @@
-package com.galaxis.rcs.connector.cl2;
+package com.galaxis.rcs.connector.amr.fm600;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.galaxis.rcs.amr.sendEntity.RcsTaskMessage;
@@ -11,10 +11,10 @@ import org.eclipse.paho.mqttv5.common.MqttException;
  * CL2 车型报文推送
  */
 @Slf4j
-public class Cl2DeviceConnector {
+public class Fm600DeviceConnector {
     private final AmrMessageHandler amrMessageHandler;
 
-    public Cl2DeviceConnector(LogisticsRuntime runtime) {
+    public Fm600DeviceConnector(LogisticsRuntime runtime) {
         this.amrMessageHandler = runtime.amrMessageHandler;
     }
 

servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600Item.java

@@ -0,0 +1,27 @@
+package com.galaxis.rcs.connector.amr.fm600;
+
+import com.galaxis.rcs.amr.AmrAgvItem;
+import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import com.galaxis.rcs.amr.PtrAgvItem;
+
+import java.util.Map;
+
+public class Fm600Item extends AmrAgvItem {
+
+    public Fm600Item(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
+        super(logisticsRuntime, raw);
+    }
+
+    @Override
+    public RcsConfigMessage getConfig() {
+        var content = new RcsConfigMessage(this.runtime);
+        content.SeqNo = this.amrMessageHandler.getNewSeqNo();
+        content.XLength = 1000;
+        content.YLength = 1000;
+        content.Gap = 1000;
+        content.HeartBeat = 60;
+        content.MqRetryTime = 3;
+        return content;
+    }
+}

servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/Fm600TaskManger.java

@@ -0,0 +1,4 @@
+package com.galaxis.rcs.connector.amr.fm600;
+
+public class Fm600TaskManger {
+}

servo/src/main/java/com/galaxis/rcs/connector/amr/fm600/VirtualFm600Connector.java

@@ -0,0 +1,8 @@
+package com.galaxis.rcs.connector.amr.fm600;
+
+import lombok.extern.slf4j.Slf4j;
+
+@Slf4j
+public class VirtualFm600Connector {
+
+}

servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2TaskManger.java

@@ -1,4 +0,0 @@
-package com.galaxis.rcs.connector.cl2;
-
-public class Cl2TaskManger {
-}

servo/src/main/java/com/galaxis/rcs/connector/clx/ClxConnector.java

@@ -1,4 +0,0 @@
-package com.galaxis.rcs.connector.clx;
-
-public interface ClxConnector {
-}

servo/src/main/java/com/galaxis/rcs/connector/clx/ClxConnectorImp.java

@@ -1,4 +0,0 @@
-package com.galaxis.rcs.connector.clx;
-
-public class ClxConnectorImp {
-}

servo/src/main/java/com/yvan/event/AgvEventManager.java

@@ -5,7 +5,7 @@ import com.galaxis.rcs.common.enums.LCCDirection;
 import com.galaxis.rcs.plan.PlanTaskSequence;
 import com.galaxis.rcs.amr.AmrTaskMode;
 import com.galaxis.rcs.amr.PosDirection;
-import com.galaxis.rcs.amr.PtrAgvDeviceTask;
+import com.galaxis.rcs.amr.AmrDeviceTask;
 import com.yvan.logisticsModel.ExecutorItem;
 
 import java.util.Set;
@@ -78,11 +78,11 @@ public class AgvEventManager {
         fireEvent(AgvEventType.PLAN_TASK_SEQUENCE_RESUME, sender, taskSequence);
     }
 
-    public void fireDeviceTaskCompleteEvent(ExecutorItem sender, PtrAgvDeviceTask deviceTask) {
+    public void fireDeviceTaskCompleteEvent(ExecutorItem sender, AmrDeviceTask deviceTask) {
         fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, sender, deviceTask);
     }
 
-    public void fireDeviceTaskExceptionEvent(ExecutorItem sender, PtrAgvDeviceTask deviceTask, Object exception) {
+    public void fireDeviceTaskExceptionEvent(ExecutorItem sender, AmrDeviceTask deviceTask, Object exception) {
         fireEvent(AgvEventType.DEVICE_TASK_EXCEPTION, sender, deviceTask, exception);
     }
 

servo/src/main/java/com/yvan/event/AgvEventType.java

@@ -63,12 +63,12 @@ public enum AgvEventType {
     PLAN_TASK_SEQUENCE_RESUME,
     /**
      * 设备任务已完成
-     * (ExecutorItem sender, PtrAgvDeviceTask deviceTask)
+     * (ExecutorItem sender, AmrDeviceTask deviceTask)
      */
     DEVICE_TASK_COMPLETE,
     /**
      * 设备任务异常
-     * (ExecutorItem sender, PtrAgvDeviceTask deviceTask, Object exception)
+     * (ExecutorItem sender, AmrDeviceTask deviceTask, Object exception)
      */
     DEVICE_TASK_EXCEPTION,
     /**

servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java

@@ -1,8 +1,10 @@
 package com.yvan.logisticsModel;
 
+import com.galaxis.rcs.amr.AmrAgvItem;
 import com.galaxis.rcs.common.entity.RcsTaskPlan;
 import com.galaxis.rcs.common.enums.PlanTaskType;
-import com.galaxis.rcs.connector.cl2.Cl2Item;
+import com.galaxis.rcs.connector.amr.cl2.Cl2Item;
+import com.galaxis.rcs.connector.amr.fm600.Fm600Item;
 import com.galaxis.rcs.inv.InvManager;
 import com.galaxis.rcs.plan.PlanTaskSequence;
 import com.galaxis.rcs.plan.path.NavigationGraph;
@@ -26,7 +28,6 @@ import lombok.extern.slf4j.Slf4j;
 import org.apache.commons.lang3.time.DateFormatUtils;
 import org.clever.core.BannerUtils;
 import org.clever.core.Conv;
-import org.clever.core.DateUtils;
 import org.clever.data.jdbc.DaoFactory;
 import org.clever.data.jdbc.QueryDSL;
 import org.clever.data.redis.Redis;
@@ -328,10 +329,18 @@ public class LogisticsRuntime {
                     break;
 
                 case "cl2":
+                case "cl3":
                 case "clx":
-                    item = new Cl2Item(this, itemObject);
+                case "cs1":
+                    item = new PtrAgvItem(this, itemObject);
                     this.executorItemMap.put(item.getId(), (ExecutorItem) item);
                     break;
+                case "fm600":
+                    item = new Fm600Item(this, itemObject);
+                    this.executorItemMap.put(item.getId(), (ExecutorItem) item);
+                    break;
+                case "cc5":
+                    break;
             }
         }