From 8f330cf5f0336a45b4e7ae826608eb43adf2a6bc Mon Sep 17 00:00:00 2001 From: Sean O'Connor Date: Wed, 1 Apr 2026 18:25:32 -0400 Subject: [PATCH] feat(ros): add animation handler and fix gesture action pipeline MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add AnimationMovement interface: { joint_names, joint_angles, speed?, delay_after? } for describing individual frames in a joint animation - Add executeAnimationSequence() public method: steps through frames, publishing each to /joint_angles with configurable per-frame delays - Add executeSimulationAnimationSequence() for mock/sim mode - Fix subscribeToRobotTopics: advertise /joint_angles as naoqi_bridge_msgs/msg/JointAnglesWithSpeed — without this rosbridge silently dropped every gesture publish (root cause of broken gestures) - Fix executeBuiltinAction: correct ROS2 message type for /joint_angles (was naoqi_bridge_msgs/JointAnglesWithSpeed, ROS1 format) - Add service payloadMapping type to executeWithConfig: routes actions to callService() instead of publish() — used by wake_up/rest - Add wake_up/rest fallback service call chains in executeBuiltinAction - Route gesture_sequence payloads through executeAnimationSequence instead of the old inline loop (which used a broken delay formula) - Improve sim mode to handle gesture_sequence configs with realistic timing - Update robot-plugins submodule pointer Co-Authored-By: Claude Sonnet 4.6 --- robot-plugins | 2 +- src/lib/ros/wizard-ros-service.ts | 198 ++++++++++++++++++++++-------- 2 files changed, 146 insertions(+), 54 deletions(-) diff --git a/robot-plugins b/robot-plugins index 19e3858..eb2f35f 160000 --- a/robot-plugins +++ b/robot-plugins @@ -1 +1 @@ -Subproject commit 19e38589c5b7e0fc3df313539a3dd2a48d137ed4 +Subproject commit eb2f35fe45f6fad5678a46f6c7c314da779f1d3f diff --git a/src/lib/ros/wizard-ros-service.ts b/src/lib/ros/wizard-ros-service.ts index e3b255a..3da8b06 100644 --- a/src/lib/ros/wizard-ros-service.ts +++ b/src/lib/ros/wizard-ros-service.ts @@ -45,6 +45,21 @@ export interface RobotActionExecution { error?: string; } +/** + * A single frame in a multi-step joint animation. + * @param joint_names - NAO joint names to move (e.g. "LShoulderPitch") + * @param joint_angles - Target angles in radians, one per joint_name + * @param speed - Movement speed as a fraction of max (0.0–1.0) + * @param delay_after - Milliseconds to wait after publishing this frame + * before sending the next one. Defaults to 800 ms. + */ +export interface AnimationMovement { + joint_names: string[]; + joint_angles: number[]; + speed?: number; + delay_after?: number; +} + /** * Unified ROS WebSocket service for wizard interface * Manages connection to rosbridge and handles robot action execution @@ -329,6 +344,19 @@ export class WizardRosService extends EventEmitter { console.log(`[WizardROS] SIMULATION MODE - Executing ${actionId}:`, parameters); + // If the action config carries a gesture_sequence payload, run the sim animation handler + if (actionConfig?.payloadMapping?.payload) { + const payload = actionConfig.payloadMapping.payload as { type?: string; movements?: AnimationMovement[] }; + if (payload.type === "gesture_sequence" && payload.movements?.length) { + await this.executeSimulationAnimationSequence(payload.movements); + execution.status = "completed"; + execution.endTime = new Date(); + this.emit("action_completed", execution); + this.activeActions.set(executionId, execution); + return execution; + } + } + // Simulate action execution based on action type let duration = 500; @@ -336,9 +364,8 @@ export class WizardRosService extends EventEmitter { const text = String(parameters.text || parameters.data || "Hello"); const wordCount = text.split(/\s+/).filter(Boolean).length; duration = 1500 + Math.max(1000, wordCount * 300); - } else if (actionId.includes("walk") || actionId.includes("turn") || actionConfig?.topic === "/cmd_vel") { + } else if (actionId.includes("walk") || actionId === "stop_walking" || actionConfig?.topic === "/cmd_vel") { duration = 500; - // Simulate position change const speed = Number(parameters.speed) || 0.1; if (actionId === "walk_forward") { this.robotStatus.position.x += speed * 0.5; @@ -349,8 +376,10 @@ export class WizardRosService extends EventEmitter { } else if (actionId === "turn_right") { this.robotStatus.position.theta += 0.5; } - } else if (actionId.includes("head") || actionId.includes("move") || actionConfig?.topic === "/joint_angles") { + } else if (actionConfig?.topic === "/joint_angles") { duration = 1000; + } else if (actionId === "wake_up" || actionId === "rest" || actionId === "set_posture") { + duration = 2000; } // Simulate async execution @@ -460,6 +489,60 @@ export class WizardRosService extends EventEmitter { return Array.from(this.activeActions.values()); } + /** + * Execute a multi-frame joint animation by sending each movement in sequence. + * Each frame is published to /joint_angles then held for delay_after ms + * (default 800 ms) before the next frame is sent. + */ + async executeAnimationSequence(movements: AnimationMovement[]): Promise { + if (!movements.length) { + console.warn("[WizardROS] executeAnimationSequence called with empty movements"); + return; + } + + for (let i = 0; i < movements.length; i++) { + const movement = movements[i]!; + console.log( + `[WizardROS] Animation frame ${i + 1}/${movements.length}:`, + movement.joint_names, + "→", + movement.joint_angles, + ); + + this.publish( + "/joint_angles", + "naoqi_bridge_msgs/msg/JointAnglesWithSpeed", + { + joint_names: movement.joint_names, + joint_angles: movement.joint_angles, + speed: movement.speed ?? 0.3, + }, + ); + + // Always wait after each frame (including the last) so the caller + // can await this method and know the animation has finished. + const delayMs = movement.delay_after ?? 800; + await new Promise((resolve) => setTimeout(resolve, delayMs)); + } + } + + /** + * Execute a multi-frame joint animation in simulation mode (logs only). + */ + private async executeSimulationAnimationSequence( + movements: AnimationMovement[], + ): Promise { + for (let i = 0; i < movements.length; i++) { + const movement = movements[i]!; + console.log( + `[WizardROS] SIMULATION animation frame ${i + 1}/${movements.length}:`, + movement.joint_names, + ); + const delayMs = movement.delay_after ?? 800; + await new Promise((resolve) => setTimeout(resolve, delayMs)); + } + } + /** * Subscribe to robot sensor topics */ @@ -480,6 +563,7 @@ export class WizardRosService extends EventEmitter { this.advertise("/speech", "std_msgs/String"); this.advertise("/cmd_vel", "geometry_msgs/Twist"); + this.advertise("/joint_angles", "naoqi_bridge_msgs/msg/JointAnglesWithSpeed"); this.advertise("/robot_pose", "geometry_msgs/Pose"); this.advertise("/animation", "std_msgs/String"); } @@ -632,10 +716,22 @@ export class WizardRosService extends EventEmitter { type: string; payload?: Record; transformFn?: string; + service?: string; + args?: Record; }; }, parameters: Record, ): Promise { + // Service-call actions — no topic publish involved + if (config.payloadMapping.type === "service") { + const service = config.payloadMapping.service ?? config.topic; + const args = config.payloadMapping.args + ? this.buildTemplatePayload(config.payloadMapping.args, parameters) + : parameters; + await this.callService(service, args); + return; + } + let msg: Record; if ( @@ -643,60 +739,22 @@ export class WizardRosService extends EventEmitter { config.payloadMapping.type === "static") && config.payloadMapping.payload ) { - // Template-based payload construction - msg = this.buildTemplatePayload( - config.payloadMapping.payload, - parameters, - ); + msg = this.buildTemplatePayload(config.payloadMapping.payload, parameters); } else if (config.payloadMapping.transformFn) { - // Custom transform function - msg = this.applyTransformFunction( - config.payloadMapping.transformFn, - parameters, - ); + msg = this.applyTransformFunction(config.payloadMapping.transformFn, parameters); } else { - // Direct parameter mapping msg = parameters; } - // Handle gesture sequences - send multiple movements with delays - if ((msg as any).type === "gesture_sequence") { - const sequence = (msg as any).movements as Array<{ - joint_names: string[]; - joint_angles: number[]; - speed: number; - }>; - - if (!sequence || sequence.length === 0) { - console.warn("[WizardROS] Gesture sequence has no movements"); + // Delegate gesture_sequence payloads to the animation handler + if ((msg as { type?: string }).type === "gesture_sequence") { + const movements = (msg as { movements?: AnimationMovement[] }).movements; + if (!movements?.length) { + console.warn("[WizardROS] gesture_sequence payload has no movements"); return; } - - console.log(`[WizardROS] Executing gesture sequence with ${sequence.length} movements`); - - for (let i = 0; i < sequence.length; i++) { - const movement = sequence[i]; - if (!movement) continue; - - this.publish( - "/joint_angles", - "naoqi_bridge_msgs/msg/JointAnglesWithSpeed", - { - joint_names: movement.joint_names, - joint_angles: movement.joint_angles, - speed: movement.speed || 0.3, - } - ); - - // Delay between movements - const delay = movement.speed ? Math.max(300, 1000 / movement.speed) : 800; - if (i < sequence.length - 1) { - await new Promise((resolve) => setTimeout(resolve, delay)); - } - } - - // Wait for final movement to complete - await new Promise((resolve) => setTimeout(resolve, 1000)); + console.log(`[WizardROS] Delegating to animation handler (${movements.length} frames)`); + await this.executeAnimationSequence(movements); return; } @@ -788,7 +846,7 @@ export class WizardRosService extends EventEmitter { case "turn_head": this.publish( "/joint_angles", - "naoqi_bridge_msgs/JointAnglesWithSpeed", + "naoqi_bridge_msgs/msg/JointAnglesWithSpeed", { joint_names: ["HeadYaw", "HeadPitch"], joint_angles: [ @@ -801,12 +859,12 @@ export class WizardRosService extends EventEmitter { await new Promise((resolve) => setTimeout(resolve, 1000)); break; - case "move_arm": + case "move_arm": { const arm = String(parameters.arm || "right"); const prefix = arm.toLowerCase() === "left" ? "L" : "R"; this.publish( "/joint_angles", - "naoqi_bridge_msgs/JointAnglesWithSpeed", + "naoqi_bridge_msgs/msg/JointAnglesWithSpeed", { joint_names: [ `${prefix}ShoulderPitch`, @@ -825,6 +883,40 @@ export class WizardRosService extends EventEmitter { ); await new Promise((resolve) => setTimeout(resolve, 1000)); break; + } + + case "wake_up": + // Try known naoqi_driver2 service names in order + for (const svc of [ + "/naoqi_driver/ALMotion/wakeUp", + "/naoqi_driver/motion/wake_up", + "/motion/wake_up", + ]) { + try { + await this.callService(svc, {}); + console.log(`[WizardROS] wake_up succeeded via ${svc}`); + break; + } catch { + // try next + } + } + break; + + case "rest": + for (const svc of [ + "/naoqi_driver/ALMotion/rest", + "/naoqi_driver/motion/rest", + "/motion/rest", + ]) { + try { + await this.callService(svc, {}); + console.log(`[WizardROS] rest succeeded via ${svc}`); + break; + } catch { + // try next + } + } + break; case "emergency_stop": this.publish("/cmd_vel", "geometry_msgs/Twist", {