Add ROS2 bridge

src/lib/nao6-transforms.ts

@@ -0,0 +1,321 @@
+/**
+ * NAO6 ROS2 Transform Functions
+ *
+ * This module provides transform functions for converting HRIStudio action parameters
+ * to ROS2 message formats for the NAO6 robot via naoqi_driver2.
+ */
+
+/**
+ * Transform velocity parameters to geometry_msgs/msg/Twist
+ */
+export function transformToTwist(
+  params: Record<string, unknown>,
+): Record<string, unknown> {
+  return {
+    linear: {
+      x: params.linear ?? 0.0,
+      y: 0.0,
+      z: 0.0,
+    },
+    angular: {
+      x: 0.0,
+      y: 0.0,
+      z: params.angular ?? 0.0,
+    },
+  };
+}
+
+/**
+ * Transform text to std_msgs/msg/String
+ */
+export function transformToStringMessage(
+  params: Record<string, unknown>,
+): Record<string, unknown> {
+  return {
+    data: params.text ?? "",
+  };
+}
+
+/**
+ * Transform joint parameters to naoqi_bridge_msgs/msg/JointAnglesWithSpeed
+ */
+export function transformToJointAngles(
+  params: Record<string, unknown>,
+): Record<string, unknown> {
+  return {
+    joint_names: [params.joint_name as string],
+    joint_angles: [params.angle as number],
+    speed: params.speed ?? 0.2,
+  };
+}
+
+/**
+ * Transform head movement parameters to naoqi_bridge_msgs/msg/JointAnglesWithSpeed
+ */
+export function transformToHeadMovement(
+  params: Record<string, unknown>,
+): Record<string, unknown> {
+  return {
+    joint_names: ["HeadYaw", "HeadPitch"],
+    joint_angles: [params.yaw as number, params.pitch as number],
+    speed: params.speed ?? 0.3,
+  };
+}
+
+/**
+ * Get camera image - returns subscription request
+ */
+export function getCameraImage(
+  params: Record<string, unknown>,
+): Record<string, unknown> {
+  const camera = params.camera as string;
+  const topic =
+    camera === "front" ? "/camera/front/image_raw" : "/camera/bottom/image_raw";
+
+  return {
+    subscribe: true,
+    topic,
+    messageType: "sensor_msgs/msg/Image",
+    once: true,
+  };
+}
+
+/**
+ * Get joint states - returns subscription request
+ */
+export function getJointStates(
+  _params: Record<string, unknown>,
+): Record<string, unknown> {
+  return {
+    subscribe: true,
+    topic: "/joint_states",
+    messageType: "sensor_msgs/msg/JointState",
+    once: true,
+  };
+}
+
+/**
+ * Get IMU data - returns subscription request
+ */
+export function getImuData(
+  _params: Record<string, unknown>,
+): Record<string, unknown> {
+  return {
+    subscribe: true,
+    topic: "/imu/torso",
+    messageType: "sensor_msgs/msg/Imu",
+    once: true,
+  };
+}
+
+/**
+ * Get bumper status - returns subscription request
+ */
+export function getBumperStatus(
+  _params: Record<string, unknown>,
+): Record<string, unknown> {
+  return {
+    subscribe: true,
+    topic: "/bumper",
+    messageType: "naoqi_bridge_msgs/msg/Bumper",
+    once: true,
+  };
+}
+
+/**
+ * Get touch sensors - returns subscription request
+ */
+export function getTouchSensors(
+  params: Record<string, unknown>,
+): Record<string, unknown> {
+  const sensorType = params.sensor_type as string;
+  const topic = sensorType === "hand" ? "/hand_touch" : "/head_touch";
+  const messageType =
+    sensorType === "hand"
+      ? "naoqi_bridge_msgs/msg/HandTouch"
+      : "naoqi_bridge_msgs/msg/HeadTouch";
+
+  return {
+    subscribe: true,
+    topic,
+    messageType,
+    once: true,
+  };
+}
+
+/**
+ * Get sonar range - returns subscription request
+ */
+export function getSonarRange(
+  params: Record<string, unknown>,
+): Record<string, unknown> {
+  const sensor = params.sensor as string;
+  let topic: string;
+
+  if (sensor === "left") {
+    topic = "/sonar/left";
+  } else if (sensor === "right") {
+    topic = "/sonar/right";
+  } else {
+    // For "both", we'll default to left and let the wizard interface handle multiple calls
+    topic = "/sonar/left";
+  }
+
+  return {
+    subscribe: true,
+    topic,
+    messageType: "sensor_msgs/msg/Range",
+    once: true,
+  };
+}
+
+/**
+ * Get robot info - returns subscription request
+ */
+export function getRobotInfo(
+  _params: Record<string, unknown>,
+): Record<string, unknown> {
+  return {
+    subscribe: true,
+    topic: "/info",
+    messageType: "naoqi_bridge_msgs/msg/RobotInfo",
+    once: true,
+  };
+}
+
+/**
+ * NAO6-specific joint limits for safety
+ */
+export const NAO6_JOINT_LIMITS = {
+  HeadYaw: { min: -2.0857, max: 2.0857 },
+  HeadPitch: { min: -0.672, max: 0.5149 },
+  LShoulderPitch: { min: -2.0857, max: 2.0857 },
+  LShoulderRoll: { min: -0.3142, max: 1.3265 },
+  LElbowYaw: { min: -2.0857, max: 2.0857 },
+  LElbowRoll: { min: -1.5446, max: -0.0349 },
+  LWristYaw: { min: -1.8238, max: 1.8238 },
+  RShoulderPitch: { min: -2.0857, max: 2.0857 },
+  RShoulderRoll: { min: -1.3265, max: 0.3142 },
+  RElbowYaw: { min: -2.0857, max: 2.0857 },
+  RElbowRoll: { min: 0.0349, max: 1.5446 },
+  RWristYaw: { min: -1.8238, max: 1.8238 },
+  LHipYawPitch: { min: -1.1453, max: 0.7408 },
+  LHipRoll: { min: -0.3793, max: 0.79 },
+  LHipPitch: { min: -1.7732, max: 0.484 },
+  LKneePitch: { min: -0.0923, max: 2.1121 },
+  LAnklePitch: { min: -1.1894, max: 0.9228 },
+  LAnkleRoll: { min: -0.3976, max: 0.769 },
+  RHipRoll: { min: -0.79, max: 0.3793 },
+  RHipPitch: { min: -1.7732, max: 0.484 },
+  RKneePitch: { min: -0.0923, max: 2.1121 },
+  RAnklePitch: { min: -1.1894, max: 0.9228 },
+  RAnkleRoll: { min: -0.769, max: 0.3976 },
+};
+
+/**
+ * Validate joint angle against NAO6 limits
+ */
+export function validateJointAngle(jointName: string, angle: number): boolean {
+  const limits = NAO6_JOINT_LIMITS[jointName as keyof typeof NAO6_JOINT_LIMITS];
+  if (!limits) {
+    console.warn(`Unknown joint: ${jointName}`);
+    return false;
+  }
+
+  return angle >= limits.min && angle <= limits.max;
+}
+
+/**
+ * Clamp joint angle to NAO6 limits
+ */
+export function clampJointAngle(jointName: string, angle: number): number {
+  const limits = NAO6_JOINT_LIMITS[jointName as keyof typeof NAO6_JOINT_LIMITS];
+  if (!limits) {
+    console.warn(`Unknown joint: ${jointName}, returning 0`);
+    return 0;
+  }
+
+  return Math.max(limits.min, Math.min(limits.max, angle));
+}
+
+/**
+ * NAO6 velocity limits for safety
+ */
+export const NAO6_VELOCITY_LIMITS = {
+  linear: { min: -0.55, max: 0.55 },
+  angular: { min: -2.0, max: 2.0 },
+};
+
+/**
+ * Validate velocity against NAO6 limits
+ */
+export function validateVelocity(linear: number, angular: number): boolean {
+  return (
+    linear >= NAO6_VELOCITY_LIMITS.linear.min &&
+    linear <= NAO6_VELOCITY_LIMITS.linear.max &&
+    angular >= NAO6_VELOCITY_LIMITS.angular.min &&
+    angular <= NAO6_VELOCITY_LIMITS.angular.max
+  );
+}
+
+/**
+ * Clamp velocity to NAO6 limits
+ */
+export function clampVelocity(
+  linear: number,
+  angular: number,
+): { linear: number; angular: number } {
+  return {
+    linear: Math.max(
+      NAO6_VELOCITY_LIMITS.linear.min,
+      Math.min(NAO6_VELOCITY_LIMITS.linear.max, linear),
+    ),
+    angular: Math.max(
+      NAO6_VELOCITY_LIMITS.angular.min,
+      Math.min(NAO6_VELOCITY_LIMITS.angular.max, angular),
+    ),
+  };
+}
+
+/**
+ * Convert degrees to radians (helper for UI)
+ */
+export function degreesToRadians(degrees: number): number {
+  return degrees * (Math.PI / 180);
+}
+
+/**
+ * Convert radians to degrees (helper for UI)
+ */
+export function radiansToDegrees(radians: number): number {
+  return radians * (180 / Math.PI);
+}
+
+/**
+ * Transform function registry for dynamic lookup
+ */
+export const NAO6_TRANSFORM_FUNCTIONS = {
+  transformToTwist,
+  transformToStringMessage,
+  transformToJointAngles,
+  transformToHeadMovement,
+  getCameraImage,
+  getJointStates,
+  getImuData,
+  getBumperStatus,
+  getTouchSensors,
+  getSonarRange,
+  getRobotInfo,
+} as const;
+
+/**
+ * Get transform function by name
+ */
+export function getTransformFunction(
+  name: string,
+): ((params: Record<string, unknown>) => Record<string, unknown>) | null {
+  return (
+    NAO6_TRANSFORM_FUNCTIONS[name as keyof typeof NAO6_TRANSFORM_FUNCTIONS] ||
+    null
+  );
+}

src/lib/ros-bridge.ts

@@ -0,0 +1,546 @@
+"use client";
+
+/* eslint-disable @typescript-eslint/no-inferrable-types */
+/* eslint-disable @typescript-eslint/consistent-generic-constructors */
+/* eslint-disable @typescript-eslint/no-unsafe-function-type */
+/* eslint-disable @typescript-eslint/no-unsafe-argument */
+/* eslint-disable @typescript-eslint/prefer-promise-reject-errors */
+/* eslint-disable @typescript-eslint/prefer-nullish-coalescing */
+/* eslint-disable @typescript-eslint/no-unsafe-call */
+
+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 RobotAction {
+  id: string;
+  type: string;
+  parameters: Record<string, unknown>;
+  status: "pending" | "executing" | "completed" | "failed";
+  startTime?: Date;
+  endTime?: Date;
+  error?: string;
+}
+
+/**
+ * ROS WebSocket Bridge for connecting to rosbridge_server
+ *
+ * This service provides a high-level interface for communicating with ROS robots
+ * through the rosbridge WebSocket protocol. It handles connection management,
+ * message publishing/subscribing, service calls, and action execution.
+ */
+export class RosBridge extends EventEmitter {
+  private ws: WebSocket | null = null;
+  private url: string;
+  private reconnectInterval: number = 3000;
+  private reconnectTimer: NodeJS.Timeout | null = null;
+  private messageId: number = 0;
+  private pendingServices: Map<
+    string,
+    { resolve: Function; reject: Function }
+  > = new Map();
+  private subscriptions: Map<string, string> = new Map(); // topic -> subscription id
+  private robotStatus: RobotStatus = {
+    connected: false,
+    battery: 0,
+    position: { x: 0, y: 0, theta: 0 },
+    joints: {},
+    sensors: {},
+    lastUpdate: new Date(),
+  };
+  private activeActions: Map<string, RobotAction> = new Map();
+
+  constructor(url: string = "ws://localhost:9090") {
+    super();
+    this.url = url;
+  }
+
+  /**
+   * Connect to the ROS bridge WebSocket server
+   */
+  async connect(): Promise<void> {
+    return new Promise((resolve, reject) => {
+      try {
+        this.ws = new WebSocket(this.url);
+
+        this.ws.onopen = () => {
+          console.log("[RosBridge] Connected to ROS bridge");
+          this.robotStatus.connected = true;
+          this.clearReconnectTimer();
+          this.setupSubscriptions();
+          this.emit("connected");
+          resolve();
+        };
+
+        this.ws.onmessage = (event) => {
+          try {
+            const message = JSON.parse(event.data) as RosMessage;
+            this.handleMessage(message);
+          } catch (error) {
+            console.error("[RosBridge] Failed to parse message:", error);
+          }
+        };
+
+        this.ws.onclose = (event) => {
+          console.log(
+            "[RosBridge] Connection closed:",
+            event.code,
+            event.reason,
+          );
+          this.robotStatus.connected = false;
+          this.emit("disconnected");
+
+          if (event.code !== 1000) {
+            // Not a normal closure
+            this.scheduleReconnect();
+          }
+        };
+
+        this.ws.onerror = (error) => {
+          console.error("[RosBridge] WebSocket error:", error);
+          this.robotStatus.connected = false;
+          this.emit("error", error);
+          reject(error);
+        };
+
+        // Connection timeout
+        setTimeout(() => {
+          if (this.ws?.readyState !== WebSocket.OPEN) {
+            reject(new Error("Connection timeout"));
+          }
+        }, 5000);
+      } catch (error) {
+        reject(error);
+      }
+    });
+  }
+
+  /**
+   * Disconnect from the ROS bridge
+   */
+  disconnect(): void {
+    this.clearReconnectTimer();
+
+    if (this.ws) {
+      this.ws.close(1000, "Manual disconnect");
+      this.ws = null;
+    }
+
+    this.robotStatus.connected = false;
+    this.emit("disconnected");
+  }
+
+  /**
+   * Check if connected to ROS bridge
+   */
+  isConnected(): boolean {
+    return this.ws?.readyState === WebSocket.OPEN;
+  }
+
+  /**
+   * Get current robot status
+   */
+  getRobotStatus(): RobotStatus {
+    return { ...this.robotStatus };
+  }
+
+  /**
+   * Subscribe to a ROS topic
+   */
+  subscribe(topic: string, messageType: string): string {
+    const id = `sub_${this.messageId++}`;
+
+    const message: RosMessage = {
+      op: "subscribe",
+      topic,
+      type: messageType,
+      id,
+    };
+
+    this.send(message);
+    this.subscriptions.set(topic, id);
+
+    return id;
+  }
+
+  /**
+   * Unsubscribe from a ROS topic
+   */
+  unsubscribe(topic: string): void {
+    const id = this.subscriptions.get(topic);
+    if (id) {
+      const message: RosMessage = {
+        op: "unsubscribe",
+        id,
+      };
+
+      this.send(message);
+      this.subscriptions.delete(topic);
+    }
+  }
+
+  /**
+   * Publish a message to a ROS topic
+   */
+  publish(
+    topic: string,
+    messageType: string,
+    msg: Record<string, unknown>,
+  ): void {
+    const message: RosMessage = {
+      op: "publish",
+      topic,
+      type: messageType,
+      msg,
+    };
+
+    this.send(message);
+  }
+
+  /**
+   * Call a ROS service
+   */
+  async callService(
+    service: string,
+    serviceType: string,
+    args: Record<string, unknown> = {},
+  ): Promise<Record<string, unknown>> {
+    return new Promise((resolve, reject) => {
+      const id = `srv_${this.messageId++}`;
+
+      const message: RosMessage = {
+        op: "call_service",
+        service,
+        type: serviceType,
+        args,
+        id,
+      };
+
+      this.pendingServices.set(id, { resolve, reject });
+      this.send(message);
+
+      // Service call timeout
+      setTimeout(() => {
+        if (this.pendingServices.has(id)) {
+          this.pendingServices.delete(id);
+          reject(new Error(`Service call timeout: ${service}`));
+        }
+      }, 10000);
+    });
+  }
+
+  /**
+   * Execute a robot action (high-level NAO action)
+   */
+  async executeAction(
+    actionType: string,
+    parameters: Record<string, unknown>,
+  ): Promise<RobotAction> {
+    const action: RobotAction = {
+      id: `action_${this.messageId++}`,
+      type: actionType,
+      parameters,
+      status: "pending",
+      startTime: new Date(),
+    };
+
+    this.activeActions.set(action.id, action);
+    this.emit("action_started", action);
+
+    try {
+      // Map action to ROS service calls based on NAO plugin configuration
+      switch (actionType) {
+        case "say_text":
+          await this.naoSayText(parameters.text as string, parameters);
+          break;
+
+        case "walk_to_position":
+          await this.naoWalkTo(
+            parameters.x as number,
+            parameters.y as number,
+            parameters.theta as number,
+          );
+          break;
+
+        case "play_animation":
+          await this.naoPlayAnimation(parameters.animation as string);
+          break;
+
+        case "set_led_color":
+          await this.naoSetLedColor(
+            parameters.color as string,
+            parameters.intensity as number,
+          );
+          break;
+
+        case "sit_down":
+          await this.naoSitDown();
+          break;
+
+        case "stand_up":
+          await this.naoStandUp();
+          break;
+
+        case "turn_head":
+          await this.naoTurnHead(
+            parameters.yaw as number,
+            parameters.pitch as number,
+            parameters.speed as number,
+          );
+          break;
+
+        default:
+          throw new Error(`Unknown action type: ${actionType}`);
+      }
+
+      action.status = "completed";
+      action.endTime = new Date();
+      this.emit("action_completed", action);
+    } catch (error) {
+      action.status = "failed";
+      action.error = error instanceof Error ? error.message : String(error);
+      action.endTime = new Date();
+      this.emit("action_failed", action);
+    }
+
+    this.activeActions.set(action.id, action);
+    return action;
+  }
+
+  /**
+   * Get list of active actions
+   */
+  getActiveActions(): RobotAction[] {
+    return Array.from(this.activeActions.values());
+  }
+
+  // Private methods
+
+  private send(message: RosMessage): void {
+    if (this.ws?.readyState === WebSocket.OPEN) {
+      this.ws.send(JSON.stringify(message));
+    } else {
+      console.warn("[RosBridge] Cannot send message - not connected");
+    }
+  }
+
+  private handleMessage(message: RosMessage): void {
+    switch (message.op) {
+      case "publish":
+        this.handleTopicMessage(message);
+        break;
+
+      case "service_response":
+        this.handleServiceResponse(message);
+        break;
+
+      case "status":
+        // Handle status messages from rosbridge
+        break;
+
+      default:
+        console.log("[RosBridge] Unhandled message:", message);
+    }
+  }
+
+  private handleTopicMessage(message: RosMessage): void {
+    if (!message.topic || !message.msg) return;
+
+    // Update robot status based on subscribed topics
+    switch (message.topic) {
+      case "/battery_state":
+        if (typeof message.msg.percentage === "number") {
+          this.robotStatus.battery = message.msg.percentage;
+        }
+        break;
+
+      case "/joint_states":
+        if (message.msg.name && message.msg.position) {
+          const names = message.msg.name as string[];
+          const positions = message.msg.position as number[];
+
+          for (let i = 0; i < names.length; i++) {
+            const jointName = names[i];
+            const jointPosition = positions[i];
+            if (jointName && jointPosition !== undefined) {
+              this.robotStatus.joints[jointName] = jointPosition;
+            }
+          }
+        }
+        break;
+
+      case "/robot_pose":
+        if (message.msg.position) {
+          const pos = message.msg.position as Record<string, number>;
+          this.robotStatus.position = {
+            x: pos.x || 0,
+            y: pos.y || 0,
+            theta: pos.theta || 0,
+          };
+        }
+        break;
+    }
+
+    this.robotStatus.lastUpdate = new Date();
+    this.emit("topic_message", message.topic, message.msg);
+    this.emit("status_update", this.robotStatus);
+  }
+
+  private handleServiceResponse(message: RosMessage): void {
+    if (!message.id) return;
+
+    const pending = this.pendingServices.get(message.id);
+    if (pending) {
+      this.pendingServices.delete(message.id);
+
+      if (message.result) {
+        pending.resolve(message.values || {});
+      } else {
+        pending.reject(new Error(`Service call failed: ${message.id}`));
+      }
+    }
+  }
+
+  private setupSubscriptions(): void {
+    // Subscribe to common robot topics
+    this.subscribe("/battery_state", "sensor_msgs/BatteryState");
+    this.subscribe("/joint_states", "sensor_msgs/JointState");
+    this.subscribe("/robot_pose", "geometry_msgs/PoseStamped");
+  }
+
+  private scheduleReconnect(): void {
+    if (this.reconnectTimer) return;
+
+    console.log(
+      `[RosBridge] Scheduling reconnect in ${this.reconnectInterval}ms`,
+    );
+    this.reconnectTimer = setTimeout(() => {
+      this.reconnectTimer = null;
+      this.connect().catch((error) => {
+        console.error("[RosBridge] Reconnect failed:", error);
+        this.scheduleReconnect();
+      });
+    }, this.reconnectInterval);
+  }
+
+  private clearReconnectTimer(): void {
+    if (this.reconnectTimer) {
+      clearTimeout(this.reconnectTimer);
+      this.reconnectTimer = null;
+    }
+  }
+
+  // NAO-specific action implementations
+
+  private async naoSayText(
+    text: string,
+    params: Record<string, unknown>,
+  ): Promise<void> {
+    await this.callService("/say_text", "nao_msgs/SayText", {
+      text,
+      volume: params.volume || 0.7,
+      speed: params.speed || 100,
+    });
+  }
+
+  private async naoWalkTo(x: number, y: number, theta: number): Promise<void> {
+    await this.callService("/walk_to", "nao_msgs/WalkTo", { x, y, theta });
+  }
+
+  private async naoPlayAnimation(animation: string): Promise<void> {
+    await this.callService("/play_animation", "nao_msgs/PlayAnimation", {
+      animation: `animations/Stand/Gestures/${animation}`,
+    });
+  }
+
+  private async naoSetLedColor(
+    color: string,
+    intensity: number = 1.0,
+  ): Promise<void> {
+    const colorMap: Record<string, [number, number, number]> = {
+      red: [1, 0, 0],
+      green: [0, 1, 0],
+      blue: [0, 0, 1],
+      yellow: [1, 1, 0],
+      magenta: [1, 0, 1],
+      cyan: [0, 1, 1],
+      white: [1, 1, 1],
+      orange: [1, 0.5, 0],
+      pink: [1, 0.7, 0.7],
+    };
+
+    const rgb = colorMap[color] ?? [0, 0, 1];
+    await this.callService("/set_led_color", "nao_msgs/SetLedColor", {
+      name: "FaceLeds",
+      r: rgb[0] * intensity,
+      g: rgb[1] * intensity,
+      b: rgb[2] * intensity,
+      duration: 1.0,
+    });
+  }
+
+  private async naoSitDown(): Promise<void> {
+    await this.callService("/sit_down", "std_srvs/Empty", {});
+  }
+
+  private async naoStandUp(): Promise<void> {
+    await this.callService("/stand_up", "std_srvs/Empty", {});
+  }
+
+  private async naoTurnHead(
+    yaw: number,
+    pitch: number,
+    speed: number = 0.3,
+  ): Promise<void> {
+    await this.callService("/move_head", "nao_msgs/MoveHead", {
+      yaw,
+      pitch,
+      speed,
+    });
+  }
+}
+
+// Global ROS bridge instance
+let rosBridgeInstance: RosBridge | null = null;
+
+/**
+ * Get or create the global ROS bridge instance
+ */
+export function getRosBridge(url?: string): RosBridge {
+  if (!rosBridgeInstance) {
+    rosBridgeInstance = new RosBridge(url);
+  }
+  return rosBridgeInstance;
+}
+
+/**
+ * Initialize ROS bridge with connection
+ */
+export async function initRosBridge(url?: string): Promise<RosBridge> {
+  const bridge = getRosBridge(url);
+
+  if (!bridge.isConnected()) {
+    await bridge.connect();
+  }
+
+  return bridge;
+}