diff --git a/src/components/trials/views/WizardView.tsx b/src/components/trials/views/WizardView.tsx index bb6195c..b149a6b 100755 --- 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; diff --git a/src/components/trials/wizard/WizardInterface.tsx b/src/components/trials/wizard/WizardInterface.tsx index 9932a86..58973d6 100755 --- 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); } }; diff --git a/src/hooks/useWizardRos.ts b/src/hooks/useWizardRos.ts index e42a6e5..8939af2 100644 --- 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, + ) => Promise; } 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, diff --git a/src/lib/ros-bridge.ts b/src/lib/ros-bridge.ts index 2dfa597..6955c6f 100755 --- 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 { diff --git a/src/lib/ros/wizard-ros-service.ts b/src/lib/ros/wizard-ros-service.ts index 9b9fa55..5e964a3 100644 --- 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 { 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, diff --git a/src/server/api/routers/robots.ts b/src/server/api/routers/robots.ts index 39f8b55..13b624a 100755 --- 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"}`, + }); + } + }), }), }); diff --git a/src/server/api/routers/trials.ts b/src/server/api/routers/trials.ts index 06644c9..c77213a 100755 --- 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, diff --git a/src/server/services/robot-communication.ts b/src/server/services/robot-communication.ts index 6928c18..a2bcb4c 100755 --- 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) {