Fix branching logic and add combo robot actions

- Fix handleNextStep to handle both string and object options in conditions - Add say_with_emotion, bow, wave, nod, shake_head, point combo actions - Update seed data with nextStepId in wizard_wait_for_response options
2026-03-23 19:27:51 -04:00 · 2026-03-21 18:51:27 -04:00
parent f8e6fccae3
commit e40c37cfd0
4 changed files with 327 additions and 27 deletions
--- a/scripts/archive/seed-story-red-rock.ts
+++ b/scripts/archive/seed-story-red-rock.ts
@@ -0,0 +1,163 @@
+import { drizzle } from "drizzle-orm/postgres-js";
+import postgres from "postgres";
+import * as schema from "../../src/server/db/schema";
+import { sql } from "drizzle-orm";
+import { v4 as uuidv4 } from "uuid";
+
+// Database connection
+const connectionString = process.env.DATABASE_URL!;
+const connection = postgres(connectionString);
+const db = drizzle(connection, { schema });
+
+async function main() {
+  console.log("🌱 Seeding 'Story: Red Rock' experiment...");
+
+  try {
+    // 1. Find Admin User & Study
+    const user = await db.query.users.findFirst({
+      where: (users, { eq }) => eq(users.email, "sean@soconnor.dev"),
+    });
+    if (!user) throw new Error("Admin user 'sean@soconnor.dev' not found.");
+
+    const study = await db.query.studies.findFirst({
+      where: (studies, { eq }) => eq(studies.name, "Comparative WoZ Study"),
+    });
+    if (!study) throw new Error("Study 'Comparative WoZ Study' not found.");
+
+    const robot = await db.query.robots.findFirst({
+      where: (robots, { eq }) => eq(robots.name, "NAO6"),
+    });
+    if (!robot) throw new Error("Robot 'NAO6' not found.");
+
+    // 2. Create Experiment
+    const [experiment] = await db
+      .insert(schema.experiments)
+      .values({
+        studyId: study.id,
+        name: "Story: Red Rock",
+        description: "A story about a red rock on Mars with comprehension check and branching.",
+        version: 1,
+        status: "draft",
+        robotId: robot.id,
+        createdBy: user.id,
+      })
+      .returning();
+
+    if (!experiment) throw new Error("Failed to create experiment");
+    console.log(`✅ Created Experiment: ${experiment.id}`);
+
+    // 3. Create Steps (in reverse for ID references if needed, but we'll use uuid placeholders)
+    const conclusionId = uuidv4();
+    const branchAId = uuidv4();
+    const branchBId = uuidv4();
+    const checkId = uuidv4();
+
+    // Step 1: The Hook
+    const [step1] = await db.insert(schema.steps).values({
+      experimentId: experiment.id,
+      name: "The Hook",
+      type: "wizard",
+      orderIndex: 0,
+    }).returning();
+
+    // Step 2: The Narrative
+    const [step2] = await db.insert(schema.steps).values({
+      experimentId: experiment.id,
+      name: "The Narrative",
+      type: "wizard",
+      orderIndex: 1,
+    }).returning();
+
+    // Step 3: Comprehension Check (Conditional)
+    const [step3] = await db.insert(schema.steps).values({
+      id: checkId,
+      experimentId: experiment.id,
+      name: "Comprehension Check",
+      type: "conditional",
+      orderIndex: 2,
+      conditions: {
+        variable: "last_wizard_response",
+        options: [
+          { label: "Answer: Red (Correct)", value: "Red", variant: "default", nextStepId: branchAId },
+          { label: "Answer: Other (Incorrect)", value: "Incorrect", variant: "destructive", nextStepId: branchBId }
+        ]
+      }
+    }).returning();
+
+    // Step 4: Branch A (Correct)
+    const [step4] = await db.insert(schema.steps).values({
+      id: branchAId,
+      experimentId: experiment.id,
+      name: "Branch A: Correct Response",
+      type: "wizard",
+      orderIndex: 3,
+      conditions: { nextStepId: conclusionId } // SKIP BRANCH B
+    }).returning();
+
+    // Step 5: Branch B (Incorrect)
+    const [step5] = await db.insert(schema.steps).values({
+      id: branchBId,
+      experimentId: experiment.id,
+      name: "Branch B: Incorrect Response",
+      type: "wizard",
+      orderIndex: 4,
+      conditions: { nextStepId: conclusionId }
+    }).returning();
+
+    // Step 6: Conclusion
+    const [step6] = await db.insert(schema.steps).values({
+      id: conclusionId,
+      experimentId: experiment.id,
+      name: "Conclusion",
+      type: "wizard",
+      orderIndex: 5,
+    }).returning();
+
+    // 4. Create Actions
+
+    // The Hook
+    await db.insert(schema.actions).values([
+      { stepId: step1!.id, name: "Say Hello", type: "nao6-ros2.say_text", orderIndex: 0, parameters: { text: "Hello! Are you ready for a story?" } },
+      { stepId: step1!.id, name: "Wave", type: "nao6-ros2.move_arm", orderIndex: 1, parameters: { arm: "right", shoulder_pitch: 0.5 } }
+    ]);
+
+    // The Narrative
+    await db.insert(schema.actions).values([
+      { stepId: step2!.id, name: "The Story", type: "nao6-ros2.say_text", orderIndex: 0, parameters: { text: "Once, a traveler went to Mars. He found a bright red rock that glowed." } },
+      { stepId: step2!.id, name: "Look Left", type: "nao6-ros2.turn_head", orderIndex: 1, parameters: { yaw: 0.5, speed: 0.3 } },
+      { stepId: step2!.id, name: "Look Right", type: "nao6-ros2.turn_head", orderIndex: 2, parameters: { yaw: -0.5, speed: 0.3 } }
+    ]);
+
+    // Comprehension Check
+    await db.insert(schema.actions).values([
+      { stepId: step3!.id, name: "Ask Color", type: "nao6-ros2.say_text", orderIndex: 0, parameters: { text: "What color was the rock I found on Mars?" } },
+      { stepId: step3!.id, name: "Wait for Color", type: "wizard_wait_for_response", orderIndex: 1, parameters: { options: ["Red", "Blue", "Green", "Incorrect"], prompt_text: "What color did the participant say?" } }
+    ]);
+
+    // Branch A (Using say_with_emotion)
+    await db.insert(schema.actions).values([
+      { stepId: step4!.id, name: "Happy Response", type: "nao6-ros2.say_with_emotion", orderIndex: 0, parameters: { text: "Exacty! It was a glowing red rock.", emotion: "happy" } }
+    ]);
+
+    // Branch B
+    await db.insert(schema.actions).values([
+      { stepId: step5!.id, name: "Correct them", type: "nao6-ros2.say_text", orderIndex: 0, parameters: { text: "Actually, it was red." } },
+      { stepId: step5!.id, name: "Shake Head", type: "nao6-ros2.turn_head", orderIndex: 1, parameters: { yaw: 0.3, speed: 0.5 } }
+    ]);
+
+    // Conclusion
+    await db.insert(schema.actions).values([
+      { stepId: step6!.id, name: "Final Goodbye", type: "nao6-ros2.say_text", orderIndex: 0, parameters: { text: "That is all for today. Goodbye!" } },
+      { stepId: step6!.id, name: "Rest", type: "nao6-ros2.move_arm", orderIndex: 1, parameters: { shoulder_pitch: 1.5 } }
+    ]);
+
+    console.log("✅ Seed completed successfully!");
+  } catch (err) {
+    console.error("❌ Seed failed:", err);
+    process.exit(1);
+  } finally {
+    await connection.end();
+  }
+}
+
+main();
--- a/scripts/seed-dev.ts
+++ b/scripts/seed-dev.ts
@@ -448,7 +448,10 @@ async function main() {
        // Define the options that will be presented to the Wizard
        parameters: {
          prompt_text: "Did participant answer 'Red' correctly?",
-          options: ["Correct", "Incorrect"],
+          options: [
+            { label: "Correct", value: "Correct", nextStepId: step4a!.id },
+            { label: "Incorrect", value: "Incorrect", nextStepId: step4b!.id },
+          ],
        },
        sourceKind: "core",
        pluginId: "hristudio-woz", // Explicit link
--- a/src/components/trials/wizard/WizardInterface.tsx
+++ b/src/components/trials/wizard/WizardInterface.tsx
@@ -613,17 +613,32 @@ export const WizardInterface = React.memo(function WizardInterface({
      currentStep.conditions?.options &&
      lastResponse
    ) {
-      const matchedOption = currentStep.conditions.options.find(
-        (opt) => opt.value === lastResponse,
-      );
-      if (matchedOption && matchedOption.nextStepId) {
+      // Handle both string options and object options
+      const matchedOption = currentStep.conditions.options.find((opt) => {
+        // If opt is a string, compare directly with lastResponse
+        if (typeof opt === "string") {
+          return opt === lastResponse;
+        }
+        // If opt is an object, check .value property
+        return opt.value === lastResponse;
+      });
+
+      if (matchedOption) {
+        // Handle both string options and object options for nextStepId
+        const nextStepId = typeof matchedOption === "string"
+          ? null  // String options don't have nextStepId
+          : matchedOption.nextStepId;
+
+        if (nextStepId) {
          // Find index of the target step
-        const targetIndex = steps.findIndex(
-          (s) => s.id === matchedOption.nextStepId,
-        );
+          const targetIndex = steps.findIndex((s) => s.id === nextStepId);
          if (targetIndex !== -1) {
+            const label = typeof matchedOption === "string"
+              ? matchedOption
+              : matchedOption.label;
+
            console.log(
-            `[WizardInterface] Branching to step ${targetIndex} (${matchedOption.label})`,
+              `[WizardInterface] Branching to step ${targetIndex} (${label})`,
            );

            logEventMutation.mutate({
@@ -632,7 +647,7 @@ export const WizardInterface = React.memo(function WizardInterface({
              data: {
                fromIndex: currentStepIndex,
                toIndex: targetIndex,
-              condition: matchedOption.label,
+                condition: label,
                value: lastResponse,
              },
            });
@@ -643,6 +658,7 @@ export const WizardInterface = React.memo(function WizardInterface({
          }
        }
      }
+    }

    // Check for explicit nextStepId in conditions (e.g. for end of branch)
    console.log(
--- a/src/lib/ros/wizard-ros-service.ts
+++ b/src/lib/ros/wizard-ros-service.ts
@@ -209,6 +209,7 @@ export class WizardRosService extends EventEmitter {
      };
    },
  ): Promise<RobotActionExecution> {
+    console.log(`[WizardROS] executeRobotAction called: plugin=${pluginName}, action=${actionId}`, { actionConfig, parameters });
    if (!this.isConnected) {
      throw new Error("Not connected to ROS bridge");
    }
@@ -298,6 +299,7 @@ export class WizardRosService extends EventEmitter {
    messageType: string,
    msg: Record<string, unknown>,
  ): void {
+    console.log(`[WizardROS] Publishing to ${topic}:`, msg);
    const message: RosMessage = {
      op: "publish",
      topic,
@@ -313,9 +315,10 @@ export class WizardRosService extends EventEmitter {
   */
  private send(message: RosMessage): void {
    if (this.ws?.readyState === WebSocket.OPEN) {
+      console.log("[WizardROS] send: Sending message:", JSON.stringify(message).substring(0, 200));
      this.ws.send(JSON.stringify(message));
    } else {
-      console.warn("[WizardROS] Cannot send message - not connected");
+      console.warn("[WizardROS] Cannot send message - not connected", { readyState: this.ws?.readyState });
    }
  }

@@ -448,9 +451,11 @@ export class WizardRosService extends EventEmitter {
    actionId: string,
    parameters: Record<string, unknown>,
  ): Promise<void> {
+    console.log(`[WizardROS] executeBuiltinAction: ${actionId}`, parameters);
    switch (actionId) {
      case "say_text":
        const text = String(parameters.text || "Hello");
+        console.log(`[WizardROS] Publishing to /speech:`, text);
        this.publish("/speech", "std_msgs/String", {
          data: text,
        });
@@ -460,6 +465,119 @@ export class WizardRosService extends EventEmitter {
        await new Promise((resolve) => setTimeout(resolve, estimatedDuration));
        break;

+      case "say_with_emotion":
+        const emotionText = String(parameters.text || "Hello");
+        const emotion = String(parameters.emotion || "neutral");
+        const speed = Number(parameters.speed || 1.0);
+        console.log(`[WizardROS] Publishing with emotion:`, emotionText, emotion);
+        // NAOqi speech format: \rspd=speed \rst=emotion text
+        this.publish("/speech", "std_msgs/String", {
+          data: `\\rspd=${speed}\\\\rst=${emotion}\\${emotionText}`,
+        });
+        const emotionWordCount = emotionText.split(/\s+/).length;
+        const emotionDuration = Math.max(800, emotionWordCount * 250 + 500);
+        await new Promise((resolve) => setTimeout(resolve, emotionDuration));
+        break;
+
+      case "bow":
+        // Combo: head look down + lean forward + return
+        console.log(`[WizardROS] Executing bow animation`);
+        this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+          joint_names: ["HeadYaw", "HeadPitch"],
+          joint_angles: [0, 0.5],
+          speed: 0.3,
+        });
+        await new Promise((resolve) => setTimeout(resolve, 500));
+        this.publish("/cmd_vel", "geometry_msgs/Twist", {
+          linear: { x: 0.1, y: 0, z: 0 },
+          angular: { x: 0, y: 0, z: 0 },
+        });
+        await new Promise((resolve) => setTimeout(resolve, 800));
+        // Return to neutral
+        this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+          joint_names: ["HeadYaw", "HeadPitch"],
+          joint_angles: [0, 0],
+          speed: 0.3,
+        });
+        this.publish("/cmd_vel", "geometry_msgs/Twist", {
+          linear: { x: 0, y: 0, z: 0 },
+          angular: { x: 0, y: 0, z: 0 },
+        });
+        break;
+
+      case "wave":
+        // Combo: right arm wave gesture
+        console.log(`[WizardROS] Executing wave gesture`);
+        this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+          joint_names: ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll"],
+          joint_angles: [1.5, 0.2, -1.0, 0.5],
+          speed: 0.4,
+        });
+        await new Promise((resolve) => setTimeout(resolve, 600));
+        // Wave motion
+        for (let i = 0; i < 3; i++) {
+          this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+            joint_names: ["RElbowRoll"],
+            joint_angles: [0.2],
+            speed: 0.6,
+          });
+          await new Promise((resolve) => setTimeout(resolve, 200));
+          this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+            joint_names: ["RElbowRoll"],
+            joint_angles: [0.8],
+            speed: 0.6,
+          });
+          await new Promise((resolve) => setTimeout(resolve, 200));
+        }
+        break;
+
+      case "nod":
+        console.log(`[WizardROS] Executing nod gesture`);
+        for (let i = 0; i < 2; i++) {
+          this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+            joint_names: ["HeadPitch"],
+            joint_angles: [0.3],
+            speed: 0.5,
+          });
+          await new Promise((resolve) => setTimeout(resolve, 300));
+          this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+            joint_names: ["HeadPitch"],
+            joint_angles: [-0.1],
+            speed: 0.5,
+          });
+          await new Promise((resolve) => setTimeout(resolve, 300));
+        }
+        break;
+
+      case "shake_head":
+        console.log(`[WizardROS] Executing head shake gesture`);
+        for (let i = 0; i < 2; i++) {
+          this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+            joint_names: ["HeadYaw"],
+            joint_angles: [0.4],
+            speed: 0.5,
+          });
+          await new Promise((resolve) => setTimeout(resolve, 250));
+          this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+            joint_names: ["HeadYaw"],
+            joint_angles: [-0.4],
+            speed: 0.5,
+          });
+          await new Promise((resolve) => setTimeout(resolve, 250));
+        }
+        break;
+
+      case "point":
+        // Point gesture with left arm
+        console.log(`[WizardROS] Executing point gesture`);
+        this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", {
+          joint_names: ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw"],
+          joint_angles: [0.8, 0.3, -1.0, 0.1, 0],
+          speed: 0.4,
+        });
+        await new Promise((resolve) => setTimeout(resolve, 1000));
+        break;
+
      case "walk_forward":
      case "walk_backward":
      case "turn_left":