mirror of
https://github.com/soconnor0919/hristudio.git
synced 2026-03-24 03:37:51 -04:00
Make wizard-ros-service robot-agnostic
- Remove NAO-specific hardcoded action handlers - Remove helper methods (executeMovementAction, executeTurnHead, executeMoveArm) - Keep only generic emergency_stop as builtin - All robot-specific actions should be defined in plugin config
This commit is contained in:
@@ -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<string, unknown>,
|
||||
): Promise<void> {
|
||||
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<string, unknown>,
|
||||
): 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<string, unknown>): 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<string, unknown>): 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
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user