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

6 months ago · 9bbea8b9b4
2 changed files with 526 additions and 36 deletions
--- a/doc/command-example/f1.json
+++ b/doc/command-example/f1.json
@ -0,0 +1,422 @@
 [
  {
    "id": "rack1",
    "t": "rack",
    "v": true,
    "tf": [
      [
        1.55,
        0,
        -1.5
      ],
      [
        0,
        0,
        0
      ],
      [
        2.2,
        2.8,
        1
      ]
    ],
    "dt": {
      "rackDepth": 1,
      "bottomBarHeight": 0.2,
      "bottomLinkHeight": 0.2,
      "topLinkDistance": 0.2,
      "levelCount": 2,
      "bayCount": 2,
      "hideFloor": 0,
      "extendColumns": 1,
      "columnSpacing": 1,
      "bays": [
        {
          "bayWidth": 1.1,
          "levelHeight": [
            1.4,
            1.4
          ]
        },
        {
          "bayWidth": 1.1,
          "levelHeight": [
            1.4,
            1.4
          ]
        }
      ],
      "center": [],
      "in": [],
      "out": [],
      "rackWidth": 2.2,
      "rackHeight": 2.8
    },
    "_rid": "_2"
  },
  {
    "id": "rack2",
    "t": "rack",
    "v": true,
    "tf": [
      [
        3.9,
        0,
        -1.5
      ],
      [
        0,
        0,
        0
      ],
      [
        2.2,
        2.8,
        1
      ]
    ],
    "dt": {
      "rackDepth": 1,
      "bottomBarHeight": 0.2,
      "bottomLinkHeight": 0.2,
      "topLinkDistance": 0.2,
      "levelCount": 2,
      "bayCount": 2,
      "hideFloor": 0,
      "extendColumns": 1,
      "columnSpacing": 1,
      "bays": [
        {
          "bayWidth": 1.1,
          "levelHeight": [
            1.4,
            1.4
          ]
        },
        {
          "bayWidth": 1.1,
          "levelHeight": [
            1.4,
            1.4
          ]
        }
      ],
      "center": [],
      "in": [],
      "out": [],
      "rackWidth": 2.2,
      "rackHeight": 2.8
    },
    "_rid": "_3"
  },
  {
    "id": "1_2",
    "t": "way",
    "v": true,
    "logicX": 1,
    "logicY": 2,
    "tf": [
      [
        1,
        0.01,
        0
      ],
      [
        0,
        0,
        0
      ],
      [
        0.25,
        0.1,
        0.25
      ]
    ],
    "dt": {
      "in": [
        "2_2"
      ],
      "out": [
        "2_2"
      ],
      "center": [],
      "linkStore": [
        {
          "item": "rack1",
          "bay": 0,
          "level": 0,
          "cell": 0,
          "direction": "up"
        },
        {
          "item": "rack1",
          "bay": 0,
          "level": 1,
          "cell": 0,
          "direction": "up"
        }
      ]
    },
    "_rid": "_4"
  },
  {
    "id": "2_2",
    "t": "way",
    "v": true,
    "logicX": 2,
    "logicY": 2,
    "tf": [
      [
        2.1,
        0.01,
        0
      ],
      [
        0,
        0,
        0
      ],
      [
        0.25,
        0.1,
        0.25
      ]
    ],
    "dt": {
      "in": [
        "1_2",
        "3_2"
      ],
      "out": [
        "1_2",
        "3_2"
      ],
      "center": [],
      "linkStore": [
        {
          "item": "rack1",
          "bay": 1,
          "level": 0,
          "cell": 0,
          "direction": "up"
        },
        {
          "item": "rack1",
          "bay": 1,
          "level": 1,
          "cell": 0,
          "direction": "up"
        }
      ]
    },
    "_rid": "_5"
  },
  {
    "id": "3_2",
    "t": "way",
    "v": true,
    "logicX": 3,
    "logicY": 2,
    "tf": [
      [
        3.39,
        0.01,
        0
      ],
      [
        0,
        0,
        0
      ],
      [
        0.25,
        0.1,
        0.25
      ]
    ],
    "dt": {
      "in": [
        "2_2",
        "4_2"
      ],
      "out": [
        "2_2",
        "4_2"
      ],
      "center": [],
      "linkStore": [
        {
          "item": "rack2",
          "bay": 0,
          "level": 0,
          "cell": 0,
          "direction": "up"
        },
        {
          "item": "rack2",
          "bay": 0,
          "level": 1,
          "cell": 0,
          "direction": "up"
        }
      ]
    },
    "_rid": "_6"
  },
  {
    "id": "4_2",
    "t": "way",
    "v": true,
    "logicX": 4,
    "logicY": 2,
    "tf": [
      [
        4.44,
        0.01,
        0
      ],
      [
        0,
        0,
        0
      ],
      [
        0.25,
        0.1,
        0.25
      ]
    ],
    "dt": {
      "in": [
        "3_2",
        "5_2"
      ],
      "out": [
        "3_2",
        "5_2"
      ],
      "center": [],
      "linkStore": [
        {
          "item": "rack2",
          "bay": 1,
          "level": 0,
          "cell": 0,
          "direction": "up"
        },
        {
          "item": "rack2",
          "bay": 1,
          "level": 1,
          "cell": 0,
          "direction": "up"
        }
      ]
    },
    "_rid": "_7"
  },
  {
    "id": "5_2",
    "t": "way",
    "v": true,
    "logicX": 5,
    "logicY": 2,
    "tf": [
      [
        5.44,
        0.01,
        0
      ],
      [
        0,
        0,
        0
      ],
      [
        0.25,
        0.1,
        0.25
      ]
    ],
    "dt": {
      "in": [
        "4_2",
        "6_2"
      ],
      "out": [
        "4_2",
        "6_2"
      ],
      "center": []
    },
    "_rid": "_8"
  },
  {
    "id": "6_2",
    "t": "way",
    "v": true,
    "logicX": 6,
    "logicY": 2,
    "tf": [
      [
        6.44,
        0.01,
        0
      ],
      [
        0,
        0,
        0
      ],
      [
        0.25,
        0.1,
        0.25
      ]
    ],
    "dt": {
      "in": [
        "5_2"
      ],
      "out": [
        "5_2"
      ],
      "center": [],
      "agvRotation": [
        "cl2",
        "clx"
      ]
    },
    "_rid": "_9"
  },
  {
    "id": "3",
    "t": "cl2",
    "v": true,
    "tf": [
      [
        6.440,
        0,
        0
      ],
      [
        0,
        0,
        0
      ],
      [
        1.5,
        1.98,
        1.5
      ]
    ],
    "dt": {
      "in": [],
      "out": [],
      "center": [],
      "ptrWidth": 1.5,
      "ptrDepth": 1.5,
      "ptrHeight": 1.98
    }
  }
 ]
--- a/src/modules/cl2/Cl23dObject.ts
+++ b/src/modules/cl2/Cl23dObject.ts
@ -25,7 +25,7 @@ export interface Cl2Task {
    GoodsSlotDirection: 0 | 1 | 2 | 3 | 15;
    GoodsId: string;
    Link: {
-      id: string; // 实际报文没有
+      // id: string; // 实际报文没有
      X: number;
      Y: number;
      Speed: number;
@ -54,17 +54,17 @@ export default class Cl23dObject extends THREE.Object3D {
    const segments = 64;       // 圆的分段精度
    // 计算切割线与圆的交点
-    const intersectY = Math.sqrt(radius*radius - lineDist*lineDist);
+    const intersectY = Math.sqrt(radius * radius - lineDist * lineDist);
-    const startAngle = Math.asin(intersectY/radius);
+    const startAngle = Math.asin(intersectY / radius);
-    const endAngle = Math.acos(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 / 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.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)
@ -170,7 +170,6 @@ export default class Cl23dObject extends THREE.Object3D {
    let geometry = new THREE.ExtrudeGeometry(shape, options)
    const fd = 0.03
    const shapeBf = new THREE.Shape();
@ -345,7 +344,8 @@ export default class Cl23dObject extends THREE.Object3D {
  private rotationAnimation: core.Tween = null;
  private riseAnimation: core.Tween = null;
  private stretchAnimation: core.Tween = null;
-
+  private currentAnimation: core.Tween = null;
  private currentDirection: number = 15;
  public get cl2Entity(): Cl2Entity {
@ -357,6 +357,7 @@ export default class Cl23dObject extends THREE.Object3D {
  }
  private clock = new THREE.Clock();
  constructor(item: ItemJson, option?: RendererCudOption) {
    super();
    console.log("time", this.clock.getElapsedTime());
@ -428,8 +429,8 @@ export default class Cl23dObject extends THREE.Object3D {
 // 事件绑定
      client.on('connect', () => {
        console.log('Connected');
-        client.subscribe(['/wcs_server/' + item.id], { qos: 0 });
+        client.subscribe(['/wcs_server/' + item.id], {qos: 0});
-        client.publish('/agv_robot/status', JSON.stringify(m20020), { retain: true });
+        client.publish('/agv_robot/status', JSON.stringify(m20020), {retain: true});
      });
      client.on('message', (topic, msg) => {
@ -461,19 +462,54 @@ export default class Cl23dObject extends THREE.Object3D {
        }
      }
      const startTask = {
        X: data.content.StartX,
        Y: data.content.StartY,
        Speed: 0
      }
      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
          }
        }
        // 添加到队列
        this.taskList.push({
          SeqNo: data.content.SeqNo,
-          OperationType: data.content.OperationType,
+          OperationType: 0,
-          PickMode: data.content.PickMode,
+          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,
        })
      }
      console.log("time", this.clock.getElapsedTime());
      this.executeTask()
@ -488,11 +524,16 @@ export default class Cl23dObject extends THREE.Object3D {
  executeTask() {
    const currentTask = this.executingTask[this.executingTask.length - 1]
    while (this.taskList.length > 0) {
      const task = this.taskList.shift()
-      this.addTravel(task.X, task.Y, task.Speed/1000)
+      if (task.OperationType === 0) {
-
+        this.addTravel(task.X, task.Y, task.Speed / 1000)
-      this.executingTask.push(task)
+      }
      if ((task.Speed > 0) != (currentTask.Speed > 1) || task.Direction != currentTask.Direction) {
        // 转向
        // this.addArmRotate(task.Direction)
      }
    }
@ -535,7 +576,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) {
@ -543,7 +584,7 @@ export default class Cl23dObject extends THREE.Object3D {
                const child = children[i]
                child.position.y = bh - a.y
              }
-            } else if (a.y <  0 ) {
+            } else if (a.y < 0) {
              for (let i = 0; i < children.length; i++) {
                const child = children[i]
                child.position.y = 0 - a.y
@ -561,28 +602,53 @@ export default class Cl23dObject extends THREE.Object3D {
  // 转
-  addRotation(rad: number): Promise<void> {
+  addRotation(direction: number): Promise<void> {
    let rad = 0;
    switch (direction) {
      case 1:
        rad = Math.PI / 2 * 3;
        break;
      case 2:
        rad = Math.PI;
        break;
      case 3:
        rad = Math.PI / 2;
        break;
      default:
        rad = 0;
    }
    const quat1 = new THREE.Quaternion().setFromEuler(this.rotation);
    const euler: Euler = new Euler(this.rotation.x, rad, this.rotation.z);
    const quat2 = new THREE.Quaternion().setFromEuler(euler);
    const angleDiff = quat1.angleTo(quat2);
    console.log(rad, this.rotation.y, angleDiff)
    const tr = this.rotation.y + angleDiff
-    let time = Math.abs(angleDiff) / (Math.PI/3)
+    let time = Math.abs(angleDiff) / (Math.PI / 7)
-    const duration = angleDiff != 0 ? Math.max(0.5, time) : 0
+    const duration = time
    if (!this.rotationAnimation) {
      return new Promise(resolve => {
-      gsap.to(this.rotation, {
+        this.rotationAnimation = gsap.to(this.rotation, {
          y: tr,
          duration,
          ease: 'none',
-        onComplete: resolve
+          onComplete: () => {
            this.rotationAnimation = null
            this.currentAnimation = null
            resolve()
          }
        })
        this.currentAnimation = this.rotationAnimation
      })
-
+    } else {
      this.rotationAnimation.vars.y = tr
      const tt = this.rotationAnimation.duration()
      this.rotationAnimation.duration(tt + duration)
    }
  }
  // 走
-  addTravel(logicX: number, logicY: number, speed : number = 1): Promise<void> {
+  addTravel(logicX: number, logicY: number, speed: number = 1): Promise<void> {
    const pos = Model.getPositionByLogicXY(logicX, logicY)
@ -599,11 +665,13 @@ export default class Cl23dObject extends THREE.Object3D {
          z: toPos.z,
          duration,
          ease: 'sine.inOut',
-          onComplete: ()=>{
+          onComplete: () => {
            this.travelAnimation = null;
            this.currentAnimation = null
            resolve()
          }
        })
        this.currentAnimation = this.travelAnimation
      })
    } else {
      this.travelAnimation.vars.x = toPos.x