api-amr/simulator.ts

796 lines
22 KiB
TypeScript
Raw Normal View History

2025-06-04 19:15:02 +08:00
// simulator.ts - Worker版本
import mqtt, { IClientOptions, MqttClient } from "npm:mqtt";
import { v4 as uuidv4 } from "npm:uuid";
import { loadConfig, RawConfig } from "./simulator_config.ts";
interface SimulatorConfig {
vdaInterface: string;
zoneSetId: string;
mapId: string;
vehicle: {
manufacturer: string;
serialNumber: string;
vdaVersion: string;
};
mqtt: {
brokerUrl: string;
options: IClientOptions;
};
settings: {
robotCount: number;
stateFrequency: number;
visualizationFrequency: number;
speed: number;
};
}
interface Header {
headerId: number;
timestamp: string;
version: string;
manufacturer: string;
serialNumber: string;
}
interface BatteryState {
batteryCharge: number;
charging: boolean;
batteryHealth?: number;
batteryVoltage?: number;
reach?: number;
}
interface SafetyState {
eStop: string;
fieldViolation: boolean;
}
interface AGVPosition {
x: number;
y: number;
theta: number;
mapId: string;
positionInitialized: boolean;
mapDescription?: string;
localizationScore?: number;
deviationRange?: number;
}
interface Velocity {
vx: number;
vy: number;
omega: number;
}
type ActionStatus = "WAITING" | "INITIALIZING" | "RUNNING" | "PAUSED" | "FINISHED" | "FAILED";
interface ActionState {
actionId: string;
actionType?: string;
actionStatus: ActionStatus;
actionDescription?: string;
resultDescription?: string;
actionParameters?: Array<{ key: string; value: string }>;
blockingType?: "NONE" | "SOFT" | "HARD";
}
interface NodeState {
nodeId: string;
sequenceId: string;
released: boolean;
nodeDescription?: string;
nodePosition: AGVPosition;
}
interface EdgeState {
edgeId: string;
sequenceId: string;
released: boolean;
edgeDescription?: string;
trajectory?: Array<Record<string,unknown>>;
}
interface Factsheet {
headerId: number;
timestamp: string;
version: string;
manufacturer: string;
serialNumber: string;
typeSpecification: {
seriesName: string;
seriesDescription: string;
agvKinematic: string;
agvClass: string;
maxLoadMass: number;
localizationTypes: string[];
navigationTypes: string[];
};
physicalParameters: {
speedMin: number;
speedMax: number;
accelerationMax: number;
decelerationMax: number;
heightMin: number;
heightMax: number;
width: number;
length: number;
};
protocolLimits: any;
protocolFeatures: any;
agvGeometry: any;
loadSpecification: any;
localizationParameters: any;
}
interface State {
headerId: number;
timestamp: string;
version: string;
manufacturer: string;
serialNumber: string;
orderId: string;
orderUpdateId: number;
zoneSetId: string;
lastNodeId: string;
lastNodeSequenceId: number;
nodeStates: NodeState[];
edgeStates: EdgeState[];
actionStates: ActionState[];
batteryState: BatteryState;
operatingMode: string;
errors: { errorType: string; errorDescription?: string; errorLevel: string }[];
safetyState: SafetyState;
driving: boolean;
paused: boolean;
newBaseRequest: boolean;
waitingForInteractionZoneRelease: boolean;
agvPosition: AGVPosition;
velocity: Velocity;
loads: any[];
information?: any[];
forkState?: { forkHeight?: number };
}
interface Visualization {
header: Header;
agvPosition: AGVPosition;
velocity: Velocity;
driving: boolean;
}
interface Connection {
headerId: number;
timestamp: string;
version: string;
manufacturer: string;
serialNumber: string;
connectionState: "ONLINE" | "OFFLINE" | "CONNECTIONBROKEN";
}
interface ActionParamValue {
Float?: number;
Str?: string;
}
interface Action {
actionId: string;
actionType?: string;
actionParameters: { key: string; value: ActionParamValue }[];
}
interface InstantActions {
instantActions: Action[];
}
interface Order {
orderId: string;
orderUpdateId: number;
nodes: any[];
edges: any[];
}
// Worker消息类型
interface WorkerMessage {
type: "init" | "close" | "reconnect";
data?: any;
}
interface MainMessage {
type: "ready" | "error" | "status" | "log" | "device_request";
data?: any;
}
class VehicleSimulator {
connectionTopic: string = "";
stateTopic: string = "";
visualizationTopic: string = "";
factsheetTopic: string = "";
connection!: Connection;
state!: State;
visualization!: Visualization;
factsheet!: Factsheet;
private lastUpdate = Date.now();
private speed: number = 0;
private boundary = 40.0;
private mqttClient?: MqttClient;
private intervals: number[] = [];
private isRunning = false;
constructor(private cfg: SimulatorConfig) {
this.speed = cfg.settings.speed;
this.initializeSimulator();
}
private initializeSimulator() {
const { manufacturer, serialNumber, vdaVersion } = this.cfg.vehicle;
const base = `${this.cfg.vdaInterface}/${vdaVersion}/${manufacturer}/${serialNumber}`;
this.connectionTopic = `${base}/connection`;
this.stateTopic = `${base}/state`;
this.visualizationTopic = `${base}/visualization`;
this.factsheetTopic = `${base}/factsheet`;
const now = () => new Date().toISOString();
const header0: Header = {
headerId: 0,
timestamp: now(),
version: vdaVersion,
manufacturer,
serialNumber
};
this.connection = {
headerId: 0,
timestamp: now(),
version: vdaVersion,
manufacturer,
serialNumber,
connectionState: "CONNECTIONBROKEN"
};
// 随机初始位置
const x0 = (Math.random() * 2 - 1) * 30;
const y0 = (Math.random() * 2 - 1) * 30;
const th0 = (Math.random() * 2 - 1) * Math.PI;
// VDA 5050 兼容的扁平化 State
this.state = {
headerId: 0,
timestamp: now(),
version: vdaVersion,
manufacturer,
serialNumber,
orderId: "",
orderUpdateId: 0,
zoneSetId: this.cfg.zoneSetId,
lastNodeId: "",
lastNodeSequenceId: 0,
nodeStates: [],
edgeStates: [],
actionStates: [],
batteryState: { batteryCharge: 1.0, charging: false },
operatingMode: "AUTOMATIC",
errors: [],
safetyState: { eStop: "NONE", fieldViolation: false },
driving: true,
paused: false,
newBaseRequest: true,
waitingForInteractionZoneRelease: false,
agvPosition: {
x: x0,
y: y0,
theta: th0,
mapId: this.cfg.mapId,
positionInitialized: true
},
velocity: {
vx: this.speed * Math.cos(th0),
vy: this.speed * Math.sin(th0),
omega: 0
},
loads: [],
information: [],
forkState: { forkHeight: 0 }
};
this.visualization = {
header: { ...header0 },
agvPosition: { ...this.state.agvPosition },
velocity: { ...this.state.velocity },
driving: true
};
this.factsheet = {
headerId: 0,
timestamp: now(),
version: "v3.4.7.1005",
manufacturer,
serialNumber,
typeSpecification: {
seriesName: serialNumber,
seriesDescription: serialNumber,
agvKinematic: "DIFF",
agvClass: "FORKLIFT",
maxLoadMass: 0.5,
localizationTypes: ["NATURAL"],
navigationTypes: ["PHYSICAL_LINE_GUIDED"]
},
physicalParameters: {
speedMin: 0.01,
speedMax: 0.8,
accelerationMax: 0.5,
decelerationMax: 0.15,
heightMin: 1.83,
heightMax: 1.83,
width: 0.885,
length: 1.5145
},
protocolLimits: null,
protocolFeatures: null,
agvGeometry: null,
loadSpecification: null,
localizationParameters: null
};
}
async start() {
if (this.isRunning) {
this.log("warn", "Simulator already running");
return;
}
try {
this.isRunning = true;
await this.connectMqtt();
await this.subscribeVda();
await this.publishConnection();
this.publishFactsheet(); // 启动时发布一次 factsheet
this.startIntervals();
this.postMessage({ type: "ready", data: { agvId: this.cfg.vehicle.serialNumber } });
this.log("info", `🚛 AGV ${this.cfg.vehicle.serialNumber} started successfully`);
} catch (error) {
this.isRunning = false;
this.postMessage({ type: "error", data: { error: (error as Error).message, agvId: this.cfg.vehicle.serialNumber } });
this.log("error", `Failed to start AGV ${this.cfg.vehicle.serialNumber}: ${(error as Error).message}`);
}
}
async stop() {
if (!this.isRunning) return;
this.isRunning = false;
this.clearIntervals();
if (this.mqttClient) {
this.connection.connectionState = "OFFLINE";
this.publishConnectionOnline();
this.mqttClient.end();
this.mqttClient = undefined;
}
this.log("info", `🛑 AGV ${this.cfg.vehicle.serialNumber} stopped`);
}
async reconnect() {
this.log("info", `🔄 Reconnecting AGV ${this.cfg.vehicle.serialNumber}`);
await this.stop();
await new Promise(resolve => setTimeout(resolve, 1000)); // 等待1秒
await this.start();
}
private async connectMqtt() {
const clientOptions: IClientOptions = {
clientId: `deno-agv-sim-${this.cfg.vehicle.serialNumber}-${uuidv4()}`,
clean: true,
keepalive: 60,
};
this.mqttClient = mqtt.connect(this.cfg.mqtt.brokerUrl, clientOptions);
return new Promise<void>((resolve, reject) => {
const timeout = setTimeout(() => {
reject(new Error("MQTT connection timeout"));
}, 10000);
this.mqttClient!.on("connect", () => {
clearTimeout(timeout);
this.log("info", `✅ MQTT connected: ${this.cfg.vehicle.serialNumber}`);
resolve();
});
this.mqttClient!.on("error", (error) => {
clearTimeout(timeout);
reject(error as Error);
});
});
}
private async subscribeVda() {
if (!this.mqttClient) throw new Error("MQTT client not connected");
const b = `${this.cfg.vdaInterface}/${this.cfg.vehicle.vdaVersion}/${this.cfg.vehicle.manufacturer}/${this.cfg.vehicle.serialNumber}`;
const topics = [`${b}/instantActions`, `${b}/order`];
await new Promise<void>((res, rej) =>
this.mqttClient!.subscribe(topics, { qos: 1 }, err => err ? rej(err) : res())
);
this.mqttClient.on("message", (topic, payload) => {
try {
const msg = JSON.parse(payload.toString());
this.log("info", `📨 Received MQTT message on topic: ${topic}`);
if (topic.endsWith("instantActions")) {
this.instantActionsAccept(msg);
} else if (topic.endsWith("order")) {
this.orderAccept(msg);
}
} catch (error) {
this.log("error", `Error processing MQTT message: ${(error as Error).message}`);
this.log("error", `Topic: ${topic}, Payload: ${payload.toString()}`);
}
});
}
private startIntervals() {
const stateInterval = setInterval(() => {
this.stateIterate();
this.publishState();
}, 2000);
const visInterval = setInterval(() => {
this.publishVisualization();
}, Math.floor(1000 / this.cfg.settings.visualizationFrequency));
const connectionInterval = setInterval(() => {
this.publishConnectionOnline();
}, 1000);
// const factsheetInterval = setInterval(() => {
// this.publishFactsheet();
// }, 30000); // 每30秒发布一次 factsheet
this.intervals = [stateInterval, visInterval, connectionInterval];
}
private clearIntervals() {
this.intervals.forEach(interval => clearInterval(interval));
this.intervals = [];
}
stateIterate() {
const nowMs = Date.now();
const dt = (nowMs - this.lastUpdate) / 1000;
this.lastUpdate = nowMs;
this.state.headerId++;
this.state.timestamp = new Date().toISOString();
if (this.state.driving) {
let { x, y, theta } = this.state.agvPosition;
const vx = this.speed * Math.cos(theta);
const vy = this.speed * Math.sin(theta);
let nx = x + vx * dt;
let ny = y + vy * dt;
let bounced = false;
if (nx > this.boundary) {
nx = this.boundary - (nx - this.boundary);
bounced = true;
}
if (nx < -this.boundary) {
nx = -this.boundary - (-this.boundary - nx);
bounced = true;
}
if (ny > this.boundary) {
ny = this.boundary - (ny - this.boundary);
bounced = true;
}
if (ny < -this.boundary) {
ny = -this.boundary - (-this.boundary - ny);
bounced = true;
}
if (bounced) {
theta += Math.PI;
if (theta > Math.PI) theta -= 2 * Math.PI;
if (theta < -Math.PI) theta += 2 * Math.PI;
}
this.state.agvPosition = {
x: nx,
y: ny,
theta,
mapId: this.state.agvPosition.mapId,
positionInitialized: true
};
this.state.velocity = {
vx: this.speed * Math.cos(theta),
vy: this.speed * Math.sin(theta),
omega: 0
};
} else {
this.state.velocity = { vx: 0, vy: 0, omega: 0 };
}
// 同步 Visualization header
this.visualization.header.headerId = this.state.headerId + 1;
this.visualization.header.timestamp = this.state.timestamp;
this.visualization.agvPosition = { ...this.state.agvPosition };
this.visualization.velocity = { ...this.state.velocity };
this.visualization.driving = this.state.driving;
}
async publishConnection() {
if (!this.mqttClient) return;
const pub = (msg: Connection) => new Promise<void>(res => {
this.mqttClient!.publish(this.connectionTopic, JSON.stringify(msg), { qos: 1 }, () => res());
});
// Broken
this.connection.headerId++;
this.connection.timestamp = new Date().toISOString();
this.connection.connectionState = "CONNECTIONBROKEN";
await pub(this.connection);
// Online
await new Promise(r => setTimeout(r, 500));
this.connection.headerId++;
this.connection.timestamp = new Date().toISOString();
this.connection.connectionState = "ONLINE";
await pub(this.connection);
}
publishConnectionOnline() {
if (!this.mqttClient) return;
this.connection.headerId++;
this.connection.timestamp = new Date().toISOString();
this.connection.connectionState = "ONLINE";
this.mqttClient.publish(this.connectionTopic, JSON.stringify(this.connection), { qos: 1 });
}
publishState() {
if (!this.mqttClient) return;
this.mqttClient.publish(this.stateTopic, JSON.stringify(this.state), { qos: 1 });
}
publishVisualization() {
if (!this.mqttClient) return;
this.mqttClient.publish(this.visualizationTopic, JSON.stringify(this.visualization), { qos: 1 });
}
publishFactsheet() {
if (!this.mqttClient) return;
this.factsheet.headerId++;
this.factsheet.timestamp = new Date().toISOString();
this.mqttClient.publish(this.factsheetTopic, JSON.stringify(this.factsheet), { qos: 1 });
}
async instantActionsAccept(req: any) {
this.log("info", `📥 Received instantActions: ${JSON.stringify(req, null, 2)}`);
// 验证消息格式
if (!req || typeof req !== 'object') {
this.log("error", "Invalid instantActions request: not an object");
return;
}
// 检查actions或instantActions字段支持两种格式
let actionsArray: any[];
if (req.instantActions && Array.isArray(req.instantActions)) {
actionsArray = req.instantActions;
} else if (req.actions && Array.isArray(req.actions)) {
actionsArray = req.actions;
this.log("info", "Using 'actions' field instead of 'instantActions'");
} else {
this.log("error", "Missing instantActions or actions field in request");
return;
}
this.log("info", `📋 Processing ${actionsArray.length} instant actions`);
for (const act of actionsArray) {
if (!act || !act.actionId) {
this.log("error", "Invalid action: missing actionId");
continue;
}
// 检查是否是设备相关的action
if (act.actionType === "deviceSetup") {
this.log("info", `🔧 Processing deviceSetup action: ${act.actionId}`);
// 提取设备信息
const deviceInfo = this.extractDeviceInfo(act.actionParameters || []);
if (deviceInfo) {
// 通知主进程创建设备模拟器
this.postMessage({
type: "device_request",
data: {
action: "create",
deviceInfo,
originalAction: act
}
});
this.log("info", `📡 Requested device simulator creation for ${deviceInfo.ip}-${deviceInfo.port}-${deviceInfo.slaveId}`);
} else {
this.log("error", `❌ Failed to extract device info from action ${act.actionId}`);
}
} else if (act.actionType === "deviceStop") {
this.log("info", `🛑 Processing deviceStop action: ${act.actionId}`);
// 提取设备ID或信息
const deviceInfo = this.extractDeviceInfo(act.actionParameters || []);
if (deviceInfo) {
const deviceId = `${deviceInfo.ip}-${deviceInfo.port}-${deviceInfo.slaveId}`;
// 通知主进程停止设备模拟器
this.postMessage({
type: "device_request",
data: {
action: "stop",
deviceId,
originalAction: act
}
});
this.log("info", `📡 Requested device simulator stop for ${deviceId}`);
}
} else if (act.actionType === "deviceDelete") {
this.log("info", `🗑️ Processing deviceDelete action: ${act.actionId}`);
// 提取设备ID或信息
const deviceInfo = this.extractDeviceInfo(act.actionParameters || []);
if (deviceInfo) {
const deviceId = `${deviceInfo.ip}-${deviceInfo.port}-${deviceInfo.slaveId}`;
// 通知主进程删除设备模拟器
this.postMessage({
type: "device_request",
data: {
action: "delete",
deviceId,
originalAction: act
}
});
this.log("info", `📡 Requested device simulator deletion for ${deviceId}`);
}
} else if (act.actionType === "deviceWrite" || act.actionType === "deviceRead") {
this.log("info", `🔧 Processing device action: ${act.actionType} - ${act.actionId}`);
// 提取设备信息
const deviceInfo = this.extractDeviceInfo(act.actionParameters || []);
if (deviceInfo) {
const deviceId = `${deviceInfo.ip}-${deviceInfo.port}-${deviceInfo.slaveId}`;
// 通知主进程转发action到对应的设备模拟器
this.postMessage({
type: "device_request",
data: {
action: "forward",
deviceId,
originalAction: act
}
});
this.log("info", `📡 Forwarded device action to ${deviceId}`);
}
}
this.state.actionStates.push({
actionId: act.actionId,
actionType: act.actionType || "unknown",
actionStatus: "WAITING"
});
this.log("info", `✅ Added action: ${act.actionId} (${act.actionType || "unknown"})`);
}
}
// 从actionParameters中提取设备信息
private extractDeviceInfo(actionParameters: any[]): any | null {
const params: Record<string, string> = {};
for (const param of actionParameters) {
if (param.key && param.value) {
// 处理不同的value格式
let value: string;
if (typeof param.value === 'string') {
value = param.value;
} else if (param.value?.Str) {
value = param.value.Str;
} else if (param.value?.Float) {
value = param.value.Float.toString();
} else {
value = String(param.value);
}
params[param.key] = value;
}
}
// 检查必需的字段
const required = ['ip', 'port', 'slaveId'];
for (const field of required) {
if (!params[field]) {
this.log("error", `❌ Missing required parameter: ${field}`);
return null;
}
}
return {
ip: params.ip,
port: params.port,
slaveId: params.slaveId,
deviceName: params.deviceName || 'Unknown Device',
protocolType: params.protocolType || 'Unknown Protocol',
brandName: params.brandName || 'Unknown Brand',
registers: params.registers || '[]'
};
}
async orderAccept(req: Order) {
this.log("info", `📥 Received order: ${JSON.stringify(req, null, 2)}`);
this.state.orderId = req.orderId;
this.state.orderUpdateId = req.orderUpdateId;
this.state.nodeStates = [];
this.state.edgeStates = [];
this.state.actionStates = [];
this.log("info", `✅ Accepted order: ${req.orderId} (updateId: ${req.orderUpdateId})`);
}
private log(level: "info" | "warn" | "error", message: string) {
const timestamp = new Date().toISOString();
const logMessage = `[${timestamp}] [${this.cfg.vehicle.serialNumber}] ${message}`;
console.log(logMessage);
this.postMessage({
type: "log",
data: { level, message: logMessage, agvId: this.cfg.vehicle.serialNumber }
});
}
private postMessage(message: MainMessage) {
self.postMessage(message);
}
}
// Worker全局变量
let simulator: VehicleSimulator | null = null;
// Worker消息处理
self.onmessage = async (event: MessageEvent<WorkerMessage>) => {
const { type, data } = event.data;
try {
switch (type) {
case "init":
if (simulator) {
await simulator.stop();
}
simulator = new VehicleSimulator(data);
await simulator.start();
break;
case "close":
if (simulator) {
await simulator.stop();
simulator = null;
}
self.postMessage({ type: "status", data: { status: "closed" } });
break;
case "reconnect":
if (simulator) {
await simulator.reconnect();
}
break;
default:
console.warn(`Unknown message type: ${type}`);
}
} catch (error) {
self.postMessage({
type: "error",
data: { error: (error as Error).message, context: type }
});
}
};