mirror of
https://github.com/soconnor0919/hristudio.git
synced 2025-12-12 07:04:44 -05:00
fix: migrate wizard from polling to WebSocket and fix duplicate ROS connections
- Removed non-functional trial WebSocket (no server exists) - Kept ROS WebSocket for robot control via useWizardRos - Fixed duplicate ROS connections by passing connection as props - WizardMonitoringPanel now receives ROS connection from parent - Trial status uses reliable tRPC polling (5-15s intervals) - Updated connection badges to show 'ROS Connected/Offline' - Added loading overlay with fade-in to designer - Fixed hash computation to include parameter values - Fixed incremental hash caching for parameter changes Fixes: - WebSocket connection errors eliminated - Connect button now works properly - No more conflicting duplicate connections - Accurate connection status display
This commit is contained in:
671
src/lib/ros/wizard-ros-service.ts
Normal file
671
src/lib/ros/wizard-ros-service.ts
Normal file
@@ -0,0 +1,671 @@
|
||||
"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 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
|
||||
// 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") {
|
||||
super();
|
||||
this.url = url;
|
||||
}
|
||||
|
||||
/**
|
||||
* Connect to ROS bridge WebSocket
|
||||
*/
|
||||
async connect(): Promise<void> {
|
||||
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.error("[WizardROS] WebSocket error:", error);
|
||||
clearTimeout(connectionTimeout);
|
||||
this.isConnecting = false;
|
||||
this.emit("error", error);
|
||||
reject(error);
|
||||
};
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Disconnect from ROS bridge
|
||||
*/
|
||||
disconnect(): void {
|
||||
this.clearReconnectTimer();
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if connected to ROS bridge
|
||||
*/
|
||||
getConnectionStatus(): boolean {
|
||||
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");
|
||||
}
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
/**
|
||||
* Subscribe to robot sensor topics
|
||||
*/
|
||||
private subscribeToRobotTopics(): void {
|
||||
const topics = [
|
||||
{ topic: "/joint_states", type: "sensor_msgs/JointState" },
|
||||
{
|
||||
topic: "/naoqi_driver/battery",
|
||||
type: "naoqi_bridge_msgs/BatteryState",
|
||||
},
|
||||
{ topic: "/naoqi_driver/bumper", type: "naoqi_bridge_msgs/Bumper" },
|
||||
{
|
||||
topic: "/naoqi_driver/hand_touch",
|
||||
type: "naoqi_bridge_msgs/HandTouch",
|
||||
},
|
||||
{
|
||||
topic: "/naoqi_driver/head_touch",
|
||||
type: "naoqi_bridge_msgs/HeadTouch",
|
||||
},
|
||||
{ topic: "/naoqi_driver/sonar/left", type: "sensor_msgs/Range" },
|
||||
{ topic: "/naoqi_driver/sonar/right", type: "sensor_msgs/Range" },
|
||||
];
|
||||
|
||||
topics.forEach(({ topic, type }) => {
|
||||
this.subscribe(topic, type);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Publish message to ROS topic
|
||||
*/
|
||||
private publish(
|
||||
topic: string,
|
||||
messageType: string,
|
||||
msg: Record<string, unknown>,
|
||||
): void {
|
||||
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 "/naoqi_driver/bumper":
|
||||
case "/naoqi_driver/hand_touch":
|
||||
case "/naoqi_driver/head_touch":
|
||||
case "/naoqi_driver/sonar/left":
|
||||
case "/naoqi_driver/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;
|
||||
};
|
||||
},
|
||||
parameters: Record<string, unknown>,
|
||||
): Promise<void> {
|
||||
let msg: Record<string, unknown>;
|
||||
|
||||
if (
|
||||
config.payloadMapping.type === "template" &&
|
||||
config.payloadMapping.payload
|
||||
) {
|
||||
// Template-based payload construction
|
||||
msg = this.buildTemplatePayload(
|
||||
config.payloadMapping.payload,
|
||||
parameters,
|
||||
);
|
||||
} else if (config.payloadMapping.transformFn) {
|
||||
// Custom transform function
|
||||
msg = this.applyTransformFunction(
|
||||
config.payloadMapping.transformFn,
|
||||
parameters,
|
||||
);
|
||||
} else {
|
||||
// Direct parameter mapping
|
||||
msg = parameters;
|
||||
}
|
||||
|
||||
this.publish(config.topic, config.messageType, msg);
|
||||
|
||||
// Wait for action completion (simple delay for now)
|
||||
await new Promise((resolve) => setTimeout(resolve, 100));
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute built-in robot actions
|
||||
*/
|
||||
private async executeBuiltinAction(
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
): Promise<void> {
|
||||
switch (actionId) {
|
||||
case "say_text":
|
||||
this.publish("/speech", "std_msgs/String", {
|
||||
data: parameters.text || "Hello",
|
||||
});
|
||||
break;
|
||||
|
||||
case "walk_forward":
|
||||
case "walk_backward":
|
||||
case "turn_left":
|
||||
case "turn_right":
|
||||
this.executeMovementAction(actionId, parameters);
|
||||
break;
|
||||
|
||||
case "turn_head":
|
||||
this.executeTurnHead(parameters);
|
||||
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;
|
||||
|
||||
default:
|
||||
throw new Error(`Unknown action: ${actionId}`);
|
||||
}
|
||||
|
||||
// Wait for action completion
|
||||
await new Promise((resolve) => setTimeout(resolve, 100));
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute movement actions
|
||||
*/
|
||||
private executeMovementAction(
|
||||
actionId: string,
|
||||
parameters: Record<string, unknown>,
|
||||
): void {
|
||||
let linear = { x: 0, y: 0, z: 0 };
|
||||
let angular = { x: 0, y: 0, z: 0 };
|
||||
|
||||
const speed = Number(parameters.speed) || 0.1;
|
||||
|
||||
switch (actionId) {
|
||||
case "walk_forward":
|
||||
linear.x = speed;
|
||||
break;
|
||||
case "walk_backward":
|
||||
linear.x = -speed;
|
||||
break;
|
||||
case "turn_left":
|
||||
angular.z = speed;
|
||||
break;
|
||||
case "turn_right":
|
||||
angular.z = -speed;
|
||||
break;
|
||||
}
|
||||
|
||||
this.publish("/cmd_vel", "geometry_msgs/Twist", { linear, angular });
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute head turn action
|
||||
*/
|
||||
private executeTurnHead(parameters: Record<string, unknown>): void {
|
||||
const yaw = Number(parameters.yaw) || 0;
|
||||
const pitch = Number(parameters.pitch) || 0;
|
||||
const speed = Number(parameters.speed) || 0.3;
|
||||
|
||||
this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
|
||||
joint_names: ["HeadYaw", "HeadPitch"],
|
||||
joint_angles: [yaw, pitch],
|
||||
speed: speed,
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 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":
|
||||
return {
|
||||
data: String(parameters.text || "Hello"),
|
||||
};
|
||||
|
||||
case "naoHeadTransform":
|
||||
return {
|
||||
joint_names: ["HeadYaw", "HeadPitch"],
|
||||
joint_angles: [
|
||||
Number(parameters.yaw) || 0,
|
||||
Number(parameters.pitch) || 0,
|
||||
],
|
||||
speed: Number(parameters.speed) || 0.3,
|
||||
};
|
||||
|
||||
default:
|
||||
console.warn(`Unknown transform function: ${transformFn}`);
|
||||
return parameters;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.error("[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(): WizardRosService {
|
||||
// Prevent multiple instances during creation
|
||||
if (isCreatingInstance && !wizardRosService) {
|
||||
throw new Error("WizardRosService is being initialized, please wait");
|
||||
}
|
||||
|
||||
if (!wizardRosService) {
|
||||
isCreatingInstance = true;
|
||||
try {
|
||||
wizardRosService = new WizardRosService();
|
||||
} finally {
|
||||
isCreatingInstance = false;
|
||||
}
|
||||
}
|
||||
return wizardRosService;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize wizard ROS service with connection
|
||||
*/
|
||||
export async function initWizardRosService(): Promise<WizardRosService> {
|
||||
const service = getWizardRosService();
|
||||
|
||||
if (!service.getConnectionStatus()) {
|
||||
await service.connect();
|
||||
}
|
||||
|
||||
return service;
|
||||
}
|
||||
Reference in New Issue
Block a user