diff --git a/src/core/manager/amr/AmrMessageDefine.ts b/src/core/manager/amr/AmrMessageDefine.ts index c48729c..f7dc3ba 100644 --- a/src/core/manager/amr/AmrMessageDefine.ts +++ b/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 { 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 = { diff --git a/src/modules/cl2/Cl23dObject.ts b/src/modules/cl2/Cl23dObject.ts index c0012ec..90a75c0 100644 --- a/src/modules/cl2/Cl23dObject.ts +++ b/src/modules/cl2/Cl23dObject.ts @@ -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(content) + this.sendMessage(m20010) + } + send20011(content: AmrMsg20011) { const m20011 = new AmrMsg>(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) { + 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(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(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 { - - 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 { - return this.animationShowFork(0) - } - - animationUpFork(y: number, time?: number = 3): Promise { - 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 { - 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 { @@ -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 { - + 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 { + + 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 { + return this.animationShowFork(0) + } + + animationUpFork(y: number, time?: number = 3): Promise { + 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 { + return this.animationUpFork(0) + } }