Get body parts and grippers dynamically

Author: MatthijsBurgh

lib/body.js

@@ -30,43 +30,27 @@ class Body {
     parameter.get(parameter_ => {
       // Joint names
       this.jointNames = {};
-      // Arm joint names. N.B.: the suffices 'left' and 'right'
-      // must be added
-      this.jointNames.arm = [];
-      this.jointNames.leftArm = [];
-      this.jointNames.rightArm = [];
-      for (const jointName in parameter_.arm.joint_names.values()) { // eslint-disable-line guard-for-in
-        this.jointNames.arm.push(`${jointName}`);
-        this.jointNames.leftArm.push(`${jointName}_left`);
-        this.jointNames.rightArm.push(`${jointName}_right`);
-      }
-
-      // Torso joint name
-      this.jointNames.torso = parameter_.torso.joint_names;
-
-      // Default joint configuration. N.B., the configurations for the left
-      // and right arm are equal
       this.defaultConfigurations = {};
-      this.defaultConfigurations.torso = parameter_.torso.default_configurations;
-      this.defaultConfigurations.arm = parameter_.arm.default_configurations;
-      this.defaultConfigurations.leftArm = parameter_.arm.default_configurations;
-      this.defaultConfigurations.rightArm = parameter_.arm.default_configurations;
-    });
-
-    // Define the gripper action
-    this.leftGripperAction = new ActionClient({
-      ros,
-      serverName: 'left_arm/gripper/action',
-      actionName: 'tue_manipulation_msgs/GripperCommandAction',
-      timeout: 0,
-    });
-
-    // Define the gripper action
-    this.rightGripperAction = new ActionClient({
-      ros,
-      serverName: 'right_arm/gripper/action',
-      actionName: 'tue_manipulation_msgs/GripperCommandAction',
-      timeout: 0,
+      this.grippers = {};
+
+      for (const [partName, part] of Object.entries(parameter_)) {
+        if (Object.prototype.hasOwnProperty.call(part, 'joint_names')) {
+          this.jointNames[partName] = part.joint_names;
+        }
+
+        if (Object.prototype.hasOwnProperty.call(part, 'default_configurations')) {
+          this.defaultConfigurations[partName] = part.default_configurations;
+        }
+
+        if (Object.prototype.hasOwnProperty.call(part, 'ac_gripper')) {
+          this.grippers[partName] = new ActionClient({
+            ros,
+            serverName: part.ac_gripper,
+            actionName: 'tue_manipulation_msgs/GripperCommandAction',
+            timeout: 0,
+          });
+        }
+      }
     });
   } // End of constructor
 
@@ -100,22 +84,18 @@ class Body {
 
   /**
    * Sends a gripper goal
-   * @param {object} cmd: object containing 'side' ('left' or 'right') and 'direction' ('open' or 'close')
+   * @param {object} cmd: object containing 'side' and 'direction' ('open' or 'close')
    */
   sendGripperGoal(cmd) {
     console.log('Robot body: gripper goal:', cmd);
 
-    let actionClient;
-    // Get the side
-    if (cmd.side === 'left') {
-      actionClient = this.leftGripperAction;
-    } else if (cmd.side === 'right') {
-      actionClient = this.rightGripperAction;
-    } else {
+    if (!Object.prototype.hasOwnProperty.call(this.grippers, cmd.side)) {
       console.error('Gripper command side must be either left or right. Right now, it is', cmd.side);
       return;
     }
 
+    const actionClient = this.grippers[cmd.side];
+
     // Get the direction: open or close. This is mapped to the enum defined in the action description
     // int8 OPEN=-1
     // int8 CLOSE=1