From d14d9cd972f04fec6e05702d05747de02f1b2741 Mon Sep 17 00:00:00 2001
From: yuliang <398780299@qq.com>
Date: Thu, 10 Jul 2025 15:52:12 +0800
Subject: [PATCH] =?UTF-8?q?=E5=8F=B0=E6=B9=BE=E5=B1=95=E4=BC=9A=E5=9C=B0?=
 =?UTF-8?q?=E5=9B=BE?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/modules/amr/ptr/PtrObject.ts | 69 ++++++++++++++++++++++++++++++++--------
 1 file changed, 55 insertions(+), 14 deletions(-)

diff --git a/src/modules/amr/ptr/PtrObject.ts b/src/modules/amr/ptr/PtrObject.ts
index 6594e3e..fd3200d 100644
--- a/src/modules/amr/ptr/PtrObject.ts
+++ b/src/modules/amr/ptr/PtrObject.ts
@@ -21,6 +21,7 @@ import {Euler} from 'three/src/math/Euler'
 import gsap from 'gsap'
 import {MeshWrap} from "@/core/manager/InstanceMeshManager";
 import {getRenderer} from "@/core/manager/ModuleManager";
+import {MathUtils} from "three";
 
 
 type CStepTaskType = 'MOVE' | 'MOVE_BACKWARD' | 'ROTATION' | 'LOAD' | 'UNLOAD' | 'CHARGE'
@@ -89,6 +90,9 @@ export default class PtrObject extends THREE.Object3D {
 
   private agvStatusVo: AgvStatusVo;
 
+  // 是否模拟设备
+  private isMonitoring: boolean = false
+
   get TaskMode(): CTaskMode {
     return this.__TaskMode
   }
@@ -171,14 +175,11 @@ export default class PtrObject extends THREE.Object3D {
     this.vehicleId = parseInt(item.id)
 
     this.viewport.addFrameTimerCallback(this.item.id, this.onFrameTimer.bind(this))
-    if (!worldModel.state.runState.isVirtual) {
-      this.bootForShow()
-    }
     window.agv3 = this
   }
 
   protected onFrameTimer() {
-    if (!worldModel.state.runState.isVirtual) {
+    if (!this.isMonitoring) {
       return
     }
     const delta = this.clock.getDelta()
@@ -220,7 +221,7 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   bootForShow() {
-
+    this.isMonitoring = false
     this.boot()
     this.subscribeMessage('/wcs_server/' + this.item.id)
     this.subscribeMessage('/agv_robot/status')
@@ -228,7 +229,7 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   bootForMonitor() {
-
+    this.isMonitoring = true
     this.boot()
     this.subscribeMessage('/wcs_server/' + this.vehicleId)
     this.send20147()
@@ -248,7 +249,7 @@ export default class PtrObject extends THREE.Object3D {
 
   // 关机
   shutdown() {
-    if (!worldModel.state.runState.isVirtual) {
+    if (!this.isMonitoring) {
       return
     }
     const content = new AmrMsg20148(this.vehicleId)
@@ -381,7 +382,7 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   sendMessage(msg: AmrMsg<any>) {
-    if (!worldModel.state.runState.isVirtual) {
+    if (!this.isMonitoring) {
       return
     }
     // console.log('send message:', JSON.stringify(msg))
@@ -396,7 +397,7 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   sendHeartBeat() {
-    if (!worldModel.state.runState.isVirtual) {
+    if (!this.isMonitoring) {
       return
     }
     const content = new AmrMsg20100(this.vehicleId)
@@ -406,7 +407,7 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   sendAck(seqNo: number, vehicleId: number) {
-    if (!worldModel.state.runState.isVirtual) {
+    if (!this.isMonitoring) {
       return
     }
     const msg20050 = new AmrMsg20050(seqNo, vehicleId)
@@ -463,13 +464,11 @@ export default class PtrObject extends THREE.Object3D {
 
 
   handle20020Message(data: AmrMsg20020) {
-    const p = Model.getPositionByLogicXY(data.CurLogicX, data.CurLogicY) as THREE.Vector3
-    this.position.set(p.x, 0, p.z)
+    this.messageSetXYAndRotation(data.CurLogicX, data.CurLogicY, data.CurDirection, data.CurOrientation)
   }
 
   handle20060Message(data: AmrMsg20060) {
-    const p = Model.getPositionByLogicXY(data.CurLogicX, data.CurLogicY) as THREE.Vector3
-    this.position.set(p.x, 0, p.z)
+    this.messageSetXYAndRotation(data.CurLogicX, data.CurLogicY, null, data.CurOrientation)
   }
 
   /*==========真车消息处理============*/
@@ -510,6 +509,47 @@ export default class PtrObject extends THREE.Object3D {
     this.currentOrientation = this.getAmrOrientation(this.rotation.y)
   }
 
+  messageSetXYAndRotation(logicX: number, logicY: number, direction: number, rotation: number) {
+    const p = Model.getPositionByLogicXY(logicX, logicY) as THREE.Vector3
+
+    let r = 0
+    if (direction != null && direction != 15) {
+      switch (direction) {
+        case 0:
+          r = 0
+          break
+        case 1:
+          r = Math.PI / 2 * 3
+          break
+        case 2:
+          r = Math.PI
+          break
+        case 3:
+          r = Math.PI / 2
+          break
+      }
+    } else {
+      r = this.getAmrOrientation(rotation)
+    }
+    const quat1 = new THREE.Quaternion().setFromEuler(this.rotation)
+    const euler: Euler = new Euler(this.rotation.x, r, this.rotation.z)
+    const quat2 = new THREE.Quaternion().setFromEuler(euler)
+    const angleDiff = quat1.angleTo(quat2)
+
+    if (p.distanceTo(this.position) < 0.5 && Math.abs(angleDiff) < Math.PI/4) {
+      return
+    }
+
+    this.position.set(p.x, 0, p.z)
+
+    const transform = new Ammo.btTransform();
+    this.boxBody.getMotionState().getWorldTransform(transform);
+    transform.setOrigin(new Ammo.btVector3(p.x, 0, p.z)); // 设置新位置
+    this.boxBody.getMotionState().setWorldTransform(transform);
+    this.viewport.ammoModel.destroy(transform)
+    this.rotation.set(0, r, 0)
+  }
+
   makeStepTask(data: AmrMsg10010) {
     console.log('makeStepTask:', data)
     let currentStepTask: StepTask = this.runningStepTask
@@ -854,6 +894,7 @@ export default class PtrObject extends THREE.Object3D {
       const transform = new this.viewport.ammoModel.btTransform()
       this.boxBody.getMotionState().getWorldTransform(transform)
       const position = transform.getOrigin()
+      this.viewport.ammoModel.destroy(transform)
       this.position.set(position.x(), position.y(), position.z())
       // this.boxBody.getActivationState()
       // 获取当前速度