From c931454a01896cc2d74ab3e59e46879cade6b07f Mon Sep 17 00:00:00 2001
From: yuliang <398780299@qq.com>
Date: Fri, 4 Jul 2025 20:35:26 +0800
Subject: [PATCH] =?UTF-8?q?cl2=20=E5=9F=BA=E4=BA=8E=E8=AE=BE=E5=A4=87?=
 =?UTF-8?q?=E7=8A=B6=E6=80=81=E5=92=8C=E4=BB=BB=E5=8A=A1=E7=8A=B6=E6=80=81?=
 =?UTF-8?q?=E5=88=86=E6=AD=A5=E7=94=9F=E6=88=90=E8=AE=BE=E5=A4=87=E4=BB=BB?=
 =?UTF-8?q?=E5=8A=A1=E6=B6=88=E6=81=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/modules/amr/ptr/PtrObject.ts | 61 ++++++++++++++++++++++++++--------------
 1 file changed, 40 insertions(+), 21 deletions(-)

diff --git a/src/modules/amr/ptr/PtrObject.ts b/src/modules/amr/ptr/PtrObject.ts
index 51488d9..51b52c0 100644
--- a/src/modules/amr/ptr/PtrObject.ts
+++ b/src/modules/amr/ptr/PtrObject.ts
@@ -385,12 +385,17 @@ export default class PtrObject extends THREE.Object3D {
   // 处理任务
   handle10010Message(data: AmrMsg10010) {
     if (this.currentStepTaskList.length > 0) {
+      const tindex = this.currentStepTaskList.length
+      const lt = this.currentStepTaskList[tindex - 1]
       if (this.runningStepTask.OperationType == 0 && this.runningStepTask.X == data.StartX && this.runningStepTask.Y == data.StartY) {
         // this.currentStepTaskList = []
         this.makeStepTask(data)
         this.executeTask()
-      } else {
+      } else if (lt.OperationType == 0 && lt.X == data.StartX && lt.Y == data.StartY) {
+        this.makeStepTask(data)
+      }else {
         // 此处应该有错误处理
+        console.log('handle10010Message:', data)
       }
     } else {
       this.makeStepTask(data)
@@ -464,31 +469,36 @@ export default class PtrObject extends THREE.Object3D {
 
     let currentStepTask: StepTask = this.runningStepTask
     if (currentStepTask == null) {
-
-      currentStepTask = {
-        SeqNo: 0,
-        StepTaskType: "MOVE",
-        OperationType: 0,
-        PickMode: 0,
-        X: this.currentLogicX,
-        Y: this.currentLogicY,
-        Speed: 1000,
-        EndDirection: this.currentDirection,
-        ChargeLocation: 0,
-        GoodsSlotHeight: 0,
-        position: this.position,
-        isCompleted: true
+      if (this.currentStepTaskList && this.currentStepTaskList.length > 0) {
+        currentStepTask = this.currentStepTaskList[this.currentStepTaskList.length - 1]
+      } else {
+        currentStepTask = {
+          SeqNo: 0,
+          StepTaskType: "MOVE",
+          OperationType: 0,
+          PickMode: 0,
+          X: this.currentLogicX,
+          Y: this.currentLogicY,
+          Speed: 1000,
+          EndDirection: this.currentDirection,
+          ChargeLocation: 0,
+          GoodsSlotHeight: 0,
+          position: this.position,
+          isCompleted: true
+        }
       }
+
     }
     let endDirection = currentStepTask.EndDirection
 
     const linkCount = data.Link?.length || 0
     if (linkCount > 0) {
+      let prevLink = {X: data.StartX, Y: data.StartY, Speed: 1000}
       for (let i = 0; i < data.Link.length; i++) {
         const link = data.Link[i]
         if ((currentStepTask.X == link.X && currentStepTask.Y == link.Y)
           || (currentStepTask.X != link.X && currentStepTask.Y != link.Y)) {
-          debugger
+          prevLink = link
           continue
         } else if (currentStepTask.X < link.X) {
           if (link.Speed > 0) {
@@ -522,13 +532,13 @@ export default class PtrObject extends THREE.Object3D {
             StepTaskType: "ROTATION",
             OperationType: 0,
             PickMode: 0,
-            X: link.X,
-            Y: link.Y,
-            Speed: link.Speed,
+            X: prevLink.X,
+            Y: prevLink.Y,
+            Speed: prevLink.Speed,
             EndDirection: endDirection,
             ChargeLocation: data.ChargeLocation,
             GoodsSlotHeight: data.GoodsSlotHeight,
-            position: Model.getPositionByLogicXY(link.X, link.Y) as THREE.Vector3,
+            position: Model.getPositionByLogicXY(prevLink.X, prevLink.Y) as THREE.Vector3,
             isCompleted: false
           }
           currentStepTask = stepTask
@@ -551,6 +561,7 @@ export default class PtrObject extends THREE.Object3D {
         }
         currentStepTask = stepTask
         this.currentStepTaskList.push(stepTask)
+        prevLink = link
       }
     }
 
@@ -569,7 +580,14 @@ export default class PtrObject extends THREE.Object3D {
           endDirection = (data.GoodsSlotDirection - 1) as LogicDirection
         }
       }
+
+      const item = this.viewport.entityManager.findItemByLogicXY(data.EndX, data.EndY)
+
       if (endDirection != currentStepTask.EndDirection) {
+        // 如果此处不能转弯，忽略结束方向 等待后续任务
+        if (!item.dt?.agvRotation || !item.dt?.agvRotation?.length <= 0) {
+          return
+        }
         const stepTask: StepTask = {
           SeqNo: data.SeqNo,
           StepTaskType: "ROTATION",
@@ -586,6 +604,7 @@ export default class PtrObject extends THREE.Object3D {
         }
         this.currentStepTaskList.push(stepTask)
       }
+
       if (data.OperationType == 3) {
 
         const stepTask: StepTask = {
@@ -709,7 +728,7 @@ export default class PtrObject extends THREE.Object3D {
 
     return new Promise(resolve => {
       this.rotationAnimation = gsap.to(this.rotation, {
-        y: tr,
+        y: rad,
         duration,
         ease: 'none',
         onComplete: ()=>{