mirror of
https://github.com/soconnor0919/hristudio.git
synced 2026-05-08 05:48:56 -04:00
1422 lines
42 KiB
TypeScript
1422 lines
42 KiB
TypeScript
"use client";
|
||
|
||
import { EventEmitter } from "events";
|
||
|
||
export interface RosMessage {
|
||
op: string;
|
||
topic?: string;
|
||
type?: string;
|
||
msg?: Record<string, unknown>;
|
||
service?: string;
|
||
args?: Record<string, unknown>;
|
||
id?: string;
|
||
result?: boolean;
|
||
values?: Record<string, unknown>;
|
||
}
|
||
|
||
export interface ServiceRequest {
|
||
service: string;
|
||
args?: Record<string, unknown>;
|
||
}
|
||
|
||
export interface ServiceResponse {
|
||
result: boolean;
|
||
values?: Record<string, unknown>;
|
||
error?: string;
|
||
}
|
||
|
||
export interface RobotStatus {
|
||
connected: boolean;
|
||
battery: number;
|
||
position: { x: number; y: number; theta: number };
|
||
joints: Record<string, number>;
|
||
sensors: Record<string, unknown>;
|
||
lastUpdate: Date;
|
||
}
|
||
|
||
export interface RobotActionExecution {
|
||
id: string;
|
||
actionId: string;
|
||
pluginName: string;
|
||
parameters: Record<string, unknown>;
|
||
status: "pending" | "executing" | "completed" | "failed";
|
||
startTime: Date;
|
||
endTime?: Date;
|
||
error?: string;
|
||
}
|
||
|
||
/**
|
||
* A single frame in a multi-step joint animation.
|
||
* @param joint_names - NAO joint names to move (e.g. "LShoulderPitch")
|
||
* @param joint_angles - Target angles in radians, one per joint_name
|
||
* @param speed - Movement speed as a fraction of max (0.0–1.0)
|
||
* @param delay_after - Milliseconds to wait after publishing this frame
|
||
* before sending the next one. Defaults to 800 ms.
|
||
*/
|
||
export interface AnimationMovement {
|
||
joint_names: string[];
|
||
joint_angles: number[];
|
||
speed?: number;
|
||
delay_after?: number;
|
||
}
|
||
|
||
/**
|
||
* Unified ROS WebSocket service for wizard interface
|
||
* Manages connection to rosbridge and handles robot action execution
|
||
*/
|
||
export class WizardRosService extends EventEmitter {
|
||
private ws: WebSocket | null = null;
|
||
private url: string;
|
||
private reconnectInterval = 3000;
|
||
private reconnectTimer: NodeJS.Timeout | null = null;
|
||
private messageId = 0;
|
||
private isConnected = false;
|
||
private connectionAttempts = 0;
|
||
private maxReconnectAttempts = 5;
|
||
private isConnecting = false;
|
||
|
||
// Simulation mode
|
||
private simulationMode: boolean;
|
||
private simulationInterval: NodeJS.Timeout | null = null;
|
||
|
||
// Robot state
|
||
private robotStatus: RobotStatus = {
|
||
connected: false,
|
||
battery: 0,
|
||
position: { x: 0, y: 0, theta: 0 },
|
||
joints: {},
|
||
sensors: {},
|
||
lastUpdate: new Date(),
|
||
};
|
||
|
||
// Active action tracking
|
||
private activeActions: Map<string, RobotActionExecution> = new Map();
|
||
|
||
constructor(url: string = "ws://localhost:9090", simulationMode: boolean = false) {
|
||
super();
|
||
this.url = url;
|
||
this.simulationMode = simulationMode ||
|
||
(typeof window !== "undefined" && process.env.NEXT_PUBLIC_SIMULATION_MODE === "true");
|
||
}
|
||
|
||
/**
|
||
* Check if running in simulation mode
|
||
*/
|
||
isSimulationMode(): boolean {
|
||
return this.simulationMode;
|
||
}
|
||
|
||
/**
|
||
* Enable or disable simulation mode
|
||
*/
|
||
setSimulationMode(enabled: boolean): void {
|
||
this.simulationMode = enabled;
|
||
if (!enabled && this.simulationInterval) {
|
||
clearInterval(this.simulationInterval);
|
||
this.simulationInterval = null;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Connect to ROS bridge WebSocket
|
||
*/
|
||
async connect(): Promise<void> {
|
||
// Simulation mode - fake connection
|
||
if (this.simulationMode) {
|
||
return this.connectSimulation();
|
||
}
|
||
|
||
return new Promise((resolve, reject) => {
|
||
if (
|
||
this.isConnected ||
|
||
this.ws?.readyState === WebSocket.OPEN ||
|
||
this.isConnecting
|
||
) {
|
||
if (this.isConnected) resolve();
|
||
return;
|
||
}
|
||
|
||
this.isConnecting = true;
|
||
console.log(`[WizardROS] Connecting to ${this.url}`);
|
||
this.ws = new WebSocket(this.url);
|
||
|
||
const connectionTimeout = setTimeout(() => {
|
||
if (this.ws?.readyState !== WebSocket.OPEN) {
|
||
this.ws?.close();
|
||
reject(new Error("Connection timeout"));
|
||
}
|
||
}, 10000);
|
||
|
||
this.ws.onopen = () => {
|
||
clearTimeout(connectionTimeout);
|
||
console.log("[WizardROS] Connected successfully");
|
||
this.isConnected = true;
|
||
this.isConnecting = false;
|
||
this.connectionAttempts = 0;
|
||
this.clearReconnectTimer();
|
||
|
||
// Subscribe to robot topics
|
||
this.subscribeToRobotTopics();
|
||
|
||
this.emit("connected");
|
||
resolve();
|
||
};
|
||
|
||
this.ws.onmessage = (event) => {
|
||
try {
|
||
const message = JSON.parse(event.data) as RosMessage;
|
||
this.handleMessage(message);
|
||
} catch (error) {
|
||
console.error("[WizardROS] Failed to parse message:", error);
|
||
}
|
||
};
|
||
|
||
this.ws.onclose = (event) => {
|
||
console.log(
|
||
`[WizardROS] Connection closed: ${event.code} - ${event.reason}`,
|
||
);
|
||
this.isConnected = false;
|
||
this.isConnecting = false;
|
||
this.emit("disconnected");
|
||
|
||
// Schedule reconnect if not manually closed
|
||
if (
|
||
event.code !== 1000 &&
|
||
this.connectionAttempts < this.maxReconnectAttempts
|
||
) {
|
||
this.scheduleReconnect();
|
||
}
|
||
};
|
||
|
||
this.ws.onerror = (error) => {
|
||
console.warn(
|
||
"[WizardROS] WebSocket error (connection may be retried):",
|
||
error,
|
||
);
|
||
clearTimeout(connectionTimeout);
|
||
this.isConnecting = false;
|
||
|
||
// Prevent unhandled error event if no listeners
|
||
if (this.listenerCount("error") > 0) {
|
||
this.emit("error", error);
|
||
}
|
||
reject(error);
|
||
};
|
||
});
|
||
}
|
||
|
||
/**
|
||
* Disconnect from ROS bridge
|
||
*/
|
||
disconnect(): void {
|
||
this.clearReconnectTimer();
|
||
|
||
if (this.simulationMode) {
|
||
this.disconnectSimulation();
|
||
return;
|
||
}
|
||
|
||
if (this.ws) {
|
||
this.ws.close(1000, "Manual disconnect");
|
||
this.ws = null;
|
||
}
|
||
|
||
this.isConnected = false;
|
||
this.isConnecting = false;
|
||
this.robotStatus.connected = false;
|
||
this.emit("disconnected");
|
||
}
|
||
|
||
/**
|
||
* Simulation mode connection - simulates robot responses
|
||
*/
|
||
private async connectSimulation(): Promise<void> {
|
||
console.log(`[WizardROS] SIMULATION MODE - Connecting to mock robot`);
|
||
this.isConnected = true;
|
||
this.isConnecting = false;
|
||
this.connectionAttempts = 0;
|
||
|
||
// Initialize mock robot state
|
||
const mockStates = this.getMockJointStates();
|
||
this.robotStatus = {
|
||
connected: true,
|
||
battery: 85,
|
||
position: { x: 0, y: 0, theta: 0 },
|
||
joints: mockStates.names.reduce((acc, name, i) => {
|
||
acc[name] = mockStates.positions[i] ?? 0;
|
||
return acc;
|
||
}, {} as Record<string, number>),
|
||
sensors: {},
|
||
lastUpdate: new Date(),
|
||
};
|
||
|
||
// Start publishing simulated sensor data
|
||
this.simulationInterval = setInterval(() => {
|
||
this.publishSimulationData();
|
||
}, 100);
|
||
|
||
this.emit("connected");
|
||
console.log(`[WizardROS] SIMULATION MODE - Connected to mock robot`);
|
||
}
|
||
|
||
/**
|
||
* Simulation mode disconnection
|
||
*/
|
||
private disconnectSimulation(): void {
|
||
console.log(`[WizardROS] SIMULATION MODE - Disconnecting`);
|
||
if (this.simulationInterval) {
|
||
clearInterval(this.simulationInterval);
|
||
this.simulationInterval = null;
|
||
}
|
||
this.isConnected = false;
|
||
this.robotStatus.connected = false;
|
||
this.emit("disconnected");
|
||
}
|
||
|
||
/**
|
||
* Publish simulated sensor data
|
||
*/
|
||
private publishSimulationData(): void {
|
||
if (!this.simulationMode || !this.isConnected) return;
|
||
|
||
const mockData = this.getMockJointStates();
|
||
this.updateJointStates(mockData);
|
||
this.robotStatus.battery = 85 + Math.random() * 2 - 1; // Slight variation
|
||
this.robotStatus.sensors = {
|
||
"/bumper": { left: false, right: false },
|
||
"/hand_touch": { leftHand: false, rightHand: false },
|
||
"/head_touch": { front: false, middle: false, rear: false },
|
||
"/sonar/left": { range: 0.5 + Math.random() * 0.5 },
|
||
"/sonar/right": { range: 0.5 + Math.random() * 0.5 },
|
||
};
|
||
this.robotStatus.lastUpdate = new Date();
|
||
this.emit("robot_status_updated", this.robotStatus);
|
||
}
|
||
|
||
/**
|
||
* Get mock joint states for simulation
|
||
*/
|
||
private getMockJointStates(): { names: string[]; positions: number[] } {
|
||
const names = [
|
||
"HeadYaw", "HeadPitch",
|
||
"LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand",
|
||
"RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand",
|
||
"LHipYawPitch", "LHipRoll", "LHipPitch", "LKneePitch", "LAnklePitch", "LAnkleRoll",
|
||
"RHipYawPitch", "RHipRoll", "RHipPitch", "RKneePitch", "RAnklePitch", "RAnkleRoll",
|
||
];
|
||
const positions = names.map(() => (Math.random() - 0.5) * 0.1);
|
||
return { names, positions };
|
||
}
|
||
|
||
/**
|
||
* Execute action in simulation mode
|
||
*/
|
||
private async executeSimulationAction(
|
||
pluginName: string,
|
||
actionId: string,
|
||
parameters: Record<string, unknown>,
|
||
actionConfig?: {
|
||
topic: string;
|
||
messageType: string;
|
||
payloadMapping: {
|
||
type: string;
|
||
payload?: Record<string, unknown>;
|
||
transformFn?: string;
|
||
};
|
||
},
|
||
): Promise<RobotActionExecution> {
|
||
const executionId = `${pluginName}_${actionId}_${Date.now()}`;
|
||
const execution: RobotActionExecution = {
|
||
id: executionId,
|
||
actionId,
|
||
pluginName,
|
||
parameters,
|
||
status: "pending",
|
||
startTime: new Date(),
|
||
};
|
||
|
||
this.activeActions.set(executionId, execution);
|
||
this.emit("action_started", execution);
|
||
|
||
try {
|
||
execution.status = "executing";
|
||
this.activeActions.set(executionId, execution);
|
||
|
||
console.log(`[WizardROS] SIMULATION MODE - Executing ${actionId}:`, parameters);
|
||
|
||
// If the action config carries a gesture_sequence payload, run the sim animation handler
|
||
if (actionConfig?.payloadMapping?.payload) {
|
||
const payload = actionConfig.payloadMapping.payload as { type?: string; movements?: AnimationMovement[] };
|
||
if (payload.type === "gesture_sequence" && payload.movements?.length) {
|
||
await this.executeSimulationAnimationSequence(payload.movements);
|
||
execution.status = "completed";
|
||
execution.endTime = new Date();
|
||
this.emit("action_completed", execution);
|
||
this.activeActions.set(executionId, execution);
|
||
return execution;
|
||
}
|
||
}
|
||
|
||
// Simulate action execution based on action type
|
||
let duration = 500;
|
||
|
||
if (actionId === "say_text" || actionId === "say_with_emotion" || actionConfig?.topic === "/speech") {
|
||
const text = String(parameters.text || parameters.data || "Hello");
|
||
const wordCount = text.split(/\s+/).filter(Boolean).length;
|
||
duration = 1500 + Math.max(1000, wordCount * 300);
|
||
} else if (actionId.includes("walk") || actionId === "stop_walking" || actionConfig?.topic === "/cmd_vel") {
|
||
duration = 500;
|
||
const speed = Number(parameters.speed) || 0.1;
|
||
if (actionId === "walk_forward") {
|
||
this.robotStatus.position.x += speed * 0.5;
|
||
} else if (actionId === "walk_backward") {
|
||
this.robotStatus.position.x -= speed * 0.5;
|
||
} else if (actionId === "turn_left") {
|
||
this.robotStatus.position.theta -= 0.5;
|
||
} else if (actionId === "turn_right") {
|
||
this.robotStatus.position.theta += 0.5;
|
||
}
|
||
} else if (actionConfig?.topic === "/joint_angles") {
|
||
duration = 1000;
|
||
} else if (actionId === "wake_up" || actionId === "rest" || actionId === "set_posture") {
|
||
duration = 2000;
|
||
}
|
||
|
||
// Simulate async execution
|
||
await new Promise((resolve) => setTimeout(resolve, duration));
|
||
|
||
execution.status = "completed";
|
||
execution.endTime = new Date();
|
||
this.emit("action_completed", execution);
|
||
} catch (error) {
|
||
execution.status = "failed";
|
||
execution.error = error instanceof Error ? error.message : String(error);
|
||
execution.endTime = new Date();
|
||
this.emit("action_failed", execution);
|
||
}
|
||
|
||
this.activeActions.set(executionId, execution);
|
||
return execution;
|
||
}
|
||
|
||
|
||
|
||
/**
|
||
* Check if connected to ROS bridge
|
||
*/
|
||
getConnectionStatus(): boolean {
|
||
if (this.simulationMode) {
|
||
return this.isConnected;
|
||
}
|
||
return this.isConnected && this.ws?.readyState === WebSocket.OPEN;
|
||
}
|
||
|
||
/**
|
||
* Get current robot status
|
||
*/
|
||
getRobotStatus(): RobotStatus {
|
||
return { ...this.robotStatus };
|
||
}
|
||
|
||
/**
|
||
* Execute robot action using plugin configuration
|
||
*/
|
||
async executeRobotAction(
|
||
pluginName: string,
|
||
actionId: string,
|
||
parameters: Record<string, unknown>,
|
||
actionConfig?: {
|
||
topic: string;
|
||
messageType: string;
|
||
payloadMapping: {
|
||
type: string;
|
||
payload?: Record<string, unknown>;
|
||
transformFn?: string;
|
||
};
|
||
},
|
||
): Promise<RobotActionExecution> {
|
||
if (!this.isConnected) {
|
||
throw new Error("Not connected to ROS bridge");
|
||
}
|
||
|
||
// Simulation mode - simulate action execution
|
||
if (this.simulationMode) {
|
||
return this.executeSimulationAction(pluginName, actionId, parameters, actionConfig);
|
||
}
|
||
|
||
const executionId = `${pluginName}_${actionId}_${Date.now()}`;
|
||
const execution: RobotActionExecution = {
|
||
id: executionId,
|
||
actionId,
|
||
pluginName,
|
||
parameters,
|
||
status: "pending",
|
||
startTime: new Date(),
|
||
};
|
||
|
||
this.activeActions.set(executionId, execution);
|
||
this.emit("action_started", execution);
|
||
|
||
try {
|
||
execution.status = "executing";
|
||
this.activeActions.set(executionId, execution);
|
||
|
||
// Execute based on action configuration or built-in mappings
|
||
if (actionConfig) {
|
||
await this.executeWithConfig(actionConfig, parameters);
|
||
} else {
|
||
await this.executeBuiltinAction(actionId, parameters);
|
||
}
|
||
|
||
execution.status = "completed";
|
||
execution.endTime = new Date();
|
||
this.emit("action_completed", execution);
|
||
} catch (error) {
|
||
execution.status = "failed";
|
||
execution.error = error instanceof Error ? error.message : String(error);
|
||
execution.endTime = new Date();
|
||
this.emit("action_failed", execution);
|
||
}
|
||
|
||
this.activeActions.set(executionId, execution);
|
||
return execution;
|
||
}
|
||
|
||
/**
|
||
* Get list of active actions
|
||
*/
|
||
getActiveActions(): RobotActionExecution[] {
|
||
return Array.from(this.activeActions.values());
|
||
}
|
||
|
||
/**
|
||
* Execute a multi-frame joint animation by sending each movement in sequence.
|
||
* Each frame is published to /joint_angles then held for delay_after ms
|
||
* (default 800 ms) before the next frame is sent.
|
||
*/
|
||
async executeAnimationSequence(movements: AnimationMovement[]): Promise<void> {
|
||
if (!movements.length) {
|
||
console.warn("[WizardROS] executeAnimationSequence called with empty movements");
|
||
return;
|
||
}
|
||
|
||
for (let i = 0; i < movements.length; i++) {
|
||
const movement = movements[i]!;
|
||
console.log(
|
||
`[WizardROS] Animation frame ${i + 1}/${movements.length}:`,
|
||
movement.joint_names,
|
||
"→",
|
||
movement.joint_angles,
|
||
);
|
||
|
||
this.publish(
|
||
"/joint_angles",
|
||
"naoqi_bridge_msgs/msg/JointAnglesWithSpeed",
|
||
{
|
||
joint_names: movement.joint_names,
|
||
joint_angles: movement.joint_angles,
|
||
speed: movement.speed ?? 0.3,
|
||
},
|
||
);
|
||
|
||
// Always wait after each frame (including the last) so the caller
|
||
// can await this method and know the animation has finished.
|
||
const delayMs = movement.delay_after ?? 800;
|
||
await new Promise<void>((resolve) => setTimeout(resolve, delayMs));
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Execute a multi-frame joint animation in simulation mode (logs only).
|
||
*/
|
||
private async executeSimulationAnimationSequence(
|
||
movements: AnimationMovement[],
|
||
): Promise<void> {
|
||
for (let i = 0; i < movements.length; i++) {
|
||
const movement = movements[i]!;
|
||
console.log(
|
||
`[WizardROS] SIMULATION animation frame ${i + 1}/${movements.length}:`,
|
||
movement.joint_names,
|
||
);
|
||
const delayMs = movement.delay_after ?? 800;
|
||
await new Promise<void>((resolve) => setTimeout(resolve, delayMs));
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Subscribe to robot sensor topics
|
||
*/
|
||
private subscribeToRobotTopics(): void {
|
||
console.log("[WizardROS] Setting up robot topics...");
|
||
const topics = [
|
||
{ topic: "/joint_states", type: "sensor_msgs/JointState" },
|
||
{ topic: "/bumper", type: "naoqi_bridge_msgs/Bumper" },
|
||
{ topic: "/hand_touch", type: "naoqi_bridge_msgs/HandTouch" },
|
||
{ topic: "/head_touch", type: "naoqi_bridge_msgs/HeadTouch" },
|
||
{ topic: "/sonar/left", type: "sensor_msgs/Range" },
|
||
{ topic: "/sonar/right", type: "sensor_msgs/Range" },
|
||
];
|
||
|
||
topics.forEach(({ topic, type }) => {
|
||
this.subscribe(topic, type);
|
||
});
|
||
|
||
this.advertise("/speech", "std_msgs/String");
|
||
this.advertise("/cmd_vel", "geometry_msgs/Twist");
|
||
this.advertise("/joint_angles", "naoqi_bridge_msgs/msg/JointAnglesWithSpeed");
|
||
this.advertise("/robot_pose", "geometry_msgs/Pose");
|
||
this.advertise("/animation", "std_msgs/String");
|
||
}
|
||
|
||
/**
|
||
* Subscribe to a ROS topic
|
||
*/
|
||
private subscribe(topic: string, messageType: string): void {
|
||
const message: RosMessage = {
|
||
op: "subscribe",
|
||
topic,
|
||
type: messageType,
|
||
id: `sub_${this.messageId++}`,
|
||
};
|
||
|
||
this.send(message);
|
||
}
|
||
|
||
/**
|
||
* Advertise a ROS topic (declare the type before publishing)
|
||
*/
|
||
private advertise(topic: string, messageType: string): void {
|
||
console.log(`[WizardROS] Advertising topic ${topic} as ${messageType}`);
|
||
const message: RosMessage = {
|
||
op: "advertise",
|
||
topic,
|
||
type: messageType,
|
||
id: `adv_${this.messageId++}`,
|
||
};
|
||
|
||
this.send(message);
|
||
}
|
||
|
||
/**
|
||
* Publish message to ROS topic
|
||
*/
|
||
private publish(
|
||
topic: string,
|
||
messageType: string,
|
||
msg: Record<string, unknown>,
|
||
): void {
|
||
console.log(`[WizardROS] Publishing to ${topic}:`, msg);
|
||
const message: RosMessage = {
|
||
op: "publish",
|
||
topic,
|
||
type: messageType,
|
||
msg,
|
||
};
|
||
|
||
this.send(message);
|
||
}
|
||
|
||
/**
|
||
* Send WebSocket message
|
||
*/
|
||
private send(message: RosMessage): void {
|
||
if (this.ws?.readyState === WebSocket.OPEN) {
|
||
this.ws.send(JSON.stringify(message));
|
||
} else {
|
||
console.warn("[WizardROS] Cannot send message - not connected");
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Handle incoming ROS messages
|
||
*/
|
||
private handleMessage(message: RosMessage): void {
|
||
if (message.topic) {
|
||
this.handleTopicMessage(message);
|
||
}
|
||
|
||
this.emit("message", message);
|
||
}
|
||
|
||
/**
|
||
* Handle topic-specific messages
|
||
*/
|
||
private handleTopicMessage(message: RosMessage): void {
|
||
if (!message.topic || !message.msg) return;
|
||
|
||
switch (message.topic) {
|
||
case "/joint_states":
|
||
this.updateJointStates(message.msg);
|
||
break;
|
||
case "/naoqi_driver/battery":
|
||
this.updateBatteryStatus(message.msg);
|
||
break;
|
||
case "/bumper":
|
||
case "/hand_touch":
|
||
case "/head_touch":
|
||
case "/sonar/left":
|
||
case "/sonar/right":
|
||
this.updateSensorData(message.topic, message.msg);
|
||
break;
|
||
}
|
||
|
||
this.robotStatus.lastUpdate = new Date();
|
||
this.emit("robot_status_updated", this.robotStatus);
|
||
}
|
||
|
||
/**
|
||
* Update joint states from ROS message
|
||
*/
|
||
private updateJointStates(msg: Record<string, unknown>): void {
|
||
if (
|
||
msg.name &&
|
||
msg.position &&
|
||
Array.isArray(msg.name) &&
|
||
Array.isArray(msg.position)
|
||
) {
|
||
const joints: Record<string, number> = {};
|
||
|
||
for (let i = 0; i < msg.name.length; i++) {
|
||
const jointName = msg.name[i] as string;
|
||
const position = msg.position[i] as number;
|
||
if (jointName && typeof position === "number") {
|
||
joints[jointName] = position;
|
||
}
|
||
}
|
||
|
||
this.robotStatus.joints = joints;
|
||
this.robotStatus.connected = true;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Update battery status from ROS message
|
||
*/
|
||
private updateBatteryStatus(msg: Record<string, unknown>): void {
|
||
if (typeof msg.percentage === "number") {
|
||
this.robotStatus.battery = msg.percentage;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Update sensor data from ROS message
|
||
*/
|
||
private updateSensorData(topic: string, msg: Record<string, unknown>): void {
|
||
this.robotStatus.sensors[topic] = msg;
|
||
}
|
||
|
||
/**
|
||
* Execute action with plugin configuration
|
||
*/
|
||
private async executeWithConfig(
|
||
config: {
|
||
topic: string;
|
||
messageType: string;
|
||
payloadMapping: {
|
||
type: string;
|
||
payload?: Record<string, unknown>;
|
||
transformFn?: string;
|
||
service?: string;
|
||
args?: Record<string, unknown>;
|
||
};
|
||
},
|
||
parameters: Record<string, unknown>,
|
||
): Promise<void> {
|
||
// Service-call actions — no topic publish involved
|
||
if (config.payloadMapping.type === "service") {
|
||
const service = config.payloadMapping.service ?? config.topic;
|
||
const args = config.payloadMapping.args
|
||
? this.buildTemplatePayload(config.payloadMapping.args, parameters)
|
||
: parameters;
|
||
await this.callService(service, args);
|
||
return;
|
||
}
|
||
|
||
let msg: Record<string, unknown>;
|
||
|
||
if (
|
||
(config.payloadMapping.type === "template" ||
|
||
config.payloadMapping.type === "static") &&
|
||
config.payloadMapping.payload
|
||
) {
|
||
msg = this.buildTemplatePayload(config.payloadMapping.payload, parameters);
|
||
} else if (config.payloadMapping.transformFn) {
|
||
msg = this.applyTransformFunction(config.payloadMapping.transformFn, parameters);
|
||
} else {
|
||
msg = parameters;
|
||
}
|
||
|
||
// Delegate gesture_sequence payloads to the animation handler
|
||
if ((msg as { type?: string }).type === "gesture_sequence") {
|
||
const movements = (msg as { movements?: AnimationMovement[] }).movements;
|
||
if (!movements?.length) {
|
||
console.warn("[WizardROS] gesture_sequence payload has no movements");
|
||
return;
|
||
}
|
||
console.log(`[WizardROS] Delegating to animation handler (${movements.length} frames)`);
|
||
await this.executeAnimationSequence(movements);
|
||
return;
|
||
}
|
||
|
||
this.publish(config.topic, config.messageType, msg);
|
||
|
||
// Wait for action completion based on topic type
|
||
if (config.topic === "/speech") {
|
||
// Estimate speech duration based on text content
|
||
const text =
|
||
typeof msg === "object" && msg !== null && "data" in msg
|
||
? String((msg as any).data || "")
|
||
: JSON.stringify(msg);
|
||
const wordCount = text.split(/\s+/).filter(Boolean).length;
|
||
// Emotion markup adds overhead: ~200ms per word base + emotion animation time
|
||
const emotionOverhead = 1500; // Animation prep time
|
||
const duration = emotionOverhead + Math.max(1000, wordCount * 300);
|
||
console.log(
|
||
`[WizardROS] Speech action estimated duration: ${duration}ms (${wordCount} words)`,
|
||
);
|
||
await new Promise((resolve) => setTimeout(resolve, duration));
|
||
} else {
|
||
// Short delay for non-speech actions
|
||
await new Promise((resolve) => setTimeout(resolve, 500));
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Execute built-in robot actions (robot-agnostic defaults)
|
||
* These are generic actions that work with standard ROS topics
|
||
*/
|
||
private async executeBuiltinAction(
|
||
actionId: string,
|
||
parameters: Record<string, unknown>,
|
||
): Promise<void> {
|
||
switch (actionId) {
|
||
case "say_text":
|
||
case "say_with_emotion":
|
||
const text = String(parameters.text || "Hello");
|
||
this.publish("/speech", "std_msgs/String", { data: text });
|
||
const wordCount = text.split(/\s+/).filter(Boolean).length;
|
||
const emotion = String(parameters.emotion || "neutral");
|
||
const emotionOverhead = 1500;
|
||
const duration = emotionOverhead + Math.max(1000, wordCount * 300);
|
||
console.log(
|
||
`[WizardROS] Speech action (${actionId}) estimated: ${duration}ms`,
|
||
);
|
||
await new Promise((resolve) => setTimeout(resolve, duration));
|
||
break;
|
||
|
||
case "wave_goodbye":
|
||
const waveText = String(parameters.text || "Goodbye");
|
||
this.publish("/speech", "std_msgs/String", { data: waveText });
|
||
await new Promise((resolve) => setTimeout(resolve, 3000));
|
||
break;
|
||
|
||
case "walk_forward":
|
||
this.publish("/cmd_vel", "geometry_msgs/Twist", {
|
||
linear: { x: Number(parameters.speed) || 0.1, y: 0, z: 0 },
|
||
angular: { x: 0, y: 0, z: 0 },
|
||
});
|
||
await new Promise((resolve) => setTimeout(resolve, 500));
|
||
break;
|
||
|
||
case "walk_backward":
|
||
this.publish("/cmd_vel", "geometry_msgs/Twist", {
|
||
linear: { x: -(Number(parameters.speed) || 0.1), y: 0, z: 0 },
|
||
angular: { x: 0, y: 0, z: 0 },
|
||
});
|
||
await new Promise((resolve) => setTimeout(resolve, 500));
|
||
break;
|
||
|
||
case "turn_left":
|
||
this.publish("/cmd_vel", "geometry_msgs/Twist", {
|
||
linear: { x: 0, y: 0, z: 0 },
|
||
angular: { x: 0, y: 0, z: -(Number(parameters.speed) || 0.3) },
|
||
});
|
||
await new Promise((resolve) => setTimeout(resolve, 500));
|
||
break;
|
||
|
||
case "turn_right":
|
||
this.publish("/cmd_vel", "geometry_msgs/Twist", {
|
||
linear: { x: 0, y: 0, z: 0 },
|
||
angular: { x: 0, y: 0, z: Number(parameters.speed) || 0.3 },
|
||
});
|
||
await new Promise((resolve) => setTimeout(resolve, 500));
|
||
break;
|
||
|
||
case "move_head":
|
||
case "turn_head":
|
||
this.publish(
|
||
"/joint_angles",
|
||
"naoqi_bridge_msgs/msg/JointAnglesWithSpeed",
|
||
{
|
||
joint_names: ["HeadYaw", "HeadPitch"],
|
||
joint_angles: [
|
||
Number(parameters.yaw) || 0,
|
||
Number(parameters.pitch) || 0,
|
||
],
|
||
speed: Number(parameters.speed) || 0.3,
|
||
},
|
||
);
|
||
await new Promise((resolve) => setTimeout(resolve, 1000));
|
||
break;
|
||
|
||
case "move_arm": {
|
||
const arm = String(parameters.arm || "right");
|
||
const prefix = arm.toLowerCase() === "left" ? "L" : "R";
|
||
this.publish(
|
||
"/joint_angles",
|
||
"naoqi_bridge_msgs/msg/JointAnglesWithSpeed",
|
||
{
|
||
joint_names: [
|
||
`${prefix}ShoulderPitch`,
|
||
`${prefix}ShoulderRoll`,
|
||
`${prefix}ElbowYaw`,
|
||
`${prefix}ElbowRoll`,
|
||
],
|
||
joint_angles: [
|
||
Number(parameters.shoulder_pitch) || 0,
|
||
Number(parameters.shoulder_roll) || 0,
|
||
Number(parameters.elbow_yaw) || 0,
|
||
Number(parameters.elbow_roll) || 0,
|
||
],
|
||
speed: Number(parameters.speed) || 0.3,
|
||
},
|
||
);
|
||
await new Promise((resolve) => setTimeout(resolve, 1000));
|
||
break;
|
||
}
|
||
|
||
case "wake_up":
|
||
// Try known naoqi_driver2 service names in order
|
||
for (const svc of [
|
||
"/naoqi_driver/ALMotion/wakeUp",
|
||
"/naoqi_driver/motion/wake_up",
|
||
"/motion/wake_up",
|
||
]) {
|
||
try {
|
||
await this.callService(svc, {});
|
||
console.log(`[WizardROS] wake_up succeeded via ${svc}`);
|
||
break;
|
||
} catch {
|
||
// try next
|
||
}
|
||
}
|
||
break;
|
||
|
||
case "rest":
|
||
for (const svc of [
|
||
"/naoqi_driver/ALMotion/rest",
|
||
"/naoqi_driver/motion/rest",
|
||
"/motion/rest",
|
||
]) {
|
||
try {
|
||
await this.callService(svc, {});
|
||
console.log(`[WizardROS] rest succeeded via ${svc}`);
|
||
break;
|
||
} catch {
|
||
// try next
|
||
}
|
||
}
|
||
break;
|
||
|
||
case "emergency_stop":
|
||
this.publish("/cmd_vel", "geometry_msgs/Twist", {
|
||
linear: { x: 0, y: 0, z: 0 },
|
||
angular: { x: 0, y: 0, z: 0 },
|
||
});
|
||
break;
|
||
|
||
case "play_animation_bow":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/BowShort_1" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
case "play_animation_hey":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/Hey_1" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
case "play_animation_show_floor":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/ShowFloor_1" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
case "play_animation_show_sole":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/ShowSole_1" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
case "play_animation_enthusiastic":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/Enthusiastic_4" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
case "play_animation_think":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/Think_1" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
case "play_animation_yes":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/Yes_1" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
case "play_animation_no":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/No_3" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
case "play_animation_idontknow":
|
||
this.publish("/animation", "std_msgs/String", { data: "animations/Stand/Gestures/IDontKnow_1" });
|
||
await new Promise((resolve) => setTimeout(resolve, 2000));
|
||
break;
|
||
|
||
default:
|
||
throw new Error(
|
||
`Unknown action: ${actionId}. Define this action in your robot plugin.`,
|
||
);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Call a ROS service
|
||
*/
|
||
async callService(
|
||
service: string,
|
||
args: Record<string, unknown> = {},
|
||
): Promise<ServiceResponse> {
|
||
if (!this.isConnected) {
|
||
throw new Error("Not connected to ROS bridge");
|
||
}
|
||
|
||
// Simulation mode - return mock responses
|
||
if (this.simulationMode) {
|
||
console.log(`[WizardROS] SIMULATION MODE - Service call: ${service}`, args);
|
||
|
||
const mockResponses: Record<string, ServiceResponse> = {
|
||
"/naoqi_driver/get_robot_info": {
|
||
result: true,
|
||
values: {
|
||
robotName: "MOCK-NAO6",
|
||
robotVersion: "6.0",
|
||
bodyType: "nao",
|
||
},
|
||
},
|
||
"/naoqi_driver/get_joint_names": {
|
||
result: true,
|
||
values: {
|
||
joint_names: [
|
||
"HeadYaw", "HeadPitch", "LShoulderPitch", "LShoulderRoll",
|
||
"LElbowYaw", "LElbowRoll", "LWristYaw", "LHand",
|
||
"RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll",
|
||
"RWristYaw", "RHand", "LHipYawPitch", "LHipRoll",
|
||
"LHipPitch", "LKneePitch", "LAnklePitch", "LAnkleRoll",
|
||
"RHipYawPitch", "RHipRoll", "RHipPitch", "RKneePitch",
|
||
"RAnklePitch", "RAnkleRoll",
|
||
],
|
||
},
|
||
},
|
||
"/naoqi_driver/get_position": {
|
||
result: true,
|
||
values: this.robotStatus.position,
|
||
},
|
||
};
|
||
|
||
return mockResponses[service] || { result: true };
|
||
}
|
||
|
||
const id = `call_${this.messageId++}`;
|
||
|
||
return new Promise((resolve, reject) => {
|
||
const handleResponse = (message: RosMessage) => {
|
||
if (message.op === "service_response" && message.id === id) {
|
||
this.off("message", handleResponse);
|
||
if (message.result === false) {
|
||
resolve({
|
||
result: false,
|
||
error: String(message.values || "Service call failed"),
|
||
});
|
||
} else {
|
||
resolve({ result: true, values: message.values });
|
||
}
|
||
}
|
||
};
|
||
|
||
this.on("message", handleResponse);
|
||
|
||
this.send({
|
||
op: "call_service",
|
||
service,
|
||
args,
|
||
id,
|
||
});
|
||
|
||
setTimeout(() => {
|
||
this.off("message", handleResponse);
|
||
reject(new Error("Service call timed out"));
|
||
}, 5000);
|
||
});
|
||
}
|
||
|
||
/**
|
||
* Set Autonomous Life state with fallbacks
|
||
*/
|
||
async setAutonomousLife(enabled: boolean): Promise<boolean> {
|
||
const desiredState = enabled ? "interactive" : "disabled";
|
||
|
||
// List of services to try in order
|
||
const attempts = [
|
||
// Standard NaoQi Bridge pattern
|
||
{
|
||
service: "/naoqi_driver/ALAutonomousLife/setState",
|
||
args: { state: desiredState },
|
||
},
|
||
{
|
||
service: "/naoqi_driver/ALAutonomousLife/set_state",
|
||
args: { state: desiredState },
|
||
},
|
||
// Direct module mapping
|
||
{
|
||
service: "/ALAutonomousLife/setState",
|
||
args: { state: desiredState },
|
||
},
|
||
// Shortcuts/Aliases
|
||
{
|
||
service: "/naoqi_driver/set_autonomous_life",
|
||
args: { state: desiredState },
|
||
},
|
||
{
|
||
service: "/autonomous_life/set_state",
|
||
args: { state: desiredState },
|
||
},
|
||
// Fallback: Enable/Disable topics/services
|
||
{
|
||
service: enabled ? "/life/enable" : "/life/disable",
|
||
args: {},
|
||
},
|
||
// Last resort: Generic proxy call (if available)
|
||
{
|
||
service: "/naoqi_driver/function_call",
|
||
args: {
|
||
service: "ALAutonomousLife",
|
||
function: "setState",
|
||
args: [desiredState],
|
||
},
|
||
},
|
||
];
|
||
|
||
console.log(`[WizardROS] Setting Autonomous Life to: ${desiredState}`);
|
||
|
||
for (const attempt of attempts) {
|
||
try {
|
||
console.log(`[WizardROS] Trying service: ${attempt.service}`);
|
||
const response = await this.callService(attempt.service, attempt.args);
|
||
|
||
// If the service call didn't timeout (it resolved), check result
|
||
if (response.result) {
|
||
console.log(`[WizardROS] Success via ${attempt.service}`);
|
||
return true;
|
||
} else {
|
||
// Resolved but failed? (e.g. internal error)
|
||
console.warn(
|
||
`[WizardROS] Service ${attempt.service} returned false result:`,
|
||
response.error,
|
||
);
|
||
}
|
||
} catch (error) {
|
||
// Service call failed or timed out
|
||
console.warn(
|
||
`[WizardROS] Service ${attempt.service} failed/timeout:`,
|
||
error,
|
||
);
|
||
}
|
||
}
|
||
|
||
console.error("[WizardROS] All Autonomous Life service attempts failed.");
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* Build template-based payload
|
||
*/
|
||
private buildTemplatePayload(
|
||
template: Record<string, unknown>,
|
||
parameters: Record<string, unknown>,
|
||
): Record<string, unknown> {
|
||
const result: Record<string, unknown> = {};
|
||
|
||
for (const [key, value] of Object.entries(template)) {
|
||
if (typeof value === "string" && value.includes("{{")) {
|
||
// Template substitution
|
||
let substituted = value;
|
||
for (const [paramKey, paramValue] of Object.entries(parameters)) {
|
||
const placeholder = `{{${paramKey}}}`;
|
||
substituted = substituted.replace(
|
||
new RegExp(placeholder.replace(/[.*+?^${}()|[\]\\]/g, "\\$&"), "g"),
|
||
String(paramValue ?? ""),
|
||
);
|
||
}
|
||
result[key] = isNaN(Number(substituted))
|
||
? substituted
|
||
: Number(substituted);
|
||
} else if (typeof value === "object" && value !== null) {
|
||
result[key] = this.buildTemplatePayload(
|
||
value as Record<string, unknown>,
|
||
parameters,
|
||
);
|
||
} else {
|
||
result[key] = value;
|
||
}
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
/**
|
||
* Apply transform function for NAO6 actions
|
||
*/
|
||
private applyTransformFunction(
|
||
transformFn: string,
|
||
parameters: Record<string, unknown>,
|
||
): Record<string, unknown> {
|
||
switch (transformFn) {
|
||
case "naoVelocityTransform":
|
||
return {
|
||
linear: {
|
||
x: Number(parameters.linear) || 0,
|
||
y: 0,
|
||
z: 0,
|
||
},
|
||
angular: {
|
||
x: 0,
|
||
y: 0,
|
||
z: Number(parameters.angular) || 0,
|
||
},
|
||
};
|
||
|
||
case "naoSpeechTransform":
|
||
case "transformToStringMessage":
|
||
return {
|
||
data: String(parameters.text || "Hello"),
|
||
};
|
||
|
||
case "naoHeadTransform":
|
||
case "transformToHeadMovement":
|
||
return {
|
||
joint_names: ["HeadYaw", "HeadPitch"],
|
||
joint_angles: [
|
||
Number(parameters.yaw) || 0,
|
||
Number(parameters.pitch) || 0,
|
||
],
|
||
speed: Number(parameters.speed) || 0.3,
|
||
};
|
||
|
||
case "transformToJointAngles":
|
||
return {
|
||
joint_names: [String(parameters.joint_name || "HeadYaw")],
|
||
joint_angles: [Number(parameters.angle) || 0],
|
||
speed: Number(parameters.speed) || 0.2,
|
||
};
|
||
|
||
case "transformToEmotionalSpeech":
|
||
return this.transformToEmotionalSpeech(parameters);
|
||
|
||
case "transformToWaveGoodbye":
|
||
return this.transformToWaveGoodbye(parameters);
|
||
|
||
case "transformToAnimation":
|
||
return this.transformToAnimation(parameters);
|
||
|
||
case "transformToGestureSequence":
|
||
return this.transformToGestureSequence(parameters);
|
||
|
||
case "transformToWaveGoodbye":
|
||
return this.transformToWaveGoodbye(parameters);
|
||
|
||
default:
|
||
console.warn(`Unknown transform function: ${transformFn}`);
|
||
return parameters;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Transform parameters for emotional speech
|
||
* NAOqi markup: \rspd=<speed>\<text>
|
||
* Using pure speech modifiers without animations to avoid sound effects
|
||
*/
|
||
private transformToEmotionalSpeech(parameters: Record<string, unknown>): {
|
||
data: string;
|
||
} {
|
||
const text = String(parameters.text || "Hello");
|
||
const emotion = String(parameters.emotion || "neutral");
|
||
const speed = Number(parameters.speed || 1.0);
|
||
const speedPercent = Math.round(speed * 100);
|
||
|
||
let markedText = text;
|
||
|
||
switch (emotion) {
|
||
case "happy":
|
||
markedText = `\\\\rspd=120\\\\vct=100\\\\ ${text}`;
|
||
break;
|
||
case "excited":
|
||
markedText = `\\\\rspd=140\\\\vct=110\\\\ ${text}`;
|
||
break;
|
||
case "sad":
|
||
markedText = `\\\\rspd=80\\\\vct=80\\\\ ${text}`;
|
||
break;
|
||
case "calm":
|
||
markedText = `\\\\rspd=90\\\\vct=90\\\\ ${text}`;
|
||
break;
|
||
case "neutral":
|
||
default:
|
||
markedText = `\\\\rspd=${speedPercent}\\\\vct=100\\\\ ${text}`;
|
||
break;
|
||
}
|
||
|
||
return { data: markedText };
|
||
}
|
||
|
||
/**
|
||
* Transform for wave goodbye - speech without animation sound
|
||
*/
|
||
private transformToWaveGoodbye(parameters: Record<string, unknown>): {
|
||
data: string;
|
||
} {
|
||
const text = String(parameters.text || "Goodbye!");
|
||
const markedText = `\\\\rspd=110\\\\ ${text}`;
|
||
return { data: markedText };
|
||
}
|
||
|
||
/**
|
||
* Transform for playing animations
|
||
*/
|
||
private transformToAnimation(parameters: Record<string, unknown>): {
|
||
data: string;
|
||
} {
|
||
const animation = String(parameters.animation || "Hey_1");
|
||
const markedText = `^start(animations/Stand/Gestures/${animation})`;
|
||
return { data: markedText };
|
||
}
|
||
|
||
/**
|
||
* Transform for gesture sequences - sends multiple joint angle movements
|
||
* Parameters: movements = [{joints: string[], angles: number[], duration: number}, ...]
|
||
*/
|
||
private transformToGestureSequence(parameters: Record<string, unknown>): {
|
||
type: string;
|
||
movements: Array<{
|
||
joint_names: string[];
|
||
joint_angles: number[];
|
||
speed: number;
|
||
}>;
|
||
} {
|
||
const movements = parameters.movements as Array<{
|
||
joints?: string[];
|
||
angles?: number[];
|
||
duration?: number;
|
||
speed?: number;
|
||
}>;
|
||
|
||
if (!Array.isArray(movements)) {
|
||
return { type: "gesture_sequence", movements: [] };
|
||
}
|
||
|
||
const parsedMovements = movements.map((m, index) => ({
|
||
joint_names: m.joints || [],
|
||
joint_angles: m.angles || [],
|
||
speed: m.speed || (m.duration ? 1 / (m.duration / 1000) : 0.3),
|
||
}));
|
||
|
||
return { type: "gesture_sequence", movements: parsedMovements };
|
||
}
|
||
|
||
/**
|
||
* Schedule reconnection attempt
|
||
*/
|
||
private scheduleReconnect(): void {
|
||
if (this.reconnectTimer) return;
|
||
|
||
this.connectionAttempts++;
|
||
console.log(
|
||
`[WizardROS] Scheduling reconnect attempt ${this.connectionAttempts}/${this.maxReconnectAttempts}`,
|
||
);
|
||
|
||
this.reconnectTimer = setTimeout(async () => {
|
||
this.reconnectTimer = null;
|
||
try {
|
||
await this.connect();
|
||
} catch (error) {
|
||
console.warn("[WizardROS] Reconnect failed:", error);
|
||
if (this.connectionAttempts < this.maxReconnectAttempts) {
|
||
this.scheduleReconnect();
|
||
} else {
|
||
this.emit("max_reconnects_reached");
|
||
}
|
||
}
|
||
}, this.reconnectInterval);
|
||
}
|
||
|
||
/**
|
||
* Clear reconnect timer
|
||
*/
|
||
private clearReconnectTimer(): void {
|
||
if (this.reconnectTimer) {
|
||
clearTimeout(this.reconnectTimer);
|
||
this.reconnectTimer = null;
|
||
}
|
||
}
|
||
}
|
||
|
||
// Global service instance
|
||
let wizardRosService: WizardRosService | null = null;
|
||
let isCreatingInstance = false;
|
||
|
||
/**
|
||
* Get or create the global wizard ROS service (true singleton)
|
||
*/
|
||
export function getWizardRosService(simulationMode?: boolean): WizardRosService {
|
||
// Prevent multiple instances during creation
|
||
if (isCreatingInstance && !wizardRosService) {
|
||
throw new Error("WizardRosService is being initialized, please wait");
|
||
}
|
||
|
||
if (!wizardRosService) {
|
||
isCreatingInstance = true;
|
||
try {
|
||
const url = typeof window !== "undefined"
|
||
? (process.env.NEXT_PUBLIC_ROS_BRIDGE_URL || "ws://localhost:9090")
|
||
: "ws://localhost:9090";
|
||
wizardRosService = new WizardRosService(url, simulationMode);
|
||
} finally {
|
||
isCreatingInstance = false;
|
||
}
|
||
}
|
||
return wizardRosService;
|
||
}
|
||
|
||
/**
|
||
* Initialize wizard ROS service with connection
|
||
*/
|
||
export async function initWizardRosService(simulationMode?: boolean): Promise<WizardRosService> {
|
||
const service = getWizardRosService(simulationMode);
|
||
|
||
if (simulationMode !== undefined) {
|
||
service.setSimulationMode(simulationMode);
|
||
}
|
||
|
||
if (!service.getConnectionStatus()) {
|
||
await service.connect();
|
||
}
|
||
|
||
return service;
|
||
}
|
||
|
||
/**
|
||
* Reset the global wizard ROS service (useful for testing or reinitializing)
|
||
*/
|
||
export function resetWizardRosService(): void {
|
||
if (wizardRosService) {
|
||
wizardRosService.disconnect();
|
||
wizardRosService = null;
|
||
}
|
||
}
|