From c13654e17f10efee79980afd471d1e4966c53bf5 Mon Sep 17 00:00:00 2001 From: luoyifan Date: Fri, 27 Jun 2025 18:22:47 +0800 Subject: [PATCH 1/5] agvCarry --- .../galaxis/rcs/common/entity/StoreLocation.java | 23 ++++++++++++++++++++++ .../yvan/workbench/controller/RcsController.java | 16 +++++++++------ 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/servo/src/main/java/com/galaxis/rcs/common/entity/StoreLocation.java b/servo/src/main/java/com/galaxis/rcs/common/entity/StoreLocation.java index a301f1f..3df6545 100644 --- a/servo/src/main/java/com/galaxis/rcs/common/entity/StoreLocation.java +++ b/servo/src/main/java/com/galaxis/rcs/common/entity/StoreLocation.java @@ -1,5 +1,7 @@ package com.galaxis.rcs.common.entity; +import org.clever.core.Conv; + /** * 存储存储位信息 * 
@@ -21,4 +23,25 @@ public record StoreLocation(
     public String toString() {
         return rackId + "_" + bay + "_" + level + "_" + cell;
     }
+
+    public static StoreLocation of(String rackPosition, String separator) {
+        // 从 'rack1/0/0/0' 解析为 'rack1_0_0_0'
+        if (rackPosition == null || rackPosition.isEmpty()) {
+            throw new RuntimeException("rackPosition cannot be null or empty");
+        }
+
+        String[] parts = rackPosition.split(separator);
+        if (parts.length == 1) {
+            return new StoreLocation(parts[0], 0, 0, 0);
+        }
+        if (parts.length != 4) {
+            throw new RuntimeException("Invalid rack position format: " + rackPosition);
+        }
+
+        String rackId = parts[0];
+        int bay = Conv.asInteger(parts[1]);
+        int level = Conv.asInteger(parts[2]);
+        int cell = Conv.asInteger(parts[3]);
+        return new StoreLocation(rackId, bay, level, cell);
+    }
 }
diff --git a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
index 48b51fd..4816106 100644
--- a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
+++ b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
@@ -172,11 +172,15 @@ public class RcsController {
         }
 
         LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId);
-        StaticItem sourceItem = runtime.getStaticItemById(fromStoreLoc);
+
+        StoreLocation sourceLocation = StoreLocation.of(fromStoreLoc, "/");
+        StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/");
+
+        StaticItem sourceItem = runtime.getStaticItemById(sourceLocation.rackId());
         if (sourceItem == null) {
             return Model.newFail("fromStoreLoc storePoint not found!");
         }
-        StaticItem targetItem = runtime.getStaticItemById(targetStoreLoc);
+        StaticItem targetItem = runtime.getStaticItemById(targetLocation.rackId());
         if (targetItem == null) {
             return Model.newFail("targetStoreLoc storePoint not found!");
         }
@@ -217,8 +221,8 @@ public class RcsController {
         bizTask.setBizType(BizTaskType.MOVE.toString());
         bizTask.setLpn("N/A");
         bizTask.setPriority(Conv.asInteger(option.get("priority"), 1));
-        bizTask.setTaskFrom(sourceItem.getId());
-        bizTask.setTaskTo(targetItem.getId());
+        bizTask.setTaskFrom(fromStoreLoc);
+        bizTask.setTaskTo(targetStoreLoc);
         bizTask.setAllocatedExecutorId(agvId);
         bizTask.setBizTaskPayload("N/A");
         bizTask.setBizTaskErrorInfo("N/A");
@@ -229,8 +233,8 @@ public class RcsController {
 
         CarryTask carryTask = new CarryTask(
             agvId, "dummy", 1,
-            new StoreLocation("rack1", 0, 1, 0),
-            new StoreLocation("54", 0, 0, 0)
+            sourceLocation,
+            targetLocation
         );
 
         runtime.pathPlannerMap.get(executorItem.getT())

From b009a93c6ece53be46a9418c36e7c3ab09d4f8b4 Mon Sep 17 00:00:00 2001
From: luoyifan 
Date: Fri, 27 Jun 2025 18:33:15 +0800
Subject: [PATCH 2/5] agvCarry

---
 .../src/main/java/com/yvan/workbench/controller/RcsController.java  | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
index 4816106..de9444d 100644
--- a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
+++ b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
@@ -239,6 +239,12 @@ public class RcsController {
 
         runtime.pathPlannerMap.get(executorItem.getT())
             .planCarryTask(planSequence, fromItem.getId(), fromDirection, carryTask);
+
+        PtrAgvItem agvItem = (PtrAgvItem) executorItem;
+        agvItem.logicX = fromItem.logicX;
+        agvItem.logicY = fromItem.logicY;
+        agvItem.dispatchTask(planSequence);
+
         return Model.newSuccess(planSequence.toPrettyMap());
     }
 

From 9b49c419a340a97247349f238287c99d8d0f5970 Mon Sep 17 00:00:00 2001
From: yuliang <398780299@qq.com>
Date: Sat, 28 Jun 2025 17:38:41 +0800
Subject: [PATCH 3/5] =?UTF-8?q?=E5=8F=96=E8=B4=A7=E6=94=BE=E8=B4=A7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../com/galaxis/rcs/ptr/AmrMessageHandler.java     |   7 ++
 .../ptr/receiveEntity/AmrTaskCompletedMessage.java |   6 +-
 .../galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java |   3 +
 .../yvan/logisticsModel/PtrAgvConnectorThread.java |  81 +++++++---------
 .../com/yvan/logisticsModel/PtrAgvDeviceTask.java  |  27 +++++-
 .../java/com/yvan/logisticsModel/PtrAgvItem.java   | 104 ++++++++++++++++++---
 6 files changed, 161 insertions(+), 67 deletions(-)

diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
index 7d1611a..8cd3005 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
@@ -140,6 +140,7 @@ public class AmrMessageHandler {
         switch (ArmMessageType.fromValue(id)) {
             case AMR_TASK_COMPLETED:
                 AmrMessage taskCompletedMessage = JacksonUtils.parse(json, typeRef20010Message);
+                handleTaskCompletedMessage(agvItem, taskCompletedMessage.content);
                 break;
             case AMR_TASK_STATUS:
                 handleTaskStatusMessage(agvItem, jw, json);
@@ -419,6 +420,11 @@ public class AmrMessageHandler {
         updateRedisNetDelay(agvItem.getId(), netDelay);
     }
 
+
+    private void handleTaskCompletedMessage(PtrAgvItem agvItem, AmrTaskCompletedMessage message) {
+        agvItem.updateTask(message.CurX, message.CurY, message.CurDirection, 4);
+    }
+
     private void handleLandmarkMessage(PtrAgvItem agvItem, AmrLandmarkMessage message) {
         // 这是源逻辑，CurLogicX / CurLogicY / CurDirection 需要到 PtrAgvItem 中更新, 因为要触发事件
         agvItem.x = message.X;
@@ -488,6 +494,7 @@ public class AmrMessageHandler {
                 // agvItem.logicY = completedMessage.Info.CurLogicY;
                 // // agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
                 // agvItem.direction = taskCompleted.content.Info.CurDirection;
+                agvItem.updateTask(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection, 4);
                 agvItem.updatePosition(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection);
                 break;
             case 8:
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java b/servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java
index b0061fc..a606a0f 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java
@@ -11,10 +11,10 @@ import com.galaxis.rcs.ptr.receiveEntity.base.SummaryData;
 @JsonIgnoreProperties(ignoreUnknown = true)
 public class AmrTaskCompletedMessage extends AmrCommonMessage {
 
-    // 当前X坐标 Double 当前实际位置在地图坐标系中的X坐标
-    public double CurX;
+    // 当前X坐标 UInt16
+    public int CurX;
     // 当前Y坐标 Double 当前实际位置在地图坐标系中的Y坐标
-    public double CurY;
+    public int CurY;
     // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
     public short CurDirection;
     // 当前方向 Double 角度
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java b/servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java
index 181debb..3236f86 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java
@@ -40,6 +40,9 @@ public class RcsTaskMessage {
     public Integer GoodsSlotHeight;
     // 目标货位朝向 UInt8	朝向定义与充电桩朝向相同。 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
     public Short GoodsSlotDirection;
+    // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public Short EndDirection;
+
     // 多机构^[1]^的拣货模式 UInt8[3] 数组形式，意义同"PickMode"
     public List MPickMode;
     // 多机构^[1]^的目标货位高度 UInt16[3] 单位：mm
diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
index e017bff..97dd691 100644
--- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
+++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
@@ -10,6 +10,7 @@ import org.eclipse.paho.mqttv5.common.MqttException;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.locks.LockSupport;
 
 @Slf4j
 public class PtrAgvConnectorThread extends Thread {
@@ -40,8 +41,6 @@ public class PtrAgvConnectorThread extends Thread {
             int taskCount = 0;
 
             PtrAgvDeviceTask startTask = null;
-            PtrAgvDeviceTask currentTask = null;
-            PtrAgvDeviceTask nextTask = null;
             RcsTaskMessage taskMessage = null;
 
             // 计算中的任务
@@ -55,26 +54,10 @@ public class PtrAgvConnectorThread extends Thread {
                     }
                 }
 
-                // for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
-                //     if (task.taskGroupStatus < 3) {
-                //         LockSupport.park(); // 阻塞当前线程
-                //         break;
-                //     }
-                // }
-
-                // 从队列中获取任务，如果队列为空则阻塞等待
-                if (nextTask == null) {
-                    nextTask = this.ptrAgvItem.deviceTaskQueue.take();
-
-                } else {
-                    currentTask = nextTask;
-                }
-
-                if (startTask == null && !nextTask.isLastTask) {
+                PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take();
+                if (startTask == null) {
+                    startTask = currentTask;
                     taskMessage = new RcsTaskMessage(this.logisticsRuntime);
-                    startTask = nextTask;
-                    currentTask = nextTask;
-                    startTask.seqNo = taskMessage.SeqNo;
                     taskMessage.OperationType = startTask.operationType;
                     taskMessage.PickMode = startTask.pickMode;
                     taskMessage.GoNow = true;
@@ -82,33 +65,28 @@ public class PtrAgvConnectorThread extends Thread {
                     taskMessage.StartY = startTask.startPoint.logicY;
                     taskMessage.Link = new ArrayList<>();
                 }
-
-                if (currentTask == nextTask && taskMessage != null) {
-                    currentTask.seqNo = taskMessage.SeqNo;
-                    RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
-                    taskMessage.Link.add(link);
-                    taskCount++;
-                    distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]);
-                    computingTaskList.add(currentTask);
-                }
-
-                // 组织设备任务并发送
-                else if (taskMessage != null &&
-                    (currentTask.operationType > 0 // 当前任务不是移动任务
-                        || currentTask.pickMode > 0 // 当前作业不是默认作业（无）
-                        || nextTask.isLastTask  // 下一个任务是结束任务
-                        || (((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction) && taskMessage.Link.size() > 0) // 下一个任务和开始任务方向不一致
-                        // 单向移动距离大于2m时并且点位数量大于1
-                        || (distance > 2 && taskCount > 1))) {
-
+                currentTask.seqNo = taskMessage.SeqNo;
+                RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
+                taskMessage.Link.add(link);
+                taskCount++;
+                distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]);
+                computingTaskList.add(currentTask);
+
+
+                PtrAgvDeviceTask 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
+                    || (distance > 2 && taskCount > 1)) {
                     taskMessage.OperationType = currentTask.operationType;
                     taskMessage.PickMode = currentTask.pickMode;
-                    taskMessage.EndX = currentTask.endPoint.logicX;
-                    taskMessage.EndY = currentTask.endPoint.logicY;
+                    taskMessage.EndX = currentTask.groupEndPoint.logicX;
+                    taskMessage.EndY = currentTask.groupEndPoint.logicY;
                     taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection;
                     taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight;
                     taskMessage.LinkCounts = (short) taskMessage.Link.size();
-
+                    taskMessage.ChargeDirection = currentTask.chargeDirection;
+                    taskMessage.ChargeLocation = currentTask.chargeLocation;
                     try {
                         // 发送任务
                         this.__currentTaskSeqNo = taskMessage.SeqNo;
@@ -121,17 +99,26 @@ public class PtrAgvConnectorThread extends Thread {
                         }
                         computingTaskList.clear();
                     } catch (MqttException | JsonProcessingException e) {
-                        log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.planTaskId, e);
+                        log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.movePlanTaskId, e);
                     }
                     distance = 0;
                     taskCount = 0;
                     taskMessage = null;
                     startTask = null;
-                }
 
-                if (nextTask == currentTask || nextTask.isLastTask) {
-                    nextTask = null;
+                    if (currentTask.isGroupEnd) {
+                        // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒
+                        LockSupport.park(); // 阻塞当前线程
+                    } else {
+                        for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
+                            if (task.taskGroupStatus < 4) {
+                                LockSupport.park(); // 阻塞当前线程
+                                break;
+                            }
+                        }
+                    }
                 }
+
             }
         } catch (InterruptedException e) {
             System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId());
diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java
index 79a93ac..0b6b70f 100644
--- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java
+++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java
@@ -1,5 +1,8 @@
 package com.yvan.logisticsModel;
 
+import java.util.HashSet;
+import java.util.Set;
+
 public class PtrAgvDeviceTask {
     //该段目标点X坐标	UInt16 逻辑单位，乘以一定系数才是物理距离
     public int x;
@@ -13,6 +16,10 @@ public class PtrAgvDeviceTask {
     public StaticItem startPoint;
     // 该段目标点
     public StaticItem endPoint;
+    // 分组起始点
+    public StaticItem groupStartPoint;
+    // 分组目标点
+    public StaticItem groupEndPoint;
     // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业）
     public short operationType;
     // 提升移栽货物拣货模式	UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
@@ -24,20 +31,30 @@ public class PtrAgvDeviceTask {
     // 目标货位相对于地面的绝对高度
     public int goodsSlotHeight;
 
+    //充电桩朝向UseBriefLocation	UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public Short chargeDirection;
+    // 充电工位坐标和充电桩之间距离	UInt16 单位：mm
+    public Integer chargeLocation;
+    // 目标货架编号 String 仅做校验使用（仅接货用）
+    public String storageRacksNo;
+    // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public Short endDirection;
 
-    /** 规划ID */
-    public Long planTaskId;
+    /** 移动规划ID */
+    public Long movePlanTaskId;
+    /** 转动取货放货充电等规划ID */
+    public Set planTaskIdSet = new HashSet<>();
     /** 业务任务ID */
     public Long bizTaskId;
     // 作业序号 发送给小车的作业序号
     public int seqNo;
 
-    // 任务状态 0创建 1发送成功 2接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
+    // 任务状态 0创建 1任务模式改变 2任务接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
     public int taskStatus = 0;
     public int taskGroupStatus = 0;
 
-    // 是否最后任务
-    public boolean isLastTask = false;
+    // 任务分组结束标记 生成报文时作为判断依据
+    public boolean isGroupEnd = false;
     public int checkLogicX;
     public int checkLogicY;
 }
diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
index 98354aa..1d53083 100644
--- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
+++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
@@ -16,6 +16,7 @@ import com.galaxis.rcs.ptr.sendEntity.RcsSRMessage;
 import com.galaxis.rcs.ptr.sendEntity.RcsSetLocationMessage;
 import com.google.common.base.Joiner;
 import com.google.common.collect.Queues;
+import com.yvan.logisticsMonitor.task.PlanTask;
 import lombok.SneakyThrows;
 import lombok.extern.slf4j.Slf4j;
 import org.clever.core.BannerUtils;
@@ -27,6 +28,7 @@ import java.util.*;
 import java.util.concurrent.BlockingQueue;
 import java.util.concurrent.CopyOnWriteArraySet;
 import java.util.concurrent.locks.LockSupport;
+import java.util.stream.Collectors;
 
 //0.4m/ss // a  max 1.2m/s
 //90 = 3.5s cl2
@@ -239,6 +241,29 @@ public abstract class PtrAgvItem extends ExecutorItem {
         return true;
     }
 
+    public void updateTask(int logicX, int logicY, short direction, int taskStatus) {
+
+        updatePosition(logicX, logicY, direction);
+        // 查找当前分组任务
+        for (PtrAgvDeviceTask task : runningDeviceTaskList) {
+            task.taskGroupStatus = taskStatus;
+            if (taskStatus == 4) {
+                // 更新计划任务
+                List planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList();
+                for (RcsTaskPlan planTask : planTaskList) {
+                    planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
+                    planTaskSequence.savePlanTask(planTask);
+                }
+            }
+        }
+
+        if (planTaskSequence.isAllCompleted()) {
+            fireEvent(AgvEventType.PLAN_COMPLETE, this);
+            this.runningDeviceTaskList.clear();
+            planTaskSequence = null;
+        }
+    }
+
     public void updatePosition(int logicX, int logicY, short direction) {
         int oldX = this.logicX;
         int oldY = this.logicY;
@@ -273,7 +298,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
             for (int i = 0; i < runningDeviceTaskList.size(); i++) {
                 PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
-                if (task.checkLogicX == logicX && task.checkLogicY == logicY) {
+                if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.taskStatus < 4) {
                     finishTargetIndex = i;
                     break;
                 }
@@ -284,13 +309,11 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 // 标记前面的任务都完成了
                 for (int i = 0; i <= finishTargetIndex; i++) {
                     PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
-
                     task.taskStatus = 4; // 标记为完成
-                    task.taskGroupStatus = 4; // 标记为任务组完成
                     fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task);
 
                     // 更新计划任务
-                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.planTaskId);
+                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
                     if (planTask != null) {
                         planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
                         planTaskSequence.savePlanTask(planTask);
@@ -299,6 +322,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
                 if (planTaskSequence.isAllCompleted()) {
                     fireEvent(AgvEventType.PLAN_COMPLETE, this);
+                    this.runningDeviceTaskList.clear();
                     planTaskSequence = null;
                 }
             }
@@ -447,13 +471,16 @@ public abstract class PtrAgvItem extends ExecutorItem {
         if (startPoint == null) {
             log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId());
         }
+        StaticItem groupStartPoint = startPoint;
 
+        Set rotationPlanTaskIdSet = new HashSet<>();
 
         // 生成移动报文
         List deviceTaskList = new ArrayList<>();
         List> linkStore = null;
         // 检查 planList 是不是全都是我的任务
-        for (RcsTaskPlan plan : sequence.taskList) {
+        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())) {
@@ -486,7 +513,9 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 deviceTask.startPoint = startPoint;
                 deviceTask.endPoint = pointItem;
                 deviceTask.bizTaskId = plan.getBizTaskId();
-                deviceTask.planTaskId = plan.getPlanTaskId();
+                deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                rotationPlanTaskIdSet.clear();
                 // 行走任务完成后，检查用的字段
                 deviceTask.checkLogicX = pointItem.logicX;
                 deviceTask.checkLogicY = pointItem.logicY;
@@ -516,12 +545,14 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 } else if (r >= 225 && r < 315) {
                     direction = CDirection.db;
                 }
+                rotationPlanTaskIdSet.add(plan.getPlanTaskId());
 
             } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) {
 
                 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 storeItemRaw = storeItem.dt;
@@ -556,17 +587,24 @@ public abstract class PtrAgvItem extends ExecutorItem {
                     }
                 }
 
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
+
             } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
                 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 storeItemRaw = storeItem.dt;
-                if (storeItemRaw.containsKey("bays") && storeItemRaw.containsKey("level")) {
+                if (storeItemRaw.containsKey("bays")) {
                     List> bays = (List>) storeItemRaw.get("bays");
                     Map bay = bays.get(plan.getTargetBay());
-                    List levelHeight = (List) bay.get("levels");
+                    List levelHeight = (List) bay.get("levelHeight");
                     deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
                 } else {
                     deviceTask.goodsSlotHeight = 1;
@@ -593,11 +631,24 @@ public abstract class PtrAgvItem extends ExecutorItem {
                         }
                     }
                 }
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
 
             } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.charge;
+                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
                 // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
+                deviceTask.chargeDirection = 0;
+                deviceTask.chargeLocation = 200;
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
             }
 
             if (!plan.getExecutorId().equals(this.getId())) {
@@ -605,10 +656,39 @@ public abstract class PtrAgvItem extends ExecutorItem {
             }
 
         }
-        // 添加结束任务
-        PtrAgvDeviceTask deviceTaskEnd = new PtrAgvDeviceTask();
-        deviceTaskEnd.isLastTask = true;
-        deviceTaskList.add(deviceTaskEnd);
+
+
+
+        // 标记任务分组结束
+        PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
+        deviceTask.groupEndPoint = deviceTask.endPoint;
+        deviceTask.groupStartPoint = groupStartPoint;
+        deviceTask.isGroupEnd = true;
+        // 最后一个规划任务为旋转时需要添加一个endDirection
+        if (rotationPlanTaskIdSet.size() > 0) {
+            deviceTask.endDirection = direction;
+            deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+            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;
+            }
+        }
+
 
 //        planQueue.addAll(sequence.taskList);
         deviceTaskQueue.addAll(deviceTaskList);

From cb3f925206ee5681246726e15cd333e3d602ee3e Mon Sep 17 00:00:00 2001
From: yuliang <398780299@qq.com>
Date: Sat, 28 Jun 2025 19:45:38 +0800
Subject: [PATCH 4/5] ddd

---
 .../com/galaxis/rcs/ptr/AmrMessageHandler.java     |  4 +-
 .../java/com/yvan/logisticsModel/PtrAgvItem.java   | 81 ++++++++++++++++++++--
 2 files changed, 76 insertions(+), 9 deletions(-)

diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
index 8cd3005..eb41787 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
@@ -422,7 +422,7 @@ public class AmrMessageHandler {
 
 
     private void handleTaskCompletedMessage(PtrAgvItem agvItem, AmrTaskCompletedMessage message) {
-        agvItem.updateTask(message.CurX, message.CurY, message.CurDirection, 4);
+        agvItem.taskCompleted(message.CurX, message.CurY, message.CurDirection, 4);
     }
 
     private void handleLandmarkMessage(PtrAgvItem agvItem, AmrLandmarkMessage message) {
@@ -494,7 +494,7 @@ public class AmrMessageHandler {
                 // agvItem.logicY = completedMessage.Info.CurLogicY;
                 // // agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
                 // agvItem.direction = taskCompleted.content.Info.CurDirection;
-                agvItem.updateTask(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection, 4);
+//                agvItem.updateTask(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection, 4);
                 agvItem.updatePosition(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection);
                 break;
             case 8:
diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
index 1d53083..2e5c610 100644
--- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
+++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
@@ -241,7 +241,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
         return true;
     }
 
-    public void updateTask(int logicX, int logicY, short direction, int taskStatus) {
+    public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) {
 
         updatePosition(logicX, logicY, direction);
         // 查找当前分组任务
@@ -257,11 +257,12 @@ public abstract class PtrAgvItem extends ExecutorItem {
             }
         }
 
-        if (planTaskSequence.isAllCompleted()) {
+        if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
             fireEvent(AgvEventType.PLAN_COMPLETE, this);
             this.runningDeviceTaskList.clear();
             planTaskSequence = null;
         }
+        LockSupport.unpark(connectorThread);
     }
 
     public void updatePosition(int logicX, int logicY, short direction) {
@@ -339,8 +340,12 @@ public abstract class PtrAgvItem extends ExecutorItem {
             )
         });
 
-        if (needCompute) {
-            LockSupport.unpark(connectorThread);
+        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);
+            }
         }
     }
 
@@ -489,18 +494,22 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 linkStore = (List>) 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;
+                    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;
+                    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("无法识别的点位关系");
                 }
@@ -549,6 +558,25 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
             } 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>) startPoint.dt.get("linkStore");
+                }
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.transplantLoadAndUnload;
                 deviceTask.pickMode = CPickMode.load;
@@ -594,6 +622,25 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 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>) startPoint.dt.get("linkStore");
+                }
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.transplantLoadAndUnload;
                 deviceTask.pickMode = CPickMode.unload;
@@ -638,6 +685,24 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 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());
@@ -657,7 +722,9 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
         }
 
-
+        if (deviceTaskList.size() <= 0) {
+            return;
+        }
 
         // 标记任务分组结束
         PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);

From dd06e970ace7df66c7e230bc2375f40bcae1dcd9 Mon Sep 17 00:00:00 2001
From: luoyifan 
Date: Sat, 28 Jun 2025 22:28:54 +0800
Subject: [PATCH 5/5] =?UTF-8?q?=E9=87=8D=E6=9E=84=20PTR=20=E7=9A=84?=
 =?UTF-8?q?=E5=B0=8F=E8=BD=A6=E5=AE=9E=E4=BD=93=E3=80=81=E6=8A=A5=E6=96=87?=
 =?UTF-8?q?=E6=94=B6=E5=8F=91=E3=80=81=E5=8D=8F=E8=AE=AE=E3=80=81=E5=85=A8?=
 =?UTF-8?q?=E6=94=BE=E5=9C=A8=E4=B8=80=E8=B5=B7=EF=BC=9B=E5=A2=9E=E5=8A=A0?=
 =?UTF-8?q?=20agvLoad/agvUnload/cancelTasks/waitTaskFinish=20=E6=96=B9?=
 =?UTF-8?q?=E6=B3=95?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../com/galaxis/rcs/common/enums/LCCDirection.java |  13 +
 .../rcs/connector/cl2/Cl2DeviceConnector.java      |   8 -
 .../com/galaxis/rcs/connector/cl2/Cl2Item.java     |   2 +-
 .../rcs/connector/cl2/VirtualCl2Connector.java     |   4 -
 .../com/galaxis/rcs/plan/path/AStarNodeState.java  |  29 -
 .../galaxis/rcs/plan/path/AStarPathPlanner.java    | 280 ++++---
 .../galaxis/rcs/plan/path/GraphInitializer.java    |  95 ---
 .../com/galaxis/rcs/plan/path/NavigationGraph.java | 166 +++--
 .../com/galaxis/rcs/plan/path/NavigationNode.java  |  25 -
 .../main/java/com/galaxis/rcs/plan/path/Node.java  |  20 +
 .../com/galaxis/rcs/plan/path/OperationPoint.java  |  33 -
 .../com/galaxis/rcs/plan/path/PathSegment.java     |  15 -
 .../java/com/galaxis/rcs/plan/path/PathUtils.java  | 183 +++++
 .../com/galaxis/rcs/plan/path/PtrPathPlanner.java  | 236 ++++++
 .../main/java/com/galaxis/rcs/plan/path/State.java |  19 +
 .../java/com/galaxis/rcs/plan/path/StoreLink.java  |  17 +
 .../galaxis/rcs/plan/path2/AStarPathPlanner.java   | 220 ------
 .../galaxis/rcs/plan/path2/NavigationGraph.java    | 147 ----
 .../main/java/com/galaxis/rcs/plan/path2/Node.java |  20 -
 .../java/com/galaxis/rcs/plan/path2/PathUtils.java | 183 -----
 .../com/galaxis/rcs/plan/path2/PtrPathPlanner.java | 143 ----
 .../java/com/galaxis/rcs/plan/path2/State.java     |  19 -
 .../java/com/galaxis/rcs/plan/path2/StoreLink.java |  17 -
 .../java/com/galaxis/rcs/plan/task/LoadTask.java   |  15 +
 .../java/com/galaxis/rcs/plan/task/MoveTask.java   |   3 +
 .../java/com/galaxis/rcs/plan/task/UnloadTask.java |  15 +
 .../java/com/galaxis/rcs/ptr/AgvEventManager.java  |   1 -
 .../com/galaxis/rcs/ptr/AmrMessageHandler.java     |   1 -
 .../java/com/galaxis/rcs/ptr/PtrAgvConnector.java  |   5 +
 .../com/galaxis/rcs/ptr/PtrAgvConnectorThread.java | 167 +++++
 .../java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java |  62 ++
 .../main/java/com/galaxis/rcs/ptr/PtrAgvItem.java  | 830 +++++++++++++++++++++
 .../java/com/yvan/logisticsModel/BaseItem.java     |  22 +-
 .../com/yvan/logisticsModel/LogisticsRuntime.java  |   4 +-
 .../com/yvan/logisticsModel/PtrAgvConnector.java   |   7 -
 .../yvan/logisticsModel/PtrAgvConnectorThread.java | 167 -----
 .../com/yvan/logisticsModel/PtrAgvDeviceTask.java  |  60 --
 .../java/com/yvan/logisticsModel/PtrAgvItem.java   | 829 --------------------
 .../yvan/workbench/controller/RcsController.java   | 327 ++++----
 .../com/yvan/workbench/model/entity/Model.java     |  13 +
 40 files changed, 2108 insertions(+), 2314 deletions(-)
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/Node.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/State.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/State.java
 delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java
 create mode 100644 servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java
 delete mode 100644 servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java
 delete mode 100644 servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
 delete mode 100644 servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java
 delete mode 100644 servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java

diff --git a/servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java b/servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java
index 7b073ef..4cc23ea 100644
--- a/servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java
+++ b/servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java
@@ -35,4 +35,17 @@ public enum LCCDirection {
 
         throw new IllegalArgumentException("No constant with name: " + value);
     }
+
+    public static LCCDirection fromString(String value, LCCDirection defaultValue) {
+        if (value == null)
+            throw new IllegalArgumentException("Value cannot be null");
+
+        for (LCCDirection type : LCCDirection.values()) {
+            if (type.toString().equalsIgnoreCase(value.trim())) {
+                return type;
+            }
+        }
+
+        return defaultValue;
+    }
 }
diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java
index 6a29965..f6fb011 100644
--- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java
+++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java
@@ -1,20 +1,12 @@
 package com.galaxis.rcs.connector.cl2;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
-import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage;
 import com.galaxis.rcs.ptr.sendEntity.RcsTaskMessage;
 import com.galaxis.rcs.ptr.AmrMessageHandler;
-import com.google.common.base.Splitter;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.extern.slf4j.Slf4j;
-import org.clever.core.BannerUtils;
-import org.clever.core.json.JsonWrapper;
 import org.eclipse.paho.mqttv5.common.MqttException;
 
-import java.util.Map;
-
 /**
  * CL2 车型报文推送
  */
diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java
index e490862..3d2d893 100644
--- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java
+++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java
@@ -2,7 +2,7 @@ package com.galaxis.rcs.connector.cl2;
 
 import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
+import com.galaxis.rcs.ptr.PtrAgvItem;
 
 import java.util.Map;
 
diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java
index 28ea82c..a42cdd1 100644
--- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java
+++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java
@@ -1,9 +1,5 @@
 package com.galaxis.rcs.connector.cl2;
 
-import com.galaxis.rcs.common.entity.RcsTaskPlan;
-import com.yvan.logisticsModel.ExecutorItem;
-import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.extern.slf4j.Slf4j;
 
 @Slf4j
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java
deleted file mode 100644
index e7f5f10..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java
+++ /dev/null
@@ -1,29 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-/**
- * A*路径节点状态
- */
-public record AStarNodeState(
-    String nodeId,          // 当前节点ID
-    int direction,          // 当前方向 (0,90,180,270)
-    float gCost,           // 实际代价
-    float hCost,           // 启发式代价
-    AStarNodeState parent   // 父节点
-) implements Comparable {
-
-    // 状态唯一标识
-    public String stateKey() {
-        return nodeId + ":" + direction;
-    }
-
-    // 总代价
-    public float fCost() {
-        return gCost + hCost;
-    }
-
-    @Override
-    public int compareTo(AStarNodeState other) {
-        return Float.compare(this.fCost(), other.fCost());
-    }
-}
-
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java
index 0c1a830..8347234 100644
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java
@@ -1,122 +1,220 @@
 package com.galaxis.rcs.plan.path;
 
+import com.galaxis.rcs.common.enums.LCCDirection;
+import com.google.common.collect.Maps;
+
 import java.util.*;
 
-/**
- * A*路径规划器
- */
+import static com.galaxis.rcs.plan.path.PathUtils.*;
+
 public class AStarPathPlanner {
+    private static final float ROTATION_COST_PER_DEGREE = 0.01f;
+    private static final float BLOCKED_COST = 10000f; // 阻塞系数成本
+    private static final float WEIGHT_FACTOR = 1.5f; // 权重因子
+
     private final NavigationGraph graph;
+    private final Map nodeWeights = Maps.newConcurrentMap();
+    private final Map blockedNodes = Maps.newConcurrentMap();
 
     public AStarPathPlanner(NavigationGraph graph) {
         this.graph = graph;
     }
 
-    /**
-     * 规划路径
-     *
-     * @param startNodeId    起始节点ID
-     * @param startDirection 起始方向
-     * @param endNodeId      目标节点ID
-     * @param endDirection   目标方向
-     * @return 路径节点序列 (包含方向信息)
-     */
-    public List planPath(String startNodeId, int startDirection, String endNodeId, int endDirection) {
-        // 开放集 (优先队列)
-        PriorityQueue openSet = new PriorityQueue<>();
-
-        // 状态管理
-        Map gScoreMap = new HashMap<>();
-        Map cameFrom = new HashMap<>();
-
-        // 初始化起点
-        AStarNodeState start = new AStarNodeState(
-            startNodeId,
-            startDirection,
-            0,
-            graph.heuristicCost(startNodeId, endNodeId),
-            null
-        );
-
-        openSet.add(start);
-        gScoreMap.put(start.stateKey(), 0.0f);
-
-        while (!openSet.isEmpty()) {
-            AStarNodeState current = openSet.poll();
-
-            // 到达目标状态
-            if (current.nodeId().equals(endNodeId) &&
-                current.direction() == endDirection) {
-                return reconstructPath(cameFrom, current);
+    // 路径规划状态
+    public List findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) {
+        Node start = graph.getNode(startId);
+        Node end = graph.getNode(endId);
+        if (start == null) {
+            throw new RuntimeException("Start node not found: " + startId);
+        }
+        if (end == null) {
+            throw new RuntimeException("End node not found: " + endId);
+        }
+
+        // 使用复合键避免状态重复
+        Map visited = new HashMap<>();
+        PriorityQueue open = new PriorityQueue<>();
+
+        // 初始状态
+        State initialState = new State(start, startDirection, 0, heuristic(start, end), null);
+        open.add(initialState);
+        visited.put(stateKey(start.id(), startDirection), initialState);
+
+        while (!open.isEmpty()) {
+            State current = open.poll();
+
+            // 到达目标节点且方向匹配
+            if (current.node().id().equals(endId) && current.direction() == endDirection) {
+                return buildPath(current);
             }
 
-            // 处理移动操作 (到相邻节点)
-            for (NavigationNode neighbor : graph.getAdjacentNodes(current.nodeId())) {
-                PathSegment segment = graph.getPathSegment(current.nodeId(), neighbor.id());
-                if (segment == null) continue;
-
-                float moveCost = segment.distance();
-                float tentativeGCost = current.gCost() + moveCost;
-                String neighborKey = neighbor.id() + ":" + current.direction();
-
-                // 发现更好路径
-                if (tentativeGCost < gScoreMap.getOrDefault(neighborKey, Float.MAX_VALUE)) {
-                    AStarNodeState neighborState = new AStarNodeState(
-                        neighbor.id(),
-                        current.direction(),
-                        tentativeGCost,
-                        graph.heuristicCost(neighbor.id(), endNodeId),
+            // 处理邻边移动
+            for (Node neighbor : graph.getNeighbors(current.node())) {
+                // 检查节点是否被阻塞
+                if (isBlocked(neighbor.id())) continue;
+
+                // 计算移动方向
+                LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
+
+                // 尝试前进
+                if (canMoveForward(current.direction(), moveDirection)) {
+                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
+                    considerState(current, neighbor, current.direction(),
+                        moveCost, open, visited, end);
+                }
+                // 尝试后退
+                else if (canMoveBackward(current.direction(), moveDirection)) {
+                    float moveCost = calculateMoveCost(current.node(), neighbor, true);
+                    considerState(current, neighbor, current.direction(),
+                        moveCost, open, visited, end);
+                }
+                // 需要旋转
+                else if (current.node().rotatable()) {
+                    // 计算需要旋转到的方向
+                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
+
+                    // 考虑旋转后移动
+                    float rotationCost = calculateRotationCost(
+                        current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
+                    );
+                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
+                    float totalCost = rotationCost + moveCost;
+
+                    // 创建旋转状态
+                    State rotatedState = new State(
+                        current.node(), requiredDirection,
+                        current.g() + rotationCost,
+                        heuristic(current.node(), end),
                         current
                     );
 
-                    cameFrom.put(neighborKey, current);
-                    gScoreMap.put(neighborKey, tentativeGCost);
-                    openSet.add(neighborState);
+                    // 考虑旋转后的移动
+                    considerState(rotatedState, neighbor, requiredDirection,
+                        moveCost, open, visited, end);
                 }
             }
 
-            // 处理旋转操作 (当前节点可旋转时)
-            NavigationNode currentNode = graph.nodes.get(current.nodeId());
-            if (currentNode != null && currentNode.rotatable()) {
-                for (int rotation : new int[]{90, -90}) {
-                    int newDirection = (current.direction() + rotation + 360) % 360;
-                    float rotateCost = 1.0f; // 旋转代价
-                    float tentativeGCost = current.gCost() + rotateCost;
-                    String rotatedKey = current.nodeId() + ":" + newDirection;
-
-                    // 发现更好路径
-                    if (tentativeGCost < gScoreMap.getOrDefault(rotatedKey, Float.MAX_VALUE)) {
-                        AStarNodeState rotatedState = new AStarNodeState(
-                            current.nodeId(),
-                            newDirection,
-                            tentativeGCost,
-                            current.hCost(), // 旋转不改变位置，启发值不变
-                            current
-                        );
+            // 处理原地旋转 - 只考虑目标方向
+            if (current.node().rotatable()) {
+                // 只考虑旋转到目标方向（如果可能）
+                if (current.direction() != endDirection) {
+                    float rotationCost = calculateRotationCost(
+                        current.direction(), endDirection, ROTATION_COST_PER_DEGREE
+                    );
+                    considerState(current, current.node(), endDirection,
+                        rotationCost, open, visited, end);
+                }
+
+                // 另外考虑旋转到移动所需的方向
+                for (Node neighbor : graph.getNeighbors(current.node())) {
+                    LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
+                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
 
-                        cameFrom.put(rotatedKey, current);
-                        gScoreMap.put(rotatedKey, tentativeGCost);
-                        openSet.add(rotatedState);
+                    if (requiredDirection != current.direction()) {
+                        float rotationCost = calculateRotationCost(
+                            current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
+                        );
+                        considerState(current, current.node(), requiredDirection,
+                            rotationCost, open, visited, end);
                     }
                 }
+
+                // for (LCCDirection rotation : LCCDirection.values()) {
+                //     if (rotation == current.direction()) continue;
+                //
+                //     float rotationCost = calculateRotationCost(
+                //         current.direction(), rotation, ROTATION_COST_PER_DEGREE
+                //     );
+                //     considerState(current, current.node(), rotation,
+                //         rotationCost, open, visited, end);
+                // }
             }
         }
-
-        return Collections.emptyList(); // 未找到路径
+        return Collections.emptyList();
     }
 
-    private List reconstructPath(
-        Map cameFrom,
-        AStarNodeState endState
-    ) {
-        LinkedList path = new LinkedList<>();
-        AStarNodeState current = endState;
-
-        while (current != null) {
-            path.addFirst(current);
-            current = cameFrom.get(current.stateKey());
+    /**
+     * 考虑新的状态并更新开放列表和访问记录
+     *
+     * @param current       当前状态
+     * @param nextNode      下一个节点
+     * @param nextDirection 下一个方向
+     * @param cost          移动成本
+     * @param open          开放列表
+     * @param visited       访问记录
+     * @param end           目标节点
+     */
+    private void considerState(State current, Node nextNode, LCCDirection nextDirection,
+                               float cost, PriorityQueue open,
+                               Map visited, Node end) {
+        String key = stateKey(nextNode.id(), nextDirection);
+        float newG = current.g() + cost;
+
+        if (!visited.containsKey(key) || visited.get(key).g() > newG) {
+            float h = heuristic(nextNode, end);
+            State newState = new State(nextNode, nextDirection, newG, h, current);
+            open.add(newState);
+            visited.put(key, newState);
         }
+    }
 
+    private List buildPath(State state) {
+        LinkedList path = new LinkedList<>();
+        while (state != null) {
+            path.addFirst(state);
+            state = state.parent();
+        }
         return path;
     }
+
+    /**
+     * 启发式函数，计算两个节点之间的距离
+     */
+    private float heuristic(Node a, Node b) {
+        return graph.distance(a, b);
+        // 使用曼哈顿距离？？
+        // return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z());
+    }
+
+    /**
+     * 生成状态的唯一键
+     */
+    private String stateKey(String nodeId, LCCDirection direction) {
+        return nodeId + "|" + direction;
+    }
+
+    public void setNodeWeight(String nodeId, float weight) {
+        nodeWeights.put(nodeId, weight);
+    }
+
+    public void setBlocked(String nodeId, float blockedFactor) {
+        blockedNodes.put(nodeId, blockedFactor);
+    }
+
+    private float calculateMoveCost(Node from, Node to, boolean isBackward) {
+        float baseCost = graph.distance(from, to);
+        float weight = nodeWeights.getOrDefault(to.id(), 1.0f);
+        float blockedFactor = blockedNodes.getOrDefault(to.id(), 0f);
+
+        // 后退移动增加额外成本??
+        // return baseCost * weight * (1 + blockedFactor) * (isBackward ? 1.2f : 1.0f);
+        return baseCost * weight * (1 + blockedFactor);
+    }
+
+    private boolean isBlocked(String nodeId) {
+        return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f;
+    }
+
+    private boolean canMoveForward(LCCDirection currentDir, LCCDirection moveDir) {
+        return currentDir == moveDir;
+    }
+
+    private boolean canMoveBackward(LCCDirection currentDir, LCCDirection moveDir) {
+        return currentDir == getOppositeDirection(moveDir);
+    }
+
+    private LCCDirection calculateRequiredDirection(LCCDirection moveDirection) {
+        // 侧插式AGV只能前进或后退，不能横移
+        return moveDirection;
+    }
 }
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java b/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java
deleted file mode 100644
index e3dd8cf..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java
+++ /dev/null
@@ -1,95 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-import com.galaxis.rcs.common.enums.OperationSide;
-import com.yvan.logisticsModel.LogisticsRuntime;
-import org.clever.core.Conv;
-
-import java.util.List;
-import java.util.Map;
-
-public class GraphInitializer {
-
-//    public NavigationGraph initializeGraph(LogisticsRuntime runtime, String agvType, List> jsonData) {
-//        if (runtime.pathPlannerMap.containsKey(agvType)) {
-//            return runtime.pathPlannerMap.get(agvType);
-//        }
-//        NavigationGraph graph = new NavigationGraph();
-//        runtime.pathPlannerMap.put(agvType, graph);
-//
-//        // 第一步：创建所有节点
-//        for (Map nodeData : jsonData) {
-//            String id = (String) nodeData.get("id");
-//
-//            // 判断是否是 way 类型才创建 NavigationNode
-//            if (!"way".equals(nodeData.get("t"))) {
-//                continue;
-//            }
-//
-//            List> tf = (List>) nodeData.get("tf");
-//            float x = tf.get(0).get(0);
-//            float z = tf.get(0).get(2);
-//
-//            // 检查是否为可旋转点
-//            Map dt = (Map) nodeData.get("dt");
-//            boolean rotatable = false;
-//            if (dt.containsKey("agvRotation")) {
-//                rotatable = true;
-//            }
-//
-//            // 添加节点
-//            graph.addNode(new NavigationNode(id, x, z, rotatable));
-//        }
-//
-//        // 第二步：添加路径连接
-//        for (Map nodeData : jsonData) {
-//            if (!"way".equals(nodeData.get("t"))) continue;
-//
-//            String id = (String) nodeData.get("id");
-//            Map dt = (Map) nodeData.get("dt");
-//
-//            List outEdges = (List) dt.get("out");
-//            if (outEdges != null) {
-//                for (String neighborId : outEdges) {
-//                    if (graph.nodes.containsKey(id) && graph.nodes.containsKey(neighborId)) {
-//                        NavigationNode from = graph.nodes.get(id);
-//                        NavigationNode to = graph.nodes.get(neighborId);
-//                        graph.addBidirectionalPath(from, to);
-//                    }
-//                }
-//            }
-//        }
-//
-//        // 第三步：添加操作点 OperationPoint
-//        for (Map nodeData : jsonData) {
-//            if (!"way".equals(nodeData.get("t"))) continue;
-//
-//            String nodeId = (String) nodeData.get("id");
-//            Map dt = (Map) nodeData.get("dt");
-//
-//            if (dt.containsKey("linkStore")) {
-//                List> linkStores = (List>) dt.get("linkStore");
-//                for (Map store : linkStores) {
-//                    String targetId = (String) store.get("item");
-//                    Integer bay = Conv.asInteger(store.get("bay"));
-//                    Integer level = Conv.asInteger(store.get("level"));
-//                    Integer cell = Conv.asInteger(store.get("cell"));
-//
-//                    // 根据位置确定方位（这里假设固定为 TOP，可根据 tf 中的方向判断更精确）
-//                    OperationSide side = OperationSide.TOP;
-//
-//                    OperationPoint point = new OperationPoint(
-//                        graph.nodes.get(nodeId),
-//                        targetId,
-//                        side,
-//                        bay,
-//                        level,
-//                        cell
-//                    );
-//                    graph.addOperationPoint(point);
-//                }
-//            }
-//        }
-//
-//        return graph;
-//    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationGraph.java b/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationGraph.java
index 08a4db7..595f381 100644
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationGraph.java
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationGraph.java
@@ -1,69 +1,147 @@
 package com.galaxis.rcs.plan.path;
 
+import com.galaxis.rcs.common.enums.LCCDirection;
+import com.google.common.collect.Maps;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import com.yvan.logisticsModel.StaticItem;
+import lombok.extern.slf4j.Slf4j;
+
 import java.util.*;
-import java.util.concurrent.ConcurrentHashMap;
 
 /**
- * 导航图管理器
+ * A* 导航图
  */
+@Slf4j
 public class NavigationGraph {
-    final Map nodes = new ConcurrentHashMap<>();
-    final Map operationPoints = new ConcurrentHashMap<>();
-    final Map pathSegments = new ConcurrentHashMap<>();
-    final Map> adjacencyList = new ConcurrentHashMap<>();
-
-    // 添加节点
-    public void addNode(NavigationNode node) {
-        nodes.put(node.id(), node);
-        adjacencyList.put(node.id(), new ArrayList<>());
+    private final LogisticsRuntime runtime;
+
+    /**
+     * 缓存距离计算结果，避免重复计算
+     */
+    private final Map distanceCache = Maps.newConcurrentMap();
+
+    /**
+     * 缓存邻居节点列表，避免重复查询
+     */
+    private final Map> neighborCache = Maps.newConcurrentMap();
+
+    /**
+     * 添加路径缓存
+     */
+    private final Map> pathCache = Maps.newConcurrentMap();
+
+    private final Map nodeMap = new HashMap<>();
+    private final Map> storeToNodes = new HashMap<>();
+
+    public NavigationGraph(LogisticsRuntime runtime) {
+        this.runtime = runtime;
     }
 
-    // 添加双向路径
-    public void addBidirectionalPath(NavigationNode a, NavigationNode b) {
-        float distance = a.distanceTo(b);
-        pathSegments.put(a.id() + "->" + b.id(), new PathSegment(a, b, distance));
-        pathSegments.put(b.id() + "->" + a.id(), new PathSegment(b, a, distance));
+    public List getCachedPath(String startId, LCCDirection startDirection,
+                                     String endId, LCCDirection endDirection) {
+        String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection;
+        return pathCache.get(cacheKey);
+    }
 
-        adjacencyList.get(a.id()).add(b);
-        adjacencyList.get(b.id()).add(a);
+    public void cachePath(String startId, LCCDirection startDirection,
+                          String endId, LCCDirection endDirection,
+                          List path) {
+        String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection;
+        pathCache.put(cacheKey, path);
     }
 
-    // 添加操作点
-    public void addOperationPoint(OperationPoint point) {
-        operationPoints.put(point.locationKey(), point);
+    public Node getNodeById(String nodeId){
+        return nodeMap.get(nodeId);
     }
 
-    // 查找操作点
-    public OperationPoint findOperationPoint(String locationKey) {
-        return operationPoints.get(locationKey);
+    public List getNodesForStore(String storeId) {
+        List nodes = new ArrayList<>();
+        List nodeIds = storeToNodes.get(storeId);
+        if (nodeIds != null) {
+            for (String id : nodeIds) {
+                Node node = nodeMap.get(id);
+                if (node != null) {
+                    nodes.add(node);
+                }
+            }
+        }
+        return nodes;
     }
 
-    // 获取路径段
-    public PathSegment getPathSegment(String startId, String endId) {
-        return pathSegments.get(startId + "->" + endId);
+    public List getNeighbors(Node node) {
+        return node.neighbors().stream()
+            .map(nodeMap::get)
+            .filter(Objects::nonNull)
+            .toList();
     }
 
-    // 获取相邻节点
-    public List getAdjacentNodes(String nodeId) {
-        return adjacencyList.getOrDefault(nodeId, Collections.emptyList());
+    public float distance(Node a, Node b) {
+        float dx = a.x() - b.x();
+        float dz = a.z() - b.z();
+        return (float) Math.sqrt(dx * dx + dz * dz);
     }
 
-    // 获取最近的旋转点
-    public NavigationNode findNearestRotationPoint(String startId) {
-        NavigationNode start = nodes.get(startId);
-        if (start == null) return null;
 
-        return nodes.values().parallelStream()
-            .filter(NavigationNode::rotatable)
-            .min(Comparator.comparing(start::distanceTo))
-            .orElse(null);
+    public void init() {
+        var items = runtime.getStaticItems();
+        for (StaticItem item : items) {
+            if ("way".equals(item.getT())) {
+                float[][] tf = item.getTf();
+                String id = item.getId();
+                Map dt = item.getDt();
+
+                // 提取坐标
+                float x = tf[0][0];
+                float z = tf[0][2]; // Z向下增长
+
+                // 检查可旋转性
+                boolean rotatable = dt.containsKey("agvRotation");
+                if (rotatable) {
+                    log.info("Node {} is rotatable", id);
+                }
+
+                // 提取邻居节点
+                List in = (List) dt.get("in");
+                List out = (List) dt.get("out");
+                Set neighbors = new HashSet<>();
+                if (in != null) neighbors.addAll(in);
+                if (out != null) neighbors.addAll(out);
+
+                // 提取货位链接
+                List storeLinks = new ArrayList<>();
+                List> links = (List>) dt.get("linkStore");
+                if (links != null) {
+                    for (Map link : links) {
+                        String storeId = (String) link.get("item");
+                        int bay = (Integer) link.get("bay");
+                        int level = (Integer) link.get("level");
+                        int cell = (Integer) link.get("cell");
+                        String direction = (String) link.get("direction");
+                        storeLinks.add(new StoreLink(storeId, bay, level, cell, LCCDirection.fromString(direction)));
+
+                        // 建立货位到节点的反向映射
+                        storeToNodes.computeIfAbsent(storeId, k -> new ArrayList<>()).add(id);
+                    }
+                }
+
+                nodeMap.put(id, new Node(id, x, z, rotatable,
+                    new ArrayList<>(neighbors), storeLinks));
+            }
+        }
+
+        // 2. 验证邻居双向连接
+        for (Node node : nodeMap.values()) {
+            Iterator it = node.neighbors().iterator();
+            while (it.hasNext()) {
+                String neighborId = it.next();
+                if (!nodeMap.containsKey(neighborId)) {
+                    it.remove(); // 移除无效邻居
+                }
+            }
+        }
     }
 
-    // 计算启发式代价 (使用欧几里得距离)
-    public float heuristicCost(String fromId, String toId) {
-        NavigationNode from = nodes.get(fromId);
-        NavigationNode to = nodes.get(toId);
-        if (from == null || to == null) return Float.MAX_VALUE;
-        return from.distanceTo(to);
+    public Node getNode(String id) {
+        return nodeMap.get(id);
     }
 }
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java b/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java
deleted file mode 100644
index 2cac8a6..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java
+++ /dev/null
@@ -1,25 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-import org.clever.core.Conv;
-
-/**
- * 路标节点（地图中的顶点）
- */
-public record NavigationNode(
-    String id,          // 节点ID (如 "20", "charger1")
-    float x,           // 向右增长
-    float z,           // 向下增长 (y无效)
-    boolean rotatable   // 是否为可旋转位
-) implements Comparable {
-    // 计算到另一个节点的欧几里得距离
-    public float distanceTo(NavigationNode other) {
-        float dx = this.x - other.x;
-        float dz = this.z - other.z;
-        return Conv.asFloat(Math.sqrt(dx * dx + dz * dz));
-    }
-
-    @Override
-    public int compareTo(NavigationNode other) {
-        return this.id.compareTo(other.id);
-    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/Node.java b/servo/src/main/java/com/galaxis/rcs/plan/path/Node.java
new file mode 100644
index 0000000..6b865bf
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/Node.java
@@ -0,0 +1,20 @@
+package com.galaxis.rcs.plan.path;
+
+import java.util.List;
+
+/**
+ * 路径节点定义
+ *
+ * @param id        节点ID
+ * @param x         节点X坐标
+ * @param z         节点Z坐标
+ * @param rotatable 节点是否可旋转
+ * @param neighbors 邻居节点ID列表
+ */
+public record Node(String id,
+                   float x,
+                   float z,
+                   boolean rotatable,
+                   List neighbors,
+                   List storeLinks) {
+}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java b/servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java
deleted file mode 100644
index 3d5926d..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java
+++ /dev/null
@@ -1,33 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-import com.galaxis.rcs.common.enums.OperationSide;
-
-/**
- * 操作位置（取货/放货/充电点）
- */
-public record OperationPoint(
-    NavigationNode node,        // 关联的路标节点
-    String targetId,            // 货位ID (如 "54") 或货架ID (如 "rack1")
-    OperationSide side,         // 方位
-    Integer bay,                // 列 (货架时非空)
-    Integer level,              // 层 (货架时非空)
-    Integer cell                // 格 (货架时非空)
-) {
-    // 计算操作所需方向
-    public int requiredDirection() {
-        return switch (side) {
-            case LEFT -> 90;
-            case RIGHT -> 270;
-            case TOP -> 0;
-            case BOTTOM -> 180;
-        };
-    }
-
-    // 位置唯一标识
-    public String locationKey() {
-        if (bay == null || level == null || cell == null) {
-            return targetId; // 地堆货位
-        }
-        return targetId + "-" + bay + "-" + level + "-" + cell;
-    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java
deleted file mode 100644
index 68c7b0e..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java
+++ /dev/null
@@ -1,15 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-/**
- * 路径段（节点之间的连接）
- */
-public record PathSegment(
-    NavigationNode start,   // 起点节点
-    NavigationNode end,     // 终点节点
-    float distance         // 路径距离
-) {
-    // 路径唯一标识
-    public String key() {
-        return start.id() + "->" + end.id();
-    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java
new file mode 100644
index 0000000..439cd4a
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java
@@ -0,0 +1,183 @@
+package com.galaxis.rcs.plan.path;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+
+import java.util.*;
+
+public class PathUtils {
+
+    /**
+     * 从 LCCDirection 转换为 AMR 的方向单位
+     * 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+     */
+    public static short convertDirectionToPtrDiretion(LCCDirection value) {
+        return switch (value) {
+            case UP -> 3;
+            case DOWN -> 1;
+            case LEFT -> 2;
+            case RIGHT -> 0;
+            default -> throw new IllegalArgumentException("Unknown direction: " + value);
+        };
+    }
+
+    /**
+     * 计算移动方向
+     */
+    public static LCCDirection calculateMoveDirection(Node from, Node to) {
+        float dx = to.x() - from.x();
+        float dz = to.z() - from.z(); // 注意：Z轴向下增长
+
+        // 考虑左手坐标系：X向右，Z向下
+        if (Math.abs(dx) > Math.abs(dz)) {
+            return dx > 0.5 ? LCCDirection.RIGHT : LCCDirection.LEFT;
+        } else {
+            // 注意：Z向下增长，所以Z值增加表示向下移动
+            return dz > 0.5 ? LCCDirection.DOWN : LCCDirection.UP;
+        }
+    }
+
+    /**
+     * 检查方向是否有效（侧插式专用）
+     */
+    public static boolean isValidForSideLoader(Node node, int bay, LCCDirection direction) {
+        Optional link = node.storeLinks().stream()
+            .filter(l -> l.bay() == bay)
+            .findFirst();
+
+        if (link.isEmpty()) return false;
+
+        LCCDirection requiredDirection = convertForSideLoader(link.get().direction());
+        return direction == requiredDirection;
+    }
+
+    /**
+     * 转换货位方向到AGV所需方向（侧插式专用）
+     */
+    public static LCCDirection convertForSideLoader(LCCDirection storeDirection) {
+        /*
+         * 侧插式AGV方向规则：
+         * - 货位在上方 → 车头向右（货叉朝左）
+         * - 货位在下方 → 车头向左（货叉朝左）
+         * - 货位在左方 → 车头向上（货叉朝左）
+         * - 货位在右方 → 车头向下（货叉朝左）
+         */
+        return switch (storeDirection) {
+            case UP -> LCCDirection.RIGHT;
+            case DOWN -> LCCDirection.LEFT;
+            case LEFT -> LCCDirection.UP;
+            case RIGHT -> LCCDirection.DOWN;
+        };
+    }
+
+    /**
+     * 获取最近的旋转点
+     */
+    public static Node findNearestRotationNode(NavigationGraph graph, Node from, LCCDirection currentDir,
+                                               LCCDirection requiredDir) {
+        // 如果当前方向已经匹配，不需要旋转
+        if (currentDir == requiredDir) return from;
+
+        // 使用Dijkstra算法查找最近的旋转点
+        Map distances = new HashMap<>();
+        Map predecessors = new HashMap<>();
+        PriorityQueue queue = new PriorityQueue<>(Comparator.comparingDouble(distances::get));
+
+        distances.put(from, 0f);
+        queue.add(from);
+
+        Node result = null;
+
+        while (!queue.isEmpty()) {
+            Node current = queue.poll();
+
+            // 找到可旋转点
+            if (current.rotatable()) {
+                result = current;
+                break;
+            }
+
+            // 探索邻居
+            for (Node neighbor : graph.getNeighbors(current)) {
+                float newDist = distances.get(current) + graph.distance(current, neighbor);
+
+                if (newDist < distances.getOrDefault(neighbor, Float.MAX_VALUE)) {
+                    distances.put(neighbor, newDist);
+                    predecessors.put(neighbor, current);
+                    queue.add(neighbor);
+                }
+            }
+        }
+
+        return result;
+    }
+
+    /**
+     * 获取相反方向
+     */
+    public static LCCDirection getOppositeDirection(LCCDirection dir) {
+        return switch (dir) {
+            case UP -> LCCDirection.DOWN;
+            case DOWN -> LCCDirection.UP;
+            case LEFT -> LCCDirection.RIGHT;
+            case RIGHT -> LCCDirection.LEFT;
+        };
+    }
+
+    /**
+     * 计算旋转代价
+     */
+    public static float calculateRotationCost(LCCDirection from, LCCDirection to, float ROTATION_COST_PER_DEGREE) {
+        float angle1 = getRequiredDirection(from);
+        float angle2 = getRequiredDirection(to);
+        float diff = Math.abs(angle1 - angle2);
+        diff = Math.min(diff, 360 - diff);
+        // return diff * ROTATION_COST_PER_DEGREE;
+        return 0.1f;
+    }
+
+    /**
+     * 获取所需方向的角度
+     */
+    public static float getRequiredDirection(LCCDirection direction) {
+        return switch (direction) {
+            case UP -> 90f;
+            case DOWN -> 270f;
+            case LEFT -> 180f;
+            case RIGHT -> 0f;
+        };
+    }
+
+    /**
+     * 转换货位方向到AGV所需方向
+     */
+    public static LCCDirection convertStoreDirection(LCCDirection storeDirection) {
+        // 转换规则: 货位在路径点的方位 -> AGV所需方向
+        return switch (storeDirection) {
+            case UP -> LCCDirection.RIGHT;    // 货位在上方 → 车头向右
+            case DOWN -> LCCDirection.LEFT;    // 货位在下方 → 车头向左
+            case LEFT -> LCCDirection.UP;      // 货位在左方 → 车头向上
+            case RIGHT -> LCCDirection.DOWN;   // 货位在右方 → 车头向下
+        };
+    }
+
+    /**
+     * 将角度转换为 LCCDirection
+     */
+    public static LCCDirection convertAngleToDirection(float angle) {
+        // 标准化角度
+        angle = ((angle % 360f) + 360f) % 360f;
+
+        if (angle >= 315f || angle < 45f) return LCCDirection.RIGHT;
+        if (angle >= 45f && angle < 135f) return LCCDirection.UP;
+        if (angle >= 135f && angle < 225f) return LCCDirection.LEFT;
+        return LCCDirection.DOWN;
+    }
+
+    /**
+     * 计算方向角度差
+     */
+    public static float angleDifference(float angle1, float angle2) {
+        float diff = Math.abs(angle1 - angle2) % 360;
+        return diff > 180 ? 360 - diff : diff;
+    }
+}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java
new file mode 100644
index 0000000..665d6f0
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java
@@ -0,0 +1,236 @@
+package com.galaxis.rcs.plan.path;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+import com.galaxis.rcs.plan.PlanTaskSequence;
+import com.galaxis.rcs.plan.task.CarryTask;
+import com.galaxis.rcs.plan.task.LoadTask;
+import com.galaxis.rcs.plan.task.MoveTask;
+import com.galaxis.rcs.plan.task.UnloadTask;
+
+import java.util.List;
+
+import static com.galaxis.rcs.plan.path.PathUtils.convertStoreDirection;
+
+public class PtrPathPlanner {
+    private final NavigationGraph graph;
+    private final AStarPathPlanner astar;
+
+    public PtrPathPlanner(NavigationGraph graph) {
+        this.graph = graph;
+        this.astar = new AStarPathPlanner(graph);
+    }
+
+
+    public void planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) {
+
+        Node node = this.graph.getNodeById(moveTask.targetWayPointId());
+
+        List toPath = astar.findPath(startNodeId, startDirection, node.id(), moveTask.targetDirection());
+        generateMoves(plan, toPath);
+        plan.addFinish();
+    }
+
+    public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) {
+        // 取货点
+        String loadRackId = task.from().rackId();
+        int pickupBay = task.from().bay();
+        NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay);
+        if (loadNodeDirection == null) {
+            throw new RuntimeException("Pickup node not found for rackId=" + loadRackId + ", bay=" + pickupBay);
+        }
+
+        // 放货点
+        String unloadRackId = task.to().rackId();
+        int unloadBay = task.to().bay();
+        NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay);
+        if (unloadNodeDirection == null) {
+            throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay);
+        }
+
+        // 规划到取货点路径
+        List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction());
+
+        // 检查方向是否匹配，如果不匹配则插入旋转点
+         if (!toLoadPath.isEmpty()) {
+             State lastState = toLoadPath.get(toLoadPath.size() - 1);
+             if (lastState.direction() != loadNodeDirection.direction()) {
+                 Node rotationNode = PathUtils.findNearestRotationNode(
+                     graph, lastState.node(), lastState.direction(), loadNodeDirection.direction()
+                 );
+
+                 if (rotationNode != null) {
+                     // 插入旋转路径
+                     List toRotation = astar.findPath(
+                         lastState.node().id(), lastState.direction(),
+                         rotationNode.id(), loadNodeDirection.direction()
+                     );
+                     toLoadPath.addAll(toRotation);
+
+                     // 从旋转点到目标点
+                     List fromRotation = astar.findPath(
+                         rotationNode.id(), loadNodeDirection.direction(),
+                         loadNodeDirection.node().id(), loadNodeDirection.direction()
+                     );
+                     toLoadPath.addAll(fromRotation);
+                 }
+             }
+         }
+
+        // 规划到放货点路径
+        List toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction());
+
+        // 生成指令序列
+        generateMoves(plan, toLoadPath);
+        plan.addLoad(task.lpn(), loadRackId, pickupBay, task.from().level(), task.from().cell());
+
+        generateMoves(plan, toUnloadPath);
+        plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell());
+
+        plan.addFinish();
+    }
+
+
+    public void planUnloadTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, UnloadTask task) {
+        Node fromNode = this.graph.getNodeById(startNodeId);
+
+        // 放货点
+        String unloadRackId = task.to().rackId();
+        int unloadBay = task.to().bay();
+        NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay);
+        if (unloadNodeDirection == null) {
+            throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay);
+        }
+
+        // 规划到放货点路径
+        List toUnloadPath = astar.findPath(startNodeId, startDirection, unloadNodeDirection.node().id(), unloadNodeDirection.direction());
+
+        // 检查方向是否匹配，如果不匹配则插入旋转点
+        // State lastState = toUnloadPath.get(toUnloadPath.size() - 1);
+        // if (startDirection != lastState.direction()) {
+        //     Node rotationNode = PathUtils.findNearestRotationNode(
+        //         graph, fromNode, startDirection, lastState.direction()
+        //     );
+        //
+        //     if (rotationNode != null) {
+        //         // 插入旋转路径
+        //         List toRotation = astar.findPath(
+        //             startNodeId, startDirection,
+        //             rotationNode.id(), unloadNodeDirection.direction()
+        //         );
+        //         toUnloadPath.addAll(toRotation);
+        //
+        //         // 从旋转点到目标点
+        //         List fromRotation = astar.findPath(
+        //             rotationNode.id(), unloadNodeDirection.direction(),
+        //             unloadNodeDirection.node().id(), unloadNodeDirection.direction()
+        //         );
+        //         toUnloadPath.addAll(fromRotation);
+        //     }
+        // }
+
+        // 生成指令序列
+        generateMoves(plan, toUnloadPath);
+        plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell());
+
+        plan.addFinish();
+    }
+
+
+    public void planLoadTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, LoadTask task) {
+        Node fromNode = this.graph.getNodeById(startNodeId);
+
+        // 放货点
+        String loadRackId = task.to().rackId();
+        int unloadBay = task.to().bay();
+        NodeDirection loadNodeDirection = findNodeForStore(loadRackId, unloadBay);
+        if (loadNodeDirection == null) {
+            throw new RuntimeException("Drop node not found for rackId=" + loadRackId + ", bay=" + unloadBay);
+        }
+
+        // 规划到取货点路径
+        List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction());
+
+        // 检查方向是否匹配，如果不匹配则插入旋转点
+        // State lastState = toLoadPath.get(toLoadPath.size() - 1);
+        // if (startDirection != lastState.direction()) {
+        //     Node rotationNode = PathUtils.findNearestRotationNode(
+        //         graph, fromNode, startDirection, lastState.direction()
+        //     );
+        //
+        //     if (rotationNode != null) {
+        //         // 插入旋转路径
+        //         List toRotation = astar.findPath(
+        //             startNodeId, startDirection,
+        //             rotationNode.id(), loadNodeDirection.direction()
+        //         );
+        //         toLoadPath.addAll(toRotation);
+        //
+        //         // 从旋转点到目标点
+        //         List fromRotation = astar.findPath(
+        //             rotationNode.id(), loadNodeDirection.direction(),
+        //             loadNodeDirection.node().id(), loadNodeDirection.direction()
+        //         );
+        //         toLoadPath.addAll(fromRotation);
+        //     }
+        // }
+
+        // 生成指令序列
+        generateMoves(plan, toLoadPath);
+        plan.addLoad("N/A", loadRackId, task.to().level(), task.to().bay(), task.to().cell());
+
+        plan.addFinish();
+    }
+
+    private NodeDirection findNodeForStore(String storeId, int bay) {
+        List nodes = this.graph.getNodesForStore(storeId);
+        for (Node node : nodes) {
+            for (StoreLink link : node.storeLinks()) {
+                if (link.storeId().equals(storeId) && link.bay() == bay) {
+                    LCCDirection agvDirection = convertStoreDirection(link.direction());
+                    return new NodeDirection(node, agvDirection);
+                }
+            }
+        }
+        return null;
+    }
+
+    /**
+     * 根据A*状态，生成移动指令序列
+     */
+    private void generateMoves(PlanTaskSequence sequence, List path) {
+        if (path.isEmpty()) return;
+
+        // 第一个状态是起点，跳过
+        State prevState = path.get(0);
+
+        for (int i = 1; i < path.size(); i++) {
+            State current = path.get(i);
+
+            // 如果是旋转动作
+            if (current.node().equals(prevState.node())) {
+                float angle = PathUtils.getRequiredDirection(current.direction());
+                sequence.addRotationTo(angle);
+            }
+            // 移动动作 - 检查是否需要后退
+            else {
+                // 检查移动方向
+                LCCDirection moveDir = PathUtils.calculateMoveDirection(prevState.node(), current.node());
+
+                // 如果移动方向与车头方向相反，需要后退
+                boolean isBackward = (current.direction() == PathUtils.getOppositeDirection(moveDir));
+
+                if (isBackward) {
+                    sequence.addMoveBackward(current.node().id());
+                } else {
+                    sequence.addMoveTo(current.node().id());
+                }
+            }
+            prevState = current;
+        }
+    }
+
+
+    // 辅助记录类
+    private record NodeDirection(Node node, LCCDirection direction) {
+    }
+}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/State.java b/servo/src/main/java/com/galaxis/rcs/plan/path/State.java
new file mode 100644
index 0000000..bc9a087
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/State.java
@@ -0,0 +1,19 @@
+package com.galaxis.rcs.plan.path;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+
+/**
+ * A* 路径规划状态
+ */
+public record State(Node node,
+                    LCCDirection direction,
+                    float g,
+                    float h,
+                    State parent)
+    implements Comparable {
+
+    @Override
+    public int compareTo(State other) {
+        return Float.compare(g + h, other.g + other.h);
+    }
+}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java b/servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java
new file mode 100644
index 0000000..d94923e
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java
@@ -0,0 +1,17 @@
+package com.galaxis.rcs.plan.path;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+
+/**
+ * A* 路径与库位的连接定义
+ *
+ * @param storeId   库位ID
+ * @param bay       列
+ * @param level     层
+ * @param cell      格
+ * @param direction 方位（路径相对于库位的方位，比如路标的"左边"是库位）
+ */
+public record StoreLink(
+    String storeId, int bay, int level, int cell, LCCDirection direction
+) {
+}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java
deleted file mode 100644
index 7b9a332..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java
+++ /dev/null
@@ -1,220 +0,0 @@
-package com.galaxis.rcs.plan.path2;
-
-import com.galaxis.rcs.common.enums.LCCDirection;
-import com.google.common.collect.Maps;
-
-import java.util.*;
-
-import static com.galaxis.rcs.plan.path2.PathUtils.*;
-
-public class AStarPathPlanner {
-    private static final float ROTATION_COST_PER_DEGREE = 0.01f;
-    private static final float BLOCKED_COST = 10000f; // 阻塞系数成本
-    private static final float WEIGHT_FACTOR = 1.5f; // 权重因子
-
-    private final NavigationGraph graph;
-    private final Map nodeWeights = Maps.newConcurrentMap();
-    private final Map blockedNodes = Maps.newConcurrentMap();
-
-    public AStarPathPlanner(NavigationGraph graph) {
-        this.graph = graph;
-    }
-
-    // 路径规划状态
-    public List findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) {
-        Node start = graph.getNode(startId);
-        Node end = graph.getNode(endId);
-        if (start == null) {
-            throw new RuntimeException("Start node not found: " + startId);
-        }
-        if (end == null) {
-            throw new RuntimeException("End node not found: " + endId);
-        }
-
-        // 使用复合键避免状态重复
-        Map visited = new HashMap<>();
-        PriorityQueue open = new PriorityQueue<>();
-
-        // 初始状态
-        State initialState = new State(start, startDirection, 0, heuristic(start, end), null);
-        open.add(initialState);
-        visited.put(stateKey(start.id(), startDirection), initialState);
-
-        while (!open.isEmpty()) {
-            State current = open.poll();
-
-            // 到达目标节点且方向匹配
-            if (current.node().id().equals(endId) && current.direction() == endDirection) {
-                return buildPath(current);
-            }
-
-            // 处理邻边移动
-            for (Node neighbor : graph.getNeighbors(current.node())) {
-                // 检查节点是否被阻塞
-                if (isBlocked(neighbor.id())) continue;
-
-                // 计算移动方向
-                LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
-
-                // 尝试前进
-                if (canMoveForward(current.direction(), moveDirection)) {
-                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
-                    considerState(current, neighbor, current.direction(),
-                        moveCost, open, visited, end);
-                }
-                // 尝试后退
-                else if (canMoveBackward(current.direction(), moveDirection)) {
-                    float moveCost = calculateMoveCost(current.node(), neighbor, true);
-                    considerState(current, neighbor, current.direction(),
-                        moveCost, open, visited, end);
-                }
-                // 需要旋转
-                else if (current.node().rotatable()) {
-                    // 计算需要旋转到的方向
-                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
-
-                    // 考虑旋转后移动
-                    float rotationCost = calculateRotationCost(
-                        current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
-                    );
-                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
-                    float totalCost = rotationCost + moveCost;
-
-                    // 创建旋转状态
-                    State rotatedState = new State(
-                        current.node(), requiredDirection,
-                        current.g() + rotationCost,
-                        heuristic(current.node(), end),
-                        current
-                    );
-
-                    // 考虑旋转后的移动
-                    considerState(rotatedState, neighbor, requiredDirection,
-                        moveCost, open, visited, end);
-                }
-            }
-
-            // 处理原地旋转 - 只考虑目标方向
-            if (current.node().rotatable()) {
-                // 只考虑旋转到目标方向（如果可能）
-                if (current.direction() != endDirection) {
-                    float rotationCost = calculateRotationCost(
-                        current.direction(), endDirection, ROTATION_COST_PER_DEGREE
-                    );
-                    considerState(current, current.node(), endDirection,
-                        rotationCost, open, visited, end);
-                }
-
-                // 另外考虑旋转到移动所需的方向
-                for (Node neighbor : graph.getNeighbors(current.node())) {
-                    LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
-                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
-
-                    if (requiredDirection != current.direction()) {
-                        float rotationCost = calculateRotationCost(
-                            current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
-                        );
-                        considerState(current, current.node(), requiredDirection,
-                            rotationCost, open, visited, end);
-                    }
-                }
-
-                // for (LCCDirection rotation : LCCDirection.values()) {
-                //     if (rotation == current.direction()) continue;
-                //
-                //     float rotationCost = calculateRotationCost(
-                //         current.direction(), rotation, ROTATION_COST_PER_DEGREE
-                //     );
-                //     considerState(current, current.node(), rotation,
-                //         rotationCost, open, visited, end);
-                // }
-            }
-        }
-        return Collections.emptyList();
-    }
-
-    /**
-     * 考虑新的状态并更新开放列表和访问记录
-     *
-     * @param current       当前状态
-     * @param nextNode      下一个节点
-     * @param nextDirection 下一个方向
-     * @param cost          移动成本
-     * @param open          开放列表
-     * @param visited       访问记录
-     * @param end           目标节点
-     */
-    private void considerState(State current, Node nextNode, LCCDirection nextDirection,
-                               float cost, PriorityQueue open,
-                               Map visited, Node end) {
-        String key = stateKey(nextNode.id(), nextDirection);
-        float newG = current.g() + cost;
-
-        if (!visited.containsKey(key) || visited.get(key).g() > newG) {
-            float h = heuristic(nextNode, end);
-            State newState = new State(nextNode, nextDirection, newG, h, current);
-            open.add(newState);
-            visited.put(key, newState);
-        }
-    }
-
-    private List buildPath(State state) {
-        LinkedList path = new LinkedList<>();
-        while (state != null) {
-            path.addFirst(state);
-            state = state.parent();
-        }
-        return path;
-    }
-
-    /**
-     * 启发式函数，计算两个节点之间的距离
-     */
-    private float heuristic(Node a, Node b) {
-        return graph.distance(a, b);
-        // 使用曼哈顿距离？？
-        // return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z());
-    }
-
-    /**
-     * 生成状态的唯一键
-     */
-    private String stateKey(String nodeId, LCCDirection direction) {
-        return nodeId + "|" + direction;
-    }
-
-    public void setNodeWeight(String nodeId, float weight) {
-        nodeWeights.put(nodeId, weight);
-    }
-
-    public void setBlocked(String nodeId, float blockedFactor) {
-        blockedNodes.put(nodeId, blockedFactor);
-    }
-
-    private float calculateMoveCost(Node from, Node to, boolean isBackward) {
-        float baseCost = graph.distance(from, to);
-        float weight = nodeWeights.getOrDefault(to.id(), 1.0f);
-        float blockedFactor = blockedNodes.getOrDefault(to.id(), 0f);
-
-        // 后退移动增加额外成本??
-        // return baseCost * weight * (1 + blockedFactor) * (isBackward ? 1.2f : 1.0f);
-        return baseCost * weight * (1 + blockedFactor);
-    }
-
-    private boolean isBlocked(String nodeId) {
-        return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f;
-    }
-
-    private boolean canMoveForward(LCCDirection currentDir, LCCDirection moveDir) {
-        return currentDir == moveDir;
-    }
-
-    private boolean canMoveBackward(LCCDirection currentDir, LCCDirection moveDir) {
-        return currentDir == getOppositeDirection(moveDir);
-    }
-
-    private LCCDirection calculateRequiredDirection(LCCDirection moveDirection) {
-        // 侧插式AGV只能前进或后退，不能横移
-        return moveDirection;
-    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java
deleted file mode 100644
index bb1722d..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java
+++ /dev/null
@@ -1,147 +0,0 @@
-package com.galaxis.rcs.plan.path2;
-
-import com.galaxis.rcs.common.enums.LCCDirection;
-import com.google.common.collect.Maps;
-import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.StaticItem;
-import lombok.extern.slf4j.Slf4j;
-
-import java.util.*;
-
-/**
- * A* 导航图
- */
-@Slf4j
-public class NavigationGraph {
-    private final LogisticsRuntime runtime;
-
-    /**
-     * 缓存距离计算结果，避免重复计算
-     */
-    private final Map distanceCache = Maps.newConcurrentMap();
-
-    /**
-     * 缓存邻居节点列表，避免重复查询
-     */
-    private final Map> neighborCache = Maps.newConcurrentMap();
-
-    /**
-     * 添加路径缓存
-     */
-    private final Map> pathCache = Maps.newConcurrentMap();
-
-    private final Map nodeMap = new HashMap<>();
-    private final Map> storeToNodes = new HashMap<>();
-
-    public NavigationGraph(LogisticsRuntime runtime) {
-        this.runtime = runtime;
-    }
-
-    public List getCachedPath(String startId, LCCDirection startDirection,
-                                     String endId, LCCDirection endDirection) {
-        String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection;
-        return pathCache.get(cacheKey);
-    }
-
-    public void cachePath(String startId, LCCDirection startDirection,
-                          String endId, LCCDirection endDirection,
-                          List path) {
-        String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection;
-        pathCache.put(cacheKey, path);
-    }
-
-    public Node getNodeById(String nodeId){
-        return nodeMap.get(nodeId);
-    }
-
-    public List getNodesForStore(String storeId) {
-        List nodes = new ArrayList<>();
-        List nodeIds = storeToNodes.get(storeId);
-        if (nodeIds != null) {
-            for (String id : nodeIds) {
-                Node node = nodeMap.get(id);
-                if (node != null) {
-                    nodes.add(node);
-                }
-            }
-        }
-        return nodes;
-    }
-
-    public List getNeighbors(Node node) {
-        return node.neighbors().stream()
-            .map(nodeMap::get)
-            .filter(Objects::nonNull)
-            .toList();
-    }
-
-    public float distance(Node a, Node b) {
-        float dx = a.x() - b.x();
-        float dz = a.z() - b.z();
-        return (float) Math.sqrt(dx * dx + dz * dz);
-    }
-
-
-    public void init() {
-        var items = runtime.getStaticItems();
-        for (StaticItem item : items) {
-            if ("way".equals(item.getT())) {
-                float[][] tf = item.getTf();
-                String id = item.getId();
-                Map dt = item.getDt();
-
-                // 提取坐标
-                float x = tf[0][0];
-                float z = tf[0][2]; // Z向下增长
-
-                // 检查可旋转性
-                boolean rotatable = dt.containsKey("agvRotation");
-                if (rotatable) {
-                    log.info("Node {} is rotatable", id);
-                }
-
-                // 提取邻居节点
-                List in = (List) dt.get("in");
-                List out = (List) dt.get("out");
-                Set neighbors = new HashSet<>();
-                if (in != null) neighbors.addAll(in);
-                if (out != null) neighbors.addAll(out);
-
-                // 提取货位链接
-                List storeLinks = new ArrayList<>();
-                List> links = (List>) dt.get("linkStore");
-                if (links != null) {
-                    for (Map link : links) {
-                        String storeId = (String) link.get("item");
-                        int bay = (Integer) link.get("bay");
-                        int level = (Integer) link.get("level");
-                        int cell = (Integer) link.get("cell");
-                        String direction = (String) link.get("direction");
-                        storeLinks.add(new StoreLink(storeId, bay, level, cell, LCCDirection.fromString(direction)));
-
-                        // 建立货位到节点的反向映射
-                        storeToNodes.computeIfAbsent(storeId, k -> new ArrayList<>()).add(id);
-                    }
-                }
-
-                nodeMap.put(id, new Node(id, x, z, rotatable,
-                    new ArrayList<>(neighbors), storeLinks));
-            }
-        }
-
-        // 2. 验证邻居双向连接
-        for (Node node : nodeMap.values()) {
-            Iterator it = node.neighbors().iterator();
-            while (it.hasNext()) {
-                String neighborId = it.next();
-                if (!nodeMap.containsKey(neighborId)) {
-                    it.remove(); // 移除无效邻居
-                }
-            }
-        }
-    }
-
-    public Node getNode(String id) {
-        return nodeMap.get(id);
-    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java
deleted file mode 100644
index 252bb11..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java
+++ /dev/null
@@ -1,20 +0,0 @@
-package com.galaxis.rcs.plan.path2;
-
-import java.util.List;
-
-/**
- * 路径节点定义
- *
- * @param id        节点ID
- * @param x         节点X坐标
- * @param z         节点Z坐标
- * @param rotatable 节点是否可旋转
- * @param neighbors 邻居节点ID列表
- */
-public record Node(String id,
-                   float x,
-                   float z,
-                   boolean rotatable,
-                   List neighbors,
-                   List storeLinks) {
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java
deleted file mode 100644
index cfffe83..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java
+++ /dev/null
@@ -1,183 +0,0 @@
-package com.galaxis.rcs.plan.path2;
-
-import com.galaxis.rcs.common.enums.LCCDirection;
-
-import java.util.*;
-
-public class PathUtils {
-
-    /**
-     * 从 LCCDirection 转换为 AMR 的方向单位
-     * 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-     */
-    public static short convertDirectionToPtrDiretion(LCCDirection value) {
-        return switch (value) {
-            case UP -> 3;
-            case DOWN -> 1;
-            case LEFT -> 2;
-            case RIGHT -> 0;
-            default -> throw new IllegalArgumentException("Unknown direction: " + value);
-        };
-    }
-
-    /**
-     * 计算移动方向
-     */
-    public static LCCDirection calculateMoveDirection(Node from, Node to) {
-        float dx = to.x() - from.x();
-        float dz = to.z() - from.z(); // 注意：Z轴向下增长
-
-        // 考虑左手坐标系：X向右，Z向下
-        if (Math.abs(dx) > Math.abs(dz)) {
-            return dx > 0.5 ? LCCDirection.RIGHT : LCCDirection.LEFT;
-        } else {
-            // 注意：Z向下增长，所以Z值增加表示向下移动
-            return dz > 0.5 ? LCCDirection.DOWN : LCCDirection.UP;
-        }
-    }
-
-    /**
-     * 检查方向是否有效（侧插式专用）
-     */
-    public static boolean isValidForSideLoader(Node node, int bay, LCCDirection direction) {
-        Optional link = node.storeLinks().stream()
-            .filter(l -> l.bay() == bay)
-            .findFirst();
-
-        if (link.isEmpty()) return false;
-
-        LCCDirection requiredDirection = convertForSideLoader(link.get().direction());
-        return direction == requiredDirection;
-    }
-
-    /**
-     * 转换货位方向到AGV所需方向（侧插式专用）
-     */
-    public static LCCDirection convertForSideLoader(LCCDirection storeDirection) {
-        /*
-         * 侧插式AGV方向规则：
-         * - 货位在上方 → 车头向右（货叉朝左）
-         * - 货位在下方 → 车头向左（货叉朝左）
-         * - 货位在左方 → 车头向上（货叉朝左）
-         * - 货位在右方 → 车头向下（货叉朝左）
-         */
-        return switch (storeDirection) {
-            case UP -> LCCDirection.RIGHT;
-            case DOWN -> LCCDirection.LEFT;
-            case LEFT -> LCCDirection.UP;
-            case RIGHT -> LCCDirection.DOWN;
-        };
-    }
-
-    /**
-     * 获取最近的旋转点
-     */
-    public static Node findNearestRotationNode(NavigationGraph graph, Node from, LCCDirection currentDir,
-                                               LCCDirection requiredDir) {
-        // 如果当前方向已经匹配，不需要旋转
-        if (currentDir == requiredDir) return from;
-
-        // 使用Dijkstra算法查找最近的旋转点
-        Map distances = new HashMap<>();
-        Map predecessors = new HashMap<>();
-        PriorityQueue queue = new PriorityQueue<>(Comparator.comparingDouble(distances::get));
-
-        distances.put(from, 0f);
-        queue.add(from);
-
-        Node result = null;
-
-        while (!queue.isEmpty()) {
-            Node current = queue.poll();
-
-            // 找到可旋转点
-            if (current.rotatable()) {
-                result = current;
-                break;
-            }
-
-            // 探索邻居
-            for (Node neighbor : graph.getNeighbors(current)) {
-                float newDist = distances.get(current) + graph.distance(current, neighbor);
-
-                if (newDist < distances.getOrDefault(neighbor, Float.MAX_VALUE)) {
-                    distances.put(neighbor, newDist);
-                    predecessors.put(neighbor, current);
-                    queue.add(neighbor);
-                }
-            }
-        }
-
-        return result;
-    }
-
-    /**
-     * 获取相反方向
-     */
-    public static LCCDirection getOppositeDirection(LCCDirection dir) {
-        return switch (dir) {
-            case UP -> LCCDirection.DOWN;
-            case DOWN -> LCCDirection.UP;
-            case LEFT -> LCCDirection.RIGHT;
-            case RIGHT -> LCCDirection.LEFT;
-        };
-    }
-
-    /**
-     * 计算旋转代价
-     */
-    public static float calculateRotationCost(LCCDirection from, LCCDirection to, float ROTATION_COST_PER_DEGREE) {
-        float angle1 = getRequiredDirection(from);
-        float angle2 = getRequiredDirection(to);
-        float diff = Math.abs(angle1 - angle2);
-        diff = Math.min(diff, 360 - diff);
-        // return diff * ROTATION_COST_PER_DEGREE;
-        return 0.1f;
-    }
-
-    /**
-     * 获取所需方向的角度
-     */
-    public static float getRequiredDirection(LCCDirection direction) {
-        return switch (direction) {
-            case UP -> 90f;
-            case DOWN -> 270f;
-            case LEFT -> 180f;
-            case RIGHT -> 0f;
-        };
-    }
-
-    /**
-     * 转换货位方向到AGV所需方向
-     */
-    public static LCCDirection convertStoreDirection(LCCDirection storeDirection) {
-        // 转换规则: 货位在路径点的方位 -> AGV所需方向
-        return switch (storeDirection) {
-            case UP -> LCCDirection.RIGHT;    // 货位在上方 → 车头向右
-            case DOWN -> LCCDirection.LEFT;    // 货位在下方 → 车头向左
-            case LEFT -> LCCDirection.UP;      // 货位在左方 → 车头向上
-            case RIGHT -> LCCDirection.DOWN;   // 货位在右方 → 车头向下
-        };
-    }
-
-    /**
-     * 将角度转换为 LCCDirection
-     */
-    public static LCCDirection convertAngleToDirection(float angle) {
-        // 标准化角度
-        angle = ((angle % 360f) + 360f) % 360f;
-
-        if (angle >= 315f || angle < 45f) return LCCDirection.RIGHT;
-        if (angle >= 45f && angle < 135f) return LCCDirection.UP;
-        if (angle >= 135f && angle < 225f) return LCCDirection.LEFT;
-        return LCCDirection.DOWN;
-    }
-
-    /**
-     * 计算方向角度差
-     */
-    public static float angleDifference(float angle1, float angle2) {
-        float diff = Math.abs(angle1 - angle2) % 360;
-        return diff > 180 ? 360 - diff : diff;
-    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java
deleted file mode 100644
index 309dfba..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java
+++ /dev/null
@@ -1,143 +0,0 @@
-package com.galaxis.rcs.plan.path2;
-
-import com.galaxis.rcs.common.enums.LCCDirection;
-import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.plan.task.CarryTask;
-import com.galaxis.rcs.plan.task.MoveTask;
-
-import java.util.List;
-
-import static com.galaxis.rcs.plan.path2.PathUtils.convertStoreDirection;
-import static com.galaxis.rcs.plan.path2.PathUtils.getRequiredDirection;
-
-public class PtrPathPlanner {
-    private final NavigationGraph graph;
-    private final AStarPathPlanner astar;
-
-    public PtrPathPlanner(NavigationGraph graph) {
-        this.graph = graph;
-        this.astar = new AStarPathPlanner(graph);
-    }
-
-
-    public void planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) {
-
-        Node node = this.graph.getNodeById(moveTask.targetWayPointId());
-
-        List toPath = astar.findPath(startNodeId, startDirection, node.id(), startDirection);
-        generateMoves(plan, toPath);
-        plan.addFinish();
-    }
-
-    public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) {
-        // 取货点
-        String loadRackId = task.from().rackId();
-        int pickupBay = task.from().bay();
-        NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay);
-        if (loadNodeDirection == null) {
-            throw new RuntimeException("Pickup node not found for rackId=" + loadRackId + ", bay=" + pickupBay);
-        }
-
-        // 放货点
-        String unloadRackId = task.to().rackId();
-        int unloadBay = task.to().bay();
-        NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay);
-        if (unloadNodeDirection == null) {
-            throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay);
-        }
-
-        // 规划到取货点路径
-        List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction());
-
-        // 检查方向是否匹配，如果不匹配则插入旋转点
-        if (!toLoadPath.isEmpty()) {
-            State lastState = toLoadPath.get(toLoadPath.size() - 1);
-            if (lastState.direction() != loadNodeDirection.direction()) {
-                Node rotationNode = PathUtils.findNearestRotationNode(
-                    graph, lastState.node(), lastState.direction(), loadNodeDirection.direction()
-                );
-
-                if (rotationNode != null) {
-                    // 插入旋转路径
-                    List toRotation = astar.findPath(
-                        lastState.node().id(), lastState.direction(),
-                        rotationNode.id(), loadNodeDirection.direction()
-                    );
-                    toLoadPath.addAll(toRotation);
-
-                    // 从旋转点到目标点
-                    List fromRotation = astar.findPath(
-                        rotationNode.id(), loadNodeDirection.direction(),
-                        loadNodeDirection.node().id(), loadNodeDirection.direction()
-                    );
-                    toLoadPath.addAll(fromRotation);
-                }
-            }
-        }
-
-        // 规划到放货点路径
-        List toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction());
-
-        // 生成指令序列
-        generateMoves(plan, toLoadPath);
-        plan.addLoad(task.lpn(), loadRackId, pickupBay, task.from().level(), task.from().cell());
-
-        generateMoves(plan, toUnloadPath);
-        plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell());
-
-        plan.addFinish();
-    }
-
-    private NodeDirection findNodeForStore(String storeId, int bay) {
-        List nodes = this.graph.getNodesForStore(storeId);
-        for (Node node : nodes) {
-            for (StoreLink link : node.storeLinks()) {
-                if (link.storeId().equals(storeId) && link.bay() == bay) {
-                    LCCDirection agvDirection = convertStoreDirection(link.direction());
-                    return new NodeDirection(node, agvDirection);
-                }
-            }
-        }
-        return null;
-    }
-
-    /**
-     * 根据A*状态，生成移动指令序列
-     */
-    private void generateMoves(PlanTaskSequence sequence, List path) {
-        if (path.isEmpty()) return;
-
-        // 第一个状态是起点，跳过
-        State prevState = path.get(0);
-
-        for (int i = 1; i < path.size(); i++) {
-            State current = path.get(i);
-
-            // 如果是旋转动作
-            if (current.node().equals(prevState.node())) {
-                float angle = PathUtils.getRequiredDirection(current.direction());
-                sequence.addRotationTo(angle);
-            }
-            // 移动动作 - 检查是否需要后退
-            else {
-                // 检查移动方向
-                LCCDirection moveDir = PathUtils.calculateMoveDirection(prevState.node(), current.node());
-
-                // 如果移动方向与车头方向相反，需要后退
-                boolean isBackward = (current.direction() == PathUtils.getOppositeDirection(moveDir));
-
-                if (isBackward) {
-                    sequence.addMoveBackward(current.node().id());
-                } else {
-                    sequence.addMoveTo(current.node().id());
-                }
-            }
-            prevState = current;
-        }
-    }
-
-
-    // 辅助记录类
-    private record NodeDirection(Node node, LCCDirection direction) {
-    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java
deleted file mode 100644
index bd7f3d7..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java
+++ /dev/null
@@ -1,19 +0,0 @@
-package com.galaxis.rcs.plan.path2;
-
-import com.galaxis.rcs.common.enums.LCCDirection;
-
-/**
- * A* 路径规划状态
- */
-public record State(Node node,
-                    LCCDirection direction,
-                    float g,
-                    float h,
-                    State parent)
-    implements Comparable {
-
-    @Override
-    public int compareTo(State other) {
-        return Float.compare(g + h, other.g + other.h);
-    }
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java
deleted file mode 100644
index f171d5d..0000000
--- a/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java
+++ /dev/null
@@ -1,17 +0,0 @@
-package com.galaxis.rcs.plan.path2;
-
-import com.galaxis.rcs.common.enums.LCCDirection;
-
-/**
- * A* 路径与库位的连接定义
- *
- * @param storeId   库位ID
- * @param bay       列
- * @param level     层
- * @param cell      格
- * @param direction 方位（路径相对于库位的方位，比如路标的"左边"是库位）
- */
-public record StoreLink(
-    String storeId, int bay, int level, int cell, LCCDirection direction
-) {
-}
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java b/servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java
new file mode 100644
index 0000000..ead42d5
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java
@@ -0,0 +1,15 @@
+package com.galaxis.rcs.plan.task;
+
+import com.galaxis.rcs.common.entity.StoreLocation;
+
+/**
+ * 装入任务
+ */
+public record LoadTask(
+    String agv,
+    int priority,
+    StoreLocation to
+) {
+}
+
+
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java b/servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java
index 43fecf0..427251b 100644
--- a/servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java
+++ b/servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java
@@ -1,8 +1,11 @@
 package com.galaxis.rcs.plan.task;
 
+import com.galaxis.rcs.common.enums.LCCDirection;
+
 public record MoveTask(
     String agv,
     String targetWayPointId,
+    LCCDirection targetDirection,
     int priority
 ) {
 }
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java b/servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java
new file mode 100644
index 0000000..1ade43a
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java
@@ -0,0 +1,15 @@
+package com.galaxis.rcs.plan.task;
+
+import com.galaxis.rcs.common.entity.StoreLocation;
+
+/**
+ * 卸载任务
+ */
+public record UnloadTask(
+    String agv,
+    int priority,
+    StoreLocation to
+) {
+}
+
+
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java b/servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java
index 5f013c8..a0c3bc8 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java
@@ -2,7 +2,6 @@ package com.galaxis.rcs.ptr;
 
 import com.galaxis.rcs.common.enums.AgvEventType;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.extern.slf4j.Slf4j;
 
 import java.util.List;
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
index eb41787..8106a7e 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
@@ -11,7 +11,6 @@ import com.google.common.collect.Maps;
 import com.google.common.collect.Sets;
 import com.yvan.logisticsEnv.EnvPayload;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.SneakyThrows;
 import lombok.extern.slf4j.Slf4j;
 import org.clever.core.BannerUtils;
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java
new file mode 100644
index 0000000..859cca1
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java
@@ -0,0 +1,5 @@
+package com.galaxis.rcs.ptr;
+
+public abstract class PtrAgvConnector {
+
+}
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java
new file mode 100644
index 0000000..472c753
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java
@@ -0,0 +1,167 @@
+package com.galaxis.rcs.ptr;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
+import com.galaxis.rcs.ptr.sendEntity.RcsTaskMessage;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import lombok.extern.slf4j.Slf4j;
+import org.eclipse.paho.mqttv5.common.MqttException;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.locks.LockSupport;
+
+@Slf4j
+public class PtrAgvConnectorThread 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;
+    private final LogisticsRuntime logisticsRuntime;
+    private volatile int __currentTaskSeqNo = 0;
+    volatile PtrAgvDeviceTask __currentTask;
+
+    public PtrAgvConnectorThread(PtrAgvItem ptrAgvItem, Cl2DeviceConnector cl2DeviceConnector, LogisticsRuntime logisticsRuntime) {
+        super("ExecutorConnector-" + ptrAgvItem.getId());
+        this.cl2DeviceConnector = cl2DeviceConnector;
+        this.ptrAgvItem = ptrAgvItem;
+        this.logisticsRuntime = logisticsRuntime;
+    }
+
+    @Override
+    public void run() {
+        running.set(true);
+        log.info("Connector thread started for executor: {}", this.ptrAgvItem.getId());
+
+        try {
+            float distance = 0;
+            int taskCount = 0;
+
+            PtrAgvDeviceTask startTask = null;
+            RcsTaskMessage taskMessage = null;
+
+            // 计算中的任务
+            List computingTaskList = new ArrayList<>();
+            while (running.get()) {
+                if (paused.get()) {
+                    synchronized (pauseLock) {
+                        while (paused.get()) {
+                            pauseLock.wait();
+                        }
+                    }
+                }
+
+                PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take();
+                if (startTask == null) {
+                    startTask = currentTask;
+                    taskMessage = new RcsTaskMessage(this.logisticsRuntime);
+                    taskMessage.OperationType = startTask.operationType;
+                    taskMessage.PickMode = startTask.pickMode;
+                    taskMessage.GoNow = true;
+                    taskMessage.StartX = startTask.startPoint.logicX;
+                    taskMessage.StartY = startTask.startPoint.logicY;
+                    taskMessage.Link = new ArrayList<>();
+                }
+                currentTask.seqNo = taskMessage.SeqNo;
+                RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
+                taskMessage.Link.add(link);
+                taskCount++;
+                distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]);
+                computingTaskList.add(currentTask);
+
+
+                PtrAgvDeviceTask 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
+                    || (distance > 2 && taskCount > 1)) {
+                    taskMessage.OperationType = currentTask.operationType;
+                    taskMessage.PickMode = currentTask.pickMode;
+                    taskMessage.EndX = currentTask.groupEndPoint.logicX;
+                    taskMessage.EndY = currentTask.groupEndPoint.logicY;
+                    taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection;
+                    taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight;
+                    taskMessage.LinkCounts = (short) taskMessage.Link.size();
+                    taskMessage.ChargeDirection = currentTask.chargeDirection;
+                    taskMessage.ChargeLocation = currentTask.chargeLocation;
+                    try {
+                        // 发送任务
+                        this.__currentTaskSeqNo = taskMessage.SeqNo;
+                        this.__currentTask = currentTask;
+                        cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage);
+                        this.ptrAgvItem.runningDeviceTaskList.addAll(computingTaskList);
+                        for (PtrAgvDeviceTask task : computingTaskList) {
+                            task.taskStatus = 1;
+                            task.taskGroupStatus = 1;
+                        }
+                        computingTaskList.clear();
+                    } catch (MqttException | JsonProcessingException e) {
+                        log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.movePlanTaskId, e);
+                    }
+                    distance = 0;
+                    taskCount = 0;
+                    taskMessage = null;
+                    startTask = null;
+
+                    if (currentTask.isGroupEnd) {
+                        // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒
+                        LockSupport.park(); // 阻塞当前线程
+                    } else {
+                        for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
+                            if (task.taskGroupStatus < 4) {
+                                LockSupport.park(); // 阻塞当前线程
+                                break;
+                            }
+                        }
+                    }
+                }
+
+            }
+        } catch (InterruptedException e) {
+            System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId());
+        } finally {
+            System.out.println("Connector thread stopped for executor: " + this.ptrAgvItem.getId());
+        }
+    }
+
+    public int getCurrentTaskSeqNo() {
+        return __currentTaskSeqNo;
+    }
+
+    /**
+     * 停止连接器线程
+     */
+    public void stopConnector() {
+        running.set(false);
+        this.interrupt(); // 中断阻塞状态
+    }
+
+    public void pauseProcessing() {
+        paused.set(true);
+    }
+
+    public void resumeProcessing() {
+        paused.set(false);
+        synchronized (pauseLock) {
+            pauseLock.notifyAll();
+        }
+    }
+
+    /**
+     * 检查连接器是否在运行
+     */
+    public boolean isRunning() {
+        return running.get();
+    }
+
+    public static float euclideanDistance(float[] p1, float[] p2) {
+        if (p1.length != 3 || p2.length != 3) throw new IllegalArgumentException();
+        float dx = p1[0] - p2[0];
+        float dy = p1[1] - p2[1];
+        float dz = p1[2] - p2[2];
+        return (float) Math.sqrt(dx * dx + dy * dy + dz * dz);
+    }
+}
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java
new file mode 100644
index 0000000..313ad9d
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java
@@ -0,0 +1,62 @@
+package com.galaxis.rcs.ptr;
+
+import com.yvan.logisticsModel.StaticItem;
+
+import java.util.HashSet;
+import java.util.Set;
+
+public class PtrAgvDeviceTask {
+    //该段目标点X坐标	UInt16 逻辑单位，乘以一定系数才是物理距离
+    public int x;
+    //该段目标点Y坐标	UInt16 逻辑单位，乘以一定系数才是物理距离
+    public int y;
+    // 该段行驶速度 Int16	mm/s
+    public int speed;
+    // 该段车头朝向
+    public short direction;
+    // 该段起始点
+    public StaticItem startPoint;
+    // 该段目标点
+    public StaticItem endPoint;
+    // 分组起始点
+    public StaticItem groupStartPoint;
+    // 分组目标点
+    public StaticItem groupEndPoint;
+    // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业）
+    public short operationType;
+    // 提升移栽货物拣货模式	UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
+    public short pickMode;
+
+    // 目标货位朝向
+    public short goodsSlotDirection;
+
+    // 目标货位相对于地面的绝对高度
+    public int goodsSlotHeight;
+
+    //充电桩朝向UseBriefLocation	UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public Short chargeDirection;
+    // 充电工位坐标和充电桩之间距离	UInt16 单位：mm
+    public Integer chargeLocation;
+    // 目标货架编号 String 仅做校验使用（仅接货用）
+    public String storageRacksNo;
+    // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public Short endDirection;
+
+    /** 移动规划ID */
+    public Long movePlanTaskId;
+    /** 转动取货放货充电等规划ID */
+    public Set planTaskIdSet = new HashSet<>();
+    /** 业务任务ID */
+    public Long bizTaskId;
+    // 作业序号 发送给小车的作业序号
+    public int seqNo;
+
+    // 任务状态 0创建 1任务模式改变 2任务接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
+    public int taskStatus = 0;
+    public int taskGroupStatus = 0;
+
+    // 任务分组结束标记 生成报文时作为判断依据
+    public boolean isGroupEnd = false;
+    public int checkLogicX;
+    public int checkLogicY;
+}
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java
new file mode 100644
index 0000000..aa1c4df
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java
@@ -0,0 +1,830 @@
+package com.galaxis.rcs.ptr;
+
+import com.fasterxml.jackson.annotation.JsonIgnore;
+import com.galaxis.rcs.common.entity.RcsTaskPlan;
+import com.galaxis.rcs.common.enums.AgvEventType;
+import com.galaxis.rcs.common.enums.LCCDirection;
+import com.galaxis.rcs.common.enums.PlanTaskStatus;
+import com.galaxis.rcs.common.enums.PlanTaskType;
+import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
+import com.galaxis.rcs.plan.PlanTaskSequence;
+import com.galaxis.rcs.ptr.receiveEntity.AmrHeartbeatMessage;
+import com.galaxis.rcs.ptr.receiveEntity.base.CurBatteryData;
+import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage;
+import com.galaxis.rcs.ptr.sendEntity.RcsSRMessage;
+import com.galaxis.rcs.ptr.sendEntity.RcsSetLocationMessage;
+import com.google.common.base.Joiner;
+import com.google.common.collect.Queues;
+import com.yvan.logisticsModel.ExecutorItem;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import com.yvan.logisticsModel.StaticItem;
+import lombok.SneakyThrows;
+import lombok.extern.slf4j.Slf4j;
+import org.clever.core.BannerUtils;
+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.CopyOnWriteArraySet;
+import java.util.concurrent.locks.LockSupport;
+
+//0.4m/ss // a  max 1.2m/s
+//90 = 3.5s cl2
+//90 = 5s // cLX
+@Slf4j
+public abstract class PtrAgvItem extends ExecutorItem {
+    private static final int BLOCKING_QUEUE_CAPACITY = 100;
+    private static final Redis redis = RedisAdmin.getRedis();
+
+    // ip
+    public String ip;
+    // agv名称
+    public String agvName;
+    // agv类型
+    public String agvType;
+    // agv型号
+    public String agvModel;
+    // AMR功能型号
+    public String agvFnModel;
+    // 电池信息
+    public CurBatteryData battery;
+    // agv当前x坐标
+    public double x;
+    // agv当前y坐标
+    public double y;
+    // agv当前z坐标
+    public double z;
+    // 当前所在站点的逻辑X坐标 Int32
+    public int logicX;
+    // 当前所在站点的逻辑Y坐标 Int32
+    public int logicY;
+    // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public short direction;
+    // agv当前转动角度值
+    public double orientation;
+
+    // 任务模式
+    public AmrTaskMode taskMode;
+
+    private volatile boolean isManualMode = false;
+    private volatile boolean isPaused = false;
+    private volatile PosDirection lastPausedPosition;
+    private volatile boolean isOnline = false;
+
+    private final Set eventListeners = new CopyOnWriteArraySet<>();
+
+    // 执行中的任务
+    @JsonIgnore
+    public List runningDeviceTaskList = new ArrayList<>();
+
+    /**
+     * 当前执行的任务规划列表
+     */
+    @JsonIgnore
+    public volatile PlanTaskSequence planTaskSequence;
+
+    /**
+     * 当前执行的设备任务列表
+     */
+    @JsonIgnore
+    final BlockingQueue deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);
+
+    final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(this.runtime);
+
+    @JsonIgnore
+    public final AmrMessageHandler amrMessageHandler;
+
+    /**
+     * 连接器线程
+     */
+    private final PtrAgvConnectorThread connectorThread;
+
+    public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map raw) {
+        super(logisticsRuntime, raw);
+        this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime);
+        this.amrMessageHandler = logisticsRuntime.amrMessageHandler;
+    }
+
+    public abstract RcsConfigMessage getConfig();
+
+    @SneakyThrows
+    public synchronized void initialize() {
+        this.amrMessageHandler.registeHeartBeatSet(this);
+
+
+        // 查询当前状态
+        requestCurrentStatus();
+        this.isInitialized = true;
+
+        this.startConnector();
+    }
+
+    public synchronized void shutdown() {
+        this.stopConnector();
+        this.amrMessageHandler.unregisteHeartBeatSet(this);
+    }
+
+    public synchronized void dispatchTask(PlanTaskSequence taskSequence) {
+        if (!isFree()) {
+            throw new IllegalStateException("AGV is not free to accept new tasks");
+        }
+
+        if (isManualMode) {
+            throw new IllegalStateException("AGV is in manual mode and cannot accept tasks");
+        }
+
+        this.planTaskSequence = taskSequence;
+        buildPlanToDeviceTask();
+        fireEvent(AgvEventType.PLAN_ACCEPT, this);
+        connectorThread.resumeProcessing();
+    }
+
+    public synchronized void pauseTask() {
+        if (planTaskSequence == null) {
+            throw new IllegalStateException("No active task to pause");
+        }
+
+        isPaused = true;
+        lastPausedPosition = new PosDirection(logicX, logicY, 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);
+        }
+
+        fireEvent(AgvEventType.PLAN_PAUSE, this);
+    }
+
+    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 ||
+            direction != lastPausedPosition.direction()) {
+            // 需要返回暂停位置
+            throw new RuntimeException("AGV position has changed since pause, cannot resume task safely");
+        }
+
+        isPaused = false;
+        connectorThread.resumeProcessing();
+        fireEvent(AgvEventType.PLAN_RESUME, this);
+    }
+
+    @SneakyThrows
+    public synchronized void cancelTask() {
+        // 发送取消指令
+        amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo());
+
+        if (planTaskSequence != null) {
+            planTaskSequence = null;
+        }
+        if (!deviceTaskQueue.isEmpty()) {
+            deviceTaskQueue.clear();
+        }
+
+        fireEvent(AgvEventType.PLAN_CANCEL, this);
+    }
+
+    @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);
+    }
+
+    public void setControlMode(ControlMode mode) {
+        // 硬件控制模式设置逻辑
+        this.isManualMode = (mode == ControlMode.MANUAL);
+
+        if (mode != ControlMode.FULL_AUTO && planTaskSequence != null) {
+            cancelTask();
+        }
+
+        // 更新Redis状态
+        updateRedisStatus();
+    }
+
+    @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;
+        }
+        return true;
+    }
+
+    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) {
+                // 更新计划任务
+                List planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList();
+                for (RcsTaskPlan planTask : planTaskList) {
+                    planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
+                    planTaskSequence.savePlanTask(planTask);
+                }
+            }
+        }
+
+        if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
+            fireEvent(AgvEventType.PLAN_COMPLETE, this);
+            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;
+
+        // 更新Redis
+        updateRedisStatus();
+
+        // 触发位置变化事件
+        if (oldX != logicX || oldY != logicY) {
+            fireEvent(AgvEventType.POS_CHANGED, this,
+                new PosDirection(oldX, oldY, oldDirection),
+                new PosDirection(logicX, logicY, direction));
+        }
+
+        if (oldDirection != direction) {
+            fireEvent(AgvEventType.DIRECTION_CHANGED, this,
+                oldDirection, direction);
+        }
+
+        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.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; // 标记为完成
+                    fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task);
+
+                    // 更新计划任务
+                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
+                    if (planTask != null) {
+                        planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
+                        planTaskSequence.savePlanTask(planTask);
+                    }
+                }
+
+                if (planTaskSequence.isAllCompleted()) {
+                    fireEvent(AgvEventType.PLAN_COMPLETE, this);
+                    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) {
+
+    }
+
+    public void updateRedisStatus() {
+        String statusKey = getRedisKey("status");
+        Map statusMap = new HashMap<>();
+        statusMap.put("x", String.valueOf(x));
+        statusMap.put("y", String.valueOf(y));
+        statusMap.put("z", String.valueOf(z));
+        statusMap.put("logicX", String.valueOf(logicX));
+        statusMap.put("logicY", String.valueOf(logicY));
+        statusMap.put("direction", String.valueOf(direction));
+        statusMap.put("orientation", String.valueOf(orientation));
+        statusMap.put("soc", this.battery == null ? "-1" : String.valueOf(this.battery.SOC));
+        statusMap.put("mode", isManualMode ? "MANUAL" : "AUTO");
+        statusMap.put("taskStatus", getTaskStatus());
+
+        redis.hPutAll(statusKey, statusMap);
+        redis.kExpire(statusKey, 10); // 10秒过期
+    }
+
+    public void handleHeartbeat(AmrHeartbeatMessage heartbeat) {
+        // 更新在线状态
+        String aliveKey = getRedisKey("alive");
+        redis.vSet(aliveKey, "1");
+        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) {
+            fireEvent(AgvEventType.LOW_BATTERY, this);
+        }
+
+        updateRedisStatus();
+    }
+
+    private void handleOnlineEvent() {
+        isOnline = true;
+        fireEvent(AgvEventType.ONLINE, this);
+        requestCurrentStatus();
+    }
+
+    private void handleOfflineEvent() {
+        isOnline = false;
+        fireEvent(AgvEventType.OFFLINE, this);
+    }
+
+    // 事件监听管理
+    public void addEventListener(AgvEventListener listener) {
+        eventListeners.add(listener);
+    }
+
+    public void removeEventListener(AgvEventListener listener) {
+        eventListeners.remove(listener);
+    }
+
+    private void fireEvent(AgvEventType type, Object... args) {
+        for (AgvEventListener listener : eventListeners) {
+            listener.onEvent(type, args);
+        }
+    }
+
+    private String getTaskStatus() {
+        if (planTaskSequence == null) return "IDLE";
+        if (isPaused) return "PAUSED";
+        return "EXECUTING";
+    }
+
+
+    /**
+     * 启动连接器线程
+     */
+    public void startConnector() {
+        if (!connectorThread.isRunning()) {
+            connectorThread.start();
+            System.out.println("Connector started for executor: " + this.getId());
+        }
+    }
+
+    /**
+     * 停止连接器线程
+     */
+    public void stopConnector() {
+        connectorThread.stop();
+        System.out.println("Connector stopped for executor: " + this.getId());
+    }
+
+    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 rotationPlanTaskIdSet = new HashSet<>();
+
+        // 生成移动报文
+        List deviceTaskList = new ArrayList<>();
+        List> 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>) 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>) 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 storeItemRaw = storeItem.dt;
+                if (storeItemRaw.containsKey("bays")) {
+                    List> bays = (List>) storeItemRaw.get("bays");
+                    Map bay = bays.get(plan.getTargetBay());
+                    List levelHeight = (List) bay.get("levelHeight");
+                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
+                } else {
+                    deviceTask.goodsSlotHeight = 1;
+                }
+                if (linkStore != null) {
+                    for (Map 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>) 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 storeItemRaw = storeItem.dt;
+                if (storeItemRaw.containsKey("bays")) {
+                    List> bays = (List>) storeItemRaw.get("bays");
+                    Map bay = bays.get(plan.getTargetBay());
+                    List levelHeight = (List) bay.get("levelHeight");
+                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
+                } else {
+                    deviceTask.goodsSlotHeight = 1;
+                }
+                if (linkStore != null) {
+                    for (Map 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.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 = 0;
+                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.size() <= 0) {
+            return;
+        }
+
+        // 标记任务分组结束
+        PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
+        deviceTask.groupEndPoint = deviceTask.endPoint;
+        deviceTask.groupStartPoint = groupStartPoint;
+        deviceTask.isGroupEnd = true;
+        // 最后一个规划任务为旋转时需要添加一个endDirection
+        if (rotationPlanTaskIdSet.size() > 0) {
+            deviceTask.endDirection = direction;
+            deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+            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;
+            }
+        }
+
+
+//        planQueue.addAll(sequence.taskList);
+        deviceTaskQueue.addAll(deviceTaskList);
+
+        String json = JsonWrapper.toJson(deviceTaskList);
+        log.info("deviceTaskList: {}", json);
+
+        // TODO: 开启轮询线程，等待下一个待执行任务
+    }
+
+    public boolean isSamePosition(PosDirection startPos) {
+        return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() && 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:rcs:%s_%s",
+            runtime.projectUUID, runtime.envId, type, this.getId());
+    }
+}
diff --git a/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java b/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java
index 77f7ac0..7c094ba 100644
--- a/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java
+++ b/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java
@@ -12,22 +12,22 @@ import java.util.Map;
  */
 @Data
 public abstract class BaseItem {
-    final String id;
-    final String t;
+    public final String id;
+    public final String t;
 
     /**
      * 变换矩阵, 3x3矩阵, tf[0]=position,采用 X向右增长，Y轴向屏幕外增长，Z向下增长, tf[1]=rotation, tf[2]=scale
      */
-    final float[][] tf;
+    public final float[][] tf;
 
     /**
      * 物品的自定义数据
      */
     @JsonIgnore
-    final Map dt;
+    public final Map dt;
 
     @JsonIgnore
-    final Map raw;
+    public final Map raw;
 
     @JsonIgnore
     public final LogisticsRuntime runtime;
@@ -60,16 +60,4 @@ public abstract class BaseItem {
 
         this.dt = (Map) map.get("dt");
     }
-
-    public float getTransformationX() {
-        return tf[0][0];
-    }
-
-    public float getTransformationY() {
-        return tf[0][2];
-    }
-
-    public float getTransformationZ() {
-        return tf[0][1];
-    }
 }
diff --git a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java
index 6694ba4..e83ee5a 100644
--- a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java
+++ b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java
@@ -2,8 +2,8 @@ package com.yvan.logisticsModel;
 
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.galaxis.rcs.connector.cl2.Cl2Item;
-import com.galaxis.rcs.plan.path2.NavigationGraph;
-import com.galaxis.rcs.plan.path2.PtrPathPlanner;
+import com.galaxis.rcs.plan.path.NavigationGraph;
+import com.galaxis.rcs.plan.path.PtrPathPlanner;
 import com.galaxis.rcs.ptr.AgvEventManager;
 import com.galaxis.rcs.ptr.AmrMessageHandler;
 import com.galaxis.rcs.task.TaskDispatchFactory;
diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java
deleted file mode 100644
index de74dbb..0000000
--- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java
+++ /dev/null
@@ -1,7 +0,0 @@
-package com.yvan.logisticsModel;
-
-import com.galaxis.rcs.common.entity.RcsTaskPlan;
-
-public abstract class PtrAgvConnector {
-
-}
diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
deleted file mode 100644
index 97dd691..0000000
--- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
+++ /dev/null
@@ -1,167 +0,0 @@
-package com.yvan.logisticsModel;
-
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
-import com.galaxis.rcs.ptr.sendEntity.RcsTaskMessage;
-import com.yvan.logisticsMonitor.task.DeviceTask;
-import lombok.extern.slf4j.Slf4j;
-import org.eclipse.paho.mqttv5.common.MqttException;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.concurrent.atomic.AtomicBoolean;
-import java.util.concurrent.locks.LockSupport;
-
-@Slf4j
-public class PtrAgvConnectorThread 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;
-    private final LogisticsRuntime logisticsRuntime;
-    private volatile int __currentTaskSeqNo = 0;
-    volatile PtrAgvDeviceTask __currentTask;
-
-    public PtrAgvConnectorThread(PtrAgvItem ptrAgvItem, Cl2DeviceConnector cl2DeviceConnector, LogisticsRuntime logisticsRuntime) {
-        super("ExecutorConnector-" + ptrAgvItem.getId());
-        this.cl2DeviceConnector = cl2DeviceConnector;
-        this.ptrAgvItem = ptrAgvItem;
-        this.logisticsRuntime = logisticsRuntime;
-    }
-
-    @Override
-    public void run() {
-        running.set(true);
-        log.info("Connector thread started for executor: {}", this.ptrAgvItem.getId());
-
-        try {
-            float distance = 0;
-            int taskCount = 0;
-
-            PtrAgvDeviceTask startTask = null;
-            RcsTaskMessage taskMessage = null;
-
-            // 计算中的任务
-            List computingTaskList = new ArrayList<>();
-            while (running.get()) {
-                if (paused.get()) {
-                    synchronized (pauseLock) {
-                        while (paused.get()) {
-                            pauseLock.wait();
-                        }
-                    }
-                }
-
-                PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take();
-                if (startTask == null) {
-                    startTask = currentTask;
-                    taskMessage = new RcsTaskMessage(this.logisticsRuntime);
-                    taskMessage.OperationType = startTask.operationType;
-                    taskMessage.PickMode = startTask.pickMode;
-                    taskMessage.GoNow = true;
-                    taskMessage.StartX = startTask.startPoint.logicX;
-                    taskMessage.StartY = startTask.startPoint.logicY;
-                    taskMessage.Link = new ArrayList<>();
-                }
-                currentTask.seqNo = taskMessage.SeqNo;
-                RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
-                taskMessage.Link.add(link);
-                taskCount++;
-                distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]);
-                computingTaskList.add(currentTask);
-
-
-                PtrAgvDeviceTask 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
-                    || (distance > 2 && taskCount > 1)) {
-                    taskMessage.OperationType = currentTask.operationType;
-                    taskMessage.PickMode = currentTask.pickMode;
-                    taskMessage.EndX = currentTask.groupEndPoint.logicX;
-                    taskMessage.EndY = currentTask.groupEndPoint.logicY;
-                    taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection;
-                    taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight;
-                    taskMessage.LinkCounts = (short) taskMessage.Link.size();
-                    taskMessage.ChargeDirection = currentTask.chargeDirection;
-                    taskMessage.ChargeLocation = currentTask.chargeLocation;
-                    try {
-                        // 发送任务
-                        this.__currentTaskSeqNo = taskMessage.SeqNo;
-                        this.__currentTask = currentTask;
-                        cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage);
-                        this.ptrAgvItem.runningDeviceTaskList.addAll(computingTaskList);
-                        for (PtrAgvDeviceTask task : computingTaskList) {
-                            task.taskStatus = 1;
-                            task.taskGroupStatus = 1;
-                        }
-                        computingTaskList.clear();
-                    } catch (MqttException | JsonProcessingException e) {
-                        log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.movePlanTaskId, e);
-                    }
-                    distance = 0;
-                    taskCount = 0;
-                    taskMessage = null;
-                    startTask = null;
-
-                    if (currentTask.isGroupEnd) {
-                        // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒
-                        LockSupport.park(); // 阻塞当前线程
-                    } else {
-                        for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
-                            if (task.taskGroupStatus < 4) {
-                                LockSupport.park(); // 阻塞当前线程
-                                break;
-                            }
-                        }
-                    }
-                }
-
-            }
-        } catch (InterruptedException e) {
-            System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId());
-        } finally {
-            System.out.println("Connector thread stopped for executor: " + this.ptrAgvItem.getId());
-        }
-    }
-
-    public int getCurrentTaskSeqNo() {
-        return __currentTaskSeqNo;
-    }
-
-    /**
-     * 停止连接器线程
-     */
-    public void stopConnector() {
-        running.set(false);
-        this.interrupt(); // 中断阻塞状态
-    }
-
-    public void pauseProcessing() {
-        paused.set(true);
-    }
-
-    public void resumeProcessing() {
-        paused.set(false);
-        synchronized (pauseLock) {
-            pauseLock.notifyAll();
-        }
-    }
-
-    /**
-     * 检查连接器是否在运行
-     */
-    public boolean isRunning() {
-        return running.get();
-    }
-
-    public static float euclideanDistance(float[] p1, float[] p2) {
-        if (p1.length != 3 || p2.length != 3) throw new IllegalArgumentException();
-        float dx = p1[0] - p2[0];
-        float dy = p1[1] - p2[1];
-        float dz = p1[2] - p2[2];
-        return (float) Math.sqrt(dx * dx + dy * dy + dz * dz);
-    }
-}
diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java
deleted file mode 100644
index 0b6b70f..0000000
--- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java
+++ /dev/null
@@ -1,60 +0,0 @@
-package com.yvan.logisticsModel;
-
-import java.util.HashSet;
-import java.util.Set;
-
-public class PtrAgvDeviceTask {
-    //该段目标点X坐标	UInt16 逻辑单位，乘以一定系数才是物理距离
-    public int x;
-    //该段目标点Y坐标	UInt16 逻辑单位，乘以一定系数才是物理距离
-    public int y;
-    // 该段行驶速度 Int16	mm/s
-    public int speed;
-    // 该段车头朝向
-    public short direction;
-    // 该段起始点
-    public StaticItem startPoint;
-    // 该段目标点
-    public StaticItem endPoint;
-    // 分组起始点
-    public StaticItem groupStartPoint;
-    // 分组目标点
-    public StaticItem groupEndPoint;
-    // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业）
-    public short operationType;
-    // 提升移栽货物拣货模式	UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
-    public short pickMode;
-
-    // 目标货位朝向
-    public short goodsSlotDirection;
-
-    // 目标货位相对于地面的绝对高度
-    public int goodsSlotHeight;
-
-    //充电桩朝向UseBriefLocation	UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-    public Short chargeDirection;
-    // 充电工位坐标和充电桩之间距离	UInt16 单位：mm
-    public Integer chargeLocation;
-    // 目标货架编号 String 仅做校验使用（仅接货用）
-    public String storageRacksNo;
-    // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-    public Short endDirection;
-
-    /** 移动规划ID */
-    public Long movePlanTaskId;
-    /** 转动取货放货充电等规划ID */
-    public Set planTaskIdSet = new HashSet<>();
-    /** 业务任务ID */
-    public Long bizTaskId;
-    // 作业序号 发送给小车的作业序号
-    public int seqNo;
-
-    // 任务状态 0创建 1任务模式改变 2任务接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
-    public int taskStatus = 0;
-    public int taskGroupStatus = 0;
-
-    // 任务分组结束标记 生成报文时作为判断依据
-    public boolean isGroupEnd = false;
-    public int checkLogicX;
-    public int checkLogicY;
-}
diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
deleted file mode 100644
index 2e5c610..0000000
--- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
+++ /dev/null
@@ -1,829 +0,0 @@
-package com.yvan.logisticsModel;
-
-import com.fasterxml.jackson.annotation.JsonIgnore;
-import com.galaxis.rcs.common.entity.RcsTaskPlan;
-import com.galaxis.rcs.common.enums.AgvEventType;
-import com.galaxis.rcs.common.enums.LCCDirection;
-import com.galaxis.rcs.common.enums.PlanTaskStatus;
-import com.galaxis.rcs.common.enums.PlanTaskType;
-import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
-import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.ptr.*;
-import com.galaxis.rcs.ptr.receiveEntity.AmrHeartbeatMessage;
-import com.galaxis.rcs.ptr.receiveEntity.base.CurBatteryData;
-import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage;
-import com.galaxis.rcs.ptr.sendEntity.RcsSRMessage;
-import com.galaxis.rcs.ptr.sendEntity.RcsSetLocationMessage;
-import com.google.common.base.Joiner;
-import com.google.common.collect.Queues;
-import com.yvan.logisticsMonitor.task.PlanTask;
-import lombok.SneakyThrows;
-import lombok.extern.slf4j.Slf4j;
-import org.clever.core.BannerUtils;
-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.CopyOnWriteArraySet;
-import java.util.concurrent.locks.LockSupport;
-import java.util.stream.Collectors;
-
-//0.4m/ss // a  max 1.2m/s
-//90 = 3.5s cl2
-//90 = 5s // cLX
-@Slf4j
-public abstract class PtrAgvItem extends ExecutorItem {
-    private static final int BLOCKING_QUEUE_CAPACITY = 100;
-    private static final Redis redis = RedisAdmin.getRedis();
-
-    // ip
-    public String ip;
-    // agv名称
-    public String agvName;
-    // agv类型
-    public String agvType;
-    // agv型号
-    public String agvModel;
-    // AMR功能型号
-    public String agvFnModel;
-    // 电池信息
-    public CurBatteryData battery;
-    // agv当前x坐标
-    public double x;
-    // agv当前y坐标
-    public double y;
-    // agv当前z坐标
-    public double z;
-    // 当前所在站点的逻辑X坐标 Int32
-    public int logicX;
-    // 当前所在站点的逻辑Y坐标 Int32
-    public int logicY;
-    // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
-    public short direction;
-    // agv当前转动角度值
-    public double orientation;
-
-    // 任务模式
-    public AmrTaskMode taskMode;
-
-    private volatile boolean isManualMode = false;
-    private volatile boolean isPaused = false;
-    private volatile PosDirection lastPausedPosition;
-    private volatile boolean isOnline = false;
-
-    private final Set eventListeners = new CopyOnWriteArraySet<>();
-
-    // 执行中的任务
-    @JsonIgnore
-    public List runningDeviceTaskList = new ArrayList<>();
-
-    /**
-     * 当前执行的任务规划列表
-     */
-    @JsonIgnore
-    private volatile PlanTaskSequence planTaskSequence;
-
-    /**
-     * 当前执行的设备任务列表
-     */
-    @JsonIgnore
-    final BlockingQueue deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);
-
-    final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(this.runtime);
-
-    @JsonIgnore
-    public final AmrMessageHandler amrMessageHandler;
-
-    /**
-     * 连接器线程
-     */
-    private final PtrAgvConnectorThread connectorThread;
-
-    public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map raw) {
-        super(logisticsRuntime, raw);
-        this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime);
-        this.amrMessageHandler = logisticsRuntime.amrMessageHandler;
-    }
-
-    public abstract RcsConfigMessage getConfig();
-
-    @SneakyThrows
-    public synchronized void initialize() {
-        this.amrMessageHandler.registeHeartBeatSet(this);
-
-
-        // 查询当前状态
-        requestCurrentStatus();
-        this.isInitialized = true;
-
-        this.startConnector();
-    }
-
-    public synchronized void shutdown() {
-        this.stopConnector();
-        this.amrMessageHandler.unregisteHeartBeatSet(this);
-    }
-
-    public synchronized void dispatchTask(PlanTaskSequence taskSequence) {
-        if (!isFree()) {
-            throw new IllegalStateException("AGV is not free to accept new tasks");
-        }
-
-        if (isManualMode) {
-            throw new IllegalStateException("AGV is in manual mode and cannot accept tasks");
-        }
-
-        this.planTaskSequence = taskSequence;
-        buildPlanToDeviceTask();
-        fireEvent(AgvEventType.PLAN_ACCEPT, this);
-        connectorThread.resumeProcessing();
-    }
-
-    public synchronized void pauseTask() {
-        if (planTaskSequence == null) {
-            throw new IllegalStateException("No active task to pause");
-        }
-
-        isPaused = true;
-        lastPausedPosition = new PosDirection(logicX, logicY, 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);
-        }
-
-        fireEvent(AgvEventType.PLAN_PAUSE, this);
-    }
-
-    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 ||
-            direction != lastPausedPosition.direction()) {
-            // 需要返回暂停位置
-            throw new RuntimeException("AGV position has changed since pause, cannot resume task safely");
-        }
-
-        isPaused = false;
-        connectorThread.resumeProcessing();
-        fireEvent(AgvEventType.PLAN_RESUME, this);
-    }
-
-    @SneakyThrows
-    public synchronized void cancelTask() {
-        if (planTaskSequence == null) {
-            throw new IllegalStateException("No active task to cancel");
-        }
-
-        // 发送取消指令
-        amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo());
-
-        planTaskSequence = null;
-        deviceTaskQueue.clear();
-        fireEvent(AgvEventType.PLAN_CANCEL, this);
-    }
-
-    @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);
-    }
-
-    public void setControlMode(ControlMode mode) {
-        // 硬件控制模式设置逻辑
-        this.isManualMode = (mode == ControlMode.MANUAL);
-
-        if (mode != ControlMode.FULL_AUTO && planTaskSequence != null) {
-            cancelTask();
-        }
-
-        // 更新Redis状态
-        updateRedisStatus();
-    }
-
-    @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;
-        }
-        return true;
-    }
-
-    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) {
-                // 更新计划任务
-                List planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList();
-                for (RcsTaskPlan planTask : planTaskList) {
-                    planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
-                    planTaskSequence.savePlanTask(planTask);
-                }
-            }
-        }
-
-        if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
-            fireEvent(AgvEventType.PLAN_COMPLETE, this);
-            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;
-
-        // 更新Redis
-        updateRedisStatus();
-
-        // 触发位置变化事件
-        if (oldX != logicX || oldY != logicY) {
-            fireEvent(AgvEventType.POS_CHANGED, this,
-                new PosDirection(oldX, oldY, oldDirection),
-                new PosDirection(logicX, logicY, direction));
-        }
-
-        if (oldDirection != direction) {
-            fireEvent(AgvEventType.DIRECTION_CHANGED, this,
-                oldDirection, direction);
-        }
-
-        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.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; // 标记为完成
-                    fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task);
-
-                    // 更新计划任务
-                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
-                    if (planTask != null) {
-                        planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
-                        planTaskSequence.savePlanTask(planTask);
-                    }
-                }
-
-                if (planTaskSequence.isAllCompleted()) {
-                    fireEvent(AgvEventType.PLAN_COMPLETE, this);
-                    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) {
-
-    }
-
-    public void updateRedisStatus() {
-        String statusKey = getRedisKey("status");
-        Map statusMap = new HashMap<>();
-        statusMap.put("x", String.valueOf(x));
-        statusMap.put("y", String.valueOf(y));
-        statusMap.put("z", String.valueOf(z));
-        statusMap.put("logicX", String.valueOf(logicX));
-        statusMap.put("logicY", String.valueOf(logicY));
-        statusMap.put("direction", String.valueOf(direction));
-        statusMap.put("orientation", String.valueOf(orientation));
-        statusMap.put("soc", this.battery == null ? "-1" : String.valueOf(this.battery.SOC));
-        statusMap.put("mode", isManualMode ? "MANUAL" : "AUTO");
-        statusMap.put("taskStatus", getTaskStatus());
-
-        redis.hPutAll(statusKey, statusMap);
-        redis.kExpire(statusKey, 10); // 10秒过期
-    }
-
-    public void handleHeartbeat(AmrHeartbeatMessage heartbeat) {
-        // 更新在线状态
-        String aliveKey = getRedisKey("alive");
-        redis.vSet(aliveKey, "1");
-        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) {
-            fireEvent(AgvEventType.LOW_BATTERY, this);
-        }
-
-        updateRedisStatus();
-    }
-
-    private void handleOnlineEvent() {
-        isOnline = true;
-        fireEvent(AgvEventType.ONLINE, this);
-        requestCurrentStatus();
-    }
-
-    private void handleOfflineEvent() {
-        isOnline = false;
-        fireEvent(AgvEventType.OFFLINE, this);
-    }
-
-    // 事件监听管理
-    public void addEventListener(AgvEventListener listener) {
-        eventListeners.add(listener);
-    }
-
-    public void removeEventListener(AgvEventListener listener) {
-        eventListeners.remove(listener);
-    }
-
-    private void fireEvent(AgvEventType type, Object... args) {
-        for (AgvEventListener listener : eventListeners) {
-            listener.onEvent(type, args);
-        }
-    }
-
-    private String getTaskStatus() {
-        if (planTaskSequence == null) return "IDLE";
-        if (isPaused) return "PAUSED";
-        return "EXECUTING";
-    }
-
-
-    /**
-     * 启动连接器线程
-     */
-    public void startConnector() {
-        if (!connectorThread.isRunning()) {
-            connectorThread.start();
-            System.out.println("Connector started for executor: " + this.getId());
-        }
-    }
-
-    /**
-     * 停止连接器线程
-     */
-    public void stopConnector() {
-        connectorThread.stop();
-        System.out.println("Connector stopped for executor: " + this.getId());
-    }
-
-    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 rotationPlanTaskIdSet = new HashSet<>();
-
-        // 生成移动报文
-        List deviceTaskList = new ArrayList<>();
-        List> 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>) 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>) 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 storeItemRaw = storeItem.dt;
-                if (storeItemRaw.containsKey("bays")) {
-                    List> bays = (List>) storeItemRaw.get("bays");
-                    Map bay = bays.get(plan.getTargetBay());
-                    List levelHeight = (List) bay.get("levelHeight");
-                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
-                } else {
-                    deviceTask.goodsSlotHeight = 1;
-                }
-                if (linkStore != null) {
-                    for (Map 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>) 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 storeItemRaw = storeItem.dt;
-                if (storeItemRaw.containsKey("bays")) {
-                    List> bays = (List>) storeItemRaw.get("bays");
-                    Map bay = bays.get(plan.getTargetBay());
-                    List levelHeight = (List) bay.get("levelHeight");
-                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
-                } else {
-                    deviceTask.goodsSlotHeight = 1;
-                }
-                if (linkStore != null) {
-                    for (Map 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.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 = 0;
-                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.size() <= 0) {
-            return;
-        }
-
-        // 标记任务分组结束
-        PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
-        deviceTask.groupEndPoint = deviceTask.endPoint;
-        deviceTask.groupStartPoint = groupStartPoint;
-        deviceTask.isGroupEnd = true;
-        // 最后一个规划任务为旋转时需要添加一个endDirection
-        if (rotationPlanTaskIdSet.size() > 0) {
-            deviceTask.endDirection = direction;
-            deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
-            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;
-            }
-        }
-
-
-//        planQueue.addAll(sequence.taskList);
-        deviceTaskQueue.addAll(deviceTaskList);
-
-        String json = JsonWrapper.toJson(deviceTaskList);
-        log.info("deviceTaskList: {}", json);
-
-        // TODO: 开启轮询线程，等待下一个待执行任务
-    }
-
-    public boolean isSamePosition(PosDirection startPos) {
-        return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() && 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:rcs:%s_%s",
-            runtime.projectUUID, runtime.envId, type, this.getId());
-    }
-}
diff --git a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
index de9444d..a18fce1 100644
--- a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
+++ b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
@@ -1,15 +1,12 @@
 package com.yvan.workbench.controller;
 
-import com.galaxis.rcs.RCSService;
-import com.galaxis.rcs.common.entity.RcsTaskBiz;
-import com.galaxis.rcs.common.entity.StoreLocation;
-import com.galaxis.rcs.common.enums.BizTaskStatus;
-import com.galaxis.rcs.common.enums.BizTaskType;
-import com.galaxis.rcs.common.enums.LCCDirection;
+import com.galaxis.rcs.common.entity.*;
+import com.galaxis.rcs.common.enums.*;
 import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.plan.task.CarryTask;
-import com.galaxis.rcs.plan.task.MoveTask;
+import com.galaxis.rcs.plan.task.*;
+import com.galaxis.rcs.ptr.PtrAgvItem;
 import com.google.common.base.Strings;
+import com.google.common.collect.Maps;
 import com.yvan.logisticsModel.*;
 import com.yvan.workbench.model.entity.Model;
 import org.apache.commons.lang3.NotImplementedException;
@@ -20,118 +17,48 @@ import org.clever.web.mvc.annotation.RequestBody;
 import java.util.Map;
 
 public class RcsController {
-//    /**
-//     * 后台机器人移动
-//     * @param agvId 机器人ID
-//     * @param targetWayPointId 目标路径点ID
-//     * @param option 选项
-//     */
-//    agvMove(agvId: string, targetWayPointId: string, option: any = {}): Promise>
-//
-//    /**
-//     * 后台机器人搬运（库存点 -> 库存点）
-//     * @param agvId 机器人ID
-//     * @param fromStoreLoc 库存点ID
-//     * @param targetStoreLoc 目标库存点ID
-//     * @param option 其他选项
-//     */
-//    agvCarry(agvId: string, fromStoreLoc: string, targetStoreLoc: string, option: any = {}): Promise>
-//
-//    /**
-//     * 机器人充电
-//     * @param agvId 机器人ID
-//     * @param chargerId 充电桩ID
-//     * @param option 其他选项
-//     */
-//    agvToCharger(agvId: string, chargerId: string, option: any = {}): Promise>
-
     static final SnowFlake snowFlake = new SnowFlake();
 
     public static Model agvMove(@RequestBody Map params) {
-        String projectUUID = Conv.asString(params.get("projectUUID"));
-        String catalogCode = Conv.asString(params.get("catalogCode"));
-        Long envId = Conv.asLong(params.get("envId"));
-        String agvId = Conv.asString(params.get("agvId"));
-        String targetWayPointId = Conv.asString(params.get("targetWayPointId"));
-        Map option = (Map) params.get("option");
-
-        if (Strings.isNullOrEmpty(agvId)) {
-            return Model.newFail("agvId Must not be empty");
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model) ret;
         }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        String targetWayPointId = Conv.asString(params.get("targetWayPointId"));
         if (Strings.isNullOrEmpty(targetWayPointId)) {
             return Model.newFail("targetWayPointId Must not be empty");
         }
+        String targetDirection = Conv.asString(params.get("targetDirection"));
+        if (Strings.isNullOrEmpty(targetDirection)) {
+            targetDirection = ps.fromDirection.toString();
+        }
 
-        LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId);
-        StaticItem toItem = runtime.getStaticItemById(targetWayPointId);
+        StaticItem toItem = ps.runtime.getStaticItemById(targetWayPointId);
         if (toItem == null) {
             return Model.newFail("target wayPoint not found!");
         }
 
-        ExecutorItem executorItem = runtime.executorItemMap.get(agvId);
-        if (executorItem == null) {
-            return Model.newFail("executor not found: " + agvId);
-        }
-        if (!(executorItem instanceof PtrAgvItem)) {
-            return Model.newFail("executor is not a PtrAgvItem id=" + agvId);
-        }
-
-        // 获取机器人当前所在位置, 也可以前端强制指定
-        // forceStartWayPointId: '6_2', forceStartDirectior: 'right'
-        StaticItem fromItem = null;
-        LCCDirection fromDirection = null;
-        if (option.get("forceStartWayPointId") != null) {
-            fromItem = runtime.getStaticItemById(Conv.asString(option.get("forceStartWayPointId")));
-        } else {
-            fromItem = runtime.getStaticItemByLogicXY(((PtrAgvItem) executorItem).logicX, ((PtrAgvItem) executorItem).logicY);
-        }
-        if (option.get("forceStartDirection") != null) {
-            fromDirection = LCCDirection.fromString(Conv.asString(option.get("forceStartDirection")));
-        } else {
-            fromDirection = ((PtrAgvItem) executorItem).getLCCDirection();
-        }
-
-        if (fromItem == null) {
-            return Model.newFail("PtrAgvItem not found at current location: " + ((PtrAgvItem) executorItem).logicX + "_" + ((PtrAgvItem) executorItem).logicY + ", id=" + agvId);
-        }
-        if (fromDirection == null) {
-            return Model.newFail("PtrAgvItem unkown direction id=" + agvId);
-        }
-
-        RcsTaskBiz bizTask = new RcsTaskBiz();
-        bizTask.setBizTaskId(snowFlake.nextId());
-        bizTask.setEnvId(envId);
-        bizTask.setBizType(BizTaskType.MOVE.toString());
-        bizTask.setLpn("N/A");
-        bizTask.setPriority(Conv.asInteger(option.get("priority"), 1));
-        bizTask.setTaskFrom(fromItem.getId());
-        bizTask.setTaskTo(targetWayPointId);
-        bizTask.setAllocatedExecutorId(agvId);
-        bizTask.setBizTaskPayload("N/A");
-        bizTask.setBizTaskErrorInfo("N/A");
-        bizTask.setBizTaskDescription("N/A");
-        bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString());
-
-        PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "demo");
+        ps.bizTask.setTaskTo(targetWayPointId);
 
         MoveTask moveTask = new MoveTask(
-            agvId, bizTask.getTaskTo(), bizTask.getPriority()
+            ps.agvId, ps.bizTask.getTaskTo(), LCCDirection.fromString(targetDirection, ps.fromDirection), ps.bizTask.getPriority()
         );
 
-        runtime.pathPlannerMap.get(executorItem.getT())
-            .planMoveTask(planSequence, fromItem.getId(), fromDirection, moveTask);
+        ps.runtime.pathPlannerMap.get(ps.agv.getT())
+            .planMoveTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, moveTask);
 
-        PtrAgvItem agvItem = (PtrAgvItem) executorItem;
-        agvItem.logicX = fromItem.logicX;
-        agvItem.logicY = fromItem.logicY;
-        agvItem.dispatchTask(planSequence);
+        ps.agv.logicX = ps.fromItem.logicX;
+        ps.agv.logicY = ps.fromItem.logicY;
+        ps.agv.dispatchTask(ps.planSequence);
 
-        return Model.newSuccess(planSequence.toPrettyMap());
+        return Model.newSuccess(ps.planSequence.toPrettyMap());
     }
 
     public static Model agvInfo(@RequestBody Map params) {
         String projectUUID = Conv.asString(params.get("projectUUID"));
-        String catalogCode = Conv.asString(params.get("catalogCode"));
         Long envId = Conv.asLong(params.get("envId"));
         String agvId = Conv.asString(params.get("agvId"));
 
@@ -152,18 +79,111 @@ public class RcsController {
         return Model.newSuccess(executorItem);
     }
 
+    public static Model agvUnload(@RequestBody Map params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        String targetStoreLoc = Conv.asString(params.get("targetStoreLoc"));
+        if (Strings.isNullOrEmpty(targetStoreLoc)) {
+            return Model.newFail("targetStoreLoc Must not be empty");
+        }
+        StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/");
+
+        UnloadTask unloadTask = new UnloadTask(
+            ps.agvId, ps.bizTask.getPriority(),
+            targetLocation
+        );
+
+        ps.runtime.pathPlannerMap.get(ps.agv.getT())
+            .planUnloadTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, unloadTask);
+
+        ps.agv.logicX = ps.fromItem.logicX;
+        ps.agv.logicY = ps.fromItem.logicY;
+        ps.agv.dispatchTask(ps.planSequence);
+
+        return Model.newSuccess(ps.planSequence.toPrettyMap());
+    }
+
+    public static Model agvLoad(@RequestBody Map params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        String targetStoreLoc = Conv.asString(params.get("targetStoreLoc"));
+        if (Strings.isNullOrEmpty(targetStoreLoc)) {
+            return Model.newFail("targetStoreLoc Must not be empty");
+        }
+        StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/");
+
+        LoadTask loadTask = new LoadTask(
+            ps.agvId, ps.bizTask.getPriority(),
+            targetLocation
+        );
+
+        ps.runtime.pathPlannerMap.get(ps.agv.getT())
+            .planLoadTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, loadTask);
+
+        ps.agv.logicX = ps.fromItem.logicX;
+        ps.agv.logicY = ps.fromItem.logicY;
+        ps.agv.dispatchTask(ps.planSequence);
+
+        return Model.newSuccess(ps.planSequence.toPrettyMap());
+    }
+
+    public static Model cancelTasks(@RequestBody Map params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        ps.agv.cancelTask();
+        return Model.newSuccess("AGV tasks cancelled successfully");
+    }
+
+    public static Model waitTaskFinish(@RequestBody Map params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        var now = System.currentTimeMillis();
+
+        // 循环等待 ps.agv.planTaskSequence 清空或没值
+        while (ps.agv.planTaskSequence != null && !ps.agv.planTaskSequence.isEmpty()) {
+            try {
+                Thread.sleep(100); // 等待100毫秒
+            } catch (InterruptedException e) {
+                Thread.currentThread().interrupt(); // 恢复中断状态
+                return Model.newFail("Interrupted while waiting for task to finish");
+            }
+        }
+
+        var cost = System.currentTimeMillis() - now;
+        return Model.newSuccess(cost);
+    }
+
     public static Model agvCarry(@RequestBody Map params) {
-        String projectUUID = Conv.asString(params.get("projectUUID"));
-        String catalogCode = Conv.asString(params.get("catalogCode"));
-        Long envId = Conv.asLong(params.get("envId"));
-        String agvId = Conv.asString(params.get("agvId"));
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
         String fromStoreLoc = Conv.asString(params.get("fromStoreLoc"));
         String targetStoreLoc = Conv.asString(params.get("targetStoreLoc"));
-        Map option = (Map) params.get("option");
 
-        if (Strings.isNullOrEmpty(agvId)) {
-            return Model.newFail("agvId Must not be empty");
-        }
         if (Strings.isNullOrEmpty(fromStoreLoc)) {
             return Model.newFail("fromStoreLoc Must not be empty");
         }
@@ -171,25 +191,67 @@ public class RcsController {
             return Model.newFail("targetStoreLoc Must not be empty");
         }
 
-        LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId);
-
         StoreLocation sourceLocation = StoreLocation.of(fromStoreLoc, "/");
         StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/");
 
-        StaticItem sourceItem = runtime.getStaticItemById(sourceLocation.rackId());
+        StaticItem sourceItem = ps.runtime.getStaticItemById(sourceLocation.rackId());
         if (sourceItem == null) {
             return Model.newFail("fromStoreLoc storePoint not found!");
         }
-        StaticItem targetItem = runtime.getStaticItemById(targetLocation.rackId());
+        StaticItem targetItem = ps.runtime.getStaticItemById(targetLocation.rackId());
         if (targetItem == null) {
             return Model.newFail("targetStoreLoc storePoint not found!");
         }
 
+        ps.bizTask.setTaskFrom(fromStoreLoc);
+        ps.bizTask.setTaskTo(targetStoreLoc);
+
+        CarryTask carryTask = new CarryTask(
+            ps.agvId, "N/A", ps.bizTask.getPriority(),
+            sourceLocation,
+            targetLocation
+        );
+
+        ps.runtime.pathPlannerMap.get(ps.agv.getT())
+            .planCarryTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, carryTask);
+
+        ps.agv.logicX = ps.fromItem.logicX;
+        ps.agv.logicY = ps.fromItem.logicY;
+        ps.agv.dispatchTask(ps.planSequence);
+
+        return Model.newSuccess(ps.planSequence.toPrettyMap());
+    }
+
+    public static Model agvToCharger(@RequestBody Map params) {
+        throw new NotImplementedException("agvToCharger not implemented yet");
+    }
+
+    public static Object getCommonParamAndCreateBizTask(@RequestBody Map params) {
+        String projectUUID = Conv.asString(params.get("projectUUID"));
+        Long envId = Conv.asLong(params.get("envId"));
+        String agvId = Conv.asString(params.get("agvId"));
+        Map option = (Map) params.get("option");
+
+        if (Strings.isNullOrEmpty(projectUUID)) {
+            return Model.newFail("projectUUID Must not be empty");
+        }
+        if (envId == null || envId < 0) {
+            return Model.newFail("envId Must not be empty");
+        }
+        if (Strings.isNullOrEmpty(agvId)) {
+            return Model.newFail("agvId Must not be empty");
+        }
+        if (option == null) {
+            option = Maps.newHashMap();
+        }
+
+        LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId);
+
         ExecutorItem executorItem = runtime.executorItemMap.get(agvId);
         if (executorItem == null) {
             return Model.newFail("executor not found: " + agvId);
         }
-        if (!(executorItem instanceof PtrAgvItem)) {
+        if (!(executorItem instanceof PtrAgvItem agv)) {
             return Model.newFail("executor is not a PtrAgvItem id=" + agvId);
         }
 
@@ -200,16 +262,16 @@ public class RcsController {
         if (option.get("forceStartWayPointId") != null) {
             fromItem = runtime.getStaticItemById(Conv.asString(option.get("forceStartWayPointId")));
         } else {
-            fromItem = runtime.getStaticItemByLogicXY(((PtrAgvItem) executorItem).logicX, ((PtrAgvItem) executorItem).logicY);
+            fromItem = runtime.getStaticItemByLogicXY(agv.logicX, agv.logicY);
         }
         if (option.get("forceStartDirection") != null) {
             fromDirection = LCCDirection.fromString(Conv.asString(option.get("forceStartDirection")));
         } else {
-            fromDirection = ((PtrAgvItem) executorItem).getLCCDirection();
+            fromDirection = agv.getLCCDirection();
         }
 
         if (fromItem == null) {
-            return Model.newFail("PtrAgvItem not found at current location: " + ((PtrAgvItem) executorItem).logicX + "_" + ((PtrAgvItem) executorItem).logicY + ", id=" + agvId);
+            return Model.newFail("PtrAgvItem not found at current location: " + agv.logicX + "_" + agv.logicY + ", id=" + agvId);
         }
         if (fromDirection == null) {
             return Model.newFail("PtrAgvItem unkown direction id=" + agvId);
@@ -217,38 +279,31 @@ public class RcsController {
 
         RcsTaskBiz bizTask = new RcsTaskBiz();
         bizTask.setBizTaskId(snowFlake.nextId());
-        bizTask.setEnvId(1L);
+        bizTask.setEnvId(envId);
         bizTask.setBizType(BizTaskType.MOVE.toString());
         bizTask.setLpn("N/A");
         bizTask.setPriority(Conv.asInteger(option.get("priority"), 1));
-        bizTask.setTaskFrom(fromStoreLoc);
-        bizTask.setTaskTo(targetStoreLoc);
+        bizTask.setTaskFrom(fromItem.getId());
         bizTask.setAllocatedExecutorId(agvId);
         bizTask.setBizTaskPayload("N/A");
         bizTask.setBizTaskErrorInfo("N/A");
         bizTask.setBizTaskDescription("N/A");
         bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString());
 
-        PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "demo");
-
-        CarryTask carryTask = new CarryTask(
-            agvId, "dummy", 1,
-            sourceLocation,
-            targetLocation
-        );
-
-        runtime.pathPlannerMap.get(executorItem.getT())
-            .planCarryTask(planSequence, fromItem.getId(), fromDirection, carryTask);
-
-        PtrAgvItem agvItem = (PtrAgvItem) executorItem;
-        agvItem.logicX = fromItem.logicX;
-        agvItem.logicY = fromItem.logicY;
-        agvItem.dispatchTask(planSequence);
-
-        return Model.newSuccess(planSequence.toPrettyMap());
+        PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "RcsScript");
+        return new RcsCommonParam(projectUUID, envId, agvId, option, bizTask, runtime, agv, planSequence, fromItem, fromDirection);
     }
 
-    public static Model agvToCharger(@RequestBody Map params) {
-        throw new NotImplementedException("agvToCharger not implemented yet");
+    public record RcsCommonParam(
+        String projectUUID,
+        Long envId,
+        String agvId,
+        Map option,
+        RcsTaskBiz bizTask,
+        LogisticsRuntime runtime,
+        PtrAgvItem agv,
+        PlanTaskSequence planSequence,
+        StaticItem fromItem,
+        LCCDirection fromDirection) {
     }
 }
diff --git a/servo/src/main/java/com/yvan/workbench/model/entity/Model.java b/servo/src/main/java/com/yvan/workbench/model/entity/Model.java
index d8ec861..ea17414 100644
--- a/servo/src/main/java/com/yvan/workbench/model/entity/Model.java
+++ b/servo/src/main/java/com/yvan/workbench/model/entity/Model.java
@@ -1,5 +1,7 @@
 package com.yvan.workbench.model.entity;
 
+import com.fasterxml.jackson.annotation.JsonInclude;
+
 public class Model implements java.io.Serializable {
     private boolean success = false;
     private T data;
@@ -43,4 +45,15 @@ public class Model implements java.io.Serializable {
         this.success = success;
         return this;
     }
+
+    @JsonInclude(JsonInclude.Include.NON_NULL)
+    private Throwable exception;
+
+    public Throwable getException() {
+        return exception;
+    }
+
+    public void setException(Throwable exception) {
+        this.exception = exception;
+    }
 }