diff --git a/src/lib/ros/wizard-ros-service.ts b/src/lib/ros/wizard-ros-service.ts index 10086c9..1548fba 100644 --- a/src/lib/ros/wizard-ros-service.ts +++ b/src/lib/ros/wizard-ros-service.ts @@ -443,153 +443,14 @@ export class WizardRosService extends EventEmitter { } /** - * Execute built-in robot actions + * Execute built-in robot actions (robot-agnostic defaults) + * These are generic actions that work with standard ROS topics */ private async executeBuiltinAction( actionId: string, parameters: Record, ): Promise { switch (actionId) { - case "say_text": - const text = String(parameters.text || "Hello"); - this.publish("/speech", "std_msgs/String", { - 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 "say_with_emotion": - const emotionText = String(parameters.text || "Hello"); - const emotion = String(parameters.emotion || "neutral"); - const speed = Number(parameters.speed || 1.0); - // 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 - 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 - 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": - 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": - 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 - 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": - case "turn_right": - 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": - await this.executeTurnHead(parameters); - await new Promise((resolve) => setTimeout(resolve, 1500)); - break; - - case "move_arm": - await this.executeMoveArm(parameters); - await new Promise((resolve) => setTimeout(resolve, 2000)); - break; - case "emergency_stop": this.publish("/cmd_vel", "geometry_msgs/Twist", { linear: { x: 0, y: 0, z: 0 }, @@ -598,88 +459,10 @@ export class WizardRosService extends EventEmitter { break; default: - throw new Error(`Unknown action: ${actionId}`); + throw new Error(`Unknown action: ${actionId}. Define this action in your robot plugin.`); } } - /** - * Execute movement actions - */ - private executeMovementAction( - actionId: string, - parameters: Record, - ): void { - let linear = { x: 0, y: 0, z: 0 }; - let angular = { x: 0, y: 0, z: 0 }; - - const speed = Number(parameters.speed) || 0.1; - - switch (actionId) { - case "walk_forward": - linear.x = speed; - break; - case "walk_backward": - linear.x = -speed; - break; - case "turn_left": - angular.z = speed; - break; - case "turn_right": - angular.z = -speed; - break; - case "strafe_left": - linear.y = speed; - break; - case "strafe_right": - linear.y = -speed; - break; - } - - this.publish("/cmd_vel", "geometry_msgs/Twist", { - linear, - angular, - }); - } - - /** - * Execute head turn action - */ - private executeTurnHead(parameters: Record): void { - const yaw = Number(parameters.yaw) || 0; - const pitch = Number(parameters.pitch) || 0; - const speed = Number(parameters.speed) || 0.3; - - this.publish("/joint_angles", "naoqi_bridge_msgs/JointAnglesWithSpeed", { - joint_names: ["HeadYaw", "HeadPitch"], - joint_angles: [yaw, pitch], - speed: speed, - }); - } - - /** - * Execute arm movement - */ - private executeMoveArm(parameters: Record): void { - const arm = String(parameters.arm || "Right"); - const roll = Number(parameters.roll) || 0; - const pitch = Number(parameters.pitch) || 0; - const speed = Number(parameters.speed) || 0.2; - - const prefix = arm === "Left" ? "L" : "R"; - const jointNames = [`${prefix}ShoulderPitch`, `${prefix}ShoulderRoll`]; - const jointAngles = [pitch, roll]; - - this.publish( - "/joint_angles", - "naoqi_bridge_msgs/JointAnglesWithSpeed", - { - joint_names: jointNames, - joint_angles: jointAngles, - speed: speed, - }, - ); - } - /** * Call a ROS service */