|
|
|
@ -57,6 +57,9 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
|
|
|
|
override AGVModel = '' |
|
|
|
override AGVFnModel = '' |
|
|
|
override rotationSpeed: number = 0 |
|
|
|
override showForkSpeed: number = 0 |
|
|
|
override upForkSpeed: number = 0 |
|
|
|
|
|
|
|
private boxBody: any = null |
|
|
|
private __toPos: THREE.Vector3 = null |
|
|
|
@ -169,6 +172,7 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
this.subscribeMessage('/wcs_server/' + this.item.id) |
|
|
|
this.subscribeMessage('/agv_robot/status') |
|
|
|
} |
|
|
|
window.agv3 = this |
|
|
|
} |
|
|
|
|
|
|
|
protected onFrameTimer() { |
|
|
|
@ -199,7 +203,7 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
|
|
|
|
const boxShape = new this.viewport.ammoModel.btBoxShape(new Ammo.btVector3(0.5, 0.5, 0.5)) |
|
|
|
|
|
|
|
const mass = 10 |
|
|
|
const mass = 1 |
|
|
|
const localInertia = new this.viewport.ammoModel.btVector3(0, 0, 0) |
|
|
|
boxShape.calculateLocalInertia(mass, localInertia) |
|
|
|
|
|
|
|
@ -486,7 +490,7 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
} |
|
|
|
|
|
|
|
makeStepTask(data: AmrMsg10010) { |
|
|
|
|
|
|
|
console.log('makeStepTask:', data) |
|
|
|
let currentStepTask: StepTask = this.runningStepTask |
|
|
|
if (currentStepTask == null) { |
|
|
|
if (this.currentStepTaskList && this.currentStepTaskList.length > 0) { |
|
|
|
@ -605,7 +609,7 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
|
|
|
|
if (endDirection != currentStepTask.EndDirection) { |
|
|
|
// 如果此处不能转弯，忽略结束方向 等待后续任务
|
|
|
|
if (!item.dt?.agvRotation || !item.dt?.agvRotation?.length <= 0) { |
|
|
|
if (!item.dt?.agvRotation || item.dt?.agvRotation?.length <= 0) { |
|
|
|
return |
|
|
|
} |
|
|
|
const stepTask: StepTask = { |
|
|
|
@ -684,7 +688,7 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
this.currentStepTaskList.shift() |
|
|
|
this.runningStepTaskList.push(stepTask) |
|
|
|
|
|
|
|
this.addTravel(stepTask.X, stepTask.Y, stepTask.Speed / 1000) |
|
|
|
this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection, stepTask.Speed / 1000) |
|
|
|
|
|
|
|
} else { |
|
|
|
break |
|
|
|
@ -694,7 +698,7 @@ 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.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') { |
|
|
|
@ -707,31 +711,23 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
} |
|
|
|
|
|
|
|
onActionCompleted() { |
|
|
|
this.runningStepTaskList = [] |
|
|
|
this.runningStepTask = null |
|
|
|
this.computeLogicXYAndDirection() |
|
|
|
this.computeRealSpeed() |
|
|
|
// 当前所有动作执行完毕
|
|
|
|
if (this.currentStepTaskList.length <= 0) { |
|
|
|
this.send20010() |
|
|
|
} |
|
|
|
this.PickMode = 0 |
|
|
|
this.OperationType = 0 |
|
|
|
this.TaskMode = 0 |
|
|
|
this.executeTask() |
|
|
|
setTimeout(()=> { |
|
|
|
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() |
|
|
|
}, 2000) |
|
|
|
} |
|
|
|
|
|
|
|
/*==========动作处理============*/ |
|
|
|
|
|
|
|
private realSpeed = 0 |
|
|
|
private __lastPosition = this.position.clone() |
|
|
|
|
|
|
|
computeRealSpeed() { |
|
|
|
this.realSpeed = this.__lastPosition.distanceTo(this.position) / this.clock.getDelta() |
|
|
|
this.__lastPosition.copy(this.position) |
|
|
|
console.log('realSpeed', this.realSpeed) |
|
|
|
} |
|
|
|
|
|
|
|
// 转
|
|
|
|
addRotation(direction: number): Promise<void> { |
|
|
|
let rad = 0 |
|
|
|
@ -754,7 +750,7 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
const angleDiff = quat1.angleTo(quat2) |
|
|
|
console.log(rad, this.rotation.y, angleDiff) |
|
|
|
const tr = this.rotation.y + angleDiff |
|
|
|
let time = Math.abs(angleDiff) / (Math.PI / 7) |
|
|
|
let time = Math.abs(angleDiff) / this.rotationSpeed |
|
|
|
const duration = time |
|
|
|
|
|
|
|
return new Promise(resolve => { |
|
|
|
@ -771,8 +767,12 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
}) |
|
|
|
} |
|
|
|
|
|
|
|
keepSpeed: any |
|
|
|
force: any |
|
|
|
reverseForce: any |
|
|
|
|
|
|
|
// 走
|
|
|
|
addTravel(logicX: number, logicY: number, speed: number = 1): Promise<void> { |
|
|
|
addTravel(logicX: number, logicY: number, direction: number, speed: number = 1): Promise<void> { |
|
|
|
|
|
|
|
|
|
|
|
this.OperationType = 0 |
|
|
|
@ -789,19 +789,31 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
} |
|
|
|
|
|
|
|
// 运动参数
|
|
|
|
const accelForce = speed > 0 ? 4 : (-4) |
|
|
|
const accelForce = speed > 0 ? a : (-a) |
|
|
|
let currentPhase = 'accelerate' |
|
|
|
|
|
|
|
this.keepSpeed = new this.viewport.ammoModel.btVector3(0, 0, 0) |
|
|
|
this.force = new this.viewport.ammoModel.btVector3(0, 0, 0) |
|
|
|
this.reverseForce = new this.viewport.ammoModel.btVector3(0, 0, 0) |
|
|
|
if (direction == 0 || direction == 2) { |
|
|
|
this.force.setValue(accelForce, 0, 0) |
|
|
|
this.reverseForce.setValue(-accelForce, 0, 0) |
|
|
|
this.keepSpeed.setValue(speed, 0, 0) |
|
|
|
} else if (direction == 1 || direction == 3) { |
|
|
|
this.force.setValue(0, 0, accelForce) |
|
|
|
this.reverseForce.setValue(0, 0, -accelForce) |
|
|
|
this.keepSpeed.setValue(0, 0, speed) |
|
|
|
} |
|
|
|
|
|
|
|
if (this.viewport.registerPhysicsUpdateCallBack.has(this.item.id)) { |
|
|
|
return null |
|
|
|
} |
|
|
|
|
|
|
|
this.travelAnimation = 'asd' |
|
|
|
|
|
|
|
const force = new this.viewport.ammoModel.btVector3(accelForce, 0, 0) |
|
|
|
this.viewport.registerPhysicsUpdateCallBack.set(this.item.id, () => { |
|
|
|
|
|
|
|
this.viewport.physicsWorld.stepSimulation(1 / 60, 20) |
|
|
|
this.viewport.physicsWorld.stepSimulation(1 / 60, 10) |
|
|
|
|
|
|
|
// 同步Three.js对象位置
|
|
|
|
const transform = new this.viewport.ammoModel.btTransform() |
|
|
|
@ -815,50 +827,55 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
const cSpeed = velocity.length() |
|
|
|
const stopTime = cSpeed / a |
|
|
|
const stopDistance = a * stopTime * stopTime / 2 |
|
|
|
if (distance <= 0.05) { |
|
|
|
if (distance <= 0.01) { |
|
|
|
const stopVel = new this.viewport.ammoModel.btVector3(0, 0, 0) |
|
|
|
this.boxBody.setLinearVelocity(stopVel) |
|
|
|
this.viewport.ammoModel.destroy(stopVel) |
|
|
|
this.viewport.registerPhysicsUpdateCallBack.delete(this.item.id) |
|
|
|
this.viewport.ammoModel.destroy(force) |
|
|
|
this.viewport.ammoModel.destroy(this.force) |
|
|
|
this.viewport.ammoModel.destroy(this.keepSpeed) |
|
|
|
this.position.set(this.__toPos.x, this.__toPos.y, this.__toPos.z) |
|
|
|
this.travelAnimation = null |
|
|
|
this.onActionCompleted() |
|
|
|
|
|
|
|
return |
|
|
|
} |
|
|
|
if (cSpeed == 0) { |
|
|
|
this.boxBody.activate() |
|
|
|
} |
|
|
|
if (distance <= stopDistance) { |
|
|
|
currentPhase = 'decelerate' |
|
|
|
if (distance <= stopDistance + 0.2) { |
|
|
|
if (currentPhase != 'decelerate') { |
|
|
|
currentPhase = 'decelerate' |
|
|
|
const sp = new this.viewport.ammoModel.btVector3(velocity.x(), velocity.y(), velocity.z()) |
|
|
|
this.boxBody.setLinearVelocity(sp) |
|
|
|
this.viewport.ammoModel.destroy(sp) |
|
|
|
} |
|
|
|
} else if (currentPhase == 'decelerate') { |
|
|
|
currentPhase = 'accelerate' |
|
|
|
} |
|
|
|
|
|
|
|
// 运动阶段控制
|
|
|
|
switch (currentPhase) { |
|
|
|
case 'accelerate': |
|
|
|
// 施加X轴正向力
|
|
|
|
|
|
|
|
if (velocity.x() == 0) { |
|
|
|
this.boxBody.activate() |
|
|
|
} |
|
|
|
this.boxBody.applyCentralForce(force) |
|
|
|
this.boxBody.applyCentralForce(this.force) |
|
|
|
|
|
|
|
// 达到最大速度进入匀速阶段
|
|
|
|
if (Math.abs(velocity.x()) >= Math.abs(speed)) { |
|
|
|
if (cSpeed >= Math.abs(speed) || distance <= stopDistance) { |
|
|
|
currentPhase = 'uniform' |
|
|
|
} |
|
|
|
this.boxBody.setDamping(0, 0); |
|
|
|
break |
|
|
|
case 'uniform': |
|
|
|
currentPhase = 'uniform' |
|
|
|
const newVel = new this.viewport.ammoModel.btVector3(speed, 0, 0) |
|
|
|
this.boxBody.setLinearVelocity(newVel) |
|
|
|
this.viewport.ammoModel.destroy(newVel) |
|
|
|
this.boxBody.setLinearVelocity(this.keepSpeed) |
|
|
|
this.boxBody.setDamping(0, 0); |
|
|
|
break |
|
|
|
case 'decelerate': |
|
|
|
// 检测停止
|
|
|
|
if (speed == 0) { |
|
|
|
const stopVel = new this.viewport.ammoModel.btVector3(0, 0, 0) |
|
|
|
this.boxBody.setLinearVelocity(stopVel) |
|
|
|
this.viewport.ammoModel.destroy(stopVel) |
|
|
|
currentPhase = 'stop' |
|
|
|
console.log('物体已完全停止') |
|
|
|
if (cSpeed > 0) { |
|
|
|
this.boxBody.setDamping(0.6, 0); |
|
|
|
} |
|
|
|
this.boxBody.activate() |
|
|
|
break |
|
|
|
} |
|
|
|
|
|
|
|
@ -882,56 +899,7 @@ export default class PtrObject extends THREE.Object3D { |
|
|
|
} |
|
|
|
|
|
|
|
}) |
|
|
|
|
|
|
|
|
|
|
|
return null |
|
|
|
//
|
|
|
|
// if (!this.travelAnimation) {
|
|
|
|
// return new Promise(resolve => {
|
|
|
|
// this.travelAnimation = gsap.to(this.position, {
|
|
|
|
// x: toPos.x,
|
|
|
|
// y: toPos.y,
|
|
|
|
// z: toPos.z,
|
|
|
|
// duration,
|
|
|
|
// ease: 'power1.inOut',
|
|
|
|
// onComplete: () => {
|
|
|
|
// this.travelAnimation = null
|
|
|
|
// resolve()
|
|
|
|
// this.onActionCompleted()
|
|
|
|
// },
|
|
|
|
// onUpdate: () => {
|
|
|
|
// this.computeRealSpeed()
|
|
|
|
// for (let i = 0; i < this.runningStepTaskList.length; i++) {
|
|
|
|
// const task = this.runningStepTaskList[i]
|
|
|
|
// if (task.isCompleted == false) {
|
|
|
|
// if (this.position.distanceTo(task.position) < 0.1) {
|
|
|
|
// task.isCompleted = true
|
|
|
|
// this.runningStepTaskList.splice(0, i + 1)
|
|
|
|
// const content: AmrMsg20020 = new AmrMsg20020(this.vehicleId)
|
|
|
|
// content.CurLogicX = task.X
|
|
|
|
// content.CurLogicY = task.Y
|
|
|
|
// content.CurX = task.X
|
|
|
|
// content.CurY = task.Y
|
|
|
|
// content.CurOrientation = task.EndDirection * 90
|
|
|
|
// content.CurDirection = task.EndDirection
|
|
|
|
// this.send20020(content)
|
|
|
|
// break
|
|
|
|
// }
|
|
|
|
// }
|
|
|
|
// }
|
|
|
|
// }
|
|
|
|
// })
|
|
|
|
// })
|
|
|
|
// } else {
|
|
|
|
// this.travelAnimation.vars.x = toPos.x
|
|
|
|
// this.travelAnimation.vars.y = toPos.y
|
|
|
|
// this.travelAnimation.vars.z = toPos.z
|
|
|
|
// this.travelAnimation.vars.ease = 'power1.out'
|
|
|
|
// const tt = this.travelAnimation.duration()
|
|
|
|
// this.travelAnimation.duration(duration)
|
|
|
|
// this.travelAnimation.invalidate().restart();
|
|
|
|
// }
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
// 取货
|
|
|
|
|
