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

src/core/manager/amr/AmrMessageDefine.ts

@@ -12,6 +12,9 @@ type CTaskMode = 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8
 // 0：无变化 1：任务模式改变 2：任务接收成功 3：任务开始 4：任务完成 5：任务已取消 6：任务已停止 7：任务已恢复 8：任务已更变
 type CEventId = 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8
 
+// 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
+type CPickMode = 0 | 1 | 2 | 3 | 4 | 5 | 6
+
 class AmrMsg<T> {
   id: number
   content: T
@@ -144,7 +147,7 @@ class AmrMsg20010 extends AmrMsg20000Base {
       // 执行器名称 String
       Name: string;
       // 任务类型 UInt8 同下发任务中PickMode的定义
-      PickMode: number;
+      PickMode: CPickMode;
     }[]
   };
   // 车载货位信息
@@ -378,7 +381,7 @@ class TaskCompletedData {
       // 执行器名称 String
       Name: string;
       // 任务类型 UInt8 同下发任务中PickMode的定义
-      PickMode: number;
+      PickMode: CPickMode;
     }[]
   };
   // 电量百分比	Uint8
@@ -539,6 +542,7 @@ export {
   COperationType,
   CTaskMode,
   CEventId,
+  CPickMode,
 }
 
 const AmrErrorCode = {

src/modules/cl2/Cl23dObject.ts

@@ -20,8 +20,8 @@ import {
   AmrMsg20150,
   AmrMsg20250,
   CurBatteryData,
-  type CEventId, type COperationType, type CTaskMode,
+  type CEventId, type COperationType, type CTaskMode, type CPickMode,
-  type LogicDirection, TaskCompletedData, TaskModeChangeData, TaskStatusChangeData, TaskTypeChangeData
+  type LogicDirection, TaskCompletedData, TaskModeChangeData, TaskStatusChangeData, TaskTypeChangeData, AmrMsg20010
 } from "@/core/manager/amr/AmrMessageDefine";
 import {worldModel} from "@/core/manager/WorldModel";
 
@@ -64,6 +64,7 @@ export default class Cl23dObject extends THREE.Object3D {
   private __TaskMode: CTaskMode = 0
   private __OperationType: COperationType = 0
   private __TaskStatus: CEventId = 0
+  private __PickMode: CPickMode = 0
 
   get TaskMode(): CTaskMode {
     return this.__TaskMode
@@ -125,6 +126,15 @@ export default class Cl23dObject extends THREE.Object3D {
     }
   }
 
+  get PickMode(): CPickMode {
+    return this.__PickMode
+  }
+  set PickMode(value: CPickMode) {
+    if (this.__PickMode !== value) {
+      this.__PickMode = value
+    }
+  }
+
   private bootTime: number = 0
 
   // 心跳间隔 UInt32 单位: s
@@ -180,6 +190,10 @@ export default class Cl23dObject extends THREE.Object3D {
 
     this.vehicleId = parseInt(this.cl2Entity.id)
     this.cl2Entity.viewport.addFrameTimerCallback(this.cl2Entity.id, this.onFrameTimer.bind(this))
+    if (!worldModel.state.runState.isVirtual) {
+      this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
+      this.subscribeMessage('/agv_robot/status')
+    }
   }
 
   private AGVModel = "CYBER-LIFT-A_V1.0"
@@ -216,7 +230,9 @@ export default class Cl23dObject extends THREE.Object3D {
     this.bootTime = Date.now();
     this.computeLogicXYAndDirection();
 
-    if (worldModel.state.runState.isVirtual) {
+    if (!worldModel.state.runState.isVirtual) {
+      return
+    }
     this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
     this.send20147()
     setTimeout(() => {
@@ -230,14 +246,14 @@ export default class Cl23dObject extends THREE.Object3D {
         }, 1000)
       }, 1000)
     }, 2000)
-    } else {
-
-    }
 
   }
 
   // 关机
   shutdown() {
+    if (!worldModel.state.runState.isVirtual) {
+      return
+    }
     const content = new AmrMsg20148(this.vehicleId)
     // 电量
     content.Battery = 100
@@ -310,6 +326,29 @@ export default class Cl23dObject extends THREE.Object3D {
     }
   }
 
+  send20010() {
+    const content: AmrMsg20010 = new AmrMsg20010(this.vehicleId)
+    content.CurX = this.currentLogicX
+    content.CurY = this.currentLogicY
+    content.CurDirection = this.currentDirection
+    content.OperationType = this.OperationType
+    content.Battery = this.Battery
+    content.OperationResult = 0
+    content.Summary = {
+      ActuatorsData: [
+        {
+          MechNo: 1,
+          Name: "Mech1",
+          PickMode: this.PickMode
+        }
+      ]
+    }
+
+
+    const m20010 = new AmrMsg<AmrMsg20010>(content)
+    this.sendMessage(m20010)
+  }
+
   send20011(content: AmrMsg20011<any>) {
     const m20011 = new AmrMsg<AmrMsg20011<any>>(content)
     this.sendMessage(m20011)
@@ -340,10 +379,13 @@ export default class Cl23dObject extends THREE.Object3D {
   }
 
   subscribeMessage(topic: string) {
-    this.cl2Entity.viewport.envManager.client.subscribe(topic, {qos: 0})
+    worldModel.envManager.client.subscribe(topic, {qos: 0})
   }
 
   sendMessage(msg: AmrMsg<any>) {
+    if (!worldModel.state.runState.isVirtual) {
+      return
+    }
     console.log('send message:', JSON.stringify(msg))
     if (this.sendMessageQueue.indexOf(msg) < 0) {
       this.sendMessageQueue.push(msg)
@@ -351,26 +393,31 @@ export default class Cl23dObject extends THREE.Object3D {
     if (this.sendMessageQueue.length <= 0) {
       this.mqRetryTimeCount = 0
     }
-    this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(msg))
+    worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(msg))
     this.heartBeatTimeCount = 0
   }
 
   sendHeartBeat() {
+    if (!worldModel.state.runState.isVirtual) {
+      return
+    }
     const content = new AmrMsg20100(this.vehicleId)
     content.Temperature = {Battery: this.Battery}
     const m20100 = new AmrMsg<AmrMsg20100>(content)
-    this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(m20100))
+    worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(m20100))
   }
 
   sendAck(seqNo: number, vehicleId: number) {
+    if (!worldModel.state.runState.isVirtual) {
+      return
+    }
     const msg20050 = new AmrMsg20050(seqNo, vehicleId)
     const ack = new AmrMsg<AmrMsg20050>(msg20050)
     this.heartBeatTimeCount = 0
-    this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(ack))
+    worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(ack))
   }
 
-  /*==========消息处理============*/
+  /*==========RCS消息处理============*/
-
   // 处理任务
   handle10010Message(data: AmrMsg10010) {
     if (this.currentStepTaskList.length > 0) {
@@ -410,6 +457,8 @@ export default class Cl23dObject extends THREE.Object3D {
 
   }
 
+  /*==========真车消息处理============*/
+
   // 计算逻辑方向
   computeLogicXYAndDirection() {
     let ra = this.rotation.y
@@ -610,7 +659,7 @@ export default class Cl23dObject extends THREE.Object3D {
   }
 
   executeTask() {
-
+    this.TaskMode = 2
     while (this.currentStepTaskList.length > 0) {
       const stepTask = this.currentStepTaskList[0]
       if (this.runningStepTask) {
@@ -635,75 +684,29 @@ export default class Cl23dObject extends THREE.Object3D {
         } else if (stepTask.StepTaskType == "ROTATION") {
           this.addRotation(stepTask.EndDirection)
         } else if (stepTask.StepTaskType == "LOAD") {
-          // this.add
+          this.addLoad(stepTask.GoodsSlotHeight/1000)
         } else if (stepTask.StepTaskType == "UNLOAD") {
-          // this.add
+          this.addUnload(stepTask.GoodsSlotHeight/1000)
         }
       }
     }
   }
 
-
+  onActionCompleted() {
-  /*==========动画处理============*/
+    this.runningStepTaskList = []
-
+    this.runningStepTask = null
-  animationShowFork(z: number): Promise<void> {
+    this.computeLogicXYAndDirection()
-
+    // 当前所有动作执行完毕
-    const ptrPillar = this.getObjectByName('ptrPillar')
+    if (this.currentStepTaskList.length <= 0) {
-    const time = 3
+      this.send20010()
-    return new Promise(resolve => {
-      gsap.to(ptrPillar.position, {
-        z: -z,
-        duration: time,
-        repeat: 0,
-        ease: 'sine.inOut',
-        onComplete: resolve
-      })
-    })
-
-  }
-
-  animationHideFork(): Promise<void> {
-    return this.animationShowFork(0)
-  }
-
-  animationUpFork(y: number, time?: number = 3): Promise<void> {
-    const ptrFork = this.getObjectByName('ptrFork')
-    const ptrPillar = this.getObjectByName('ptrPillar')
-    const pz = ptrPillar.position.z
-    return new Promise(resolve => {
-      const bh = 0.22
-      const children = ptrFork.children
-
-      gsap.to(ptrFork.position, {
-        y: y,
-        duration: time,
-        repeat: 0,
-        ease: 'sine.inOut',
-        onComplete: resolve,
-        onUpdate: function () {
-          const a = this.targets()[0]
-          if (a.y < bh) {
-            if (pz > -1) {
-              for (let i = 0; i < children.length; i++) {
-                const child = children[i]
-                child.position.y = bh - a.y
-              }
-            } else if (a.y < 0) {
-              for (let i = 0; i < children.length; i++) {
-                const child = children[i]
-                child.position.y = 0 - a.y
-              }
-            }
-          }
-        }
-      })
-    })
     }
-
+    this.PickMode = 0
-  animationDownFork(): Promise<void> {
+    this.OperationType = 0
-    return this.animationUpFork(0)
+    this.TaskMode = 0
+    this.executeTask()
   }
 
+  /*==========动画处理============*/
 
   // 转
   addRotation(direction: number): Promise<void> {
@@ -737,8 +740,7 @@ export default class Cl23dObject extends THREE.Object3D {
         ease: 'none',
         onComplete: ()=>{
           resolve()
-          this.runningStepTaskList = []
+          this.onActionCompleted()
-          this.runningStepTask = null
         }
       })
     })
@@ -746,7 +748,8 @@ export default class Cl23dObject extends THREE.Object3D {
 
   // 走
   addTravel(logicX: number, logicY: number, speed: number = 1): Promise<void> {
-
+    this.OperationType = 0
+    this.PickMode = 0
     const pos = Model.getPositionByLogicXY(logicX, logicY)
 
     const fromPos = this.position
@@ -769,9 +772,7 @@ export default class Cl23dObject extends THREE.Object3D {
           onComplete: () => {
             this.travelAnimation = null
             resolve()
-            this.runningStepTaskList = []
+            this.onActionCompleted()
-            this.runningStepTask = null
-            this.computeLogicXYAndDirection()
           },
           onUpdate: () => {
 
@@ -807,10 +808,96 @@ export default class Cl23dObject extends THREE.Object3D {
 
   }
 
-  // fn = _.debounce((cl2: Cl2Entity) => {
+  // 取货
-  //   cl2.taskStartRun()
+  addLoad(height: number): void {
-  // }, 2000)
+    this.PickMode = 1
+    this.OperationType = 4
+    this.animationUpFork(height).then(
+      () => this.animationShowFork(1.4).then(
+        ()=>this.animationUpFork(height + 0.2).then(
+          ()=>this.animationHideFork().then(
+            ()=>this.animationDownFork().then(()=>{
+              this.onActionCompleted()
+            })
+          )
+        )
+      )
+    )
+  }
+  // 卸货
+  addUnload(height: number): void {
+    this.PickMode = 2
+    this.OperationType = 4
+    this.animationUpFork(height + 0.2).then(
+      () => this.animationShowFork(1.4).then(
+        ()=>this.animationUpFork(height).then(
+          ()=>this.animationHideFork().then(
+            ()=>this.animationDownFork().then(()=>{
+              this.onActionCompleted()
+            })
+          )
+        )
+      )
+    )
+  }
+
+  animationShowFork(z: number): Promise<void> {
+
+    const ptrPillar = this.getObjectByName('ptrPillar')
+    const time = 3
+    return new Promise(resolve => {
+      gsap.to(ptrPillar.position, {
+        z: -z,
+        duration: time,
+        repeat: 0,
+        ease: 'sine.inOut',
+        onComplete: resolve
+      })
+    })
+
+  }
 
+  animationHideFork(): Promise<void> {
+    return this.animationShowFork(0)
+  }
+
+  animationUpFork(y: number, time?: number = 3): Promise<void> {
+    const ptrFork = this.getObjectByName('ptrFork')
+    const ptrPillar = this.getObjectByName('ptrPillar')
+    const pz = ptrPillar.position.z
+    return new Promise(resolve => {
+      const bh = 0.22
+      const children = ptrFork.children
+
+      gsap.to(ptrFork.position, {
+        y: y,
+        duration: time,
+        repeat: 0,
+        ease: 'sine.inOut',
+        onComplete: resolve,
+        onUpdate: function () {
+          const a = this.targets()[0]
+          if (a.y < bh) {
+            if (pz > -1) {
+              for (let i = 0; i < children.length; i++) {
+                const child = children[i]
+                child.position.y = bh - a.y
+              }
+            } else if (a.y < 0) {
+              for (let i = 0; i < children.length; i++) {
+                const child = children[i]
+                child.position.y = 0 - a.y
+              }
+            }
+          }
+        }
+      })
+    })
+  }
+
+  animationDownFork(): Promise<void> {
+    return this.animationUpFork(0)
+  }
 }