Merge remote-tracking branch 'origin/master'

src/core/manager/TaskManager.ts

@@ -1,7 +1,7 @@
 import * as THREE from 'three'
 import gsap from 'gsap'
 import Viewport from '@/core/engine/Viewport'
-import Clx3dObject from "@/modules/clx/Clx3dObject";
+import Clx3dObject from "@/modules/amr/ptr/clx/Clx3dObject";
 
 export default class TaskManager implements TaskManagerIF {
 

src/core/manager/WorldModel.ts

@@ -108,8 +108,8 @@ export default class WorldModel {
       import('../../modules/pallet'),
       import('../../modules/tote'),
       import('../../modules/carton'),
-      import('../../modules/cl2'),
-      import('../../modules/clx'),
+      import('../../modules/amr/ptr/cl2'),
+      import('../../modules/amr/ptr/clx'),
       import('../../modules/charger')
 
     ]).then(() => {

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/core/manager/amr/AmrMessageManager.ts

@@ -1,5 +1,5 @@
 import { AmrMsg, AmrMsg10010, AmrMsg10050, AmrMsg10060, AmrMsg10110, AmrMsg10120, AmrMsg20050 } from '@/core/manager/amr/AmrMessageDefine'
-import Cl23dObject from '@/modules/cl2/Cl23dObject'
+import Cl23dObject from '@/modules/amr/ptr/cl2/Cl23dObject'
 import Viewport from '@/core/engine/Viewport'
 
 export default class AmrMessageManager {

src/core/script/ModelManager.ts

@@ -4,8 +4,8 @@ import * as THREE from 'three'
 import { getMatrixFromTf } from '@/core/ModelUtils.ts'
 import type { Object3DLike } from '@/types/ModelTypes.ts'
 import TaskManager from '../manager/TaskManager.ts'
-import Cl2Entity from '@/modules/cl2/Cl2Entity.ts'
-import ClxEntity from '@/modules/clx/ClxEntity.ts'
+import Cl2Entity from '@/modules/amr/ptr/cl2/Cl2Entity.ts'
+import ClxEntity from '@/modules/amr/ptr/clx/ClxEntity.ts'
 import { getRenderer } from '@/core/manager/ModuleManager.ts'
 
 export default class ModelManager implements IControls, Model {

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

@@ -3,8 +3,8 @@ import {CSG} from 'three-csg-ts'
 import gsap from 'gsap'
 import mqtt from 'mqtt'
 import {Euler} from 'three/src/math/Euler'
-import Cl2Entity from '@/modules/cl2/Cl2Entity'
-import Cl23DGraphics from "@/modules/cl2/Cl23DGraphics"
+import Cl2Entity from '@/modules/amr/ptr/cl2/Cl2Entity'
+import Cl23DGraphics from "@/modules/amr/ptr/cl2/Cl23DGraphics"
 import {
   AmrErrorCode,
   AmrMsg,
@@ -20,8 +20,8 @@ import {
   AmrMsg20150,
   AmrMsg20250,
   CurBatteryData,
-  type CEventId, type COperationType, type CTaskMode,
-  type LogicDirection, TaskCompletedData, TaskModeChangeData, TaskStatusChangeData, TaskTypeChangeData
+  type CEventId, type COperationType, type CTaskMode, type CPickMode,
+  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,28 +230,30 @@ export default class Cl23dObject extends THREE.Object3D {
     this.bootTime = Date.now();
     this.computeLogicXYAndDirection();
 
-    if (worldModel.state.runState.isVirtual) {
-      this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
-      this.send20147()
+    if (!worldModel.state.runState.isVirtual) {
+      return
+    }
+    this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
+    this.send20147()
+    setTimeout(() => {
+      this.send20149()
+      this.TaskMode = 1
+      // 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标
       setTimeout(() => {
-        this.send20149()
-        this.TaskMode = 1
-        // 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标
+        this.sendCurrentPositionAndDirection()
         setTimeout(() => {
-          this.sendCurrentPositionAndDirection()
-          setTimeout(() => {
-            this.send20150()
-          }, 1000)
+          this.send20150()
         }, 1000)
-      }, 2000)
-    } else {
-
-    }
+      }, 1000)
+    }, 2000)
 
   }
 
   // 关机
   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)
         }
       }
     }
   }
 
-
-  /*==========动画处理============*/
-
-  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)
+  onActionCompleted() {
+    this.runningStepTaskList = []
+    this.runningStepTask = null
+    this.computeLogicXYAndDirection()
+    // 当前所有动作执行完毕
+    if (this.currentStepTaskList.length <= 0) {
+      this.send20010()
+    }
+    this.PickMode = 0
+    this.OperationType = 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.runningStepTask = null
+          this.onActionCompleted()
         }
       })
     })
@@ -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.runningStepTask = null
-            this.computeLogicXYAndDirection()
+            this.onActionCompleted()
           },
           onUpdate: () => {
 
@@ -807,10 +808,96 @@ export default class Cl23dObject extends THREE.Object3D {
 
   }
 
-  // fn = _.debounce((cl2: Cl2Entity) => {
-  //   cl2.taskStartRun()
-  // }, 2000)
+  // 取货
+  addLoad(height: number): void {
+    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)
+  }
 }
 
 

src/modules/cl2/Cl2Entity.ts → src/modules/amr/ptr/cl2/Cl2Entity.ts

@@ -3,7 +3,7 @@ import BaseEntity from '@/core/base/BaseItemEntity.ts'
 import type Viewport from '@/core/engine/Viewport.ts'
 import gsap from 'gsap'
 import { nextTick } from 'vue'
-import Cl23dObject, {type Cl2Task} from "@/modules/cl2/Cl23dObject";
+import Cl23dObject from "./Cl23dObject";
 /**
  * CL2 机械臂实体类
  * 0.4m/ss // a  max 1.2m/s
@@ -16,82 +16,7 @@ export default class Cl2Entity extends BaseEntity {
   }
 
 
-  // 移动
-  addRobotTask(task: Cl2Task) {
 
-    if (task.id === 10010) {
-      let startX = task.content.StartX
-      let startY = task.content.StartY
-
-      if (task.content.Link?.length > 0) {
-        for (let i = 0; i < task.content.Link.length; i++) {
-          const link = task.content.Link[i];
-          let moveDirection: 0 | 1 | 2 | 3 | 15 = 15;
-          // 计算 移动方向
-          if (startX !== link.X && startY !== link.Y) {
-            throw new Error('Invalid task');
-          } else if (startX === link.X) {
-            if (startY < link.Y && link.Speed > 0 || startY > link.Y && link.Speed < 0) {
-              moveDirection = 3
-            } else {
-              moveDirection = 1
-            }
-          } else {
-            if (startX > link.X && link.Speed > 0 || startX < link.X && link.Speed < 0) {
-              moveDirection = 0
-            } else {
-              moveDirection = 2
-            }
-          }
-          this.taskQueue.add(this.createTask('ROTATION',
-            () => this.cl2Object.addRotation(moveDirection * Math.PI/2)
-          ))
-          this.taskQueue.add(this.createTask('TRAVEL',
-            () => this.cl2Object.addTravel(Model.getPositionByLogicXY(link.X, link.Y) ,Math.abs(link.Speed/1000))
-          ))
-          startX = link.X
-          startY = link.Y
-        }
-      }
-      //移动
-      if (task.content.OperationType === 0) {
-
-      } else if (task.content.OperationType === 1) {
-
-      } else if (task.content.OperationType === 2) {
-
-      } else if (task.content.OperationType === 3) {
-
-      } else if (task.content.OperationType === 4) {
-        // 取货
-        if (task.content.PickMode === 1) {
-          const gh = task.content.GoodsSlotHeight/1000
-          this.addArmRaise(gh)
-          this.addArmExtender()
-          // this.addArmRaise(gh + 0.1)
-          this.addLoad(task.content.GoodsId)
-          this.addArmRetractor()
-          this.addArmLower()
-          // 放货
-        } else if (task.content.PickMode === 2) {
-          this.addArmRaise(task.content.GoodsSlotHeight/1000 + 0.2)
-          this.addArmExtender()
-          this.addArmLower()
-          this.addUnload(task.content.GoodsId, '')
-          this.addArmRetractor()
-        }
-
-      } else if (task.content.OperationType === 5) {
-
-      } else if (task.content.OperationType === 135) {
-
-      } else if (task.content.OperationType === 136) {}
-
-
-    }
-
-
-  }
 
   // 抬
   addArmRaise(height: number) {

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

@@ -1,7 +1,7 @@
 import * as THREE from 'three'
 import BaseRenderer from '@/core/base/BaseRenderer.ts'
 import Constract from '@/core/Constract.ts'
-import Cl23dObject from "@/modules/cl2/Cl23dObject";
+import Cl23dObject from "./Cl23dObject";
 
 /**
  * ptr侧叉渲染器

src/modules/clx/ClxEntity.ts → src/modules/amr/ptr/clx/ClxEntity.ts

@@ -2,7 +2,7 @@ import BaseEntity from '@/core/base/BaseItemEntity.ts'
 import type Viewport from '@/core/engine/Viewport.ts'
 import * as THREE from 'three'
 import gsap from 'gsap'
-import Clx3dObject from '@/modules/clx/Clx3dObject.ts'
+import Clx3dObject from './Clx3dObject.ts'
 
 export default class ClxEntity extends BaseEntity {
   constructor(viewport: Viewport, id: string) {

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

@@ -1,7 +1,7 @@
 import * as THREE from 'three'
 import BaseRenderer from '@/core/base/BaseRenderer.ts'
 import Constract from '@/core/Constract.ts'
-import Clx3dObject from "@/modules/clx/Clx3dObject";
+import Clx3dObject from "./Clx3dObject";
 
 /**
  * clx渲染器

src/modules/clx/index.ts → src/modules/amr/ptr/clx/index.ts

@@ -1,12 +1,11 @@
 import { defineModule } from '@/core/manager/ModuleManager.ts'
 import ClxRenderer from './ClxRenderer.ts'
 import ClxInteraction from './ClxInteraction.ts'
-import propertySetter from '@/modules/clx/ClxPropertySetter.ts'
+import propertySetter from './ClxPropertySetter.ts'
 
 export const ITEM_TYPE_NAME = 'clx'
 
 export default defineModule(ITEM_TYPE_NAME, () => ({
-  name: ITEM_TYPE_NAME,
   renderer: new ClxRenderer(ITEM_TYPE_NAME),
   interaction: new ClxInteraction(ITEM_TYPE_NAME),
   setter: propertySetter