yvan-rcs-web/src/modules/amr/ptr/cl2/Cl23dObject.ts

import * as THREE from 'three'
import {CSG} from 'three-csg-ts'
import gsap from 'gsap'
import mqtt from 'mqtt'
import {Euler} from 'three/src/math/Euler'
import Cl2Entity from '@/modules/amr/ptr/cl2/Cl2Entity'
import Cl23DGraphics from "@/modules/amr/ptr/cl2/Cl23DGraphics"
import {
  AmrErrorCode,
  AmrMsg,
  AmrMsg10010,
  AmrMsg10050,
  AmrMsg10060,
  AmrMsg10110, AmrMsg10120, AmrMsg20011,
  AmrMsg20020,
  AmrMsg20050,
  AmrMsg20060,
  AmrMsg20100, AmrMsg20147, AmrMsg20148,
  AmrMsg20149,
  AmrMsg20150,
  AmrMsg20250,
  CurBatteryData,
  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";


type CStepTaskType = "MOVE" | "MOVE_BACKWARD" | "ROTATION" | "LOAD" | "UNLOAD" | "CHARGE"

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 Cl23dObject extends THREE.Object3D {

  private item: ItemJson

  private _cl2Entity: Cl2Entity = null

  private currentStepTaskList: StepTask[] = []
  private runningStepTask: StepTask = null
  private runningStepTaskList: StepTask[] = []
  private travelAnimation: core.Tween = null

  private currentLogicX: number = -1
  private currentLogicY: number = -1
  private currentDirection: LogicDirection = 15
  private sendMessageQueue: AmrMsg<any>[] = []

  public Battery: number = 100

  private __TaskMode: CTaskMode = 0
  private __OperationType: COperationType = 0
  private __TaskStatus: CEventId = 0
  private __PickMode: CPickMode = 0

  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 = THREE.MathUtils.radToDeg(this.rotation.y)
        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

  // 心跳间隔 UInt32 单位: s
  private heartBeatInterval: number = 0
  // 小车所有上报消息重试间隔(未收到应答消息时重发消息)  UInt32 单位: s
  private mqRetryInterval: number = 3

  public get cl2Entity(): Cl2Entity {
    if (!this._cl2Entity) {
      const cl2: Cl2Entity = Model.getCl2(this.item.id) as Cl2Entity
      this._cl2Entity = cl2
    }
    return this._cl2Entity
  }

  public vehicleId: number

  private clock = new THREE.Clock()

  constructor(item: ItemJson, option?: RendererCudOption) {
    super()
    this.item = item
    if (!Cl23DGraphics.ptrPedestalGeometry) {
      Cl23DGraphics.ptrPedestalGeometry = Cl23DGraphics.createPtrPedestal()
    }
    const ptrPedestalGeometry = Cl23DGraphics.ptrPedestalGeometry
    const ptrPedestalMaterial = new THREE.MeshPhongMaterial({color: 0xffdddbca})
    const ptrPedestalMesh = new THREE.Mesh(ptrPedestalGeometry, ptrPedestalMaterial)
    ptrPedestalMesh.name = 'ptrPedestal'

    if (!Cl23DGraphics.ptrPillarGeometry) {
      Cl23DGraphics.ptrPillarGeometry = Cl23DGraphics.createPtrPillar()
    }
    const ptrPillarGeometry = Cl23DGraphics.ptrPillarGeometry
    const ptrPillarMaterial = new THREE.MeshPhongMaterial({color: 0xff6c6956})
    const ptrPillarMesh = new THREE.Mesh(ptrPillarGeometry, ptrPillarMaterial)


    if (!Cl23DGraphics.ptrForkGeometry) {
      Cl23DGraphics.ptrForkGeometry = Cl23DGraphics.createPtrFork()
    }
    const ptrForkGeometry = Cl23DGraphics.ptrForkGeometry
    const ptrForkMaterial = new THREE.MeshPhongMaterial({color: 0xff444444})
    const ptrForkMesh = new THREE.Mesh(ptrForkGeometry, ptrForkMaterial)
    ptrForkMesh.name = 'ptrFork'

    this.add(ptrPedestalMesh)
    const groupPillar = new THREE.Group()
    groupPillar.name = 'ptrPillar'
    groupPillar.add(ptrPillarMesh)
    groupPillar.add(ptrForkMesh)
    this.add(groupPillar)

    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"
  private AGVFnModel = "FITBOTS-CYBER-LIFT-1000_V1.0"


  private heartBeatTimeCount: number = 0
  private mqRetryTimeCount: number = 0

  private 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();

    if (!worldModel.state.runState.isVirtual) {
      return
    }
    this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
    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.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 = THREE.MathUtils.radToDeg(this.rotation.y);
    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) {
      if (this.runningStepTask.OperationType == 0 && this.runningStepTask.X == data.StartX && this.runningStepTask.Y == data.StartY) {
        // this.currentStepTaskList = []
        this.makeStepTask(data)
        this.executeTask()
      } else {
        // 此处应该有错误处理
      }
    } 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>) {

  }

  /*==========真车消息处理============*/

  // 计算逻辑方向
  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 / 8
    if (ra >= ddra * 7 || ra < ddra) {
      this.currentDirection = 0;

    } else if (ra >= ddra && ra < ddra * 3) {
      this.currentDirection = 3;

    } else if (ra >= ddra * 3 && ra < ddra * 5) {
      this.currentDirection = 2;

    } else if (ra >= ddra * 5 && ra < ddra * 7) {
      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;
    }

  }

  makeStepTask(data: AmrMsg10010) {

    let currentStepTask: StepTask = this.runningStepTask
    if (currentStepTask == null) {

      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

    if (data.Link.length > 0) {
      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)) {
          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 stepTask: StepTask = {
            SeqNo: data.SeqNo,
            StepTaskType: "ROTATION",
            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)
        }

        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)
      }
    }


    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 == 0) {
        endDirection = 3
      } else {
        endDirection = (data.GoodsSlotDirection - 1) as LogicDirection
      }
    }
    if (endDirection != currentStepTask.EndDirection) {
      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 {

    }

  }

  executeTask() {
    this.TaskMode = 2
    while (this.currentStepTaskList.length > 0) {
      const stepTask = this.currentStepTaskList[0]
      if (this.runningStepTask) {
        if ((stepTask.StepTaskType == "MOVE" || stepTask.StepTaskType == "MOVE_BACKWARD")
          && stepTask.EndDirection == this.runningStepTask.EndDirection
          && (stepTask.Speed > 0) == (this.runningStepTask.Speed > 0)) {
          this.runningStepTask = stepTask
          this.currentStepTaskList.shift()
          this.runningStepTaskList.push(stepTask)

          this.addTravel(stepTask.X, stepTask.Y, stepTask.Speed/1000)

        } else {
          break
        }
      } else {
        this.runningStepTask = stepTask
        this.currentStepTaskList.shift()
        this.runningStepTaskList.push(stepTask)
        if (stepTask.StepTaskType == "MOVE" || stepTask.StepTaskType == "MOVE_BACKWARD") {
          this.addTravel(stepTask.X, stepTask.Y, stepTask.Speed/1000)
        } else if (stepTask.StepTaskType == "ROTATION") {
          this.addRotation(stepTask.EndDirection)
        } else if (stepTask.StepTaskType == "LOAD") {
          this.addLoad(stepTask.GoodsSlotHeight/1000)
        } else if (stepTask.StepTaskType == "UNLOAD") {
          this.addUnload(stepTask.GoodsSlotHeight/1000)
        }
      }
    }
  }

  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<void> {
    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) / (Math.PI / 7)
    const duration = time

    return new Promise(resolve => {
      gsap.to(this.rotation, {
        y: tr,
        duration,
        ease: 'none',
        onComplete: ()=>{
          resolve()
          this.onActionCompleted()
        }
      })
    })
  }

  // 走
  addTravel(logicX: number, logicY: number, speed: number = 1): Promise<void> {
    this.OperationType = 0
    this.PickMode = 0
    const pos = Model.getPositionByLogicXY(logicX, logicY)

    const fromPos = this.position
    const toPos = pos as THREE.Vector3
    const distance = fromPos.distanceTo(toPos)
    const duration = Math.max(1.0, distance / speed)

    if (!this.travelAnimation) {
      return new Promise(resolve => {
        this.travelAnimation = gsap.fromTo(this.position, {
          x: fromPos.x,
          y: fromPos.y,
          z: fromPos.z,
        }, {
          x: toPos.x,
          y: toPos.y,
          z: toPos.z,
          duration,
          ease: 'power2.inOut',
          onComplete: () => {
            this.travelAnimation = null
            resolve()
            this.onActionCompleted()
          },
          onUpdate: () => {

            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.Orientation
                  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
      const tt = this.travelAnimation.duration()
      this.travelAnimation.duration(tt + duration)
      this.travelAnimation.invalidate().restart();
    }

  }

  // 取货
  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<void> {

    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<void> {
    return this.animationShowFork(0)
  }

  animationUpFork(y: number, time?: number = 3): Promise<void> {
    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<void> {
    return this.animationUpFork(0)
  }
}