You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1073 lines
32 KiB
1073 lines
32 KiB
import * as THREE from 'three'
|
|
import {
|
|
AmrErrorCode,
|
|
AmrMsg, AmrMsg10010, AmrMsg10050, AmrMsg10060, AmrMsg10110, AmrMsg10120, AmrMsg20010,
|
|
AmrMsg20011, AmrMsg20020, AmrMsg20050, AmrMsg20060, AmrMsg20100,
|
|
AmrMsg20147,
|
|
AmrMsg20148, AmrMsg20149, AmrMsg20150, AmrMsg20250,
|
|
type CEventId,
|
|
type COperationType,
|
|
type CPickMode,
|
|
type CTaskMode, CurBatteryData,
|
|
type LogicDirection,
|
|
TaskCompletedData,
|
|
TaskModeChangeData,
|
|
TaskStatusChangeData,
|
|
TaskTypeChangeData
|
|
} from '@/core/manager/amr/AmrMessageDefine'
|
|
import {worldModel} from '@/core/manager/WorldModel'
|
|
import Viewport from '@/core/engine/Viewport'
|
|
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'
|
|
|
|
export interface StepTask {
|
|
SeqNo: number;
|
|
StepTaskType: CStepTaskType;
|
|
OperationType: 0 | 1 | 2 | 3 | 4 | 5 | 135 | 136;
|
|
PickMode: 0 | 1 | 2 | 3 | 4 | 5 | 6;
|
|
X: number;
|
|
Y: number;
|
|
Speed: number;
|
|
EndDirection: 0 | 1 | 2 | 3 | 15;
|
|
ChargeLocation: number;
|
|
GoodsSlotHeight: number;
|
|
position: THREE.Vector3;
|
|
isCompleted: boolean;
|
|
}
|
|
|
|
export default class PtrObject extends THREE.Object3D {
|
|
|
|
public item: ItemJson
|
|
public readonly viewport: Viewport
|
|
|
|
public currentLogicX: number = -1
|
|
public currentLogicY: number = -1
|
|
public currentDirection: LogicDirection = 15
|
|
public currentOrientation: number = 0
|
|
public sendMessageQueue: AmrMsg<any>[] = []
|
|
|
|
public Battery: number = 100
|
|
|
|
public vehicleId: number
|
|
|
|
public clock = new THREE.Clock()
|
|
|
|
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
|
|
|
|
private currentStepTaskList: StepTask[] = []
|
|
private runningStepTask: StepTask = null
|
|
public runningStepTaskList: StepTask[] = []
|
|
|
|
// 移动动画
|
|
public travelAnimation: core.Tween = null
|
|
// 旋转动画
|
|
public rotationAnimation: core.Tween = null
|
|
// 动作动画
|
|
public actionAnimation: core.Tween = null
|
|
|
|
// 心跳间隔 UInt32 单位: s
|
|
private heartBeatInterval: number = 0
|
|
// 小车所有上报消息重试间隔(未收到应答消息时重发消息) UInt32 单位: s
|
|
private mqRetryInterval: number = 3
|
|
|
|
private __TaskMode: CTaskMode = 0
|
|
private __OperationType: COperationType = 0
|
|
private __TaskStatus: CEventId = 0
|
|
private __PickMode: CPickMode = 0
|
|
|
|
private agvStatusVo: AgvStatusVo;
|
|
|
|
get TaskMode(): CTaskMode {
|
|
return this.__TaskMode
|
|
}
|
|
|
|
set TaskMode(value: CTaskMode) {
|
|
if (this.__TaskMode != value) {
|
|
const info = new TaskModeChangeData(this.__TaskMode, value)
|
|
const msg = new AmrMsg20011(this.vehicleId, info)
|
|
msg.TaskMode = value
|
|
this.send20011(msg)
|
|
}
|
|
this.__TaskMode = value
|
|
}
|
|
|
|
get OperationType(): COperationType {
|
|
return this.__OperationType
|
|
}
|
|
|
|
set OperationType(value: COperationType) {
|
|
if (this.__OperationType != value) {
|
|
const info = new TaskTypeChangeData(this.__OperationType, value)
|
|
const msg = new AmrMsg20011(this.vehicleId, info)
|
|
msg.TaskMode = this.TaskMode
|
|
this.send20011(msg)
|
|
}
|
|
this.__OperationType = value
|
|
}
|
|
|
|
get TaskStatus(): CEventId {
|
|
return this.__TaskStatus
|
|
}
|
|
|
|
set TaskStatus(value: CEventId) {
|
|
if (this.__TaskStatus != value) {
|
|
|
|
if (value != 1 && value != 4 && value != 8) {
|
|
const info = new TaskStatusChangeData(this.OperationType)
|
|
const msg = new AmrMsg20011(this.vehicleId, info)
|
|
msg.TaskMode = this.TaskMode
|
|
msg.EventId = value
|
|
this.send20011(msg)
|
|
} else if (value == 4) {
|
|
|
|
const info = new TaskCompletedData(this.OperationType)
|
|
info.Battery = this.Battery
|
|
info.OperationResult = 0
|
|
info.CurLogicX = this.currentLogicX
|
|
info.CurLogicY = this.currentLogicY
|
|
info.CurX = this.currentLogicX
|
|
info.CurY = this.currentLogicY
|
|
info.CurDirection = this.currentDirection
|
|
info.CurOrientation = this.currentOrientation
|
|
const msg = new AmrMsg20011(this.vehicleId, info)
|
|
msg.TaskMode = this.TaskMode
|
|
this.send20011(msg)
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
get PickMode(): CPickMode {
|
|
return this.__PickMode
|
|
}
|
|
|
|
set PickMode(value: CPickMode) {
|
|
if (this.__PickMode !== value) {
|
|
this.__PickMode = value
|
|
}
|
|
}
|
|
|
|
private bootTime: number = 0
|
|
|
|
private heartBeatTimeCount: number = 0
|
|
private mqRetryTimeCount: number = 0
|
|
|
|
constructor(item: ItemJson, viewport: Viewport) {
|
|
super()
|
|
this.viewport = viewport
|
|
this.item = item
|
|
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) {
|
|
return
|
|
}
|
|
const delta = this.clock.getDelta()
|
|
this.mqRetryTimeCount += delta
|
|
if (this.mqRetryInterval > 0 && this.mqRetryTimeCount >= this.mqRetryInterval) {
|
|
this.mqRetryTimeCount = 0
|
|
// 在此处理消息重试
|
|
if (this.sendMessageQueue.length > 0) {
|
|
this.sendMessage(this.sendMessageQueue[0])
|
|
}
|
|
}
|
|
this.heartBeatTimeCount += delta
|
|
if (this.heartBeatInterval > 0 && this.heartBeatTimeCount >= this.heartBeatInterval) {
|
|
this.heartBeatTimeCount = 0
|
|
// 在此处发送心跳报文
|
|
this.sendHeartBeat()
|
|
}
|
|
}
|
|
|
|
// 开机
|
|
boot() {
|
|
this.bootTime = Date.now()
|
|
this.computeLogicXYAndDirection()
|
|
|
|
const boxShape = new this.viewport.ammoModel.btBoxShape(new Ammo.btVector3(0.5, 0.5, 0.5))
|
|
|
|
const mass = 1
|
|
const localInertia = new this.viewport.ammoModel.btVector3(0, 0, 0)
|
|
boxShape.calculateLocalInertia(mass, localInertia)
|
|
|
|
const boxTransform = new this.viewport.ammoModel.btTransform()
|
|
boxTransform.setIdentity()
|
|
boxTransform.setOrigin(new this.viewport.ammoModel.btVector3(this.position.x, this.position.y, this.position.z))
|
|
|
|
const boxMotionState = new this.viewport.ammoModel.btDefaultMotionState(boxTransform)
|
|
const rbInfo = new this.viewport.ammoModel.btRigidBodyConstructionInfo(mass, boxMotionState, boxShape, localInertia)
|
|
this.boxBody = new this.viewport.ammoModel.btRigidBody(rbInfo)
|
|
this.viewport.physicsWorld.addRigidBody(this.boxBody)
|
|
}
|
|
|
|
bootForShow() {
|
|
|
|
this.boot()
|
|
this.subscribeMessage('/wcs_server/' + this.item.id)
|
|
this.subscribeMessage('/agv_robot/status')
|
|
|
|
}
|
|
|
|
bootForMonitor() {
|
|
|
|
this.boot()
|
|
this.subscribeMessage('/wcs_server/' + this.vehicleId)
|
|
this.send20147()
|
|
setTimeout(() => {
|
|
this.send20149()
|
|
this.TaskMode = 1
|
|
// 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标
|
|
setTimeout(() => {
|
|
this.sendCurrentPositionAndDirection()
|
|
setTimeout(() => {
|
|
this.send20150()
|
|
}, 1000)
|
|
}, 1000)
|
|
}, 2000)
|
|
|
|
}
|
|
|
|
// 关机
|
|
shutdown() {
|
|
if (!worldModel.state.runState.isVirtual) {
|
|
return
|
|
}
|
|
const content = new AmrMsg20148(this.vehicleId)
|
|
// 电量
|
|
content.Battery = 100
|
|
content.CreateMonoTime = Date.now() - this.bootTime
|
|
content.Uptime = content.CreateMonoTime
|
|
const m20148 = new AmrMsg<AmrMsg20148>(content)
|
|
this.sendMessage(m20148)
|
|
}
|
|
|
|
// 开机上报
|
|
send20147() {
|
|
const content = new AmrMsg20147(this.vehicleId)
|
|
content.AGVModel = this.AGVModel
|
|
content.AGVFnModel = this.AGVFnModel
|
|
// 电量
|
|
content.Battery = 100
|
|
content.CreateMonoTime = Date.now() - this.bootTime
|
|
const m20147 = new AmrMsg<AmrMsg20147>(content)
|
|
this.sendMessage(m20147)
|
|
}
|
|
|
|
// 主程序启动上报
|
|
send20149() {
|
|
const content = new AmrMsg20149(this.vehicleId)
|
|
content.AGVModel = this.AGVModel
|
|
content.AGVFnModel = this.AGVFnModel
|
|
// 电量
|
|
content.Battery = 100
|
|
content.CreateMonoTime = Date.now() - this.bootTime
|
|
const m20149 = new AmrMsg<AmrMsg20149>(content)
|
|
this.sendMessage(m20149)
|
|
}
|
|
|
|
// 上线上报
|
|
send20150() {
|
|
const content = new AmrMsg20150(this.vehicleId)
|
|
content.AGVModel = this.AGVModel
|
|
content.AGVFnModel = this.AGVFnModel
|
|
content.Battery = 100
|
|
content.CreateMonoTime = Date.now() - this.bootTime
|
|
const m20150 = new AmrMsg<AmrMsg20150>(content)
|
|
this.sendMessage(m20150)
|
|
}
|
|
|
|
// 上报当前位姿，地标和方向
|
|
sendCurrentPositionAndDirection() {
|
|
if (this.currentLogicX <= 0 || this.currentLogicY <= 0) {
|
|
// 当前车辆所在位置未找到
|
|
const content = new AmrMsg20250(this.vehicleId)
|
|
content.Duration = 0
|
|
content.ErrCode = 5
|
|
content.ErrCodeName = AmrErrorCode[5].ErrCodeName
|
|
content.ErrEvtType = 1
|
|
content.ErrLevel = 14
|
|
content.ErrLifecycle = 2
|
|
content.CreateMonoTime = Date.now() - this.bootTime
|
|
const m20250 = new AmrMsg<AmrMsg20250>(content)
|
|
this.sendMessage(m20250)
|
|
} else {
|
|
// 发送正常地标信息
|
|
const content = new AmrMsg20020(this.vehicleId)
|
|
content.CurDirection = this.currentDirection
|
|
content.CurOrientation = this.currentOrientation
|
|
content.CurLogicX = this.currentLogicX
|
|
content.CurLogicY = this.currentLogicY
|
|
content.CurX = this.currentLogicX
|
|
content.CurY = this.currentLogicY
|
|
content.CreateMonoTime = Date.now() - this.bootTime
|
|
const m20020 = new AmrMsg<AmrMsg20020>(content)
|
|
this.sendMessage(m20020)
|
|
}
|
|
}
|
|
|
|
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)
|
|
}
|
|
|
|
send20020(content: AmrMsg20020) {
|
|
const m20020 = new AmrMsg<AmrMsg20020>(content)
|
|
this.sendMessage(m20020)
|
|
}
|
|
|
|
send20060() {
|
|
const content = new AmrMsg20060(this.vehicleId)
|
|
content.CreateMonoTime = Date.now() - this.bootTime
|
|
content.CurBattery = new CurBatteryData()
|
|
content.CurLogicX = this.currentLogicX
|
|
content.CurLogicY = this.currentLogicY
|
|
content.CurOrientation = this.currentOrientation
|
|
content.CurX = this.position.x
|
|
content.CurY = this.position.z
|
|
content.X = this.position.x
|
|
content.Y = this.position.z
|
|
const m20060 = new AmrMsg<AmrMsg20060>(content)
|
|
this.sendMessage(m20060)
|
|
}
|
|
|
|
send20250(content: AmrMsg20250) {
|
|
this.sendMessage(new AmrMsg<AmrMsg20250>(content))
|
|
}
|
|
|
|
subscribeMessage(topic: string) {
|
|
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)
|
|
}
|
|
if (this.sendMessageQueue.length <= 0) {
|
|
this.mqRetryTimeCount = 0
|
|
}
|
|
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)
|
|
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
|
|
worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(ack))
|
|
}
|
|
|
|
/*==========RCS消息处理============*/
|
|
|
|
// 处理任务
|
|
handle10010Message(data: AmrMsg10010) {
|
|
if (this.currentStepTaskList.length > 0) {
|
|
const tindex = this.currentStepTaskList.length
|
|
const lt = this.currentStepTaskList[tindex - 1]
|
|
if (this.runningStepTask.OperationType == 0 && this.runningStepTask.X == data.StartX && this.runningStepTask.Y == data.StartY) {
|
|
// this.currentStepTaskList = []
|
|
this.makeStepTask(data)
|
|
this.executeTask()
|
|
} else if (lt.OperationType == 0 && lt.X == data.StartX && lt.Y == data.StartY) {
|
|
this.makeStepTask(data)
|
|
} else {
|
|
// 此处应该有错误处理
|
|
console.log('handle10010Message:', data)
|
|
}
|
|
} else {
|
|
this.makeStepTask(data)
|
|
this.executeTask()
|
|
}
|
|
}
|
|
|
|
handle10050Message(data: AmrMsg<AmrMsg10050>) {
|
|
if (this.sendMessageQueue.length > 0 && data.content.SeqNo === this.sendMessageQueue[0].content.SeqNo) {
|
|
this.mqRetryTimeCount = 0
|
|
this.sendMessageQueue.shift()
|
|
}
|
|
}
|
|
|
|
handle10060Message(data: AmrMsg<AmrMsg10060>) {
|
|
this.mqRetryInterval = data.content.MqRetryTime
|
|
this.heartBeatInterval = data.content.HeartBeat
|
|
this.TaskMode = 0
|
|
}
|
|
|
|
// 处理状态查询
|
|
handle10110Message(data: AmrMsg<AmrMsg10110>) {
|
|
this.send20060()
|
|
}
|
|
|
|
// 取消任务
|
|
handle10120Message(data: AmrMsg<AmrMsg10120>) {
|
|
|
|
}
|
|
|
|
|
|
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)
|
|
}
|
|
|
|
/*==========真车消息处理============*/
|
|
|
|
// 计算逻辑方向
|
|
computeLogicXYAndDirection() {
|
|
let ra = this.rotation.y
|
|
while (ra > Math.PI * 2) {
|
|
ra -= Math.PI * 2
|
|
}
|
|
while (ra < 0) {
|
|
ra += Math.PI * 2
|
|
}
|
|
const ddra = Math.PI / 180
|
|
if (ra >= Math.PI * 2 - ddra || ra <= ddra) {
|
|
this.currentDirection = 0
|
|
|
|
} else if (ra >= Math.PI / 2 - ddra && ra <= Math.PI / 2 + ddra) {
|
|
this.currentDirection = 3
|
|
|
|
} else if (ra >= Math.PI - ddra && ra <= Math.PI + ddra) {
|
|
this.currentDirection = 2
|
|
|
|
} else if (ra >= Math.PI / 2 * 3 - ddra && ra <= Math.PI / 2 * 3 + ddra) {
|
|
this.currentDirection = 1
|
|
} else {
|
|
this.currentDirection = 15
|
|
}
|
|
|
|
const pointItem = Model.getItemByXYZ(this.position.x, this.position.y, this.position.z)
|
|
if (!pointItem || !pointItem.logicX || !pointItem.logicY) {
|
|
this.currentLogicX = -1
|
|
this.currentLogicY = -1
|
|
} else {
|
|
this.currentLogicX = pointItem.logicX
|
|
this.currentLogicY = pointItem.logicY
|
|
}
|
|
this.currentOrientation = this.getAmrOrientation(this.rotation.y)
|
|
}
|
|
|
|
makeStepTask(data: AmrMsg10010) {
|
|
console.log('makeStepTask:', data)
|
|
let currentStepTask: StepTask = this.runningStepTask
|
|
if (currentStepTask == null) {
|
|
if (this.currentStepTaskList && this.currentStepTaskList.length > 0) {
|
|
currentStepTask = this.currentStepTaskList[this.currentStepTaskList.length - 1]
|
|
} else {
|
|
currentStepTask = {
|
|
SeqNo: 0,
|
|
StepTaskType: 'MOVE',
|
|
OperationType: 0,
|
|
PickMode: 0,
|
|
X: this.currentLogicX,
|
|
Y: this.currentLogicY,
|
|
Speed: 1000,
|
|
EndDirection: this.currentDirection,
|
|
ChargeLocation: 0,
|
|
GoodsSlotHeight: 0,
|
|
position: this.position,
|
|
isCompleted: true
|
|
}
|
|
}
|
|
|
|
}
|
|
let endDirection = currentStepTask.EndDirection
|
|
|
|
const linkCount = data.Link?.length || 0
|
|
if (linkCount > 0) {
|
|
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)
|
|
|| (currentStepTask.X != link.X && currentStepTask.Y != link.Y)) {
|
|
prevLink = link
|
|
continue
|
|
} else if (currentStepTask.X < link.X) {
|
|
if (link.Speed > 0) {
|
|
endDirection = 0
|
|
} else {
|
|
endDirection = 2
|
|
}
|
|
} else if (currentStepTask.X > link.X) {
|
|
if (link.Speed > 0) {
|
|
endDirection = 2
|
|
} else {
|
|
endDirection = 0
|
|
}
|
|
} else if (currentStepTask.Y < link.Y) {
|
|
if (link.Speed > 0) {
|
|
endDirection = 1
|
|
} else {
|
|
endDirection = 3
|
|
}
|
|
} else if (currentStepTask.Y > link.Y) {
|
|
if (link.Speed > 0) {
|
|
endDirection = 3
|
|
} else {
|
|
endDirection = 1
|
|
}
|
|
}
|
|
|
|
if (endDirection != currentStepTask.EndDirection) {
|
|
const item = this.viewport.entityManager.findItemByLogicXY(prevLink.X, prevLink.Y)
|
|
if (item.dt?.agvRotation && item.dt?.agvRotation?.length > 0) {
|
|
const stepTask: StepTask = {
|
|
SeqNo: data.SeqNo,
|
|
StepTaskType: 'ROTATION',
|
|
OperationType: 0,
|
|
PickMode: 0,
|
|
X: prevLink.X,
|
|
Y: prevLink.Y,
|
|
Speed: prevLink.Speed,
|
|
EndDirection: endDirection,
|
|
ChargeLocation: data.ChargeLocation,
|
|
GoodsSlotHeight: data.GoodsSlotHeight,
|
|
position: Model.getPositionByLogicXY(prevLink.X, prevLink.Y) as THREE.Vector3,
|
|
isCompleted: false
|
|
}
|
|
currentStepTask = stepTask
|
|
this.currentStepTaskList.push(stepTask)
|
|
}
|
|
}
|
|
|
|
const stepTask: StepTask = {
|
|
SeqNo: data.SeqNo,
|
|
StepTaskType: link.Speed > 0 ? 'MOVE' : 'MOVE_BACKWARD',
|
|
OperationType: 0,
|
|
PickMode: 0,
|
|
X: link.X,
|
|
Y: link.Y,
|
|
Speed: link.Speed,
|
|
EndDirection: endDirection,
|
|
ChargeLocation: data.ChargeLocation,
|
|
GoodsSlotHeight: data.GoodsSlotHeight,
|
|
position: Model.getPositionByLogicXY(link.X, link.Y) as THREE.Vector3,
|
|
isCompleted: false
|
|
}
|
|
currentStepTask = stepTask
|
|
this.currentStepTaskList.push(stepTask)
|
|
prevLink = link
|
|
}
|
|
}
|
|
|
|
// 如果没有link 或者当前link的XY是任务结束点位
|
|
if (linkCount <= 0
|
|
|| (linkCount > 0 && data.Link[linkCount - 1].X == data.EndX && data.Link[linkCount - 1].Y == data.EndY)) {
|
|
|
|
if (data.OperationType == 0 && data.EndDirection >= 0 && data.EndDirection <= 3) {
|
|
endDirection = data.EndDirection
|
|
} 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 == 3) {
|
|
endDirection = 0
|
|
} else {
|
|
endDirection = (data.GoodsSlotDirection + 1) as LogicDirection
|
|
}
|
|
}
|
|
|
|
const item = this.viewport.entityManager.findItemByLogicXY(data.EndX, data.EndY)
|
|
|
|
if (endDirection != currentStepTask.EndDirection) {
|
|
// 如果此处不能转弯，忽略结束方向 等待后续任务
|
|
if (!item.dt?.agvRotation || item.dt?.agvRotation?.length <= 0) {
|
|
debugger
|
|
return
|
|
}
|
|
const stepTask: StepTask = {
|
|
SeqNo: data.SeqNo,
|
|
StepTaskType: 'ROTATION',
|
|
OperationType: 0,
|
|
PickMode: 0,
|
|
X: data.EndX,
|
|
Y: data.EndY,
|
|
Speed: currentStepTask.Speed,
|
|
EndDirection: endDirection,
|
|
ChargeLocation: data.ChargeLocation,
|
|
GoodsSlotHeight: data.GoodsSlotHeight,
|
|
position: Model.getPositionByLogicXY(data.EndX, data.EndY) as THREE.Vector3,
|
|
isCompleted: false
|
|
}
|
|
this.currentStepTaskList.push(stepTask)
|
|
}
|
|
|
|
if (data.OperationType == 3) {
|
|
|
|
const stepTask: StepTask = {
|
|
SeqNo: data.SeqNo,
|
|
StepTaskType: 'CHARGE',
|
|
OperationType: 3,
|
|
PickMode: 0,
|
|
X: data.EndX,
|
|
Y: data.EndY,
|
|
Speed: currentStepTask.Speed,
|
|
EndDirection: endDirection,
|
|
ChargeLocation: data.ChargeLocation,
|
|
GoodsSlotHeight: data.GoodsSlotHeight,
|
|
position: Model.getPositionByLogicXY(data.EndX, data.EndY) as THREE.Vector3,
|
|
isCompleted: false
|
|
}
|
|
this.currentStepTaskList.push(stepTask)
|
|
|
|
} else if (data.OperationType == 4) {
|
|
|
|
const stepTask: StepTask = {
|
|
SeqNo: data.SeqNo,
|
|
StepTaskType: data.PickMode == 1 ? 'LOAD' : 'UNLOAD',
|
|
OperationType: 4,
|
|
PickMode: data.PickMode,
|
|
X: data.EndX,
|
|
Y: data.EndY,
|
|
Speed: currentStepTask.Speed,
|
|
EndDirection: endDirection,
|
|
ChargeLocation: data.ChargeLocation,
|
|
GoodsSlotHeight: data.GoodsSlotHeight,
|
|
position: Model.getPositionByLogicXY(data.EndX, data.EndY) as THREE.Vector3,
|
|
isCompleted: false
|
|
}
|
|
this.currentStepTaskList.push(stepTask)
|
|
|
|
} else {
|
|
|
|
}
|
|
}
|
|
|
|
|
|
console.log(JSON.stringify(this.currentStepTaskList))
|
|
|
|
}
|
|
|
|
executeTask() {
|
|
if (this.runningStepTaskList.length > 0) {
|
|
this.TaskMode = 2
|
|
}
|
|
while (this.currentStepTaskList.length > 0) {
|
|
const stepTask = this.currentStepTaskList[0]
|
|
if (this.runningStepTask) {
|
|
if ((stepTask.StepTaskType == 'MOVE' || stepTask.StepTaskType == 'MOVE_BACKWARD')
|
|
&& this.rotationAnimation == null && this.actionAnimation == null
|
|
&& stepTask.EndDirection == this.runningStepTask.EndDirection
|
|
&& (stepTask.Speed > 0) == (this.runningStepTask.Speed > 0)) {
|
|
this.runningStepTask = stepTask
|
|
this.currentStepTaskList.shift()
|
|
this.runningStepTaskList.push(stepTask)
|
|
console.log('移动')
|
|
this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection, stepTask.Speed / 1000)
|
|
|
|
} else {
|
|
break
|
|
}
|
|
} else {
|
|
this.runningStepTask = stepTask
|
|
this.currentStepTaskList.shift()
|
|
this.runningStepTaskList.push(stepTask)
|
|
if (stepTask.StepTaskType == 'MOVE' || stepTask.StepTaskType == 'MOVE_BACKWARD') {
|
|
console.log('移动')
|
|
this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection, stepTask.Speed / 1000)
|
|
} else if (stepTask.StepTaskType == 'ROTATION') {
|
|
console.log('转动')
|
|
this.addRotation(stepTask.EndDirection)
|
|
} else if (stepTask.StepTaskType == 'LOAD') {
|
|
console.log('取货')
|
|
this.addLoad(stepTask.GoodsSlotHeight / 1000, this.agvStatusVo.bizLpn)
|
|
} else if (stepTask.StepTaskType == 'UNLOAD') {
|
|
console.log('放货')
|
|
this.addUnload(stepTask.GoodsSlotHeight / 1000, this.agvStatusVo.bizLpn)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
onActionCompleted() {
|
|
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)
|
|
}
|
|
|
|
/*==========动作处理============*/
|
|
|
|
// 转
|
|
addRotation(direction: number): Promise<void> {
|
|
this.OperationType = 0
|
|
this.PickMode = 0
|
|
this.TaskMode = 2
|
|
let rad = 0
|
|
switch (direction) {
|
|
case 1:
|
|
rad = Math.PI / 2 * 3
|
|
break
|
|
case 2:
|
|
rad = Math.PI
|
|
break
|
|
case 3:
|
|
rad = Math.PI / 2
|
|
break
|
|
default:
|
|
rad = 0
|
|
}
|
|
const quat1 = new THREE.Quaternion().setFromEuler(this.rotation)
|
|
const euler: Euler = new Euler(this.rotation.x, rad, this.rotation.z)
|
|
const quat2 = new THREE.Quaternion().setFromEuler(euler)
|
|
const angleDiff = quat1.angleTo(quat2)
|
|
console.log(rad, this.rotation.y, angleDiff)
|
|
const tr = this.rotation.y + angleDiff
|
|
let time = Math.abs(angleDiff) / this.rotationSpeed
|
|
const duration = time
|
|
|
|
return new Promise(resolve => {
|
|
this.rotationAnimation = gsap.to(this.rotation, {
|
|
y: rad,
|
|
duration,
|
|
ease: 'none',
|
|
onComplete: () => {
|
|
resolve()
|
|
this.rotationAnimation = null
|
|
this.onActionCompleted()
|
|
}
|
|
})
|
|
})
|
|
}
|
|
|
|
keepSpeed: any
|
|
force: any
|
|
reverseForce: any
|
|
|
|
// 走
|
|
addTravel(logicX: number, logicY: number, direction: number, speed: number = 1): Promise<void> {
|
|
|
|
|
|
this.OperationType = 0
|
|
this.PickMode = 0
|
|
this.TaskMode = 2
|
|
const pos = Model.getPositionByLogicXY(logicX, logicY)
|
|
this.__toPos = pos as THREE.Vector3
|
|
//加速度
|
|
const a = 0.4
|
|
|
|
if (this.position.x < this.__toPos.x || this.position.z < this.__toPos.z) {
|
|
speed = Math.abs(speed)
|
|
} else if (this.position.x > this.__toPos.x || this.position.z > this.__toPos.z) {
|
|
speed = 0 - Math.abs(speed)
|
|
}
|
|
|
|
// 运动参数
|
|
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'
|
|
|
|
this.viewport.registerPhysicsUpdateCallBack.set(this.item.id, () => {
|
|
|
|
this.viewport.physicsWorld.stepSimulation(1 / 60, 10)
|
|
|
|
// 同步Three.js对象位置
|
|
const transform = new this.viewport.ammoModel.btTransform()
|
|
this.boxBody.getMotionState().getWorldTransform(transform)
|
|
const position = transform.getOrigin()
|
|
this.position.set(position.x(), position.y(), position.z())
|
|
// this.boxBody.getActivationState()
|
|
// 获取当前速度
|
|
const velocity = this.boxBody.getLinearVelocity()
|
|
const distance = this.position.distanceTo(this.__toPos)
|
|
const cSpeed = velocity.length()
|
|
const stopTime = cSpeed / a
|
|
const stopDistance = a * stopTime * stopTime / 2
|
|
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(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 + 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轴正向力
|
|
this.boxBody.applyCentralForce(this.force)
|
|
|
|
// 达到最大速度进入匀速阶段
|
|
if (cSpeed >= Math.abs(speed) || distance <= stopDistance) {
|
|
currentPhase = 'uniform'
|
|
}
|
|
this.boxBody.setDamping(0, 0);
|
|
break
|
|
case 'uniform':
|
|
currentPhase = 'uniform'
|
|
this.boxBody.setLinearVelocity(this.keepSpeed)
|
|
this.boxBody.setDamping(0, 0);
|
|
break
|
|
case 'decelerate':
|
|
// 检测停止
|
|
if (cSpeed > 0) {
|
|
this.boxBody.setDamping(0.6, 0);
|
|
}
|
|
this.boxBody.activate()
|
|
break
|
|
}
|
|
|
|
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
|
|
}
|
|
}
|
|
}
|
|
|
|
})
|
|
return null
|
|
}
|
|
|
|
// 取货
|
|
addLoad(height: number, goodsId: string): void {
|
|
this.actionAnimation = 'wq'
|
|
console.log('取货')
|
|
this.TaskMode = 2
|
|
this.PickMode = 1
|
|
this.OperationType = 4
|
|
this.animationUpFork(height).then(
|
|
() => 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, goodsId: string): void {
|
|
this.actionAnimation = 'wq'
|
|
console.log('卸货')
|
|
this.TaskMode = 2
|
|
this.PickMode = 2
|
|
this.OperationType = 4
|
|
this.animationUpFork(height + 0.2).then(
|
|
() => 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()
|
|
})
|
|
)
|
|
}
|
|
)
|
|
)
|
|
)
|
|
}
|
|
|
|
override animationShowFork(z: number): Promise<void> {
|
|
return null
|
|
};
|
|
|
|
override animationHideFork(): Promise<void> {
|
|
return null
|
|
}
|
|
|
|
override animationUpFork(y: number, time?: number = 3): Promise<void> {
|
|
return null
|
|
}
|
|
|
|
override animationDownFork(): Promise<void> {
|
|
return null
|
|
}
|
|
|
|
override getArmObject(): THREE.Object3D | undefined {
|
|
return null
|
|
}
|
|
|
|
//获取ptr的角度朝向
|
|
getAmrOrientation(radY: number) {
|
|
while (radY < 0) {
|
|
radY += Math.PI * 2
|
|
}
|
|
while (radY > Math.PI * 2) {
|
|
radY -= Math.PI * 2
|
|
}
|
|
radY = (Math.PI * 2 - radY) % (Math.PI * 2)
|
|
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
|
|
}
|
|
|
|
}
|
|
|