Update (#302)

Author: MatthijsBurgh

lib/action-server.js

@@ -26,12 +26,12 @@ class ActionServer extends EventEmitter2 {
     // Add feedback callback
     goal.on('feedback', feedback => {
       this.emit('feedback', feedback);
-    }).bind(this);
+    });
 
     // Add status callback
     goal.on('status', status => {
       this.emit('status', status);
-    }).bind(this);
+    });
 
     // Send goal
     goal.send();

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
 
@@ -86,36 +70,36 @@ class Body {
         trajectory: {
           /* eslint camelcase:0 */
           joint_names: this.jointNames[cmd.body_part],
-          points: [{positions: this.defaultConfigurations[cmd.body_part][cmd.configuration]}],
+          points: [{
+            positions: this.defaultConfigurations[cmd.body_part][cmd.configuration],
+            time_from_start: {secs: 2},
+          }],
         },
         goal_time_tolerance: {
           secs: 5,
         },
       },
     });
+    console.debug(goal);
 
     // Send the goal with a default timeout of 10.0 seconds
     goal.send(10);
   } // End of sendGoal
 
   /**
    * 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 {
-      console.error('Gripper command side must be either left or right. Right now, it is', cmd.side);
+    if (!Object.prototype.hasOwnProperty.call(this.grippers, cmd.side)) {
+      console.error(`Gripper command side must be one of [${Object.keys(this.grippers)}]. 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