yuliang
/
yvan-rcs-web
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

master
yuliang 6 months ago
parent
7e5e46ec70
commit
8b937851c9
2 changed files with 186 additions and 95 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      src/core/manager/amr/AmrMessageDefine.ts
  2. 273
      src/modules/cl2/Cl23dObject.ts

8
src/core/manager/amr/AmrMessageDefine.ts
View File

@ -12,6 +12,9 @@ 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
// 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
type CPickMode = 0 | 1 | 2 | 3 | 4 | 5 | 6
class AmrMsg<T> {
id: number
content: T
@ -144,7 +147,7 @@ class AmrMsg20010 extends AmrMsg20000Base {
// 执行器名称 String
Name: string;
// 任务类型 UInt8 同下发任务中PickMode的定义
PickMode: number;
PickMode: CPickMode;
}[]
};
// 车载货位信息
@ -378,7 +381,7 @@ class TaskCompletedData {
// 执行器名称 String
Name: string;
// 任务类型 UInt8 同下发任务中PickMode的定义
PickMode: number;
PickMode: CPickMode;
}[]
};
// 电量百分比 Uint8
@ -539,6 +542,7 @@ export {
COperationType,
CTaskMode,
CEventId,
CPickMode,
}
const AmrErrorCode = {

273
src/modules/cl2/Cl23dObject.ts
View File

@ -20,8 +20,8 @@ import {
AmrMsg20150,
AmrMsg20250,
CurBatteryData,
type CEventId, type COperationType, type CTaskMode,
type LogicDirection, TaskCompletedData, TaskModeChangeData, TaskStatusChangeData, TaskTypeChangeData
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";
@ -64,6 +64,7 @@ export default class Cl23dObject extends THREE.Object3D {
private __TaskMode: CTaskMode = 0
private __OperationType: COperationType = 0
private __TaskStatus: CEventId = 0
private __PickMode: CPickMode = 0
get TaskMode(): CTaskMode {
return this.__TaskMode
@ -125,6 +126,15 @@ export default class Cl23dObject extends THREE.Object3D {
}
}
get PickMode(): CPickMode {
return this.__PickMode
}
set PickMode(value: CPickMode) {
if (this.__PickMode !== value) {
this.__PickMode = value
}
}
private bootTime: number = 0
// 心跳间隔 UInt32 单位: s
@ -180,6 +190,10 @@ export default class Cl23dObject extends THREE.Object3D {
this.vehicleId = parseInt(this.cl2Entity.id)
this.cl2Entity.viewport.addFrameTimerCallback(this.cl2Entity.id, this.onFrameTimer.bind(this))
if (!worldModel.state.runState.isVirtual) {
this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
this.subscribeMessage('/agv_robot/status')
}
}
private AGVModel = "CYBER-LIFT-A_V1.0"
@ -216,28 +230,30 @@ export default class Cl23dObject extends THREE.Object3D {
this.bootTime = Date.now();
this.computeLogicXYAndDirection();
if (worldModel.state.runState.isVirtual) {
this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
this.send20147()
if (!worldModel.state.runState.isVirtual) {
return
}
this.subscribeMessage('/wcs_server/' + this.cl2Entity.id)
this.send20147()
setTimeout(() => {
this.send20149()
this.TaskMode = 1
// 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标
setTimeout(() => {
this.send20149()
this.TaskMode = 1
// 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标
this.sendCurrentPositionAndDirection()
setTimeout(() => {
this.sendCurrentPositionAndDirection()
setTimeout(() => {
this.send20150()
}, 1000)
this.send20150()
}, 1000)
}, 2000)
} else {
}
}, 1000)
}, 2000)
}
// 关机
shutdown() {
if (!worldModel.state.runState.isVirtual) {
return
}
const content = new AmrMsg20148(this.vehicleId)
// 电量
content.Battery = 100
@ -310,6 +326,29 @@ export default class Cl23dObject extends THREE.Object3D {
}
}
send20010() {
const content: AmrMsg20010 = new AmrMsg20010(this.vehicleId)
content.CurX = this.currentLogicX
content.CurY = this.currentLogicY
content.CurDirection = this.currentDirection
content.OperationType = this.OperationType
content.Battery = this.Battery
content.OperationResult = 0
content.Summary = {
ActuatorsData: [
{
MechNo: 1,
Name: "Mech1",
PickMode: this.PickMode
}
]
}
const m20010 = new AmrMsg<AmrMsg20010>(content)
this.sendMessage(m20010)
}
send20011(content: AmrMsg20011<any>) {
const m20011 = new AmrMsg<AmrMsg20011<any>>(content)
this.sendMessage(m20011)
@ -340,10 +379,13 @@ export default class Cl23dObject extends THREE.Object3D {
}
subscribeMessage(topic: string) {
this.cl2Entity.viewport.envManager.client.subscribe(topic, {qos: 0})
worldModel.envManager.client.subscribe(topic, {qos: 0})
}
sendMessage(msg: AmrMsg<any>) {
if (!worldModel.state.runState.isVirtual) {
return
}
console.log('send message:', JSON.stringify(msg))
if (this.sendMessageQueue.indexOf(msg) < 0) {
this.sendMessageQueue.push(msg)
@ -351,26 +393,31 @@ export default class Cl23dObject extends THREE.Object3D {
if (this.sendMessageQueue.length <= 0) {
this.mqRetryTimeCount = 0
}
this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(msg))
worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(msg))
this.heartBeatTimeCount = 0
}
sendHeartBeat() {
if (!worldModel.state.runState.isVirtual) {
return
}
const content = new AmrMsg20100(this.vehicleId)
content.Temperature = {Battery: this.Battery}
const m20100 = new AmrMsg<AmrMsg20100>(content)
this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(m20100))
worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(m20100))
}
sendAck(seqNo: number, vehicleId: number) {
if (!worldModel.state.runState.isVirtual) {
return
}
const msg20050 = new AmrMsg20050(seqNo, vehicleId)
const ack = new AmrMsg<AmrMsg20050>(msg20050)
this.heartBeatTimeCount = 0
this.cl2Entity.viewport.envManager.client.publish('/agv_robot/status', JSON.stringify(ack))
worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(ack))
}
/*==========消息处理============*/
/*==========RCS消息处理============*/
// 处理任务
handle10010Message(data: AmrMsg10010) {
if (this.currentStepTaskList.length > 0) {
@ -410,6 +457,8 @@ export default class Cl23dObject extends THREE.Object3D {
}
/*==========真车消息处理============*/
// 计算逻辑方向
computeLogicXYAndDirection() {
let ra = this.rotation.y
@ -610,7 +659,7 @@ export default class Cl23dObject extends THREE.Object3D {
}
executeTask() {
this.TaskMode = 2
while (this.currentStepTaskList.length > 0) {
const stepTask = this.currentStepTaskList[0]
if (this.runningStepTask) {
@ -635,75 +684,29 @@ export default class Cl23dObject extends THREE.Object3D {
} else if (stepTask.StepTaskType == "ROTATION") {
this.addRotation(stepTask.EndDirection)
} else if (stepTask.StepTaskType == "LOAD") {
// this.add
this.addLoad(stepTask.GoodsSlotHeight/1000)
} else if (stepTask.StepTaskType == "UNLOAD") {
// this.add
this.addUnload(stepTask.GoodsSlotHeight/1000)
}
}
}
}
/*==========动画处理============*/
animationShowFork(z: number): Promise<void> {
const ptrPillar = this.getObjectByName('ptrPillar')
const time = 3
return new Promise(resolve => {
gsap.to(ptrPillar.position, {
z: -z,
duration: time,
repeat: 0,
ease: 'sine.inOut',
onComplete: resolve
})
})
}
animationHideFork(): Promise<void> {
return this.animationShowFork(0)
}
animationUpFork(y: number, time?: number = 3): Promise<void> {
const ptrFork = this.getObjectByName('ptrFork')
const ptrPillar = this.getObjectByName('ptrPillar')
const pz = ptrPillar.position.z
return new Promise(resolve => {
const bh = 0.22
const children = ptrFork.children
gsap.to(ptrFork.position, {
y: y,
duration: time,
repeat: 0,
ease: 'sine.inOut',
onComplete: resolve,
onUpdate: function () {
const a = this.targets()[0]
if (a.y < bh) {
if (pz > -1) {
for (let i = 0; i < children.length; i++) {
const child = children[i]
child.position.y = bh - a.y
}
} else if (a.y < 0) {
for (let i = 0; i < children.length; i++) {
const child = children[i]
child.position.y = 0 - a.y
}
}
}
}
})
})
}
animationDownFork(): Promise<void> {
return this.animationUpFork(0)
onActionCompleted() {
this.runningStepTaskList = []
this.runningStepTask = null
this.computeLogicXYAndDirection()
// 当前所有动作执行完毕
if (this.currentStepTaskList.length <= 0) {
this.send20010()
}
this.PickMode = 0
this.OperationType = 0
this.TaskMode = 0
this.executeTask()
}
/*==========动画处理============*/
// 转
addRotation(direction: number): Promise<void> {
@ -737,8 +740,7 @@ export default class Cl23dObject extends THREE.Object3D {
ease: 'none',
onComplete: ()=>{
resolve()
this.runningStepTaskList = []
this.runningStepTask = null
this.onActionCompleted()
}
})
})
@ -746,7 +748,8 @@ export default class Cl23dObject extends THREE.Object3D {
// 走
addTravel(logicX: number, logicY: number, speed: number = 1): Promise<void> {
this.OperationType = 0
this.PickMode = 0
const pos = Model.getPositionByLogicXY(logicX, logicY)
const fromPos = this.position
@ -769,9 +772,7 @@ export default class Cl23dObject extends THREE.Object3D {
onComplete: () => {
this.travelAnimation = null
resolve()
this.runningStepTaskList = []
this.runningStepTask = null
this.computeLogicXYAndDirection()
this.onActionCompleted()
},
onUpdate: () => {
@ -807,10 +808,96 @@ export default class Cl23dObject extends THREE.Object3D {
}
// fn = _.debounce((cl2: Cl2Entity) => {
// cl2.taskStartRun()
// }, 2000)
// 取货
addLoad(height: number): void {
this.PickMode = 1
this.OperationType = 4
this.animationUpFork(height).then(
() => this.animationShowFork(1.4).then(
()=>this.animationUpFork(height + 0.2).then(
()=>this.animationHideFork().then(
()=>this.animationDownFork().then(()=>{
this.onActionCompleted()
})
)
)
)
)
}
// 卸货
addUnload(height: number): void {
this.PickMode = 2
this.OperationType = 4
this.animationUpFork(height + 0.2).then(
() => this.animationShowFork(1.4).then(
()=>this.animationUpFork(height).then(
()=>this.animationHideFork().then(
()=>this.animationDownFork().then(()=>{
this.onActionCompleted()
})
)
)
)
)
}
animationShowFork(z: number): Promise<void> {
const ptrPillar = this.getObjectByName('ptrPillar')
const time = 3
return new Promise(resolve => {
gsap.to(ptrPillar.position, {
z: -z,
duration: time,
repeat: 0,
ease: 'sine.inOut',
onComplete: resolve
})
})
}
animationHideFork(): Promise<void> {
return this.animationShowFork(0)
}
animationUpFork(y: number, time?: number = 3): Promise<void> {
const ptrFork = this.getObjectByName('ptrFork')
const ptrPillar = this.getObjectByName('ptrPillar')
const pz = ptrPillar.position.z
return new Promise(resolve => {
const bh = 0.22
const children = ptrFork.children
gsap.to(ptrFork.position, {
y: y,
duration: time,
repeat: 0,
ease: 'sine.inOut',
onComplete: resolve,
onUpdate: function () {
const a = this.targets()[0]
if (a.y < bh) {
if (pz > -1) {
for (let i = 0; i < children.length; i++) {
const child = children[i]
child.position.y = bh - a.y
}
} else if (a.y < 0) {
for (let i = 0; i < children.length; i++) {
const child = children[i]
child.position.y = 0 - a.y
}
}
}
}
})
})
}
animationDownFork(): Promise<void> {
return this.animationUpFork(0)
}
}

Write Preview
Loading…
Cancel
Save