Update robot-plugins submodule

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

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "robot-plugins"]
+	path = robot-plugins
+	url = git@github.com:soconnor0919/robot-plugins.git
+	branch = main

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;

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);
     }
   };
 

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,

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 {

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,

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"}`,
+          });
+        }
+      }),
   }),
 });

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,

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) {