diff --git a/src/modules/amr/ptr/PtrObject.ts b/src/modules/amr/ptr/PtrObject.ts index 6594e3e..fd3200d 100644 --- a/src/modules/amr/ptr/PtrObject.ts +++ b/src/modules/amr/ptr/PtrObject.ts @@ -21,6 +21,7 @@ import {Euler} from 'three/src/math/Euler' import gsap from 'gsap' import {MeshWrap} from "@/core/manager/InstanceMeshManager"; import {getRenderer} from "@/core/manager/ModuleManager"; +import {MathUtils} from "three"; type CStepTaskType = 'MOVE' | 'MOVE_BACKWARD' | 'ROTATION' | 'LOAD' | 'UNLOAD' | 'CHARGE' @@ -89,6 +90,9 @@ export default class PtrObject extends THREE.Object3D { private agvStatusVo: AgvStatusVo; + // 是否模拟设备 + private isMonitoring: boolean = false + get TaskMode(): CTaskMode { return this.__TaskMode } @@ -171,14 +175,11 @@ export default class PtrObject extends THREE.Object3D { this.vehicleId = parseInt(item.id) this.viewport.addFrameTimerCallback(this.item.id, this.onFrameTimer.bind(this)) - if (!worldModel.state.runState.isVirtual) { - this.bootForShow() - } window.agv3 = this } protected onFrameTimer() { - if (!worldModel.state.runState.isVirtual) { + if (!this.isMonitoring) { return } const delta = this.clock.getDelta() @@ -220,7 +221,7 @@ export default class PtrObject extends THREE.Object3D { } bootForShow() { - + this.isMonitoring = false this.boot() this.subscribeMessage('/wcs_server/' + this.item.id) this.subscribeMessage('/agv_robot/status') @@ -228,7 +229,7 @@ export default class PtrObject extends THREE.Object3D { } bootForMonitor() { - + this.isMonitoring = true this.boot() this.subscribeMessage('/wcs_server/' + this.vehicleId) this.send20147() @@ -248,7 +249,7 @@ export default class PtrObject extends THREE.Object3D { // 关机 shutdown() { - if (!worldModel.state.runState.isVirtual) { + if (!this.isMonitoring) { return } const content = new AmrMsg20148(this.vehicleId) @@ -381,7 +382,7 @@ export default class PtrObject extends THREE.Object3D { } sendMessage(msg: AmrMsg) { - if (!worldModel.state.runState.isVirtual) { + if (!this.isMonitoring) { return } // console.log('send message:', JSON.stringify(msg)) @@ -396,7 +397,7 @@ export default class PtrObject extends THREE.Object3D { } sendHeartBeat() { - if (!worldModel.state.runState.isVirtual) { + if (!this.isMonitoring) { return } const content = new AmrMsg20100(this.vehicleId) @@ -406,7 +407,7 @@ export default class PtrObject extends THREE.Object3D { } sendAck(seqNo: number, vehicleId: number) { - if (!worldModel.state.runState.isVirtual) { + if (!this.isMonitoring) { return } const msg20050 = new AmrMsg20050(seqNo, vehicleId) @@ -463,13 +464,11 @@ 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) + this.messageSetXYAndRotation(data.CurLogicX, data.CurLogicY, data.CurDirection, data.CurOrientation) } handle20060Message(data: AmrMsg20060) { - const p = Model.getPositionByLogicXY(data.CurLogicX, data.CurLogicY) as THREE.Vector3 - this.position.set(p.x, 0, p.z) + this.messageSetXYAndRotation(data.CurLogicX, data.CurLogicY, null, data.CurOrientation) } /*==========真车消息处理============*/ @@ -510,6 +509,47 @@ export default class PtrObject extends THREE.Object3D { this.currentOrientation = this.getAmrOrientation(this.rotation.y) } + messageSetXYAndRotation(logicX: number, logicY: number, direction: number, rotation: number) { + const p = Model.getPositionByLogicXY(logicX, logicY) as THREE.Vector3 + + let r = 0 + if (direction != null && direction != 15) { + switch (direction) { + case 0: + r = 0 + break + case 1: + r = Math.PI / 2 * 3 + break + case 2: + r = Math.PI + break + case 3: + r = Math.PI / 2 + break + } + } else { + r = this.getAmrOrientation(rotation) + } + const quat1 = new THREE.Quaternion().setFromEuler(this.rotation) + const euler: Euler = new Euler(this.rotation.x, r, this.rotation.z) + const quat2 = new THREE.Quaternion().setFromEuler(euler) + const angleDiff = quat1.angleTo(quat2) + + if (p.distanceTo(this.position) < 0.5 && Math.abs(angleDiff) < Math.PI/4) { + return + } + + this.position.set(p.x, 0, p.z) + + const transform = new Ammo.btTransform(); + this.boxBody.getMotionState().getWorldTransform(transform); + transform.setOrigin(new Ammo.btVector3(p.x, 0, p.z)); // 设置新位置 + this.boxBody.getMotionState().setWorldTransform(transform); + this.viewport.ammoModel.destroy(transform) + this.rotation.set(0, r, 0) + } + makeStepTask(data: AmrMsg10010) { console.log('makeStepTask:', data) let currentStepTask: StepTask = this.runningStepTask @@ -854,6 +894,7 @@ export default class PtrObject extends THREE.Object3D { const transform = new this.viewport.ammoModel.btTransform() this.boxBody.getMotionState().getWorldTransform(transform) const position = transform.getOrigin() + this.viewport.ammoModel.destroy(transform) this.position.set(position.x(), position.y(), position.z()) // this.boxBody.getActivationState() // 获取当前速度