cl2 基于设备状态和任务状态分步生成设备任务消息

src/core/manager/EnvManager.ts

@@ -20,8 +20,9 @@ export default class EnvManager {
     if (topic.startsWith('/wcs_server/')) {
       const message: AmrMsg<any> = JSON.parse(payload.toString())
       this.amrMessageManager.handleMessage(topic, message)
-    } else {
+    } else if (topic.startsWith('/agv_robot/status')) {
-
+      const message: AmrMsg<any> = JSON.parse(payload.toString())
+      this.amrMessageManager.handleStatusMessage(topic, message)
     }
   }
 

src/core/manager/amr/AmrMessageManager.ts

@@ -54,10 +54,15 @@ export default class AmrMessageManager {
       // 等待就绪 19997
       case 19997:
         break
+
     }
     if (amrMsg.id != 10050 && amrMsg.id != 10100) {
       const seqNo = amrMsg.content.SeqNo
       amrItem.sendAck(seqNo, vehicleId)
     }
   }
+
+  handleStatusMessage(topic, amrMsg: AmrMsg<any>) {
+    // switch ()
+  }
 }

src/modules/amr/ptr/PtrObject.ts

@@ -15,10 +15,12 @@ import {
   TaskStatusChangeData,
   TaskTypeChangeData
 } from '@/core/manager/amr/AmrMessageDefine'
-import { worldModel } from '@/core/manager/WorldModel'
+import {worldModel} from '@/core/manager/WorldModel'
 import Viewport from '@/core/engine/Viewport'
-import { Euler } from 'three/src/math/Euler'
+import {Euler} from 'three/src/math/Euler'
 import gsap from 'gsap'
+import {MeshWrap} from "@/core/manager/InstanceMeshManager";
+import {getRenderer} from "@/core/manager/ModuleManager";
 
 
 type CStepTaskType = 'MOVE' | 'MOVE_BACKWARD' | 'ROTATION' | 'LOAD' | 'UNLOAD' | 'CHARGE'
@@ -85,7 +87,7 @@ export default class PtrObject extends THREE.Object3D {
   private __TaskStatus: CEventId = 0
   private __PickMode: CPickMode = 0
 
-  private agvStatusVo : AgvStatusVo;
+  private agvStatusVo: AgvStatusVo;
 
   get TaskMode(): CTaskMode {
     return this.__TaskMode
@@ -172,6 +174,7 @@ export default class PtrObject extends THREE.Object3D {
     if (!worldModel.state.runState.isVirtual) {
       this.subscribeMessage('/wcs_server/' + this.item.id)
       this.subscribeMessage('/agv_robot/status')
+      this.boot()
     }
     window.agv3 = this
   }
@@ -367,7 +370,7 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   subscribeMessage(topic: string) {
-    worldModel.envManager.client.subscribe(topic, { qos: 0 })
+    worldModel.envManager.client.subscribe(topic, {qos: 0})
   }
 
   sendMessage(msg: AmrMsg<any>) {
@@ -390,7 +393,7 @@ export default class PtrObject extends THREE.Object3D {
       return
     }
     const content = new AmrMsg20100(this.vehicleId)
-    content.Temperature = { Battery: this.Battery }
+    content.Temperature = {Battery: this.Battery}
     const m20100 = new AmrMsg<AmrMsg20100>(content)
     worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(m20100))
   }
@@ -452,6 +455,16 @@ 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)
+  }
+
+  handle20060Message(data: AmrMsg20060) {
+    const p = Model.getPositionByLogicXY(data.CurLogicX, data.CurLogicY) as THREE.Vector3
+    this.position.set(p.x, 0, p.z)
+  }
+
   /*==========真车消息处理============*/
 
   // 计算逻辑方向
@@ -518,7 +531,7 @@ export default class PtrObject extends THREE.Object3D {
 
     const linkCount = data.Link?.length || 0
     if (linkCount > 0) {
-      let prevLink = { X: data.StartX, Y: data.StartY, Speed: 1000 }
+      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)
@@ -599,10 +612,10 @@ export default class PtrObject extends THREE.Object3D {
       } else if (data.OperationType == 3 && data.ChargeDirection >= 0 && data.ChargeDirection <= 3) {
         endDirection = data.ChargeDirection
       } else if (data.OperationType == 4 && data.GoodsSlotDirection >= 0 && data.GoodsSlotDirection <= 3) {
-        if (data.GoodsSlotDirection == 0) {
+        if (data.GoodsSlotDirection == 3) {
-          endDirection = 3
+          endDirection = 0
         } else {
-          endDirection = (data.GoodsSlotDirection - 1) as LogicDirection
+          endDirection = (data.GoodsSlotDirection + 1) as LogicDirection
         }
       }
 
@@ -611,6 +624,7 @@ export default class PtrObject extends THREE.Object3D {
       if (endDirection != currentStepTask.EndDirection) {
         // 如果此处不能转弯，忽略结束方向 等待后续任务
         if (!item.dt?.agvRotation || item.dt?.agvRotation?.length <= 0) {
+          debugger
           return
         }
         const stepTask: StepTask = {
@@ -677,7 +691,9 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   executeTask() {
+    if (this.runningStepTaskList.length > 0) {
       this.TaskMode = 2
+    }
     while (this.currentStepTaskList.length > 0) {
       const stepTask = this.currentStepTaskList[0]
       if (this.runningStepTask) {
@@ -699,20 +715,20 @@ export default class PtrObject extends THREE.Object3D {
         this.currentStepTaskList.shift()
         this.runningStepTaskList.push(stepTask)
         if (stepTask.StepTaskType == 'MOVE' || stepTask.StepTaskType == 'MOVE_BACKWARD') {
-          this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection,stepTask.Speed / 1000)
+          this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection, stepTask.Speed / 1000)
         } else if (stepTask.StepTaskType == 'ROTATION') {
           this.addRotation(stepTask.EndDirection)
         } else if (stepTask.StepTaskType == 'LOAD') {
-          this.addLoad(stepTask.GoodsSlotHeight / 1000)
+          this.addLoad(stepTask.GoodsSlotHeight / 1000, this.agvStatusVo.bizLpn)
         } else if (stepTask.StepTaskType == 'UNLOAD') {
-          this.addUnload(stepTask.GoodsSlotHeight / 1000)
+          this.addUnload(stepTask.GoodsSlotHeight / 1000, this.agvStatusVo.bizLpn)
         }
       }
     }
   }
 
   onActionCompleted() {
-    setTimeout(()=> {
+    setTimeout(() => {
       this.runningStepTaskList = []
       this.runningStepTask = null
       this.computeLogicXYAndDirection()
@@ -904,12 +920,21 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   // 取货
-  addLoad(height: number): void {
+  addLoad(height: number, goodsId: string): void {
+    this.actionAnimation = 'wq'
+    console.log('取货')
     this.PickMode = 1
     this.OperationType = 4
     this.animationUpFork(height).then(
-      () => this.animationShowFork(1.4).then(
+      () => this.animationShowFork(1.35).then(
-        () => this.animationUpFork(height + 0.2).then(
+        () => {
+          // 将物品拾取到机械臂上
+          const mesh = this.pickupItem(goodsId)
+          mesh.position.set(0, 0, -0.15)
+          mesh.rotation.set(0, THREE.MathUtils.degToRad(90), 0)
+          this.getArmObject().add(mesh)
+
+          this.animationUpFork(height + 0.2).then(
             () => this.animationHideFork().then(
               () => this.animationDownFork().then(() => {
                 this.actionAnimation = null
@@ -917,23 +942,32 @@ export default class PtrObject extends THREE.Object3D {
               })
             )
           )
+        }
       )
     )
   }
 
   // 卸货
-  addUnload(height: number): void {
+  addUnload(height: number, goodsId: string): void {
+    this.actionAnimation = 'wq'
+    console.log('卸货')
     this.PickMode = 2
     this.OperationType = 4
     this.animationUpFork(height + 0.2).then(
-      () => this.animationShowFork(1.4).then(
+      () => this.animationShowFork(1.35).then(
-        () => this.animationUpFork(height).then(
+        ()=>this.animationUpFork(height).then(
-          () => this.animationHideFork().then(
+          () => {
+            const a = this.agvStatusVo.unloadBasLocationVo
+            // 将物品从机械臂上卸下
+            this.dropItem(goodsId, a.rack, a.bay, a.level, a.cell)
+
+            this.animationHideFork().then(
               () => this.animationDownFork().then(() => {
                 this.actionAnimation = null
                 this.onActionCompleted()
               })
             )
+          }
         )
       )
     )
@@ -955,6 +989,10 @@ export default class PtrObject extends THREE.Object3D {
     return null
   }
 
+  override getArmObject(): THREE.Object3D | undefined {
+    return null
+  }
+
   //获取ptr的角度朝向
   getAmrOrientation(radY: number) {
     while (radY < 0) {
@@ -967,4 +1005,49 @@ export default class PtrObject extends THREE.Object3D {
     return THREE.MathUtils.radToDeg(radY)
   }
 
+  /**
+   * 拾取物品
+   */
+  pickupItem(id: string): THREE.Object3D {
+    // 找到物品所在的地方, 删除他
+    const item = this.viewport.entityManager.findItemById(id)
+    const wrap = this.viewport.entityManager.findObjectById(id) as MeshWrap
+
+    if (wrap.type !== 'MeshWrap') {
+      throw new Error(`无法拾取物品 ${id}，它不是一个有效的 MeshWrap`)
+    }
+    item.dt.storeAt = {
+      item: this.id + ""
+    }
+    const mesh = wrap.manager.wrapToObject3D(wrap)
+    this.viewport.entityManager.replaceObject(id, mesh)
+    return mesh
+  }
+
+  /**
+   * 卸下物品
+   */
+  dropItem(itemId: string, storeItemId: string, bay?: number, level?: number, cell?: number): void {
+
+    const item = this.viewport.entityManager.findItemById(itemId)
+
+    item.dt.storeAt = {
+      item: storeItemId,
+      bay: bay,
+      level: level,
+      cell: cell
+    }
+
+    const itemRenderer = getRenderer(item.t)
+    if (!itemRenderer) {
+      throw new Error(`Renderer for type ${item.t} not found`)
+    }
+
+    this.getArmObject().clear()
+
+    itemRenderer.tempViewport = this.viewport
+    itemRenderer.createOrUpdatePointForRuntime(item)
+    itemRenderer.tempViewport = null
+  }
+
 }

src/modules/amr/ptr/cl2/Cl23dObject.ts

@@ -13,8 +13,8 @@ export default class Cl23dObject extends PtrObject {
   private _cl2Entity: Cl2Entity = null
 
   override rotationSpeed = (Math.PI / 7)
-  override showForkSpeed: number = 0.15
+  override showForkSpeed: number = 0.2
-  override upForkSpeed: number = 0.15
+  override upForkSpeed: number = 0.2
 
   public get cl2Entity(): Cl2Entity {
     if (!this._cl2Entity) {
@@ -66,13 +66,13 @@ export default class Cl23dObject extends PtrObject {
   override animationShowFork(z: number): Promise<void> {
 
     const ptrPillar = this.getObjectByName('ptrPillar')
-    const time = Math.abs(z/ this.showForkSpeed)
+    const time = Math.abs((ptrPillar.position.z - z) / this.showForkSpeed)
     return new Promise(resolve => {
       this.actionAnimation = gsap.to(ptrPillar.position, {
         z: -z,
         duration: time,
         repeat: 0,
-        ease: 'sine.inOut',
+        ease: 'power1.inOut',
         onComplete: ()=>{
           setTimeout(() => {
             resolve()
@@ -131,6 +131,16 @@ export default class Cl23dObject extends PtrObject {
   override animationDownFork(): Promise<void> {
     return this.animationUpFork(0)
   }
+
+  override getArmObject(): THREE.Object3D | undefined {
+    if (this.children.length > 1) {
+      const pillar = this.children[1]
+      if (pillar.children.length > 1) {
+        return pillar.children[1]
+      }
+    }
+    return undefined
+  }
 }
 
 

src/modules/amr/ptr/cl2/Cl2Renderer.ts

@@ -34,7 +34,7 @@ export default class PtrRenderer extends BaseRenderer {
   } {
 
     return {
-      position: [0, 0.2, 0],
+      position: [0, 0.2, -0.15],
       rotation: [0, 90, 0],
       getParentObject3D: this.getArmObject.bind(this)
     }

src/modules/amr/ptr/clx/Clx3dObject.ts

@@ -109,7 +109,7 @@ export default class Clx3dObject extends PtrObject {
 
     const fy = clxFork.position.y
 
-    const time = Math.abs(z / this.upForkSpeed)
+    const time = Math.abs((clxFork.position.z - z) / this.upForkSpeed)
 
     const ac = Math.asin(z / (2 * 0.8))
 
@@ -121,13 +121,13 @@ export default class Clx3dObject extends PtrObject {
       y: fy + 0.72,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
     gsap.to(clxGemelMeshL1.rotation, {
       x: -ac,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
     // clxGemelMeshL2.position.z = 0.5 - z/4 * 3
     // clxGemelMeshL2.position.y = fy + 0.72
@@ -137,13 +137,13 @@ export default class Clx3dObject extends PtrObject {
       y: fy + 0.72,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
     gsap.to(clxGemelMeshL2.rotation, {
       x: -ac,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
 
     // clxGemelMeshR1.position.z = 0.5 - z/4
@@ -154,13 +154,13 @@ export default class Clx3dObject extends PtrObject {
       y: fy + 0.72,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
     gsap.to(clxGemelMeshR1.rotation, {
       x: ac,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
 
     // clxGemelMeshR2.position.z = 0.5 - z/4 * 3
@@ -171,20 +171,20 @@ export default class Clx3dObject extends PtrObject {
       y: fy + 0.72,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
     gsap.to(clxGemelMeshR2.rotation, {
       x: ac,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
 
     gsap.to(clxForkBasePlateMesh.position, {
       y: fy,
       duration: time,
       repeat: 0,
-      ease: 'sine.inOut'
+      ease: 'power1.inOut'
     })
 
     return new Promise(resolve => {
@@ -192,7 +192,7 @@ export default class Clx3dObject extends PtrObject {
         z: -z,
         duration: time,
         repeat: 0,
-        ease: 'sine.inOut',
+        ease: 'power1.inOut',
         onComplete: ()=>{
           setTimeout(() => {
             resolve()
@@ -287,4 +287,8 @@ export default class Clx3dObject extends PtrObject {
     return this.animationUpFork(0)
   }
 
+  override getArmObject(): THREE.Object3D | undefined {
+    const clxFork = this.getObjectByName('clxFork')
+    return clxFork
+  }
 }

src/modules/amr/ptr/clx/ClxRenderer.ts

@@ -71,7 +71,7 @@ export default class ClxRenderer extends BaseRenderer {
   } {
 
     return {
-      position: [0, 0.2, 0],
+      position: [0, 0.2, -0.15],
       rotation: [0, 90, 0],
       getParentObject3D: this.getArmObject.bind(this)
     }

src/modules/rack/RackRenderer.ts

@@ -103,7 +103,7 @@ export default class RackRenderer extends BaseRenderer {
     return {
       position: [
         worldPosition.x,
-        worldPosition.y, //+ 0.05, // 加上横梁高度
+        worldPosition.y,
         worldPosition.z
       ],
       rotation: [