From 78691ff8a4df9f12385887f576d88033949888bc Mon Sep 17 00:00:00 2001 From: yuliang <398780299@qq.com> Date: Wed, 9 Jul 2025 18:59:40 +0800 Subject: [PATCH] =?UTF-8?q?cl2=20=E5=9F=BA=E4=BA=8E=E8=AE=BE=E5=A4=87?= =?UTF-8?q?=E7=8A=B6=E6=80=81=E5=92=8C=E4=BB=BB=E5=8A=A1=E7=8A=B6=E6=80=81?= =?UTF-8?q?=E5=88=86=E6=AD=A5=E7=94=9F=E6=88=90=E8=AE=BE=E5=A4=87=E4=BB=BB?= =?UTF-8?q?=E5=8A=A1=E6=B6=88=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/core/manager/EnvManager.ts | 5 +- src/core/manager/amr/AmrMessageManager.ts | 5 + src/modules/amr/ptr/PtrObject.ts | 147 +++++++++++++++++++++++------- src/modules/amr/ptr/cl2/Cl23dObject.ts | 18 +++- src/modules/amr/ptr/cl2/Cl2Renderer.ts | 2 +- src/modules/amr/ptr/clx/Clx3dObject.ts | 26 +++--- src/modules/amr/ptr/clx/ClxRenderer.ts | 2 +- src/modules/rack/RackRenderer.ts | 2 +- 8 files changed, 155 insertions(+), 52 deletions(-) diff --git a/src/core/manager/EnvManager.ts b/src/core/manager/EnvManager.ts index 53339ee..f5d75ef 100644 --- a/src/core/manager/EnvManager.ts +++ b/src/core/manager/EnvManager.ts @@ -20,8 +20,9 @@ export default class EnvManager { if (topic.startsWith('/wcs_server/')) { const message: AmrMsg = JSON.parse(payload.toString()) this.amrMessageManager.handleMessage(topic, message) - } else { - + } else if (topic.startsWith('/agv_robot/status')) { + const message: AmrMsg = JSON.parse(payload.toString()) + this.amrMessageManager.handleStatusMessage(topic, message) } } diff --git a/src/core/manager/amr/AmrMessageManager.ts b/src/core/manager/amr/AmrMessageManager.ts index 5d3e73d..86fbe02 100644 --- a/src/core/manager/amr/AmrMessageManager.ts +++ b/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) { + // switch () + } } diff --git a/src/modules/amr/ptr/PtrObject.ts b/src/modules/amr/ptr/PtrObject.ts index b9f7fe9..ce37605 100644 --- a/src/modules/amr/ptr/PtrObject.ts +++ b/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) { @@ -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(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) { - endDirection = 3 + if (data.GoodsSlotDirection == 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() { - this.TaskMode = 2 + 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,36 +920,54 @@ 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.animationUpFork(height + 0.2).then( - () => this.animationHideFork().then( - () => this.animationDownFork().then(() => { - this.actionAnimation = null - this.onActionCompleted() - }) + () => this.animationShowFork(1.35).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 + this.onActionCompleted() + }) + ) ) - ) + } ) ) } // 卸货 - 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.animationUpFork(height).then( - () => this.animationHideFork().then( - () => this.animationDownFork().then(() => { - this.actionAnimation = null - this.onActionCompleted() - }) - ) + () => this.animationShowFork(1.35).then( + ()=>this.animationUpFork(height).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 + } + } diff --git a/src/modules/amr/ptr/cl2/Cl23dObject.ts b/src/modules/amr/ptr/cl2/Cl23dObject.ts index dabbea2..357293e 100644 --- a/src/modules/amr/ptr/cl2/Cl23dObject.ts +++ b/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 upForkSpeed: number = 0.15 + override showForkSpeed: number = 0.2 + 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 { 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 { 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 + } } diff --git a/src/modules/amr/ptr/cl2/Cl2Renderer.ts b/src/modules/amr/ptr/cl2/Cl2Renderer.ts index 586ce1e..916989c 100644 --- a/src/modules/amr/ptr/cl2/Cl2Renderer.ts +++ b/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) } diff --git a/src/modules/amr/ptr/clx/Clx3dObject.ts b/src/modules/amr/ptr/clx/Clx3dObject.ts index 3dc1b43..a49b07c 100644 --- a/src/modules/amr/ptr/clx/Clx3dObject.ts +++ b/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 + } } diff --git a/src/modules/amr/ptr/clx/ClxRenderer.ts b/src/modules/amr/ptr/clx/ClxRenderer.ts index 47708c3..5d7d366 100644 --- a/src/modules/amr/ptr/clx/ClxRenderer.ts +++ b/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) } diff --git a/src/modules/rack/RackRenderer.ts b/src/modules/rack/RackRenderer.ts index dab1850..d15deb6 100644 --- a/src/modules/rack/RackRenderer.ts +++ b/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: [