diff --git a/src/core/engine/Viewport.ts b/src/core/engine/Viewport.ts index a80bf90..c87dfa9 100644 --- a/src/core/engine/Viewport.ts +++ b/src/core/engine/Viewport.ts @@ -69,6 +69,8 @@ export default class Viewport { markRaw(this.envManager) ] + registerFrameTimerCallBack: Map void> = new Map() + // 对象实例管理器 moduleName -> InstanceMeshManager meshManager: Map = 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 diff --git a/src/core/manager/amr/AmrMessageDefine.ts b/src/core/manager/amr/AmrMessageDefine.ts index 4c7569d..c48729c 100644 --- a/src/core/manager/amr/AmrMessageDefine.ts +++ b/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 { id: number content: T constructor(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 { 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 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: { diff --git a/src/core/manager/amr/AmrMessageManager.ts b/src/core/manager/amr/AmrMessageManager.ts index 0864b8f..04be1a8 100644 --- a/src/core/manager/amr/AmrMessageManager.ts +++ b/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) { 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) + amrItem.handle10010Message(amrMsg.content as AmrMsg10010) break; // 停止/解除 10040 case 10040: break; // 电文应答 10050 case 10050: + amrItem.handle10050Message(amrMsg as AmrMsg) break; // 配置信息 10060 case 10060: + amrItem.handle10060Message(amrMsg as AmrMsg) break; // 旋转货架 10080 case 10080: @@ -38,9 +40,11 @@ export default class AmrMessageManager { break; // 状态查询 10110 case 10110: + amrItem.handle10110Message(amrMsg as AmrMsg) break; // 取消已下发小车任务 10120 case 10120: + amrItem.handle10120Message(amrMsg as AmrMsg) 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(msg20050) - this.sendMessage(ack) - } - - sendMessage(amrMsg: AmrMsg) { - this.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(amrMsg)) - } - } diff --git a/src/core/script/RCSScript.ts b/src/core/script/RCSScript.ts index 7d5bb2a..7d588d4 100644 --- a/src/core/script/RCSScript.ts +++ b/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> { 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 }) } diff --git a/src/modules/cl2/Cl23dObject.ts b/src/modules/cl2/Cl23dObject.ts index 50d7fb6..c0012ec 100644 --- a/src/modules/cl2/Cl23dObject.ts +++ b/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[] = [] + + 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(); + + if (worldModel.state.runState.isVirtual) { + 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) + } else { + + } - setTimeout(()=>{ - this.send20149() - // 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标 - setTimeout(()=>{ - this.sendCurrentPositionAndDirection() - }, 1000) - }, 2000) } + // 关机 shutdown() { + const content = new AmrMsg20148(this.vehicleId) + // 电量 + content.Battery = 100 + content.CreateMonoTime = Date.now() - this.bootTime + content.Uptime = content.CreateMonoTime + const m20148 = new AmrMsg(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(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(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(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(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(content) this.sendMessage(m20020) } } - subscribeMessage(topic: string) { - this.cl2Entity.viewport.envManager.client.subscribe(topic, { qos: 0 }) + send20011(content: AmrMsg20011) { + const m20011 = new AmrMsg>(content) + this.sendMessage(m20011) } - sendMessage(msg: AmrMsg) { - this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(msg)) + + send20020(content: AmrMsg20020) { + const m20020 = new AmrMsg(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(content) + this.sendMessage(m20060) + } + + send20250(content: AmrMsg20250) { + this.sendMessage(new AmrMsg(content)) + } + + subscribeMessage(topic: string) { + this.cl2Entity.viewport.envManager.client.subscribe(topic, {qos: 0}) + } - 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 + sendMessage(msg: AmrMsg) { + 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 + } - client.subscribe(['/wcs_server/' + item.id], { qos: 0 }) - client.publish('/agv_robot/status', JSON.stringify(m20020), { retain: true }) + sendHeartBeat() { + const content = new AmrMsg20100(this.vehicleId) + content.Temperature = {Battery: this.Battery} + const m20100 = new AmrMsg(content) + this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(m20100)) } - handleMessage(data: AmrMsg) { - return - if (data.id === 10010) { + sendAck(seqNo: number, vehicleId: number) { + const msg20050 = new AmrMsg20050(seqNo, vehicleId) + const ack = new AmrMsg(msg20050) + this.heartBeatTimeCount = 0 + this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(ack)) + } - 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('当前车辆所在位置未找到') - } - } + /*==========消息处理============*/ - const startTask = { - X: data.content.StartX, - Y: data.content.StartY, - Speed: 0 + // 处理任务 + 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 { + // 此处应该有错误处理 } - 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 - } - } - if (startTask.Y < item.Y) { - if (item.Speed > 0) { - moveDirection = 1 - } else { - moveDirection = 3 - } - } + } else { + 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) { + if (this.sendMessageQueue.length > 0 && data.content.SeqNo === this.sendMessageQueue[0].content.SeqNo) { + this.mqRetryTimeCount = 0 + this.sendMessageQueue.shift() + } + } - } + handle10060Message(data: AmrMsg) { + this.mqRetryInterval = data.content.MqRetryTime + this.heartBeatInterval = data.content.HeartBeat + this.TaskMode = 0 + } - console.log('time', this.clock.getElapsedTime()) + // 处理状态查询 + handle10110Message(data: AmrMsg) { + this.send20060() + } - this.executeTask() - return + // 取消任务 + handle10120Message(data: AmrMsg) { - 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,8 +419,8 @@ export default class Cl23dObject extends THREE.Object3D { while (ra < 0) { ra += Math.PI * 2 } - const ddra = Math.PI/8 - if (ra >= ddra * 7 || ra < ddra) { + const ddra = Math.PI / 8 + if (ra >= ddra * 7 || ra < ddra) { this.currentDirection = 0; } else if (ra >= ddra && ra < ddra * 3) { @@ -284,38 +434,211 @@ export default class Cl23dObject extends THREE.Object3D { } else { this.currentDirection = 15; } - } - executeTask() { - - if (this.currentAnimation) { - return + 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; } - while (this.taskList.length > 0) { - const task = this.taskList[0] - if (this.currentTask == null) { - this.currentTask = task + + } + + 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 (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) } - } 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 + + 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() { + 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, { - y: tr, - duration, - ease: 'none', - onComplete: () => { - this.rotationAnimation = null - this.currentAnimation = null - resolve() - } - }) - this.currentAnimation = this.rotationAnimation + return new Promise(resolve => { + gsap.to(this.rotation, { + y: tr, + duration, + ease: 'none', + onComplete: ()=>{ + resolve() + this.runningStepTaskList = [] + this.runningStepTask = null + } }) - } 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) => {