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

src/core/Constract.ts

@@ -29,7 +29,7 @@ const Constract = Object.freeze({
   HEIGHT_GSTORE: 0.03,
   HEIGHT_MEASURE: 0.02,
   HEIGHT_RACK: 0,
-  HEIGHT_WAY: 0.01,
+  HEIGHT_WAY: 0,
   HEIGHT_WAY_LABEL: 0.03,
   HEIGHT_WAY_LINE: 0.02,
 

src/modules/amr/ptr/PtrObject.ts

@@ -57,6 +57,9 @@ export default class PtrObject extends THREE.Object3D {
 
   override AGVModel = ''
   override AGVFnModel = ''
+  override rotationSpeed: number = 0
+  override showForkSpeed: number = 0
+  override upForkSpeed: number = 0
 
   private boxBody: any = null
   private __toPos: THREE.Vector3 = null
@@ -169,6 +172,7 @@ export default class PtrObject extends THREE.Object3D {
       this.subscribeMessage('/wcs_server/' + this.item.id)
       this.subscribeMessage('/agv_robot/status')
     }
+    window.agv3 = this
   }
 
   protected onFrameTimer() {
@@ -199,7 +203,7 @@ export default class PtrObject extends THREE.Object3D {
 
     const boxShape = new this.viewport.ammoModel.btBoxShape(new Ammo.btVector3(0.5, 0.5, 0.5))
 
-    const mass = 10
+    const mass = 1
     const localInertia = new this.viewport.ammoModel.btVector3(0, 0, 0)
     boxShape.calculateLocalInertia(mass, localInertia)
 
@@ -486,7 +490,7 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   makeStepTask(data: AmrMsg10010) {
-
+    console.log('makeStepTask:', data)
     let currentStepTask: StepTask = this.runningStepTask
     if (currentStepTask == null) {
       if (this.currentStepTaskList && this.currentStepTaskList.length > 0) {
@@ -605,7 +609,7 @@ export default class PtrObject extends THREE.Object3D {
 
       if (endDirection != currentStepTask.EndDirection) {
         // 如果此处不能转弯，忽略结束方向 等待后续任务
-        if (!item.dt?.agvRotation || !item.dt?.agvRotation?.length <= 0) {
+        if (!item.dt?.agvRotation || item.dt?.agvRotation?.length <= 0) {
           return
         }
         const stepTask: StepTask = {
@@ -684,7 +688,7 @@ export default class PtrObject extends THREE.Object3D {
           this.currentStepTaskList.shift()
           this.runningStepTaskList.push(stepTask)
 
-          this.addTravel(stepTask.X, stepTask.Y, stepTask.Speed / 1000)
+          this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection, stepTask.Speed / 1000)
 
         } else {
           break
@@ -694,7 +698,7 @@ export default class PtrObject extends THREE.Object3D {
         this.currentStepTaskList.shift()
         this.runningStepTaskList.push(stepTask)
         if (stepTask.StepTaskType == 'MOVE' || stepTask.StepTaskType == 'MOVE_BACKWARD') {
-          this.addTravel(stepTask.X, stepTask.Y, stepTask.Speed / 1000)
+          this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection,stepTask.Speed / 1000)
         } else if (stepTask.StepTaskType == 'ROTATION') {
           this.addRotation(stepTask.EndDirection)
         } else if (stepTask.StepTaskType == 'LOAD') {
@@ -707,31 +711,23 @@ export default class PtrObject extends THREE.Object3D {
   }
 
   onActionCompleted() {
-    this.runningStepTaskList = []
+    setTimeout(()=> {
-    this.runningStepTask = null
+      this.runningStepTaskList = []
-    this.computeLogicXYAndDirection()
+      this.runningStepTask = null
-    this.computeRealSpeed()
+      this.computeLogicXYAndDirection()
-    // 当前所有动作执行完毕
+      // 当前所有动作执行完毕
-    if (this.currentStepTaskList.length <= 0) {
+      if (this.currentStepTaskList.length <= 0) {
-      this.send20010()
+        this.send20010()
-    }
+      }
-    this.PickMode = 0
+      this.PickMode = 0
-    this.OperationType = 0
+      this.OperationType = 0
-    this.TaskMode = 0
+      this.TaskMode = 0
-    this.executeTask()
+      this.executeTask()
+    }, 2000)
   }
 
   /*==========动作处理============*/
 
-  private realSpeed = 0
-  private __lastPosition = this.position.clone()
-
-  computeRealSpeed() {
-    this.realSpeed = this.__lastPosition.distanceTo(this.position) / this.clock.getDelta()
-    this.__lastPosition.copy(this.position)
-    console.log('realSpeed', this.realSpeed)
-  }
-
   // 转
   addRotation(direction: number): Promise<void> {
     let rad = 0
@@ -754,7 +750,7 @@ export default class PtrObject extends THREE.Object3D {
     const angleDiff = quat1.angleTo(quat2)
     console.log(rad, this.rotation.y, angleDiff)
     const tr = this.rotation.y + angleDiff
-    let time = Math.abs(angleDiff) / (Math.PI / 7)
+    let time = Math.abs(angleDiff) / this.rotationSpeed
     const duration = time
 
     return new Promise(resolve => {
@@ -771,8 +767,12 @@ export default class PtrObject extends THREE.Object3D {
     })
   }
 
+  keepSpeed: any
+  force: any
+  reverseForce: any
+
   // 走
-  addTravel(logicX: number, logicY: number, speed: number = 1): Promise<void> {
+  addTravel(logicX: number, logicY: number, direction: number, speed: number = 1): Promise<void> {
 
 
     this.OperationType = 0
@@ -789,19 +789,31 @@ export default class PtrObject extends THREE.Object3D {
     }
 
     // 运动参数
-    const accelForce = speed > 0 ? 4 : (-4)
+    const accelForce = speed > 0 ? a : (-a)
     let currentPhase = 'accelerate'
 
+    this.keepSpeed = new this.viewport.ammoModel.btVector3(0, 0, 0)
+    this.force = new this.viewport.ammoModel.btVector3(0, 0, 0)
+    this.reverseForce = new this.viewport.ammoModel.btVector3(0, 0, 0)
+    if (direction == 0 || direction == 2) {
+      this.force.setValue(accelForce, 0, 0)
+      this.reverseForce.setValue(-accelForce, 0, 0)
+      this.keepSpeed.setValue(speed, 0, 0)
+    } else if (direction == 1 || direction == 3) {
+      this.force.setValue(0, 0, accelForce)
+      this.reverseForce.setValue(0, 0, -accelForce)
+      this.keepSpeed.setValue(0, 0, speed)
+    }
+
     if (this.viewport.registerPhysicsUpdateCallBack.has(this.item.id)) {
       return null
     }
 
     this.travelAnimation = 'asd'
 
-    const force = new this.viewport.ammoModel.btVector3(accelForce, 0, 0)
     this.viewport.registerPhysicsUpdateCallBack.set(this.item.id, () => {
 
-      this.viewport.physicsWorld.stepSimulation(1 / 60, 20)
+      this.viewport.physicsWorld.stepSimulation(1 / 60, 10)
 
       // 同步Three.js对象位置
       const transform = new this.viewport.ammoModel.btTransform()
@@ -815,50 +827,55 @@ export default class PtrObject extends THREE.Object3D {
       const cSpeed = velocity.length()
       const stopTime = cSpeed / a
       const stopDistance = a * stopTime * stopTime / 2
-      if (distance <= 0.05) {
+      if (distance <= 0.01) {
         const stopVel = new this.viewport.ammoModel.btVector3(0, 0, 0)
         this.boxBody.setLinearVelocity(stopVel)
         this.viewport.ammoModel.destroy(stopVel)
         this.viewport.registerPhysicsUpdateCallBack.delete(this.item.id)
-        this.viewport.ammoModel.destroy(force)
+        this.viewport.ammoModel.destroy(this.force)
+        this.viewport.ammoModel.destroy(this.keepSpeed)
         this.position.set(this.__toPos.x, this.__toPos.y, this.__toPos.z)
         this.travelAnimation = null
         this.onActionCompleted()
-
+        return
+      }
+      if (cSpeed == 0) {
+        this.boxBody.activate()
       }
-      if (distance <= stopDistance) {
+      if (distance <= stopDistance + 0.2) {
-        currentPhase = 'decelerate'
+        if (currentPhase != 'decelerate') {
+          currentPhase = 'decelerate'
+          const sp = new this.viewport.ammoModel.btVector3(velocity.x(), velocity.y(), velocity.z())
+          this.boxBody.setLinearVelocity(sp)
+          this.viewport.ammoModel.destroy(sp)
+        }
+      } else if (currentPhase == 'decelerate') {
+        currentPhase = 'accelerate'
       }
+
       // 运动阶段控制
       switch (currentPhase) {
         case 'accelerate':
           // 施加X轴正向力
-
+          this.boxBody.applyCentralForce(this.force)
-          if (velocity.x() == 0) {
-            this.boxBody.activate()
-          }
-          this.boxBody.applyCentralForce(force)
 
           // 达到最大速度进入匀速阶段
-          if (Math.abs(velocity.x()) >= Math.abs(speed)) {
+          if (cSpeed >= Math.abs(speed) || distance <= stopDistance) {
             currentPhase = 'uniform'
           }
+          this.boxBody.setDamping(0, 0);
           break
         case 'uniform':
           currentPhase = 'uniform'
-          const newVel = new this.viewport.ammoModel.btVector3(speed, 0, 0)
+          this.boxBody.setLinearVelocity(this.keepSpeed)
-          this.boxBody.setLinearVelocity(newVel)
+          this.boxBody.setDamping(0, 0);
-          this.viewport.ammoModel.destroy(newVel)
           break
         case 'decelerate':
           // 检测停止
-          if (speed == 0) {
+          if (cSpeed > 0) {
-            const stopVel = new this.viewport.ammoModel.btVector3(0, 0, 0)
+            this.boxBody.setDamping(0.6, 0);
-            this.boxBody.setLinearVelocity(stopVel)
-            this.viewport.ammoModel.destroy(stopVel)
-            currentPhase = 'stop'
-            console.log('物体已完全停止')
           }
+          this.boxBody.activate()
           break
       }
 
@@ -882,56 +899,7 @@ export default class PtrObject extends THREE.Object3D {
       }
 
     })
-
-
     return null
-    //
-    // if (!this.travelAnimation) {
-    //   return new Promise(resolve => {
-    //     this.travelAnimation = gsap.to(this.position, {
-    //       x: toPos.x,
-    //       y: toPos.y,
-    //       z: toPos.z,
-    //       duration,
-    //       ease: 'power1.inOut',
-    //       onComplete: () => {
-    //         this.travelAnimation = null
-    //         resolve()
-    //         this.onActionCompleted()
-    //       },
-    //       onUpdate: () => {
-    //         this.computeRealSpeed()
-    //         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.EndDirection * 90
-    //               content.CurDirection = task.EndDirection
-    //               this.send20020(content)
-    //               break
-    //             }
-    //           }
-    //         }
-    //       }
-    //     })
-    //   })
-    // } else {
-    //   this.travelAnimation.vars.x = toPos.x
-    //   this.travelAnimation.vars.y = toPos.y
-    //   this.travelAnimation.vars.z = toPos.z
-    //   this.travelAnimation.vars.ease = 'power1.out'
-    //   const tt = this.travelAnimation.duration()
-    //   this.travelAnimation.duration(duration)
-    //   this.travelAnimation.invalidate().restart();
-    // }
-
   }
 
   // 取货

src/modules/amr/ptr/cl2/Cl23DGraphics.ts

@@ -233,7 +233,7 @@ export default class Cl23DGraphics {
 
     // 拉伸轨迹线
     const curve = new THREE.CatmullRomCurve3(
-      [new THREE.Vector3(0, 0.02, 0), new THREE.Vector3(0, 0.482, 0)],
+      [new THREE.Vector3(0, /*0.02*/-0.8, 0), new THREE.Vector3(0, 0.482, 0)],
       false,  // 闭合曲线
       'catmullrom',
       0
@@ -259,7 +259,7 @@ export default class Cl23DGraphics {
     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()
+    shapeD.closePath()
 
     // 拉伸轨迹线
     const curveD = new THREE.CatmullRomCurve3(
@@ -282,8 +282,38 @@ export default class Cl23DGraphics {
 
     meshD.updateMatrix()
 
+
+    const shapeDD = new THREE.Shape()
+
+
+    shapeDD.moveTo(0, 0)
+    shapeDD.lineTo(0, 2)
+    shapeDD.lineTo(2, 2)
+    shapeDD.lineTo(2, 0)
+    shapeDD.closePath()
+
+    // 拉伸轨迹线
+    const curveDD = new THREE.CatmullRomCurve3(
+      [new THREE.Vector3(-1, 0.03, -1), new THREE.Vector3(-1, 0.03, 0.45)],
+      false,  // 闭合曲线
+      'catmullrom',
+      0
+    )
+
+    // 挤出几何图形 参数
+    const optionsDD = {
+      steps: 1,
+      bevelSegments: 0.05,
+      bevelEnabled: true,
+      extrudePath: curveDD //  设置挤出轨迹
+    }
+    // 创建挤出几何体
+    const geometryDD = new THREE.ExtrudeGeometry(shapeDD, optionsDD)
+    const meshDD = new THREE.Mesh(geometryDD, material)
+    meshDD.updateMatrix()
     // 布尔运算
-    const result = CSG.subtract(mesh, meshD)
+    let result = CSG.subtract(mesh, meshD)
+    result = CSG.subtract(result, meshDD)
 
     return result.geometry
   }

src/modules/amr/ptr/cl2/Cl23dObject.ts

@@ -5,25 +5,6 @@ import mqtt from 'mqtt'
 import {Euler} from 'three/src/math/Euler'
 import Cl2Entity from '@/modules/amr/ptr/cl2/Cl2Entity'
 import Cl23DGraphics from "@/modules/amr/ptr/cl2/Cl23DGraphics"
-import {
-  AmrErrorCode,
-  AmrMsg,
-  AmrMsg10010,
-  AmrMsg10050,
-  AmrMsg10060,
-  AmrMsg10110, AmrMsg10120, AmrMsg20011,
-  AmrMsg20020,
-  AmrMsg20050,
-  AmrMsg20060,
-  AmrMsg20100, AmrMsg20147, AmrMsg20148,
-  AmrMsg20149,
-  AmrMsg20150,
-  AmrMsg20250,
-  CurBatteryData,
-  type CEventId, type COperationType, type CTaskMode, type CPickMode,
-  type LogicDirection, TaskCompletedData, TaskModeChangeData, TaskStatusChangeData, TaskTypeChangeData, AmrMsg20010
-} from "@/core/manager/amr/AmrMessageDefine";
-import {worldModel} from "@/core/manager/WorldModel";
 import PtrObject from "../PtrObject";
 import Viewport from "@/core/engine/Viewport";
 
@@ -31,6 +12,10 @@ export default class Cl23dObject extends PtrObject {
 
   private _cl2Entity: Cl2Entity = null
 
+  override rotationSpeed = (Math.PI / 7)
+  override showForkSpeed: number = 0.15
+  override upForkSpeed: number = 0.15
+
   public get cl2Entity(): Cl2Entity {
     if (!this._cl2Entity) {
       const cl2: Cl2Entity = Model.getCl2(this.item.id) as Cl2Entity
@@ -81,14 +66,18 @@ export default class Cl23dObject extends PtrObject {
   override animationShowFork(z: number): Promise<void> {
 
     const ptrPillar = this.getObjectByName('ptrPillar')
-    const time = 3
+    const time = Math.abs(z/ this.showForkSpeed)
     return new Promise(resolve => {
       this.actionAnimation = gsap.to(ptrPillar.position, {
         z: -z,
         duration: time,
         repeat: 0,
         ease: 'sine.inOut',
-        onComplete: resolve
+        onComplete: ()=>{
+          setTimeout(() => {
+            resolve()
+          }, 1000)
+        },
       })
     })
 
@@ -98,10 +87,13 @@ export default class Cl23dObject extends PtrObject {
     return this.animationShowFork(0)
   }
 
-  override animationUpFork(y: number, time?: number = 3): Promise<void> {
+  override animationUpFork(y: number): Promise<void> {
     const ptrFork = this.getObjectByName('ptrFork')
     const ptrPillar = this.getObjectByName('ptrPillar')
     const pz = ptrPillar.position.z
+
+    const time = Math.abs((ptrFork.position.y - y) / this.upForkSpeed)
+
     return new Promise(resolve => {
       const bh = 0.22
       const children = ptrFork.children
@@ -111,7 +103,11 @@ export default class Cl23dObject extends PtrObject {
         duration: time,
         repeat: 0,
         ease: 'sine.inOut',
-        onComplete: resolve,
+        onComplete: ()=>{
+          setTimeout(() => {
+            resolve()
+          }, 1000)
+        },
         onUpdate: function () {
           const a = this.targets()[0]
           if (a.y < bh) {

src/modules/amr/ptr/clx/Clx3DGraphics.ts

@@ -377,7 +377,7 @@ export default class Clx3DGraphics {
     shape.closePath()
 
     const curve = new THREE.CatmullRomCurve3(
-      [new THREE.Vector3(0, 0.02, 0), new THREE.Vector3(0, 1.287, 0)],
+      [new THREE.Vector3(0, -1.5/*0.02*/, 0), new THREE.Vector3(0, 1.287, 0)],
       false,  // 闭合曲线
       'catmullrom',
       0

src/modules/amr/ptr/clx/Clx3dObject.ts

@@ -10,6 +10,10 @@ import PtrObject from "@/modules/amr/ptr/PtrObject";
 
 export default class Clx3dObject extends PtrObject {
 
+  override rotationSpeed = (Math.PI / 10)
+  override showForkSpeed: number = 0.15
+  override upForkSpeed: number = 0.15
+
   constructor(item: ItemJson, viewport: Viewport, option?: RendererCudOption) {
     super(item, viewport)
 
@@ -105,7 +109,7 @@ export default class Clx3dObject extends PtrObject {
 
     const fy = clxFork.position.y
 
-    const time = 3
+    const time = Math.abs(z / this.upForkSpeed)
 
     const ac = Math.asin(z / (2 * 0.8))
 
@@ -189,7 +193,11 @@ export default class Clx3dObject extends PtrObject {
         duration: time,
         repeat: 0,
         ease: 'sine.inOut',
-        onComplete: resolve
+        onComplete: ()=>{
+          setTimeout(() => {
+            resolve()
+          }, 1000)
+        },
       })
     })
 
@@ -199,7 +207,7 @@ export default class Clx3dObject extends PtrObject {
     return this.animationShowFork(0)
   }
 
-  override animationUpFork(y: number, time?: number = 3): Promise<void> {
+  override animationUpFork(y: number): Promise<void> {
     const clxFork = this.getObjectByName('clxFork')
     const clxGemelMeshL1 = this.getObjectByName('clxGemelMeshL1')
     const clxGemelMeshL2 = this.getObjectByName('clxGemelMeshL2')
@@ -208,6 +216,8 @@ export default class Clx3dObject extends PtrObject {
     const clxForkBasePlateMesh = this.getObjectByName('clxForkBasePlateMesh')
 
 
+    const time = Math.abs((clxFork.position.y - y) / this.upForkSpeed)
+
     gsap.to(clxGemelMeshL1.position, {
       y: y + 0.72,
       duration: time,
@@ -248,7 +258,11 @@ export default class Clx3dObject extends PtrObject {
         duration: time,
         repeat: 0,
         ease: 'sine.inOut',
-        onComplete: resolve,
+        onComplete: ()=>{
+          setTimeout(() => {
+            resolve()
+          }, 1000)
+        },
         onUpdate: function() {
           const a = this.targets()[0]
           if (a.y <  bh) {

src/modules/amr/ptr/clx/ClxRenderer.ts

@@ -3,6 +3,7 @@ import BaseRenderer from '@/core/base/BaseRenderer.ts'
 import Constract from '@/core/Constract.ts'
 import Clx3dObject from './Clx3dObject'
 import type { Object3DLike } from '@/types/ModelTypes.ts'
+import Viewport from "@/core/engine/Viewport";
 
 /**
  * clx渲染器
@@ -60,6 +61,34 @@ export default class ClxRenderer extends BaseRenderer {
     return group
   }
 
+  /**
+   * 如果某物品要放到 Cl2 上 返回可存放的位置和角度一个 Position 和 Rotation
+   */
+  getStorePlacement(storeItem: ItemJson, bay = 0, level = 0, cell = 0)
+    : {
+    position: [number, number, number], rotation: [number, number, number],
+    getParentObject3D?: (viewport: Viewport, parent: THREE.Object3D) => THREE.Object3D
+  } {
+
+    return {
+      position: [0, 0.2, 0],
+      rotation: [0, 90, 0],
+      getParentObject3D: this.getArmObject.bind(this)
+    }
+  }
+  getArmObject(viewport: Viewport, item: ItemJson): THREE.Object3D {
+    // 获取机械臂对象
+    const object = viewport.entityManager.findObjectById(item.dt.storeAt?.item)
+    if (!object) {
+      console.warn('PtrRenderer: getArmObject failed, not found Cl2:', item.dt.storeAt?.item)
+      return
+    }
+
+    const agv = object as THREE.Group
+    const clxFork = agv.getObjectByName('clxFork')
+    return clxFork
+  }
+
   updatePoint(item: ItemJson, object: Object3DLike, option?: RendererCudOption): Object3DLike {
     const group: THREE.Group = object as THREE.Group
 

src/modules/rack/RackRenderer.ts

@@ -82,11 +82,7 @@ export default class RackRenderer extends BaseRenderer {
     }
     localX += bays[bay].bayWidth / 2 // 居中
 
-    let localY = 0
+    let localY = levelHeights[level]
-    for (let i = 0; i <= level; i++) {
-      localY += levelHeights[i]
-    }
-
     // 构建局部偏移向量
     const rackPos = new THREE.Vector3(...rack.tf[0])
     const offset = new THREE.Vector3(
@@ -107,7 +103,7 @@ export default class RackRenderer extends BaseRenderer {
     return {
       position: [
         worldPosition.x,
-        worldPosition.y + rack.dt.bottomBarHeight + 0.05, // 加上横梁高度
+        worldPosition.y, //+ 0.05, // 加上横梁高度
         worldPosition.z
       ],
       rotation: [