cl2 基于设备状态和任务状态分步生成设备任务消息

src/core/engine/Viewport.ts

@@ -69,6 +69,8 @@ export default class Viewport {
     markRaw(this.envManager)
   ]
 
+  registerFrameTimerCallBack: Map<string, ()=> void>  = new Map()
+
   // 对象实例管理器 moduleName -> InstanceMeshManager
   meshManager: Map<string, InstanceMeshManager> = new Map()
 
@@ -116,6 +118,12 @@ export default class Viewport {
     this.stateManager = stateManager
   }
 
+  addFrameTimerCallback(id: string, callback: () => void) {
+    this.registerFrameTimerCallBack.set(id, callback)
+  }
+  removeFrameTimerCallback(id: string) {
+    this.registerFrameTimerCallBack.delete(id)
+  }
   /**
    * 根据实体 ID 查找实体
    * @param entityId
@@ -371,7 +379,9 @@ export default class Viewport {
   }
 
   offset = 0
-
+  clock = new THREE.Clock();
+  elapsedTime = 0;
+  interval = 1; // 触发间隔（秒）
   /**
    * 动画循环
    */
@@ -389,6 +399,17 @@ export default class Viewport {
 
     this.css2DRenderer.render(this.scene.scene, this.camera)
     this.css3DRenderer.render(this.scene.scene, this.camera)
+    const delta = this.clock.getDelta();
+    this.elapsedTime += delta;
+
+    if (this.elapsedTime >= this.interval) { // 每1秒触发一次
+      this.elapsedTime = 0;
+      this.registerFrameTimerCallBack.forEach(callback => {
+        if (typeof callback === 'function') {
+          callback()
+        }
+      })
+    }
 
     // if (window['lineMaterial']) {
     //   this.offset -= 0.002

src/core/manager/amr/AmrMessageDefine.ts

@@ -1,19 +1,46 @@
 import type Viewport from '@/core/engine/Viewport.ts'
 
+// 逻辑方向 0 X+  1 Y+  2 X-  3 Y-  15 未知
 type LogicDirection = 0 | 1 | 2 | 3 | 15
 
+// 0 运输 1 接货 2 卸货 3 充电 4 提升移栽取货或卸货 5 滚筒取货或卸货 135 旋转货架 136 旋转车身 143 卷帘门控制 224 等待就绪
+type COperationType = 0 | 1 | 2 | 3 | 4 | 5 | 135 | 136 | 143 | 224
+
+// 0 空闲模式 1 初始化模式 2 任务模式 3 单动作模式 4 手动模式 5 遥控器模式 6 充电模式 7 任务被中断模式 8 自定义模式
+type CTaskMode = 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8
+
+// 0：无变化 1：任务模式改变 2：任务接收成功 3：任务开始 4：任务完成 5：任务已取消 6：任务已停止 7：任务已恢复 8：任务已更变
+type CEventId = 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8
 
 class AmrMsg<T> {
   id: number
   content: T
 
   constructor<T>(content: T) {
-    if (content instanceof AmrMsg20020) {
+    if (content instanceof AmrMsg20010) {
+      this.id = 20010
+    } else if (content instanceof AmrMsg20011) {
+      this.id = 20011
+    } else if (content instanceof AmrMsg20012) {
+      this.id = 20012
+    } else if (content instanceof AmrMsg20020) {
       this.id = 20020
     } else if (content instanceof AmrMsg20050) {
       this.id = 20050
+    } else if (content instanceof AmrMsg20060) {
+      this.id = 20060
+    } else if (content instanceof AmrMsg20100) {
+      this.id = 20100
+    } else if (content instanceof AmrMsg20147) {
+      this.id = 20147
+    } else if (content instanceof AmrMsg20148) {
+      this.id = 20148
     } else if (content instanceof AmrMsg20149) {
       this.id = 20149
+    } else if (content instanceof AmrMsg20150) {
+      this.id = 20150
+    } else if (content instanceof AmrMsg20200) {
+      this.id = 20200
     } else if (content instanceof AmrMsg20250) {
       this.id = 20250
     }
@@ -23,8 +50,10 @@ class AmrMsg<T> {
 
 class AmrMsg10010 {
   SeqNo: number;
-  OperationType: 0 | 1 | 2 | 3 | 4 | 5 | 135 | 136;
+  OperationType:COperationType;
   UseBriefLocation: boolean;
+  ChargeDirection: LogicDirection;
+  ChargeLocation: number;
   StartX: number;
   StartY: number;
   EndX: number;
@@ -43,6 +72,36 @@ class AmrMsg10010 {
   }[]
 }
 
+class AmrMsg10050 {
+  // 作业序号 UInt32
+  SeqNo: number;
+}
+
+class AmrMsg10060 {
+  // 作业序号 UInt32
+  SeqNo: number;
+  // X坐标最大长度 UInt16 暂时未使用
+  XLength: number;
+  // Y坐标最大长度 UInt16 暂时未使用
+  YLength: number;
+  // 相邻地标间距 UInt16 单位:mm （目前等距）
+  Gap: number;
+  // 心跳间隔 UInt32 单位: s
+  HeartBeat: number;
+  // 小车所有上报消息重试间隔(未收到应答消息时重发消息)  UInt32 单位: s
+  MqRetryTime: number;
+
+}
+
+// 状态查询 10110
+class AmrMsg10110 {
+  SeqNo: number;
+}
+
+// 状态查询 10110
+class AmrMsg10120 {
+  SeqNo: number;
+}
 
 class AmrMsg20000Base {
   SeqNo: number
@@ -51,14 +110,92 @@ class AmrMsg20000Base {
   SendTime: number
   CreateMonoTime: number
 
-  constructor() {
+  constructor(vehicleId: number) {
     this.SeqNo = getAmrMsgSeqNo()
     this.CreateTime = Date.now()
     this.SendTime = Date.now()
+    this.VehicleId = vehicleId
+  }
+}
+
+// 小车作业完成 20010
+class AmrMsg20010 extends AmrMsg20000Base {
+  // 当前X坐标 UInt16
+  CurX: number;
+  // 当前Y坐标 UInt16
+  CurY: number;
+  // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+  CurDirection: LogicDirection;
+  // 当前方向 Double 角度
+  CurOrientation: number;
+  // 作业类型	UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业） 135：旋转货架 136：旋转车身
+  OperationType: COperationType;
+  // 作业结果 Int32 参考linux errno
+  OperationResult: number;
+  // 货物ID String 在肥波类车型中是货架二维码值；在飞梭车中是箱码（目前未传，未来可能会有）；在皮带飞梭中是货物上贴的二维码的码值（如果检测到多个码，则以"
+  StorageRacksNo: string;
+  // 电量百分比	Uint8
+  Battery: number;
+  // 任务描述 Object 目前仅在飞梭和侧叉车型的报文中有这个字段，字段详情见下文
+  Summary: {
+    ActuatorsData: {
+      // 执行器编号 Uint8 从1开始
+      MechNo: number;
+      // 执行器名称 String
+      Name: string;
+      // 任务类型 UInt8 同下发任务中PickMode的定义
+      PickMode: number;
+    }[]
+  };
+  // 车载货位信息
+  GoodsSlots: LocationData[];
+
+  constructor(vehicleId: number) {
+    super(vehicleId)
+  }
+}
+
+// 任务状态上报 20011
+class AmrMsg20011<T> extends AmrMsg20000Base {
+  // 事件ID UInt8 0：无变化 1：任务模式改变 2：任务接收成功 3：任务开始 4：任务完成 5：任务已取消 6：任务已停止 7：任务已恢复 8：任务已更变
+  EventId: CEventId;
+  // 任务模式 Byte
+  TaskMode: CTaskMode;
+  // 任务信息 由事件ID决定 Object 默认消息（0，2，3，5，6，7）：任务状态改变消息 1：任务模式消息 4：任务完成消息 8：任务类型改变消息
+  Info: T;
+
+  constructor(vehicleId: number, info: T) {
+    super(vehicleId)
+    if (info instanceof TaskCompletedData) {
+      this.EventId = 4
+    } else if (info instanceof TaskModeChangeData) {
+      this.EventId = 1
+    } else if (info instanceof TaskTypeChangeData) {
+      this.EventId = 8
+    }
+    this.Info = info
   }
 }
 
+// 小车子模块任务状态 20012
+class AmrMsg20012 extends AmrMsg20000Base {
+  CurDirection: LogicDirection
+  CurLogicX: number
+  CurLogicY: number
+  CurX: number
+  CurY: number
+  // 地标类型 二维码 1 默认
+  MarkerType: number = 1
+  CurOrientation: number = 0
+  X: number = 0
+  Y: number = 0
 
+  constructor(vehicleId: number) {
+    super(vehicleId)
+  }
+}
+
+// 地标报告 20020
 class AmrMsg20020 extends AmrMsg20000Base {
   CurDirection: LogicDirection
   CurLogicX: number
@@ -71,29 +208,123 @@ class AmrMsg20020 extends AmrMsg20000Base {
   X: number = 0
   Y: number = 0
 
-  constructor() {
-    super()
+  constructor(vehicleId: number) {
+    super(vehicleId)
   }
 }
 
+// 消息应答 20050
 class AmrMsg20050 extends AmrMsg20000Base {
   constructor(seqNo: number, vehicleId: number) {
-    super()
+    super(vehicleId)
     this.SeqNo = seqNo
-    this.VehicleId = vehicleId
   }
 }
 
+// 状态上报 20060
+class AmrMsg20060 extends AmrMsg20000Base {
+  // 电池状态
+  CurBattery: CurBatteryData;
+  // 当前方向 Double 角度
+  CurOrientation: number = 0;
+  // 当前所在站点的逻辑X坐标 Int32
+  CurLogicX: number = -1;
+  // 当前所在站点的逻辑Y坐标 Int32
+  CurLogicY: number = -1;
+  // 当前X坐标 Double 当前实际位置在地图坐标系中的X坐标
+  CurX: number = 0;
+  // 当前Y坐标 Double 当前实际位置在地图坐标系中的Y坐标
+  CurY: number = 0;
+  // 货架当前方向  Double
+  RackCurOrientation: number;
+  // 货架当前位置 Double
+  RackCurPosition: number;
+  // 货架号 String
+  StorageRacksNo: string;
+  // 多载货机构上每个机构上面货物的ID String[]
+  MStorageRacksNo: string[];
+  // 任务模式 Byte
+  TaskMode: CTaskMode;
+  // 当前标准X坐标 double
+  X: number = 0;
+  // 当前标准Y坐标 double
+  Y: number = 0;
+  // 当前货物数量。数组形式。 对于单滚筒等单个托盘的机型，只需关注数据0； 对于双滚筒，滚筒1的数量放到数据0中，滚筒2的货物数量放到数据1中。 其它机型以此类推。UInt16 [4]
+  GoodsQuantity: number[];
+  // 异常数组，存放异常的所有异常ID 	UInt16 [4]
+  Exceptions: number[];
+  // 是否已进行精准停靠（是否能在原地直接执行对接任务） bool
+  InDock: boolean;
+  // 初始化状态 bool
+  Initialized: boolean;
+  // 车载货位信息
+  GoodsSlots: LocationData[];
+
+  constructor(vehicleId: number) {
+    super(vehicleId);
+  }
+}
+
+//心跳 20100
+class AmrMsg20100 extends AmrMsg20000Base {
+  Temperature: { Battery: number }
+
+  constructor(vehicleId: number) {
+    super(vehicleId);
+  }
+}
+
+// 开机上报 20147 在开机后上报一次，此后AMR程序重启不会重新上报（与ID#20149的差异），除非人为清除内部记录已上报的标志。
+class AmrMsg20147 extends AmrMsg20000Base {
+  AGVFnModel: string
+  AGVModel: string
+  Battery: number
+
+  constructor(vehicleId: number) {
+    super(vehicleId);
+  }
+}
+// 关机上报 20148 在收到关机信号后上报此消息。
+class AmrMsg20148 extends AmrMsg20000Base {
+  // 电量百分比	Uint8
+  Battery: number;
+  // 上电至今的毫秒数 	Uint64
+  Uptime: number;
+  constructor(vehicleId: number) {
+    super(vehicleId);
+  }
+}
+
+// 小车主程序启动 20149
 class AmrMsg20149 extends AmrMsg20000Base {
   AGVFnModel: string
   AGVModel: string
   Battery: number
 
-  constructor() {
-    super()
+  constructor(vehicleId: number) {
+    super(vehicleId)
   }
 }
 
+// 小车上线 20150  代表车已经完成初始化，可以接收任务了。
+class AmrMsg20150 extends AmrMsg20000Base {
+  AGVFnModel: string
+  AGVModel: string
+  Battery: number
+
+  constructor(vehicleId: number) {
+    super(vehicleId)
+  }
+}
+
+// 小车离线 20200 注意：目前未实现此报文
+class AmrMsg20200 extends AmrMsg20000Base {
+  constructor(vehicleId: number) {
+    super(vehicleId);
+  }
+}
+
+// 异常上报 20250
 class AmrMsg20250 extends AmrMsg20000Base {
   // 异常持续时间 秒
   Duration: number = 0
@@ -110,14 +341,160 @@ class AmrMsg20250 extends AmrMsg20000Base {
   // 异常详情 根据不同的异常，字段不一样，比如针对ErrCode为2电量低的情况，ErrMsg中使用CurBattery标识当前电量；
   ErrMsg: object
 
-  constructor() {
-    super()
+  constructor(vehicleId: number) {
+    super(vehicleId)
   }
 
   // 地标异常
   // {"content":{"CreateMonoTime":64489153,"CreateTime":1751337599272,"Duration":3,"ErrCode":5,"ErrCodeName":"kLocationMarkNotFound","ErrEvtType":1,"ErrLevel":14,"ErrLifecycle":2,"ErrMsg":{"ErrDesc":"无法在 (-1.00, -1.00) 位置使用扫描设备 kLocationMarkCamera(ID: 1) 找到码","ErrDescEn":"Unable to use scanner kLocationMarkCamera(id: 1) to find a mark at (-1.00, -1.00)","ErrPrivInfo":{"Camera":1,"CurLogicX":-1,"CurLogicY":-1,"ExpectCode":"","X":-1000.0,"Y":-1000.0}},"SendTime":1751337602101,"SeqNo":4,"VehicleId":3},"id":20250}
 }
 
+class TaskCompletedData {
+  // 作业类型	UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业） 135：旋转货架 136：旋转车身
+  OperationType: COperationType;
+  // 作业结果 Int32 参考linux errno
+  OperationResult: number;
+  // 当前所在站点的逻辑X坐标 Int32
+  CurLogicX: number;
+  // 当前所在站点的逻辑Y坐标 Int32
+  CurLogicY: number;
+  // 当前X坐标 Double 当前实际位置在地图坐标系中的X坐标
+  CurX: number;
+  // 当前Y坐标 Double 当前实际位置在地图坐标系中的Y坐标
+  CurY: number;
+  // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+  CurDirection: LogicDirection;
+  // 当前方向 Double 角度
+  CurOrientation: number;
+  // 货架号 String 在肥波类车型中是货架二维码值；在飞梭车中是箱码（目前未传，未来可能会有）；在皮带飞梭中是货物上贴的二维码的码值（如果检测到多个码，则以"
+  StorageRacksNo: string;
+  // 货架朝向  UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+  StorageDirection: LogicDirection;
+  // 任务描述 Object 目前仅在飞梭和侧叉车型的报文中有这个字段，字段详情见下文
+  Summary: {
+    ActuatorsData: {
+      // 执行器编号 Uint8 从1开始
+      MechNo: number;
+      // 执行器名称 String
+      Name: string;
+      // 任务类型 UInt8 同下发任务中PickMode的定义
+      PickMode: number;
+    }[]
+  };
+  // 电量百分比	Uint8
+  Battery: number;
+  // 车载货位信息
+  GoodsSlots: LocationData[];
+
+  constructor(operationType: COperationType) {
+    this.OperationType = operationType
+  }
+
+}
+
+class TaskModeChangeData {
+  // 上一个任务模式 Uint8  0: 空闲模式 1: 初始化模式 2: 任务模式 3: 单动作模式 4: 手动模式 5: 遥控器模式 6: 充电模式 7: 任务被中断模式 有任务，但未收到终点坐标 8: 自定义模式
+  PrevTaskMode: CTaskMode;
+  // 当前任务模式 Uint8  0: 空闲模式 1: 初始化模式 2: 任务模式 3: 单动作模式 4: 手动模式 5: 遥控器模式 6: 充电模式 7: 任务被中断模式 有任务，但未收到终点坐标 8: 自定义模式
+  TaskMode: CTaskMode;
+  constructor(pre: CTaskMode, cur: CTaskMode) {
+    this.PrevTaskMode = pre
+    this.TaskMode = cur
+  }
+}
+
+class TaskStatusChangeData {
+  // 当前任务类型 0: 运输 1: 接货 2: 卸货 3: 充电 4: 提升移栽取货或卸货 5: 滚筒取货或卸货（双向作业） 135: 旋转货架 136: 旋转车身 143: 卷帘门控制 224: 等待就绪
+  OperationType: COperationType;
+  constructor(operationType: COperationType) {
+    this.OperationType = operationType
+  }
+}
+
+class TaskTypeChangeData {
+  // 上一个任务类型 0: 运输 1: 接货 2: 卸货 3: 充电 4: 提升移栽取货或卸货 5: 滚筒取货或卸货（双向作业） 135: 旋转货架 136: 旋转车身 143: 卷帘门控制 224: 等待就绪
+  PrevOperationType: COperationType;
+  // 当前任务类型 0: 运输 1: 接货 2: 卸货 3: 充电 4: 提升移栽取货或卸货 5: 滚筒取货或卸货（双向作业） 135: 旋转货架 136: 旋转车身 143: 卷帘门控制 224: 等待就绪
+  OperationType: COperationType;
+
+  constructor(pre: COperationType, cur: COperationType) {
+    this.PrevOperationType = pre
+    this.OperationType = cur
+  }
+
+}
+
+class CurBatteryData {
+  // 充电电流
+  ChargingCurrent: number;
+  // 放电电流
+  DischargingCurrent: number;
+  // 电量
+  SOC: number;
+  // 电压
+  Voltage: number;
+  // 温度
+  Temperature: number;
+
+  constructor() {
+    this.ChargingCurrent = 0;
+    this.DischargingCurrent = 1;
+    this.SOC = 100;
+    this.Voltage = 50;
+    this.Temperature = 30;
+  }
+}
+
+class LocationData {
+  // 货位ID UInt8 从0开始，每款车自己定义的车载货位的ID
+  ID: number;
+  // 货位名称 String 每款车自己定义的货位的名称
+  Name: string;
+  // 逻辑上是否应该有货 bool 有些车型有传感器来检测货物状态，这时实际的货物状态跟逻辑状态就可能不同
+  ShouldHaveGoods: boolean;
+  // 检测到的货位状态 UInt8 0: 正常 1: 冲突，即传感器检测到的状态跟逻辑状态有冲突。逻辑上有货，实际无货，是一种冲突；逻辑上货的属性（比如高度）跟实际检测到的属性不同，也会产生冲突；对于一个货位有多个货物的情况，需要查看货物信息来查找具体是哪个货物出现了什么冲突
+  DetectedStatus: number;
+  // 货位放货平面离地高度 Double
+  Height: number;
+  // 货位相对原点（初始化后货位所在位置）的相对位置  Object 包含以下成员： OffsetX：Double类型 OffsetY：Double类型 OffsetZ：Double类型，代表在高度上的位移 Orientation：Double类型，用角度（Degree）表示
+  Position: number;
+  // 货物信息  Object
+  Goods: GoodsData[];
+}
+
+class OffsetPosition {
+  // OffsetX：Double类型 OffsetY：Double类型 OffsetZ：Double类型，代表在高度上的位移 Orientation：Double类型，用角度（Degree）表示
+  OffsetX: number;
+  OffsetY: number;
+  OffsetZ: number;
+  Orientation: number;
+}
+
+class GoodsData {
+  // 货物类型	UInt 对于一类设备可能取放不同货物时有用；如果只有一类货物，则可以不（在地图或者RCS下发的任务报文中）指定，此种情况下报文中不包含此字段
+  Category: number;
+  // 传感器检测到的货物ID String 比如飞梭通过摄像头检测到的箱码、FM系列通过对上摄像头检测到的货架底部二维码等；如果车型没配备可以检测货物ID的传感器，则报文中不包含此字段
+  DetectedId: string;
+  // 上位系统传过来的货物ID String 通常上位传过来的ID应该跟检测到的ID一致，否则会报错；对于没有检测ID功能的车型，不做校验而只是回传该值；有些项目可能会希望两个ID分开，对应此类需求可以特别设置不校验两类ID；也许有些项目会同时传入两种ID，当前暂不支持；如果上位没传入ID，则报文中不包含此字段
+  UpperSystemDefinedId: string;
+  // 高度等级 UInt8 部分车型能检测货物高度，等级从0（最矮，默认值）开始，每一级递增1，各个等级的具体含义与车型有关，需要另行约定；如果有传感器但是由于传感器异常等原因无法获取到检测结果，则设置该值为0xff，代表状态未知。
+  HeightLevel: number;
+  // 长度等级
+  LengthLevel: number;
+  // 宽度等级
+  WidthLevel: number;
+  // 重量等级
+  WeightLevel: number;
+  // 测算的重量，不一定准	Double 单位是千克
+  ApproximateWeight: number;
+  // 货物在地图上的朝向 Double 角度（Degree）值
+  Orientation: number;
+  // 货物相对机器人的位置	包含以下成员： OffsetX：Int32类型 OffsetY：Int32类型 Orientation：Double类型，用角度（Degree）表示
+  RelativePosition: OffsetPosition;
+  // 检测到的货物状态冲突	[UInt8] 1：货物ID不匹配 2：货物高度不匹配 3：货物宽度不匹配 4：货物长度不匹配 5：应该有货但没检测到有货 6：应该没货但检测到有货
+  DetectedStatusConflicts: number[];
+}
+
 let __AmrMsgSeqNo__: number = 0
 
 const getAmrMsgSeqNo = () => {
@@ -129,7 +506,40 @@ const getAmrMsgSeqNo = () => {
   return __AmrMsgSeqNo__
 }
 
-export {AmrMsg, AmrMsg10010, AmrMsg20020, AmrMsg20050, AmrMsg20149, AmrMsg20250, AmrErrorCode, LogicDirection}
+export {
+  AmrMsg,
+  AmrMsg10010,
+  AmrMsg10050,
+  AmrMsg10060,
+  AmrMsg10110,
+  AmrMsg10120,
+  AmrMsg20010,
+  AmrMsg20011,
+  AmrMsg20012,
+  AmrMsg20020,
+  AmrMsg20050,
+  AmrMsg20060,
+  AmrMsg20100,
+  AmrMsg20147,
+  AmrMsg20148,
+  AmrMsg20149,
+  AmrMsg20150,
+  AmrMsg20200,
+  AmrMsg20250,
+  AmrErrorCode,
+  CurBatteryData,
+  LocationData,
+  OffsetPosition,
+  GoodsData,
+  TaskCompletedData,
+  TaskModeChangeData,
+  TaskStatusChangeData,
+  TaskTypeChangeData,
+  LogicDirection,
+  COperationType,
+  CTaskMode,
+  CEventId,
+}
 
 const AmrErrorCode = {
   0: {

src/core/manager/amr/AmrMessageManager.ts

@@ -1,4 +1,4 @@
-import {AmrMsg, AmrMsg10010, AmrMsg20050} from "@/core/manager/amr/AmrMessageDefine";
+import {AmrMsg, AmrMsg10010, AmrMsg10050, AmrMsg10060, AmrMsg10110, AmrMsg10120, AmrMsg20050} from "@/core/manager/amr/AmrMessageDefine";
 import Cl23dObject from "@/modules/cl2/Cl23dObject";
 import Viewport from "@/core/engine/Viewport";
 
@@ -9,20 +9,22 @@ export default class AmrMessageManager {
   handleMessage(topic, amrMsg: AmrMsg<any>) {
 
     const vehicleId = parseInt(topic.replace("/wcs_server/", ""))
+    const amrItem = this.viewport.entityManager.findObjectById(vehicleId + "") as Cl23dObject
     switch (amrMsg.id) {
       // AMR作业指令 10010
       case 10010:
-        const cl2 = this.viewport.entityManager.findObjectById(amrMsg.content.VehicleId + "") as Cl23dObject
-        cl2.handleMessage(amrMsg as AmrMsg<AmrMsg10010>)
+        amrItem.handle10010Message(amrMsg.content as AmrMsg10010)
         break;
       // 停止/解除 10040
       case 10040:
         break;
       // 电文应答 10050
       case 10050:
+        amrItem.handle10050Message(amrMsg as AmrMsg<AmrMsg10050>)
         break;
       // 配置信息 10060
       case 10060:
+        amrItem.handle10060Message(amrMsg as AmrMsg<AmrMsg10060>)
         break;
       // 旋转货架 10080
       case 10080:
@@ -38,9 +40,11 @@ export default class AmrMessageManager {
         break;
       // 状态查询 10110
       case 10110:
+        amrItem.handle10110Message(amrMsg as AmrMsg<AmrMsg10110>)
         break;
       // 取消已下发小车任务 10120
       case 10120:
+        amrItem.handle10120Message(amrMsg as AmrMsg<AmrMsg10120>)
         break;
       // 设置小车坐标 10200
       case 10200:
@@ -51,18 +55,7 @@ export default class AmrMessageManager {
     }
     if (amrMsg.id != 10050 && amrMsg.id != 10100) {
       const seqNo = amrMsg.content.SeqNo;
-      this.sendAck(seqNo, vehicleId);
+      amrItem.sendAck(seqNo, vehicleId);
     }
   }
-
-  sendAck(seqNo: number, vehicleId: number) {
-    const msg20050 = new AmrMsg20050(seqNo, vehicleId)
-    const ack = new AmrMsg<AmrMsg20050>(msg20050)
-    this.sendMessage(ack)
-  }
-
-  sendMessage(amrMsg: AmrMsg<any>) {
-    this.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(amrMsg))
-  }
-
 }

src/core/script/RCSScript.ts

@@ -35,7 +35,11 @@ export default class RCSScript implements RCS {
   agvMove(agvId: string, targetWayPointId: string, targetDirection: '' | LLCDirection = '', option: AgvOptions = {}): Promise<ServerResponse<boolean>> {
     return Request.request.post('/api/workbench/RcsController@agvMove', {
       projectUUID: worldModel.state.project_uuid,
-      envId: worldModel.state.runState.currentEnvId
+      envId: worldModel.state.runState.currentEnvId,
+      agvId,
+      targetWayPointId,
+      targetDirection,
+      option
     })
   }
 

src/modules/cl2/Cl23dObject.ts

@@ -1,22 +1,46 @@
 import * as THREE from 'three'
-import { CSG } from 'three-csg-ts'
+import {CSG} from 'three-csg-ts'
 import gsap from 'gsap'
 import mqtt from 'mqtt'
-import { Euler } from 'three/src/math/Euler'
+import {Euler} from 'three/src/math/Euler'
 import Cl2Entity from '@/modules/cl2/Cl2Entity'
 import Cl23DGraphics from "@/modules/cl2/Cl23DGraphics"
-import {AmrErrorCode, AmrMsg, AmrMsg10010, AmrMsg20020, AmrMsg20149, AmrMsg20250, type LogicDirection} from "@/core/manager/amr/AmrMessageDefine";
-
-
-interface Task {
+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 LogicDirection, TaskCompletedData, TaskModeChangeData, TaskStatusChangeData, TaskTypeChangeData
+} 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;
-  GoodsSlotHeight: number;
-  GoodsSlotDirection: 0 | 1 | 2 | 3 | 15;
   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 {
@@ -25,19 +49,88 @@ export default class Cl23dObject extends THREE.Object3D {
 
   private _cl2Entity: Cl2Entity = null
 
-  private taskList: Task[] = []
-  private currentTask: any = null
-
+  private currentStepTaskList: StepTask[] = []
+  private runningStepTask: StepTask = null
+  private runningStepTaskList: StepTask[] = []
   private travelAnimation: core.Tween = null
-  private rotationAnimation: core.Tween = null
-  private riseAnimation: core.Tween = null
-  private stretchAnimation: core.Tween = null
-  private currentAnimation: 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
+
+  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)
+      }
+
+    }
+  }
 
   private bootTime: number = 0
 
-  private seqNoName: string = ""
+  // 心跳间隔 UInt32 单位: s
+  private heartBeatInterval: number = 0
+  // 小车所有上报消息重试间隔(未收到应答消息时重发消息)  UInt32 单位: s
+  private mqRetryInterval: number = 3
 
   public get cl2Entity(): Cl2Entity {
     if (!this._cl2Entity) {
@@ -47,17 +140,18 @@ export default class Cl23dObject extends THREE.Object3D {
     return this._cl2Entity
   }
 
+  public vehicleId: number
+
   private clock = new THREE.Clock()
 
   constructor(item: ItemJson, option?: RendererCudOption) {
     super()
-    console.log('time', this.clock.getElapsedTime())
     this.item = item
     if (!Cl23DGraphics.ptrPedestalGeometry) {
       Cl23DGraphics.ptrPedestalGeometry = Cl23DGraphics.createPtrPedestal()
     }
     const ptrPedestalGeometry = Cl23DGraphics.ptrPedestalGeometry
-    const ptrPedestalMaterial = new THREE.MeshPhongMaterial({ color: 0xffdddbca })
+    const ptrPedestalMaterial = new THREE.MeshPhongMaterial({color: 0xffdddbca})
     const ptrPedestalMesh = new THREE.Mesh(ptrPedestalGeometry, ptrPedestalMaterial)
     ptrPedestalMesh.name = 'ptrPedestal'
 
@@ -65,7 +159,7 @@ export default class Cl23dObject extends THREE.Object3D {
       Cl23DGraphics.ptrPillarGeometry = Cl23DGraphics.createPtrPillar()
     }
     const ptrPillarGeometry = Cl23DGraphics.ptrPillarGeometry
-    const ptrPillarMaterial = new THREE.MeshPhongMaterial({ color: 0xff6c6956 })
+    const ptrPillarMaterial = new THREE.MeshPhongMaterial({color: 0xff6c6956})
     const ptrPillarMesh = new THREE.Mesh(ptrPillarGeometry, ptrPillarMaterial)
 
 
@@ -73,7 +167,7 @@ export default class Cl23dObject extends THREE.Object3D {
       Cl23DGraphics.ptrForkGeometry = Cl23DGraphics.createPtrFork()
     }
     const ptrForkGeometry = Cl23DGraphics.ptrForkGeometry
-    const ptrForkMaterial = new THREE.MeshPhongMaterial({ color: 0xff444444 })
+    const ptrForkMaterial = new THREE.MeshPhongMaterial({color: 0xff444444})
     const ptrForkMesh = new THREE.Mesh(ptrForkGeometry, ptrForkMaterial)
     ptrForkMesh.name = 'ptrFork'
 
@@ -84,51 +178,115 @@ export default class Cl23dObject extends THREE.Object3D {
     groupPillar.add(ptrForkMesh)
     this.add(groupPillar)
 
-    this.seqNoName = 'CL2' + this.cl2Entity.id
-
+    this.vehicleId = parseInt(this.cl2Entity.id)
+    this.cl2Entity.viewport.addFrameTimerCallback(this.cl2Entity.id, this.onFrameTimer.bind(this))
   }
 
   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.computeLogicDirection();
-    this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
+    this.computeLogicXYAndDirection();
 
-    setTimeout(()=>{
+    if (worldModel.state.runState.isVirtual) {
+      this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
+      this.send20147()
+      setTimeout(() => {
         this.send20149()
+        this.TaskMode = 1
         // 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标
-      setTimeout(()=>{
+        setTimeout(() => {
           this.sendCurrentPositionAndDirection()
+          setTimeout(() => {
+            this.send20150()
+          }, 1000)
         }, 1000)
       }, 2000)
+    } else {
+
+    }
+
   }
+
   // 关机
   shutdown() {
+    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()
+    const content = new AmrMsg20149(this.vehicleId)
     content.AGVModel = this.AGVModel
     content.AGVFnModel = this.AGVFnModel
     // 电量
     content.Battery = 100
     content.CreateMonoTime = Date.now() - this.bootTime
-    content.VehicleId = parseInt(this.cl2Entity.id)
     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() {
-    const pointItem = Model.getItemByXYZ(this.position.x, this.position.y, this.position.z)
-    if (!pointItem || !pointItem.logicX || !pointItem.logicY) {
+    if (this.currentLogicX <= 0 || this.currentLogicY <= 0) {
       // 当前车辆所在位置未找到
-      const content = new AmrMsg20250()
+      const content = new AmrMsg20250(this.vehicleId)
       content.Duration = 0
       content.ErrCode = 5
       content.ErrCodeName = AmrErrorCode[5].ErrCodeName
@@ -136,132 +294,124 @@ export default class Cl23dObject extends THREE.Object3D {
       content.ErrLevel = 14
       content.ErrLifecycle = 2
       content.CreateMonoTime = Date.now() - this.bootTime
-      content.VehicleId = parseInt(this.cl2Entity.id)
       const m20250 = new AmrMsg<AmrMsg20250>(content)
       this.sendMessage(m20250)
     } else {
       // 发送正常地标信息
-      const content = new AmrMsg20020()
+      const content = new AmrMsg20020(this.vehicleId)
       content.CurDirection = this.currentDirection
-      content.CurLogicX = pointItem.logicX
-      content.CurLogicY = pointItem.logicY
-      content.CurX = pointItem.logicX
-      content.CurY = pointItem.logicY
+      content.CurLogicX = this.currentLogicX
+      content.CurLogicY = this.currentLogicY
+      content.CurX = this.currentLogicX
+      content.CurY = this.currentLogicY
       content.CreateMonoTime = Date.now() - this.bootTime
-      content.VehicleId = parseInt(this.cl2Entity.id)
       const m20020 = new AmrMsg<AmrMsg20020>(content)
       this.sendMessage(m20020)
     }
   }
 
-  subscribeMessage(topic: string) {
-    this.cl2Entity.viewport.envManager.client.subscribe(topic, { qos: 0 })
-  }
-  sendMessage(msg: AmrMsg<any>) {
-    this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(msg))
+  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)
+  }
 
-  onMqttConnect(item: ItemJson, client: mqtt.MqttClient) {
-    const m20020 = {
-      'content': {
-        'CreateMonoTime': 233701185,
-        'CreateTime': 1750638957541,
-        'CurDirection': 0,
-        'CurLogicX': 6,
-        'CurLogicY': 2,
-        'CurOrientation': -3.1375624383367926,
-        'CurX': 6,
-        'CurY': 2,
-        'MarkerType': 1,
-        'SendTime': 1750638957541,
-        'SeqNo': 11,
-        'VehicleId': 3,
-        'X': 2652.477598132277,
-        'Y': 3944.4427159671854
-      },
-      'id': 20020
+  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)
   }
 
-    client.subscribe(['/wcs_server/' + item.id], { qos: 0 })
-    client.publish('/agv_robot/status', JSON.stringify(m20020), { retain: true })
+  send20250(content: AmrMsg20250) {
+    this.sendMessage(new AmrMsg<AmrMsg20250>(content))
   }
 
-  handleMessage(data: AmrMsg<AmrMsg10010>) {
-    return
-    if (data.id === 10010) {
+  subscribeMessage(topic: string) {
+    this.cl2Entity.viewport.envManager.client.subscribe(topic, {qos: 0})
+  }
 
-      if (this.taskList.length <= 0) {
-        //当队列为空时，检查当前车辆所在位置
-        const pointItem = Model.getItemByXYZ(this.position.x, this.position.y, this.position.z)
-        if (!pointItem || data.content.StartX != pointItem.logicX || data.content.StartY != pointItem.logicY) {
-          // throw new Error('当前车辆所在位置未找到')
+  sendMessage(msg: AmrMsg<any>) {
+    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
+    }
+    this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(msg))
+    this.heartBeatTimeCount = 0
   }
 
-      const startTask = {
-        X: data.content.StartX,
-        Y: data.content.StartY,
-        Speed: 0
+  sendHeartBeat() {
+    const content = new AmrMsg20100(this.vehicleId)
+    content.Temperature = {Battery: this.Battery}
+    const m20100 = new AmrMsg<AmrMsg20100>(content)
+    this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(m20100))
   }
-      for (const item of data.content.Link) {
-        let moveDirection: 0 | 1 | 2 | 3 | 15 = 15
-        if (startTask.X < item.X) {
-          if (item.Speed > 0) {
-            moveDirection = 0
-          } else {
-            moveDirection = 2
+
+  sendAck(seqNo: number, vehicleId: number) {
+    const msg20050 = new AmrMsg20050(seqNo, vehicleId)
+    const ack = new AmrMsg<AmrMsg20050>(msg20050)
+    this.heartBeatTimeCount = 0
+    this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(ack))
   }
+
+  /*==========消息处理============*/
+
+  // 处理任务
+  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 {
+        // 此处应该有错误处理
       }
-        if (startTask.Y < item.Y) {
-          if (item.Speed > 0) {
-            moveDirection = 1
     } else {
-            moveDirection = 3
+      this.makeStepTask(data)
+      this.executeTask()
     }
   }
 
-        // 添加到队列
-        this.taskList.push({
-          SeqNo: data.content.SeqNo,
-          OperationType: 0,
-          PickMode: 0,
-          GoodsSlotHeight: data.content.GoodsSlotHeight,
-          GoodsSlotDirection: data.content.GoodsSlotDirection,
-          X: item.X,
-          Y: item.Y,
-          Speed: item.Speed,
-          Direction: moveDirection
-        })
-        startTask.X = item.X
-        startTask.Y = item.Y
-        startTask.Speed = item.Speed
-      }
-      if (data.content.OperationType === 4 || data.content.OperationType === 5) {
-        this.taskList.push({
-          OperationType: data.content.OperationType,
-          PickMode: data.content.PickMode,
-          GoodsSlotHeight: data.content.GoodsSlotHeight,
-          GoodsSlotDirection: data.content.GoodsSlotDirection
-        })
+  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
   }
 
-      console.log('time', this.clock.getElapsedTime())
+  // 处理状态查询
+  handle10110Message(data: AmrMsg<AmrMsg10110>) {
+    this.send20060()
+  }
 
-      this.executeTask()
-      return
+  // 取消任务
+  handle10120Message(data: AmrMsg<AmrMsg10120>) {
 
-      this.cl2Entity.addRobotTask(data)
-      if (!this.cl2Entity.taskIsRunning) {
-        this.cl2Entity.taskStartRun()
-      }
-    }
   }
 
   // 计算逻辑方向
-  computeLogicDirection() {
+  computeLogicXYAndDirection() {
     let ra = this.rotation.y
     while (ra > Math.PI * 2) {
       ra -= Math.PI * 2
@@ -269,7 +419,7 @@ export default class Cl23dObject extends THREE.Object3D {
     while (ra < 0) {
       ra += Math.PI * 2
     }
-    const ddra = Math.PI/8
+    const ddra = Math.PI / 8
     if (ra >= ddra * 7 || ra < ddra) {
       this.currentDirection = 0;
 
@@ -284,39 +434,212 @@ export default class Cl23dObject extends THREE.Object3D {
     } 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;
     }
 
-  executeTask() {
+  }
 
-    if (this.currentAnimation) {
-      return
+  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
           }
-    while (this.taskList.length > 0) {
-      const task = this.taskList[0]
-      if (this.currentTask == null) {
-        this.currentTask = task
         }
 
-      if (task.OperationType == 0
-        && ((task.Speed > 0) != (this.currentTask.Speed > 0) || task.Direction != this.currentTask.Direction)) {
-        if (!this.currentAnimation || this.currentAnimation == this.rotationAnimation) {
-          // 转向
-          this.addRotation(task.Direction).then(() => {
-            this.executeTask()
-          })
-          this.currentAnimation = this.rotationAnimation
-          this.currentTask = task
+        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
       }
-      } else if (task.OperationType === 0) {
-        if (!this.currentAnimation || this.currentAnimation == this.travelAnimation) {
-          this.taskList.shift()
-          this.addTravel(task.X, task.Y, task.Speed / 1000)
-          this.currentAnimation = this.travelAnimation
-          this.currentTask = task
     }
+    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() {
+
+    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.add
+        } else if (stepTask.StepTaskType == "UNLOAD") {
+          // this.add
+        }
+      }
     }
   }
 
@@ -357,7 +680,7 @@ export default class Cl23dObject extends THREE.Object3D {
         repeat: 0,
         ease: 'sine.inOut',
         onComplete: resolve,
-        onUpdate: function() {
+        onUpdate: function () {
           const a = this.targets()[0]
           if (a.y < bh) {
             if (pz > -1) {
@@ -407,25 +730,18 @@ export default class Cl23dObject extends THREE.Object3D {
     let time = Math.abs(angleDiff) / (Math.PI / 7)
     const duration = time
 
-    if (!this.rotationAnimation) {
     return new Promise(resolve => {
-        this.rotationAnimation = gsap.to(this.rotation, {
+      gsap.to(this.rotation, {
         y: tr,
         duration,
         ease: 'none',
-          onComplete: () => {
-            this.rotationAnimation = null
-            this.currentAnimation = null
+        onComplete: ()=>{
           resolve()
+          this.runningStepTaskList = []
+          this.runningStepTask = null
         }
       })
-        this.currentAnimation = this.rotationAnimation
     })
-    } else {
-      this.rotationAnimation.vars.y = tr
-      const tt = this.rotationAnimation.duration()
-      this.rotationAnimation.duration(tt + duration)
-    }
   }
 
   // 走
@@ -434,26 +750,51 @@ export default class Cl23dObject extends THREE.Object3D {
     const pos = Model.getPositionByLogicXY(logicX, logicY)
 
     const fromPos = this.position
-    const toPos = new THREE.Vector3(pos.x, pos.y, pos.z)
+    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.to(this.position, {
+        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: 'sine.inOut',
+          ease: 'power2.inOut',
           onComplete: () => {
             this.travelAnimation = null
-            this.currentAnimation = null
-            this.executeTask()
-            // resolve()
+            resolve()
+            this.runningStepTaskList = []
+            this.runningStepTask = null
+            this.computeLogicXYAndDirection()
+          },
+          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
+                }
+              }
+            }
           }
         })
-        this.currentAnimation = this.travelAnimation
       })
     } else {
       this.travelAnimation.vars.x = toPos.x
@@ -461,9 +802,9 @@ export default class Cl23dObject extends THREE.Object3D {
       this.travelAnimation.vars.z = toPos.z
       const tt = this.travelAnimation.duration()
       this.travelAnimation.duration(tt + duration)
+      this.travelAnimation.invalidate().restart();
     }
 
-
   }
 
   // fn = _.debounce((cl2: Cl2Entity) => {