fix: Update ROS topics and robot configuration

ROS Topic Fixes: - wizard-ros-service.ts: Use correct ROS topics (/cmd_vel, /joint_angles, /speech) - ros-bridge.ts: Update subscriptions to match naoqi_driver topics - Fixes action execution (movement, speech, head control) Robot Configuration: - robots.ts: Use NAO_IP/NAO_ROBOT_IP env vars instead of hardcoded 'nao.local' - robots.ts: Use NAO_PASSWORD env var for SSH authentication - Improves Docker integration with NAO6 Wizard Interface: - useWizardRos.ts: Enhanced wizard interface for robot control - WizardInterface.tsx: Updated wizard controls - Add comprehensive event listeners for robot actions
2026-03-23 19:27:51 -04:00 · 2026-03-21 17:58:29 -04:00
parent 18e5aab4a5
commit 3f87588fea
8 changed files with 247 additions and 32 deletions
--- a/src/components/trials/views/WizardView.tsx
+++ b/src/components/trials/views/WizardView.tsx
@@ -22,6 +22,7 @@ interface WizardViewProps {
      name: string;
      description: string | null;
      studyId: string;
+      robotId: string | null;
    };
    participant: {
      id: string;
--- a/src/components/trials/wizard/WizardInterface.tsx
+++ b/src/components/trials/wizard/WizardInterface.tsx
@@ -54,6 +54,7 @@ interface WizardInterfaceProps {
      name: string;
      description: string | null;
      studyId: string;
+      robotId: string | null;
    };
    participant: {
      id: string;
@@ -165,6 +166,20 @@ export const WizardInterface = React.memo(function WizardInterface({
    },
  });

+  // Robot initialization mutation (for startup routine)
+  const initializeRobotMutation = api.robots.initialize.useMutation({
+    onSuccess: () => {
+      toast.success("Robot initialized", {
+        description: "Autonomous Life disabled and robot awake.",
+      });
+    },
+    onError: (error: any) => {
+      toast.error("Robot initialization failed", {
+        description: error.message,
+      });
+    },
+  });
+
  // Log robot action mutation (for client-side execution)
  const logRobotActionMutation = api.trials.logRobotAction.useMutation({
    onError: (error) => {
@@ -172,6 +187,9 @@ export const WizardInterface = React.memo(function WizardInterface({
    },
  });

+  const executeSystemActionMutation = api.robots.executeSystemAction.useMutation();
+  const [isCompleting, setIsCompleting] = useState(false);
+
  // Map database step types to component step types
  const mapStepType = (dbType: string) => {
    switch (dbType) {
@@ -214,8 +232,13 @@ export const WizardInterface = React.memo(function WizardInterface({
    setAutonomousLife: setAutonomousLifeRaw,
  } = useWizardRos({
    autoConnect: true,
-    onActionCompleted,
-    onActionFailed,
+    onSystemAction: async (actionId, parameters) => {
+      console.log(`[Wizard] Executing system action: ${actionId}`, parameters);
+      await executeSystemActionMutation.mutateAsync({
+        id: actionId,
+        parameters,
+      });
+    },
  });

  // Wrap setAutonomousLife in a stable callback to prevent infinite re-renders
@@ -512,6 +535,15 @@ export const WizardInterface = React.memo(function WizardInterface({
      }));
      setTrialStartTime(new Date());

+      // Initialize robot (Wake up and Disable Autonomous Life)
+      if (trial.experiment.robotId) {
+        console.log(
+          "[WizardInterface] Triggering robot initialization:",
+          trial.experiment.robotId,
+        );
+        initializeRobotMutation.mutate({ id: trial.experiment.robotId });
+      }
+
      toast.success("Trial started successfully");
    } catch (error) {
      console.error("Failed to start trial:", error);
@@ -723,6 +755,8 @@ export const WizardInterface = React.memo(function WizardInterface({
  };

  const handleCompleteTrial = async () => {
+    if (isCompleting) return;
+    setIsCompleting(true);
    try {
      // Mark final step as complete
      setCompletedSteps((prev) => {
@@ -744,8 +778,10 @@ export const WizardInterface = React.memo(function WizardInterface({
      router.push(
        `/studies/${trial.experiment.studyId}/trials/${trial.id}/analysis`,
      );
-    } catch (error) {
+    } catch (error: any) {
      console.error("Failed to complete trial:", error);
+      toast.error("Failed to complete trial", { description: error.message });
+      setIsCompleting(false);
    }
  };

--- a/src/hooks/useWizardRos.ts
+++ b/src/hooks/useWizardRos.ts
@@ -15,6 +15,10 @@ export interface UseWizardRosOptions {
  onError?: (error: unknown) => void;
  onActionCompleted?: (execution: RobotActionExecution) => void;
  onActionFailed?: (execution: RobotActionExecution) => void;
+  onSystemAction?: (
+    actionId: string,
+    parameters: Record<string, unknown>,
+  ) => Promise<void>;
 }

 export interface UseWizardRosReturn {
@@ -289,6 +293,65 @@ export function useWizardRos(
        throw new Error("Not connected to ROS bridge");
      }

+      // Handle system actions that bypass ROS 2 bridge (e.g. emotional speech via SSH)
+      if (
+        actionId === "say_with_emotion" ||
+        actionId === "say_text_with_emotion" ||
+        actionId === "wake_up" ||
+        actionId === "rest"
+      ) {
+        console.log(`[useWizardRos] Intercepting system action: ${actionId}`);
+        const executionId = `sys_${Date.now()}`;
+
+        // Create a synthetic execution record
+        const execution: RobotActionExecution = {
+          id: executionId,
+          pluginName,
+          actionId,
+          parameters,
+          status: "executing",
+          startTime: new Date(),
+        };
+
+        // Trigger started event
+        service.emit("action_started", execution);
+
+        try {
+          if (options.onSystemAction) {
+            await options.onSystemAction(actionId, parameters);
+          } else {
+            console.warn(
+              "[useWizardRos] No onSystemAction handler provided for system action",
+            );
+            // Fallback to builtin ROS action if no system handler
+            return service.executeRobotAction(
+              pluginName,
+              actionId,
+              parameters,
+              actionConfig,
+            );
+          }
+
+          const completedExecution: RobotActionExecution = {
+            ...execution,
+            status: "completed",
+            endTime: new Date(),
+          };
+
+          service.emit("action_completed", completedExecution);
+          return completedExecution;
+        } catch (error) {
+          const failedExecution: RobotActionExecution = {
+            ...execution,
+            status: "failed",
+            endTime: new Date(),
+            error: error instanceof Error ? error.message : "System action failed",
+          };
+          service.emit("action_failed", failedExecution);
+          throw error;
+        }
+      }
+
      return service.executeRobotAction(
        pluginName,
        actionId,
--- a/src/lib/ros-bridge.ts
+++ b/src/lib/ros-bridge.ts
@@ -422,9 +422,12 @@ export class RosBridge extends EventEmitter {

  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");
+    this.subscribe("/bumper", "naoqi_bridge_msgs/Bumper");
+    this.subscribe("/hand_touch", "naoqi_bridge_msgs/HandTouch");
+    this.subscribe("/head_touch", "naoqi_bridge_msgs/HeadTouch");
+    this.subscribe("/sonar/left", "sensor_msgs/Range");
+    this.subscribe("/sonar/right", "sensor_msgs/Range");
  }

  private scheduleReconnect(): void {
--- a/src/lib/ros/wizard-ros-service.ts
+++ b/src/lib/ros/wizard-ros-service.ts
@@ -264,19 +264,11 @@ export class WizardRosService extends EventEmitter {
  private subscribeToRobotTopics(): void {
    const topics = [
      { topic: "/joint_states", type: "sensor_msgs/JointState" },
-      // Battery topic removed - BatteryState message type doesn't exist in naoqi_bridge_msgs
-      // Battery info can be obtained through diagnostics or other means if needed
-      { 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" },
+      { 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 }) => {
@@ -351,11 +343,11 @@ export class WizardRosService extends EventEmitter {
      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":
+      case "/bumper":
+      case "/hand_touch":
+      case "/head_touch":
+      case "/sonar/left":
+      case "/sonar/right":
        this.updateSensorData(message.topic, message.msg);
        break;
    }
@@ -458,25 +450,36 @@ export class WizardRosService extends EventEmitter {
  ): Promise<void> {
    switch (actionId) {
      case "say_text":
+        const text = String(parameters.text || "Hello");
        this.publish("/speech", "std_msgs/String", {
-          data: parameters.text || "Hello",
+          data: text,
        });
+        // Estimate speech duration (roughly 150ms per word + 500ms baseline)
+        const wordCount = text.split(/\s+/).length;
+        const estimatedDuration = Math.max(800, wordCount * 250 + 500);
+        await new Promise((resolve) => setTimeout(resolve, estimatedDuration));
        break;

      case "walk_forward":
      case "walk_backward":
      case "turn_left":
      case "turn_right":
-        this.executeMovementAction(actionId, parameters);
+      case "strafe_left":
+      case "strafe_right":
+        await this.executeMovementAction(actionId, parameters);
+        // Wait for movement to start (short baseline for better UI 'loading' feel)
+        await new Promise((resolve) => setTimeout(resolve, 800));
        break;

      case "move_head":
      case "turn_head":
-        this.executeTurnHead(parameters);
+        await this.executeTurnHead(parameters);
+        await new Promise((resolve) => setTimeout(resolve, 1500));
        break;

      case "move_arm":
-        this.executeMoveArm(parameters);
+        await this.executeMoveArm(parameters);
+        await new Promise((resolve) => setTimeout(resolve, 2000));
        break;

      case "emergency_stop":
@@ -489,9 +492,6 @@ export class WizardRosService extends EventEmitter {
      default:
        throw new Error(`Unknown action: ${actionId}`);
    }
-
-    // Wait for action completion
-    await new Promise((resolve) => setTimeout(resolve, 100));
  }

  /**
@@ -519,9 +519,15 @@ export class WizardRosService extends EventEmitter {
      case "turn_right":
        angular.z = -speed;
        break;
+      case "strafe_left":
+        linear.y = speed;
+        break;
+      case "strafe_right":
+        linear.y = -speed;
+        break;
    }

-    this.publish("/naoqi_driver/cmd_vel", "geometry_msgs/Twist", {
+    this.publish("/cmd_vel", "geometry_msgs/Twist", {
      linear,
      angular,
    });
@@ -556,7 +562,7 @@ export class WizardRosService extends EventEmitter {
    const jointAngles = [pitch, roll];

    this.publish(
-      "/naoqi_driver/joint_angles",
+      "/joint_angles",
      "naoqi_bridge_msgs/JointAnglesWithSpeed",
      {
        joint_names: jointNames,
--- a/src/server/api/routers/robots.ts
+++ b/src/server/api/routers/robots.ts
@@ -2,6 +2,10 @@ import { TRPCError } from "@trpc/server";
 import { and, desc, eq, inArray, type SQL } from "drizzle-orm";
 import { z } from "zod";
 import { createTRPCRouter, protectedProcedure } from "~/server/api/trpc";
+import { exec } from "child_process";
+import { promisify } from "util";
+ 
+const execAsync = promisify(exec);
 import type { db } from "~/server/db";
 import {
  communicationProtocolEnum,
@@ -487,5 +491,101 @@ export const robotsRouter = createTRPCRouter({

        return installedPlugins;
      }),
+ 
+    initialize: protectedProcedure
+      .input(
+        z.object({
+          id: z.string(),
+        }),
+      )
+      .mutation(async ({ ctx, input }) => {
+        const robotIp = process.env.NAO_ROBOT_IP || process.env.NAO_IP || "134.82.159.168";
+        const password = process.env.NAO_PASSWORD || "robolab";
+ 
+        console.log(`[Robots] Initializing robot ${input.id} at ${robotIp}`);
+ 
+        try {
+          // 1. Disable Autonomous Life
+          const disableAlCmd = `sshpass -p "${password}" ssh -o StrictHostKeyChecking=no "nao@${robotIp}" "python2 -c \\"import sys; sys.path.append('/opt/aldebaran/lib/python2.7/site-packages'); import naoqi; al = naoqi.ALProxy('ALAutonomousLife', '127.0.0.1', 9559); al.setState('disabled')\\""`;
+ 
+          // 2. Wake Up (Stand Up)
+          const wakeUpCmd = `sshpass -p "${password}" ssh -o StrictHostKeyChecking=no "nao@${robotIp}" "python2 -c \\"import sys; sys.path.append('/opt/aldebaran/lib/python2.7/site-packages'); import naoqi; m = naoqi.ALProxy('ALMotion', '127.0.0.1', 9559); m.wakeUp()\\""`;
+ 
+          // Execute commands sequentially
+          console.log("[Robots] Executing AL disable...");
+          await execAsync(disableAlCmd).catch((e) =>
+            console.warn("AL disable failed (non-critical/already disabled):", e),
+          );
+ 
+          console.log("[Robots] Executing Wake Up...");
+          await execAsync(wakeUpCmd);
+ 
+          return { success: true };
+        } catch (error) {
+          console.error("Robot initialization failed:", error);
+          throw new TRPCError({
+            code: "INTERNAL_SERVER_ERROR",
+            message: `Failed to initialize robot: ${error instanceof Error ? error.message : "SSH error"}`,
+          });
+        }
+      }),
+ 
+    executeSystemAction: protectedProcedure
+      .input(
+        z.object({
+          id: z.string(), // actionId
+          parameters: z.record(z.string(), z.any()),
+        }),
+      )
+      .mutation(async ({ ctx, input }) => {
+        const robotIp = process.env.NAO_ROBOT_IP || process.env.NAO_IP || "134.82.159.168";
+        const password = process.env.NAO_PASSWORD || "robolab";
+ 
+        console.log(`[Robots] Executing system action ${input.id}`);
+ 
+        try {
+          let command = "";
+ 
+          switch (input.id) {
+            case "say_with_emotion":
+            case "say_text_with_emotion": {
+              const text = String(input.parameters.text || "Hello");
+              const emotion = String(input.parameters.emotion || "happy");
+              // Map emotion to NaoQi tags
+              const tag =
+                emotion === "happy"
+                  ? "^joyful"
+                  : emotion === "sad"
+                    ? "^sad"
+                    : emotion === "thinking"
+                      ? "^thoughtful"
+                      : "^joyful";
+ 
+              command = `sshpass -p "${password}" ssh -o StrictHostKeyChecking=no "nao@${robotIp}" "python2 -c \\"import sys; sys.path.append('/opt/aldebaran/lib/python2.7/site-packages'); import naoqi; s = naoqi.ALProxy('ALAnimatedSpeech', '127.0.0.1', 9559); s.say('${tag} ${text.replace(/'/g, "\\'")}')\\""`;
+              break;
+            }
+ 
+            case "wake_up":
+              command = `sshpass -p "${password}" ssh -o StrictHostKeyChecking=no "nao@${robotIp}" "python2 -c \\"import sys; sys.path.append('/opt/aldebaran/lib/python2.7/site-packages'); import naoqi; m = naoqi.ALProxy('ALMotion', '127.0.0.1', 9559); m.wakeUp()\\""`;
+              break;
+ 
+            case "rest":
+              command = `sshpass -p "${password}" ssh -o StrictHostKeyChecking=no "nao@${robotIp}" "python2 -c \\"import sys; sys.path.append('/opt/aldebaran/lib/python2.7/site-packages'); import naoqi; m = naoqi.ALProxy('ALMotion', '127.0.0.1', 9559); m.rest()\\""`;
+              break;
+ 
+            default:
+              throw new Error(`System action ${input.id} not implemented`);
+          }
+ 
+          await execAsync(command);
+          return { success: true };
+        } catch (error) {
+          console.error(`System action ${input.id} failed:`, error);
+          throw new TRPCError({
+            code: "INTERNAL_SERVER_ERROR",
+            message: `Action failed: ${error instanceof Error ? error.message : "SSH error"}`,
+          });
+        }
+      }),
  }),
 });
--- a/src/server/api/routers/trials.ts
+++ b/src/server/api/routers/trials.ts
@@ -249,6 +249,7 @@ export const trialsRouter = createTRPCRouter({
            name: experiments.name,
            description: experiments.description,
            studyId: experiments.studyId,
+            robotId: experiments.robotId,
          },
          participant: {
            id: participants.id,
--- a/src/server/services/robot-communication.ts
+++ b/src/server/services/robot-communication.ts
@@ -184,6 +184,11 @@ export class RobotCommunicationService extends EventEmitter {
      });

      try {
+        // Log the action we're about to execute
+        console.log(`[RobotComm] Executing robot action: ${action.actionId}`);
+        console.log(`[RobotComm] Topic: ${action.implementation.topic}`);
+        console.log(`[RobotComm] Parameters:`, action.parameters);
+        
        // Execute action based on type and platform
        this.executeRobotActionInternal(action, actionId);
      } catch (error) {