From ea2d1982ffc0d9757c3800836bb6233bca9b8640 Mon Sep 17 00:00:00 2001 From: yuliang <398780299@qq.com> Date: Tue, 1 Jul 2025 19:35:10 +0800 Subject: [PATCH] =?UTF-8?q?cl2=20=E5=9F=BA=E4=BA=8E=E8=AE=BE=E5=A4=87?= =?UTF-8?q?=E7=8A=B6=E6=80=81=E5=92=8C=E4=BB=BB=E5=8A=A1=E7=8A=B6=E6=80=81?= =?UTF-8?q?=E5=88=86=E6=AD=A5=E7=94=9F=E6=88=90=E8=AE=BE=E5=A4=87=E4=BB=BB?= =?UTF-8?q?=E5=8A=A1=E6=B6=88=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/core/base/BaseItemEntity.ts | 2 +- src/core/manager/EnvManager.ts | 22 +- src/core/manager/amr/AmrMessageDefine.ts | 174 ++++++++ src/core/manager/amr/AmrMessageManager.ts | 68 +++ src/editor/propEditors/PtrController.vue | 40 ++ .../widgets/property/PropertyPanelConstant.ts | 2 + src/modules/cl2/Cl23DGraphics.ts | 295 +++++++++++++ src/modules/cl2/Cl23dObject.ts | 475 +++++---------------- src/modules/cl2/Cl2PropertySetter.ts | 4 + 9 files changed, 714 insertions(+), 368 deletions(-) create mode 100644 src/core/manager/amr/AmrMessageDefine.ts create mode 100644 src/core/manager/amr/AmrMessageManager.ts create mode 100644 src/editor/propEditors/PtrController.vue create mode 100644 src/modules/cl2/Cl23DGraphics.ts diff --git a/src/core/base/BaseItemEntity.ts b/src/core/base/BaseItemEntity.ts index 19d772c..87f5def 100644 --- a/src/core/base/BaseItemEntity.ts +++ b/src/core/base/BaseItemEntity.ts @@ -10,7 +10,7 @@ import { getRenderer } from '@/core/manager/ModuleManager.ts' * 物流实体基类 */ export default abstract class BaseEntity implements EntityIf, Loadable, Carry, Walk, ForkArm, LiftingArm { - protected readonly viewport: Viewport + public readonly viewport: Viewport protected readonly taskQueue: TaskQueue = new TaskQueue() readonly id: string readonly item: ItemJson diff --git a/src/core/manager/EnvManager.ts b/src/core/manager/EnvManager.ts index b52ab3c..43464e9 100644 --- a/src/core/manager/EnvManager.ts +++ b/src/core/manager/EnvManager.ts @@ -1,17 +1,23 @@ import type Viewport from '@/core/engine/Viewport.ts' import { worldModel } from '@/core/manager/WorldModel.ts' import mqtt, { type IConnackPacket, type IPublishPacket } from 'mqtt' -import type { Cl2Task } from '@/modules/cl2/Cl23dObject.ts' import type { ErrorWithReasonCode } from 'mqtt/src/lib/shared.ts' import type Cl23dObject from '@/modules/cl2/Cl23dObject.ts' import { Request } from '@ease-forge/shared' +import AmrMessageManager from "@/core/manager/amr/AmrMessageManager"; +import {AmrMsg} from "@/core/manager/amr/AmrMessageDefine"; export default class EnvManager { private viewport: Viewport - private client: mqtt.MqttClient = null + private amrMessageManager: AmrMessageManager = null + public client: mqtt.MqttClient = null init(viewport: Viewport): void { this.viewport = viewport + if (!this.amrMessageManager) { + this.amrMessageManager = new AmrMessageManager() + } + this.amrMessageManager.viewport = viewport } // 从后台读取所有车 @@ -92,10 +98,14 @@ export default class EnvManager { } onMqttMessage = (topic: string, payload: Buffer, packet: IPublishPacket) => { - console.log(`[${topic}] ${msg}`) - debugger - const a: Cl2Task = JSON.parse(msg.toString()) - this.handleMessage(a) + console.log(`[${topic}]-> ${payload.toString()}`) + if (topic.startsWith("/wcs_server/")) { + const message: AmrMsg = JSON.parse(payload.toString()) + this.amrMessageManager.handleMessage(topic, message) + } else { + + } + } onMqttError = (error: Error | ErrorWithReasonCode) => { diff --git a/src/core/manager/amr/AmrMessageDefine.ts b/src/core/manager/amr/AmrMessageDefine.ts new file mode 100644 index 0000000..4c7569d --- /dev/null +++ b/src/core/manager/amr/AmrMessageDefine.ts @@ -0,0 +1,174 @@ +import type Viewport from '@/core/engine/Viewport.ts' + +type LogicDirection = 0 | 1 | 2 | 3 | 15 + + +class AmrMsg { + id: number + content: T + + constructor(content: T) { + if (content instanceof AmrMsg20020) { + this.id = 20020 + } else if (content instanceof AmrMsg20050) { + this.id = 20050 + } else if (content instanceof AmrMsg20149) { + this.id = 20149 + } else if (content instanceof AmrMsg20250) { + this.id = 20250 + } + this.content = content + } +} + +class AmrMsg10010 { + SeqNo: number; + OperationType: 0 | 1 | 2 | 3 | 4 | 5 | 135 | 136; + UseBriefLocation: boolean; + StartX: number; + StartY: number; + EndX: number; + EndY: number; + EndDirection: 0 | 1 | 2 | 3 | 15; + // 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作 + PickMode: 0 | 1 | 2 | 3 | 4 | 5 | 6; + GoodsSlotHeight: number; + GoodsSlotDirection: 0 | 1 | 2 | 3 | 15; + GoodsId: string; + Link: { + // id: string; // 实际报文没有 + X: number; + Y: number; + Speed: number; + }[] +} + + +class AmrMsg20000Base { + SeqNo: number + VehicleId: number + CreateTime: number + SendTime: number + CreateMonoTime: number + + constructor() { + this.SeqNo = getAmrMsgSeqNo() + this.CreateTime = Date.now() + this.SendTime = Date.now() + } +} + + +class AmrMsg20020 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() { + super() + } +} + +class AmrMsg20050 extends AmrMsg20000Base { + constructor(seqNo: number, vehicleId: number) { + super() + this.SeqNo = seqNo + this.VehicleId = vehicleId + } +} + +class AmrMsg20149 extends AmrMsg20000Base { + AGVFnModel: string + AGVModel: string + Battery: number + + constructor() { + super() + } +} + +class AmrMsg20250 extends AmrMsg20000Base { + // 异常持续时间 秒 + Duration: number = 0 + // 异常代码 + ErrCode: number + // 异常代码 名称 + ErrCodeName: string + // 异常事件类型 0：无 1：开始 2：更新 3：结束 4：开始并结束 + ErrEvtType: number + // 异常等级 0：Undetermined 未确定 12：Info 信息 13：Warning 警告 14：Critical 严重 15：Fatal 致命 + ErrLevel: number + // 异常生命周期 0：易变的 1：一次性 2：持续性 3：任务期间 + ErrLifecycle: number + // 异常详情 根据不同的异常，字段不一样，比如针对ErrCode为2电量低的情况，ErrMsg中使用CurBattery标识当前电量； + ErrMsg: object + + constructor() { + super() + } + + // 地标异常 + // {"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} +} + +let __AmrMsgSeqNo__: number = 0 + +const getAmrMsgSeqNo = () => { + if (__AmrMsgSeqNo__ >= 0Xfffffff) { + __AmrMsgSeqNo__ = 0 + } else { + __AmrMsgSeqNo__++ + } + return __AmrMsgSeqNo__ +} + +export {AmrMsg, AmrMsg10010, AmrMsg20020, AmrMsg20050, AmrMsg20149, AmrMsg20250, AmrErrorCode, LogicDirection} + +const AmrErrorCode = { + 0: { + // 无异常 + ErrCode: 0, + ErrCodeName: "", + ErrMsg: {} + }, + 5: { + // 未找到地面二维码 "ErrEvtType":1,"ErrLevel":14,"ErrLifecycle":2 + ErrCode: 5, + ErrCodeName: "kLocationMarkNotFound", + 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} + } + }, + 15: { + // 未记录的地标码 "ErrEvtType":4,"ErrLevel":13,"ErrLifecycle":1 + ErrCode: 15, + ErrCodeName: "kUnrecognizedMark", + ErrMsg: { + "ErrDesc": "在位置 (-799.00, -1004.00)（逻辑位置：(-1.00, -1.00)）发现了一个未识别的码（224842）", + "ErrDescEn": "Found an unrecognized mark(224842) at location (-799.00, -1004.00)(logic location: (-1.00, -1.00))", + "ErrPrivInfo": {"CurLogicX": -1, "CurLogicY": -1, "MarkName": "224842", "X": -799.0, "Y": -1004.0} + } + }, + 67: { + // 障碍物检测功能异常 "ErrEvtType":1,"ErrLevel":14,"ErrLifecycle":2, + ErrCode: 67, + ErrCodeName: "kObstacleDetectionMalfunction", + ErrMsg: {"ErrDesc": "障碍物检测功能异常", "ErrDescEn": "Obstacle detection malfunction", "ErrPrivInfo": {"CurLogicX": 6, "CurLogicY": 2, "X": 10816.0, "Y": 2115.0}} + }, + 85: { + // 需要人工介入处理 "ErrEvtType":1,"ErrLevel":14,"ErrLifecycle":2 + // 当设备遇到无法/不敢自动恢复的异常时，如果导致执行中断的异常结束了，则会报这个异常。比如行驶过程中防撞条被碰了一下而后马上松开了，由于防撞条触发很可能有撞击，所以检测到防撞条触发后车会停止不动，这种情况下需要人工介入检查处理。由于被撞物体与防撞条已经不接触了，所以防撞条触发异常（异常号8）会结束，结束的同时设备主动产生本异常，以提示人工介入。此异常消息中会包含一个字段，用以提示可能的导致异常的原因；由于异常过程可能伴随多种严重异常（比如碰撞到物体到停车过程可能伴随有失控、检测不到地标码等异常），这个字段所提示原因并不一定是确切的原因 + ErrCode: 85, + ErrCodeName: "kRequestForIntervention", + ErrMsg: {"ErrDesc": "需要人工协助。无法恢复的原因：kLocationMarkNotFound（ID：5）", "ErrDescEn": "Manual assistance is required. Likely reason for my inability: kLocationMarkNotFound(id: 5)", "ErrPrivInfo": {"ReasonException": 5}} + } +} diff --git a/src/core/manager/amr/AmrMessageManager.ts b/src/core/manager/amr/AmrMessageManager.ts new file mode 100644 index 0000000..0864b8f --- /dev/null +++ b/src/core/manager/amr/AmrMessageManager.ts @@ -0,0 +1,68 @@ +import {AmrMsg, AmrMsg10010, AmrMsg20050} from "@/core/manager/amr/AmrMessageDefine"; +import Cl23dObject from "@/modules/cl2/Cl23dObject"; +import Viewport from "@/core/engine/Viewport"; + +export default class AmrMessageManager { + + public viewport: Viewport + + handleMessage(topic, amrMsg: AmrMsg) { + + const vehicleId = parseInt(topic.replace("/wcs_server/", "")) + switch (amrMsg.id) { + // AMR作业指令 10010 + case 10010: + const cl2 = this.viewport.entityManager.findObjectById(amrMsg.content.VehicleId + "") as Cl23dObject + cl2.handleMessage(amrMsg as AmrMsg) + break; + // 停止/解除 10040 + case 10040: + break; + // 电文应答 10050 + case 10050: + break; + // 配置信息 10060 + case 10060: + break; + // 旋转货架 10080 + case 10080: + break; + // 旋转车身 10081 + case 10081: + break; + // 控制卷帘门 10082 + case 10082: + break; + // 心跳 10100 + case 10100: + break; + // 状态查询 10110 + case 10110: + break; + // 取消已下发小车任务 10120 + case 10120: + break; + // 设置小车坐标 10200 + case 10200: + break; + // 等待就绪 19997 + case 19997: + break; + } + if (amrMsg.id != 10050 && amrMsg.id != 10100) { + const seqNo = amrMsg.content.SeqNo; + this.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/editor/propEditors/PtrController.vue b/src/editor/propEditors/PtrController.vue new file mode 100644 index 0000000..423b11c --- /dev/null +++ b/src/editor/propEditors/PtrController.vue @@ -0,0 +1,40 @@ + + + + + diff --git a/src/editor/widgets/property/PropertyPanelConstant.ts b/src/editor/widgets/property/PropertyPanelConstant.ts index ea6c113..59577b5 100644 --- a/src/editor/widgets/property/PropertyPanelConstant.ts +++ b/src/editor/widgets/property/PropertyPanelConstant.ts @@ -5,6 +5,7 @@ import { dataFormInputComponents } from "@/components/data-form/DataFormConstant import TransformEditor from "@/editor/propEditors/TransformEditor.vue"; import InOutCenterEditor from "@/editor/propEditors/InOutCenterEditor.vue"; import BayEditor from "@/editor/propEditors/BayEditor.vue"; +import PtrController from "@/editor/propEditors/PtrController.vue"; const defDataFormProps: DataFormProps = { columnCount: 1, @@ -16,6 +17,7 @@ const defDataFormProps: DataFormProps = { dataFormInputComponents.TransformEditor = markRaw(TransformEditor); dataFormInputComponents.InOutCenterEditor = markRaw(InOutCenterEditor); dataFormInputComponents.BayEditor = markRaw(BayEditor); +dataFormInputComponents.PtrController = markRaw(PtrController); const basicFieldsSetter: Array = [ { diff --git a/src/modules/cl2/Cl23DGraphics.ts b/src/modules/cl2/Cl23DGraphics.ts new file mode 100644 index 0000000..2f6a9dc --- /dev/null +++ b/src/modules/cl2/Cl23DGraphics.ts @@ -0,0 +1,295 @@ +import * as THREE from "three"; +import {CSG} from "three-csg-ts"; +//@ts-ignore +import { mergeGeometries } from 'three/addons/utils/BufferGeometryUtils.js' + +export default class Cl23DGraphics { + +// 创建ptr的底座 + public static createPtrPedestal(): THREE.BufferGeometry { + // 参数配置 + const radius = 0.8 // 圆半径 + const lineDist = 0.75 // 切割线距离圆心距离 + const segments = 64 // 圆的分段精度 + + // 计算切割线与圆的交点 + const intersectY = Math.sqrt(radius * radius - lineDist * lineDist) + const startAngle = Math.asin(intersectY / radius) + const endAngle = Math.acos(intersectY / radius) + + + const shape = new THREE.Shape() + shape.moveTo(0, 0) // 起点在圆心 + shape.absarc(0, 0, 0.8, startAngle, endAngle, false) // 从0到π绘制半圆 + shape.absarc(0, 0, 0.8, startAngle + Math.PI / 2, endAngle + Math.PI / 2, false) + shape.absarc(0, 0, 0.8, startAngle + Math.PI, endAngle + Math.PI, false) + shape.absarc(0, 0, 0.8, startAngle + Math.PI / 2 * 3, endAngle + Math.PI / 2 * 3, false) + + shape.lineTo(-0.5, -intersectY) + shape.lineTo(-0.5, -intersectY + 0.17) + shape.lineTo(0.75, -intersectY + 0.17) + shape.lineTo(0.75, intersectY - 0.17) + shape.lineTo(-0.5, intersectY - 0.17) + shape.lineTo(-0.5, intersectY) + shape.closePath() // 闭合路径 + + // 拉伸轨迹线 + const curve = new THREE.CatmullRomCurve3( + [new THREE.Vector3(0, 0.02, 0), new THREE.Vector3(0, 0.22, 0)], + false, // 闭合曲线 + 'catmullrom', + 0 + ) + + // 挤出几何图形 参数 + const options = { + steps: 1, + bevelEnabled: false, + extrudePath: curve // 设置挤出轨迹 + } + // 创建挤出几何体 + const geometry = new THREE.ExtrudeGeometry(shape, options) + const material = new THREE.MeshBasicMaterial({color: 0xffdddbca}) + const mesh = new THREE.Mesh(geometry, material) + + mesh.updateMatrix() + + // 倒角 + const shapeD = new THREE.Shape() + shapeD.moveTo(-0.02, -0.02) + shapeD.lineTo(0.02, 0.02) + shapeD.lineTo(-0.02, 0.02) + shapeD.closePath() + + const curveD = new THREE.EllipseCurve( + 0, 0, radius, radius, 0, Math.PI * 2, false, 0 + ) + // 生成拉伸路径点 + const pointsD = curveD.getPoints(segments).map(p => + new THREE.Vector3(p.x, 0.20, p.y) + ) + // 3. 挤出成型 + const optionsD = { + steps: segments, + bevelEnabled: false, + // bevelSegments: 0.01, + extrudePath: new THREE.CatmullRomCurve3(pointsD) + } + + const geometryD = new THREE.ExtrudeGeometry(shapeD, optionsD) + const meshD = new THREE.Mesh(geometryD, material) + meshD.updateMatrix() + + // 布尔运算 + const result = CSG.subtract(mesh, meshD) + + return result.geometry + } + +// 创建ptr的立柱 + public static createPtrPillar(): THREE.BufferGeometry { + // 606.5 + const width = 0.245 + const depth = 0.716 + const dd = 0.05 + const shape = new THREE.Shape() + shape.moveTo(-0.728, -0.358 + dd) + shape.lineTo(-0.728 + dd, -0.358) + shape.lineTo(-0.728 + width - dd, -0.358) + shape.lineTo(-0.728 + width, -0.358 + dd) + + shape.lineTo(-0.728 + width, -0.28) + shape.lineTo(-0.728 + width - 0.08, -0.28) + shape.lineTo(-0.728 + width - 0.08, 0.28) + shape.lineTo(-0.728 + width, 0.28) + + shape.lineTo(-0.728 + width, -0.358 + depth - dd) + shape.lineTo(-0.728 + width - dd, -0.358 + depth) + shape.lineTo(-0.728 + dd, -0.358 + depth) + shape.lineTo(-0.728, -0.358 + depth - dd) + // shape.lineTo(-0.728, -0.358 + dd); + // shape.closePath() + + // 拉伸轨迹线 + const curve = new THREE.CatmullRomCurve3( + [new THREE.Vector3(0, 0.22, 0), new THREE.Vector3(0, 1.872, 0)], + false, // 闭合曲线 + 'catmullrom', + 0 + ) + + // 挤出几何图形 参数 + const options = { + steps: 1, + bevelSegments: 0.05, + bevelEnabled: true, + extrudePath: curve // 设置挤出轨迹 + } + // 创建挤出几何体 + let geometry = new THREE.ExtrudeGeometry(shape, options) + + + const fd = 0.03 + const shapeBf = new THREE.Shape() + + shapeBf.moveTo(-0.728 + width - 0.08, -0.28) + shapeBf.lineTo(-0.728 + width + 0.02, -0.28) + shapeBf.lineTo(-0.728 + width + 0.02, -0.25) + shapeBf.lineTo(-0.728 + width + 0.02 + 1.130 - fd, -0.25) + shapeBf.lineTo(-0.728 + width + 0.02 + 1.130, -0.25 + fd) + shapeBf.lineTo(-0.728 + width + 0.02 + 1.130, -0.14 - fd) + shapeBf.lineTo(-0.728 + width + 0.02 + 1.130 - fd, -0.14) + shapeBf.lineTo(-0.728 + width + 0.02, -0.14) + shapeBf.lineTo(-0.728 + width + 0.02, 0.14) + shapeBf.lineTo(-0.728 + width + 0.02 + 1.130 - fd, 0.14) + shapeBf.lineTo(-0.728 + width + 0.02 + 1.130, 0.14 + fd) + shapeBf.lineTo(-0.728 + width + 0.02 + 1.130, 0.25 - fd) + shapeBf.lineTo(-0.728 + width + 0.02 + 1.130 - fd, 0.25) + shapeBf.lineTo(-0.728 + width + 0.02, 0.25) + shapeBf.lineTo(-0.728 + width + 0.02, 0.28) + shapeBf.lineTo(-0.728 + width - 0.08, 0.28) + + shapeBf.closePath() + + // 拉伸轨迹线 + const curveBf = new THREE.CatmullRomCurve3( + [new THREE.Vector3(0, 0.02, 0), new THREE.Vector3(0, 0.06, 0)], + false, // 闭合曲线 + 'catmullrom', + 0 + ) + + // 挤出几何图形 参数 + const optionsBf = { + steps: 1, + bevelSegments: 0.05, + bevelEnabled: true, + extrudePath: curveBf // 设置挤出轨迹 + } + // 创建挤出几何体 + const geometryBf = new THREE.ExtrudeGeometry(shapeBf, optionsBf) + geometry = mergeGeometries([geometry, geometryBf]) + const material = new THREE.MeshBasicMaterial({color: 0xffdddbca}) + const mesh = new THREE.Mesh(geometry, material) + + const shapeD = new THREE.Shape() + shapeD.moveTo(-0.3, -0.25) + shapeD.lineTo(-0.3, 0.25) + shapeD.lineTo(0.3, 0.25) + shapeD.lineTo(0.3, -0.25) + shapeD.closePath() + + const curveD = new THREE.CatmullRomCurve3( + [new THREE.Vector3(0, 1.45, -1), new THREE.Vector3(0, 1.45, 1)], + false, // 闭合曲线 + 'catmullrom', + 0 + ) + + const optionsD = { + steps: 1, + bevelSegments: 0.05, + bevelEnabled: true, + extrudePath: curveD // 设置挤出轨迹 + } + + const geometryD = new THREE.ExtrudeGeometry(shapeD, optionsD) + const meshD = new THREE.Mesh(geometryD, material) + mesh.updateMatrix() + meshD.updateMatrix() + const result = CSG.subtract(mesh, meshD) + + return result.geometry + + } + +// 创建ptr的立柱 + public static createPtrFork(): THREE.BufferGeometry { + // 606.5 + const width = 0.245 + const depth = 0.716 + const dd = 0.05 + const fd = 0.03 + const shape = new THREE.Shape() + + shape.moveTo(-0.728 + width - 0.08, -0.28) + shape.lineTo(-0.728 + width + 0.02, -0.28) + shape.lineTo(-0.728 + width + 0.02, -0.27) + shape.lineTo(-0.728 + width + 0.02 + 1.130 - fd, -0.27) + shape.lineTo(-0.728 + width + 0.02 + 1.130, -0.27 + fd) + shape.lineTo(-0.728 + width + 0.02 + 1.130, -0.12 - fd) + shape.lineTo(-0.728 + width + 0.02 + 1.130 - fd, -0.12) + shape.lineTo(-0.728 + width + 0.02, -0.12) + shape.lineTo(-0.728 + width + 0.02, 0.12) + shape.lineTo(-0.728 + width + 0.02 + 1.130 - fd, 0.12) + shape.lineTo(-0.728 + width + 0.02 + 1.130, 0.12 + fd) + shape.lineTo(-0.728 + width + 0.02 + 1.130, 0.27 - fd) + shape.lineTo(-0.728 + width + 0.02 + 1.130 - fd, 0.27) + shape.lineTo(-0.728 + width + 0.02, 0.27) + shape.lineTo(-0.728 + width + 0.02, 0.28) + shape.lineTo(-0.728 + width - 0.08, 0.28) + + shape.closePath() + + // 拉伸轨迹线 + const curve = new THREE.CatmullRomCurve3( + [new THREE.Vector3(0, 0.02, 0), new THREE.Vector3(0, 0.482, 0)], + false, // 闭合曲线 + 'catmullrom', + 0 + ) + + // 挤出几何图形 参数 + const options = { + steps: 1, + bevelSegments: 0.05, + bevelEnabled: true, + extrudePath: curve // 设置挤出轨迹 + } + // 创建挤出几何体 + const geometry = new THREE.ExtrudeGeometry(shape, options) + const material = new THREE.MeshBasicMaterial({color: 0xffdddbca}) + const mesh = new THREE.Mesh(geometry, material) + mesh.updateMatrix() + + const shapeD = new THREE.Shape() + + + shapeD.moveTo(-0.728 + width + 0.02, -0.3) + shapeD.lineTo(-0.728 + width + 0.02 + 1.2, -0.3) + shapeD.lineTo(-0.728 + width + 0.02 + 1.2, 0.3) + shapeD.lineTo(-0.728 + width + 0.02, 0.3) + shape.closePath() + + // 拉伸轨迹线 + const curveD = new THREE.CatmullRomCurve3( + [new THREE.Vector3(0, 0.07, 0), new THREE.Vector3(0, 0.482, 0)], + false, // 闭合曲线 + 'catmullrom', + 0 + ) + + // 挤出几何图形 参数 + const optionsD = { + steps: 1, + bevelSegments: 0.05, + bevelEnabled: true, + extrudePath: curveD // 设置挤出轨迹 + } + // 创建挤出几何体 + const geometryD = new THREE.ExtrudeGeometry(shapeD, optionsD) + const meshD = new THREE.Mesh(geometryD, material) + + meshD.updateMatrix() + + // 布尔运算 + const result = CSG.subtract(mesh, meshD) + + return result.geometry + } + + public static ptrPedestalGeometry: THREE.BufferGeometry = null + public static ptrPillarGeometry: THREE.BufferGeometry = null + public static ptrForkGeometry: THREE.BufferGeometry = null + +} diff --git a/src/modules/cl2/Cl23dObject.ts b/src/modules/cl2/Cl23dObject.ts index 51b513d..50d7fb6 100644 --- a/src/modules/cl2/Cl23dObject.ts +++ b/src/modules/cl2/Cl23dObject.ts @@ -1,37 +1,12 @@ import * as THREE from 'three' import { CSG } from 'three-csg-ts' -//@ts-ignore -import { mergeGeometries } from 'three/addons/utils/BufferGeometryUtils.js' import gsap from 'gsap' import mqtt from 'mqtt' 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"; -export interface Cl2Task { - // 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业） 135：旋转货架 136：旋转车身 - id: number; - content: { - SeqNo: number; - OperationType: 0 | 1 | 2 | 3 | 4 | 5 | 135 | 136; - UseBriefLocation: boolean; - StartX: number; - StartY: number; - EndX: number; - EndY: number; - EndDirection: 0 | 1 | 2 | 3 | 15; - // 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作 - PickMode: 0 | 1 | 2 | 3 | 4 | 5 | 6; - GoodsSlotHeight: number; - GoodsSlotDirection: 0 | 1 | 2 | 3 | 15; - GoodsId: string; - Link: { - // id: string; // 实际报文没有 - X: number; - Y: number; - Speed: number; - }[] - } -} interface Task { SeqNo: number; @@ -46,293 +21,6 @@ interface Task { export default class Cl23dObject extends THREE.Object3D { - // 创建ptr的底座 - private static createPtrPedestal(): THREE.BufferGeometry { - // 参数配置 - const radius = 0.8 // 圆半径 - const lineDist = 0.75 // 切割线距离圆心距离 - const segments = 64 // 圆的分段精度 - - // 计算切割线与圆的交点 - const intersectY = Math.sqrt(radius * radius - lineDist * lineDist) - const startAngle = Math.asin(intersectY / radius) - const endAngle = Math.acos(intersectY / radius) - - - const shape = new THREE.Shape() - shape.moveTo(0, 0) // 起点在圆心 - shape.absarc(0, 0, 0.8, startAngle, endAngle, false) // 从0到π绘制半圆 - shape.absarc(0, 0, 0.8, startAngle + Math.PI / 2, endAngle + Math.PI / 2, false) - shape.absarc(0, 0, 0.8, startAngle + Math.PI, endAngle + Math.PI, false) - shape.absarc(0, 0, 0.8, startAngle + Math.PI / 2 * 3, endAngle + Math.PI / 2 * 3, false) - - shape.lineTo(-0.5, -intersectY) - shape.lineTo(-0.5, -intersectY + 0.17) - shape.lineTo(0.75, -intersectY + 0.17) - shape.lineTo(0.75, intersectY - 0.17) - shape.lineTo(-0.5, intersectY - 0.17) - shape.lineTo(-0.5, intersectY) - shape.closePath() // 闭合路径 - - // 拉伸轨迹线 - const curve = new THREE.CatmullRomCurve3( - [new THREE.Vector3(0, 0.02, 0), new THREE.Vector3(0, 0.22, 0)], - false, // 闭合曲线 - 'catmullrom', - 0 - ) - - // 挤出几何图形 参数 - const options = { - steps: 1, - bevelEnabled: false, - extrudePath: curve // 设置挤出轨迹 - } - // 创建挤出几何体 - const geometry = new THREE.ExtrudeGeometry(shape, options) - const material = new THREE.MeshBasicMaterial({ color: 0xffdddbca }) - const mesh = new THREE.Mesh(geometry, material) - - mesh.updateMatrix() - - // 倒角 - const shapeD = new THREE.Shape() - shapeD.moveTo(-0.02, -0.02) - shapeD.lineTo(0.02, 0.02) - shapeD.lineTo(-0.02, 0.02) - shapeD.closePath() - - const curveD = new THREE.EllipseCurve( - 0, 0, radius, radius, 0, Math.PI * 2, false, 0 - ) - // 生成拉伸路径点 - const pointsD = curveD.getPoints(segments).map(p => - new THREE.Vector3(p.x, 0.20, p.y) - ) - // 3. 挤出成型 - const optionsD = { - steps: segments, - bevelEnabled: false, - // bevelSegments: 0.01, - extrudePath: new THREE.CatmullRomCurve3(pointsD) - } - - const geometryD = new THREE.ExtrudeGeometry(shapeD, optionsD) - const meshD = new THREE.Mesh(geometryD, material) - meshD.updateMatrix() - - // 布尔运算 - const result = CSG.subtract(mesh, meshD) - - return result.geometry - } - - // 创建ptr的立柱 - private static createPtrPillar(): THREE.BufferGeometry { - // 606.5 - const width = 0.245 - const depth = 0.716 - const dd = 0.05 - const shape = new THREE.Shape() - shape.moveTo(-0.728, -0.358 + dd) - shape.lineTo(-0.728 + dd, -0.358) - shape.lineTo(-0.728 + width - dd, -0.358) - shape.lineTo(-0.728 + width, -0.358 + dd) - - shape.lineTo(-0.728 + width, -0.28) - shape.lineTo(-0.728 + width - 0.08, -0.28) - shape.lineTo(-0.728 + width - 0.08, 0.28) - shape.lineTo(-0.728 + width, 0.28) - - shape.lineTo(-0.728 + width, -0.358 + depth - dd) - shape.lineTo(-0.728 + width - dd, -0.358 + depth) - shape.lineTo(-0.728 + dd, -0.358 + depth) - shape.lineTo(-0.728, -0.358 + depth - dd) - // shape.lineTo(-0.728, -0.358 + dd); - // shape.closePath() - - // 拉伸轨迹线 - const curve = new THREE.CatmullRomCurve3( - [new THREE.Vector3(0, 0.22, 0), new THREE.Vector3(0, 1.872, 0)], - false, // 闭合曲线 - 'catmullrom', - 0 - ) - - // 挤出几何图形 参数 - const options = { - steps: 1, - bevelSegments: 0.05, - bevelEnabled: true, - extrudePath: curve // 设置挤出轨迹 - } - // 创建挤出几何体 - let geometry = new THREE.ExtrudeGeometry(shape, options) - - - const fd = 0.03 - const shapeBf = new THREE.Shape() - - shapeBf.moveTo(-0.728 + width - 0.08, -0.28) - shapeBf.lineTo(-0.728 + width + 0.02, -0.28) - shapeBf.lineTo(-0.728 + width + 0.02, -0.25) - shapeBf.lineTo(-0.728 + width + 0.02 + 1.130 - fd, -0.25) - shapeBf.lineTo(-0.728 + width + 0.02 + 1.130, -0.25 + fd) - shapeBf.lineTo(-0.728 + width + 0.02 + 1.130, -0.14 - fd) - shapeBf.lineTo(-0.728 + width + 0.02 + 1.130 - fd, -0.14) - shapeBf.lineTo(-0.728 + width + 0.02, -0.14) - shapeBf.lineTo(-0.728 + width + 0.02, 0.14) - shapeBf.lineTo(-0.728 + width + 0.02 + 1.130 - fd, 0.14) - shapeBf.lineTo(-0.728 + width + 0.02 + 1.130, 0.14 + fd) - shapeBf.lineTo(-0.728 + width + 0.02 + 1.130, 0.25 - fd) - shapeBf.lineTo(-0.728 + width + 0.02 + 1.130 - fd, 0.25) - shapeBf.lineTo(-0.728 + width + 0.02, 0.25) - shapeBf.lineTo(-0.728 + width + 0.02, 0.28) - shapeBf.lineTo(-0.728 + width - 0.08, 0.28) - - shapeBf.closePath() - - // 拉伸轨迹线 - const curveBf = new THREE.CatmullRomCurve3( - [new THREE.Vector3(0, 0.02, 0), new THREE.Vector3(0, 0.06, 0)], - false, // 闭合曲线 - 'catmullrom', - 0 - ) - - // 挤出几何图形 参数 - const optionsBf = { - steps: 1, - bevelSegments: 0.05, - bevelEnabled: true, - extrudePath: curveBf // 设置挤出轨迹 - } - // 创建挤出几何体 - const geometryBf = new THREE.ExtrudeGeometry(shapeBf, optionsBf) - geometry = mergeGeometries([geometry, geometryBf]) - const material = new THREE.MeshBasicMaterial({ color: 0xffdddbca }) - const mesh = new THREE.Mesh(geometry, material) - - const shapeD = new THREE.Shape() - shapeD.moveTo(-0.3, -0.25) - shapeD.lineTo(-0.3, 0.25) - shapeD.lineTo(0.3, 0.25) - shapeD.lineTo(0.3, -0.25) - shapeD.closePath() - - const curveD = new THREE.CatmullRomCurve3( - [new THREE.Vector3(0, 1.45, -1), new THREE.Vector3(0, 1.45, 1)], - false, // 闭合曲线 - 'catmullrom', - 0 - ) - - const optionsD = { - steps: 1, - bevelSegments: 0.05, - bevelEnabled: true, - extrudePath: curveD // 设置挤出轨迹 - } - - const geometryD = new THREE.ExtrudeGeometry(shapeD, optionsD) - const meshD = new THREE.Mesh(geometryD, material) - mesh.updateMatrix() - meshD.updateMatrix() - const result = CSG.subtract(mesh, meshD) - - return result.geometry - - } - - // 创建ptr的立柱 - private static createPtrFork(): THREE.BufferGeometry { - // 606.5 - const width = 0.245 - const depth = 0.716 - const dd = 0.05 - const fd = 0.03 - const shape = new THREE.Shape() - - shape.moveTo(-0.728 + width - 0.08, -0.28) - shape.lineTo(-0.728 + width + 0.02, -0.28) - shape.lineTo(-0.728 + width + 0.02, -0.27) - shape.lineTo(-0.728 + width + 0.02 + 1.130 - fd, -0.27) - shape.lineTo(-0.728 + width + 0.02 + 1.130, -0.27 + fd) - shape.lineTo(-0.728 + width + 0.02 + 1.130, -0.12 - fd) - shape.lineTo(-0.728 + width + 0.02 + 1.130 - fd, -0.12) - shape.lineTo(-0.728 + width + 0.02, -0.12) - shape.lineTo(-0.728 + width + 0.02, 0.12) - shape.lineTo(-0.728 + width + 0.02 + 1.130 - fd, 0.12) - shape.lineTo(-0.728 + width + 0.02 + 1.130, 0.12 + fd) - shape.lineTo(-0.728 + width + 0.02 + 1.130, 0.27 - fd) - shape.lineTo(-0.728 + width + 0.02 + 1.130 - fd, 0.27) - shape.lineTo(-0.728 + width + 0.02, 0.27) - shape.lineTo(-0.728 + width + 0.02, 0.28) - shape.lineTo(-0.728 + width - 0.08, 0.28) - - shape.closePath() - - // 拉伸轨迹线 - const curve = new THREE.CatmullRomCurve3( - [new THREE.Vector3(0, 0.02, 0), new THREE.Vector3(0, 0.482, 0)], - false, // 闭合曲线 - 'catmullrom', - 0 - ) - - // 挤出几何图形 参数 - const options = { - steps: 1, - bevelSegments: 0.05, - bevelEnabled: true, - extrudePath: curve // 设置挤出轨迹 - } - // 创建挤出几何体 - const geometry = new THREE.ExtrudeGeometry(shape, options) - const material = new THREE.MeshBasicMaterial({ color: 0xffdddbca }) - const mesh = new THREE.Mesh(geometry, material) - mesh.updateMatrix() - - const shapeD = new THREE.Shape() - - - shapeD.moveTo(-0.728 + width + 0.02, -0.3) - shapeD.lineTo(-0.728 + width + 0.02 + 1.2, -0.3) - shapeD.lineTo(-0.728 + width + 0.02 + 1.2, 0.3) - shapeD.lineTo(-0.728 + width + 0.02, 0.3) - shape.closePath() - - // 拉伸轨迹线 - const curveD = new THREE.CatmullRomCurve3( - [new THREE.Vector3(0, 0.07, 0), new THREE.Vector3(0, 0.482, 0)], - false, // 闭合曲线 - 'catmullrom', - 0 - ) - - // 挤出几何图形 参数 - const optionsD = { - steps: 1, - bevelSegments: 0.05, - bevelEnabled: true, - extrudePath: curveD // 设置挤出轨迹 - } - // 创建挤出几何体 - const geometryD = new THREE.ExtrudeGeometry(shapeD, optionsD) - const meshD = new THREE.Mesh(geometryD, material) - - meshD.updateMatrix() - - // 布尔运算 - const result = CSG.subtract(mesh, meshD) - - return result.geometry - } - - private static ptrPedestalGeometry: THREE.BufferGeometry = null - private static ptrPillarGeometry: THREE.BufferGeometry = null - private static ptrForkGeometry: THREE.BufferGeometry = null - private item: ItemJson private _cl2Entity: Cl2Entity = null @@ -345,8 +33,11 @@ export default class Cl23dObject extends THREE.Object3D { private riseAnimation: core.Tween = null private stretchAnimation: core.Tween = null private currentAnimation: core.Tween = null - private currentDirection: number = 15 + private currentDirection: LogicDirection = 15 + + private bootTime: number = 0 + private seqNoName: string = "" public get cl2Entity(): Cl2Entity { if (!this._cl2Entity) { @@ -362,26 +53,26 @@ export default class Cl23dObject extends THREE.Object3D { super() console.log('time', this.clock.getElapsedTime()) this.item = item - if (!Cl23dObject.ptrPedestalGeometry) { - Cl23dObject.ptrPedestalGeometry = Cl23dObject.createPtrPedestal() + if (!Cl23DGraphics.ptrPedestalGeometry) { + Cl23DGraphics.ptrPedestalGeometry = Cl23DGraphics.createPtrPedestal() } - const ptrPedestalGeometry = Cl23dObject.ptrPedestalGeometry + const ptrPedestalGeometry = Cl23DGraphics.ptrPedestalGeometry const ptrPedestalMaterial = new THREE.MeshPhongMaterial({ color: 0xffdddbca }) const ptrPedestalMesh = new THREE.Mesh(ptrPedestalGeometry, ptrPedestalMaterial) ptrPedestalMesh.name = 'ptrPedestal' - if (!Cl23dObject.ptrPillarGeometry) { - Cl23dObject.ptrPillarGeometry = Cl23dObject.createPtrPillar() + if (!Cl23DGraphics.ptrPillarGeometry) { + Cl23DGraphics.ptrPillarGeometry = Cl23DGraphics.createPtrPillar() } - const ptrPillarGeometry = Cl23dObject.ptrPillarGeometry + const ptrPillarGeometry = Cl23DGraphics.ptrPillarGeometry const ptrPillarMaterial = new THREE.MeshPhongMaterial({ color: 0xff6c6956 }) const ptrPillarMesh = new THREE.Mesh(ptrPillarGeometry, ptrPillarMaterial) - if (!Cl23dObject.ptrForkGeometry) { - Cl23dObject.ptrForkGeometry = Cl23dObject.createPtrFork() + if (!Cl23DGraphics.ptrForkGeometry) { + Cl23DGraphics.ptrForkGeometry = Cl23DGraphics.createPtrFork() } - const ptrForkGeometry = Cl23dObject.ptrForkGeometry + const ptrForkGeometry = Cl23DGraphics.ptrForkGeometry const ptrForkMaterial = new THREE.MeshPhongMaterial({ color: 0xff444444 }) const ptrForkMesh = new THREE.Mesh(ptrForkGeometry, ptrForkMaterial) ptrForkMesh.name = 'ptrFork' @@ -393,48 +84,82 @@ export default class Cl23dObject extends THREE.Object3D { groupPillar.add(ptrForkMesh) this.add(groupPillar) - try { - // 安全连接配置 - // const client = mqtt.connect('mqtt://127.0.0.1:9001', { - // path: '/mqtt', - // clientId: 'virtual-' + item.id, - // clean: true, - // connectTimeout: 10000, - // username: 'user', - // password: 'user', - // unixSocket: true, - // keepalive: 60 - // }) - const client = { - on() { - } - } - // debugger - - /* - // 事件绑定 - client.on('connect', () => { - console.log('Connected') - client.subscribe(['/wcs_server/' + item.id], { qos: 0 }) - client.publish('/agv_robot/status', JSON.stringify(m20020), { retain: true }) - }) + this.seqNoName = 'CL2' + this.cl2Entity.id - client.on('message', (topic, msg) => { - console.log(`[${topic}] ${msg}`) - const a: Cl2Task = JSON.parse(msg.toString()) - this.handleMessage(a) + } - }) + private AGVModel = "CYBER-LIFT-A_V1.0" + private AGVFnModel = "FITBOTS-CYBER-LIFT-1000_V1.0" - client.on('error', (error) => { - console.error('Error:', error) - }) - */ - } catch (e) { - console.error(e) + + // 开机 + boot() { + this.bootTime = Date.now(); + this.computeLogicDirection(); + this.subscribeMessage('/wcs_server/' + this.cl2Entity.id) + + setTimeout(()=>{ + this.send20149() + // 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标 + setTimeout(()=>{ + this.sendCurrentPositionAndDirection() + }, 1000) + }, 2000) + } + // 关机 + shutdown() { + + } + + // 主程序启动上报 + send20149() { + const content = new AmrMsg20149() + 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) + } + // 上报当前位姿，地标和方向 + sendCurrentPositionAndDirection() { + const pointItem = Model.getItemByXYZ(this.position.x, this.position.y, this.position.z) + if (!pointItem || !pointItem.logicX || !pointItem.logicY) { + // 当前车辆所在位置未找到 + const content = new AmrMsg20250() + 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 + content.VehicleId = parseInt(this.cl2Entity.id) + const m20250 = new AmrMsg(content) + this.sendMessage(m20250) + } else { + // 发送正常地标信息 + const content = new AmrMsg20020() + content.CurDirection = this.currentDirection + content.CurLogicX = pointItem.logicX + content.CurLogicY = pointItem.logicY + content.CurX = pointItem.logicX + content.CurY = pointItem.logicY + 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 }) + } + sendMessage(msg: AmrMsg) { + this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(msg)) + } /*==========消息处理============*/ @@ -463,7 +188,7 @@ export default class Cl23dObject extends THREE.Object3D { client.publish('/agv_robot/status', JSON.stringify(m20020), { retain: true }) } - handleMessage(data: Cl2Task) { + handleMessage(data: AmrMsg) { return if (data.id === 10010) { @@ -535,6 +260,32 @@ export default class Cl23dObject extends THREE.Object3D { } } + // 计算逻辑方向 + computeLogicDirection() { + 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; + } + } + executeTask() { if (this.currentAnimation) { @@ -720,3 +471,5 @@ export default class Cl23dObject extends THREE.Object3D { // }, 2000) } + + diff --git a/src/modules/cl2/Cl2PropertySetter.ts b/src/modules/cl2/Cl2PropertySetter.ts index 4a5cfc9..2cc8a18 100644 --- a/src/modules/cl2/Cl2PropertySetter.ts +++ b/src/modules/cl2/Cl2PropertySetter.ts @@ -13,6 +13,10 @@ const propertySetter: PropertySetter = { dataPath: 'dt.palletDepth', label: '托盘深度', input: 'InputNumber', inputProps: {}, }, + { + dataPath: 'state', label: 'ptr控制', input: 'PtrController', + inputProps: {}, + }, ], }, };