major changes

bower.json

@@ -10,6 +10,7 @@
   "dependencies": {
     "selectize": "^0.12.6",
     "pretty-checkbox": "^3.0.3",
-    "remarkable-bootstrap-notify": "^3.1.3"
+    "remarkable-bootstrap-notify": "^3.1.3",
+    "async": "^3.1.0"
   }
 }

client/app/components/coordinator/coordinator.html

@@ -125,30 +125,9 @@
                                 </div>
                                 <div class="panel-body text-center">
                                         <div class="status-list">
-                                            <div class="observed">
-                                                <div class="progress" id="frankaForceObservedLeft">
-                                                    <div class="progress-bar bg-success" role="progressbar" id="frankaForceObservedBar" aria-valuenow="25" aria-valuemin="0" aria-valuemax="200">
-                                                        <span>N/A</span>
-                                                        <div>
-                                                            <span>Observed</span>
-                                                        </div>
-                                                    </div>
-                                                </div>
-                                                <ul class="list-group list-group-horizontal text-center">
-                                                    <li class="list-group-item force-value">
-                                                        <h4>X: <span id="frankaForceLeftObservedX">0.0</span></h4>
-                                                    </li>
-                                                    <li class="list-group-item force-value">
-                                                        <h4>Y: <span id="frankaForceLeftObservedY">0.0</span></h4>
-                                                    </li>
-                                                    <li class="list-group-item force-value">
-                                                        <h4>Z: <span id="frankaForceLeftObservedZ">0.0</span></h4>
-                                                    </li>        
-                                                </ul>
-                                            </div>
                                             <div class="commanded">
-                                                <div class="progress" id="frankaForceCommandedLeft">
+                                                <div class="progress">
-                                                    <div class="progress-bar bg-success" role="progressbar" id="frankaForceCommandedBar" aria-valuenow="25" aria-valuemin="0" aria-valuemax="200">N/A</div>
+                                                    <div class="progress-bar bg-success" role="progressbar" id="frankaForceCommandedLeft" aria-valuenow="25" aria-valuemin="0" aria-valuemax="200">N/A</div>
                                                 </div>
                                                 <ul class="list-group list-group-horizontal text-center">
                                                     <li class="list-group-item force-value">
@@ -162,12 +141,31 @@
                                                     </li>        
                                                 </ul>
                                             </div>
+                                            <div class="observed">
+                                              <div class="progress">
+                                                  <div class="progress-bar bg-success" role="progressbar" id="frankaForceObservedLeft" aria-valuenow="25" aria-valuemin="0" aria-valuemax="200">
+                                                      <span>N/A</span>
+                                                      <div>
+                                                          <span>Observed</span>
+                                                      </div>
+                                                  </div>
+                                              </div>
+                                              <ul class="list-group list-group-horizontal text-center">
+                                                  <li class="list-group-item force-value">
+                                                      <h4>X: <span id="frankaForceLeftObservedX">0.0</span></h4>
+                                                  </li>
+                                                  <li class="list-group-item force-value">
+                                                      <h4>Y: <span id="frankaForceLeftObservedY">0.0</span></h4>
+                                                  </li>
+                                                  <li class="list-group-item force-value">
+                                                      <h4>Z: <span id="frankaForceLeftObservedZ">0.0</span></h4>
+                                                  </li>        
+                                              </ul>
+                                          </div>
                                         </div>
                                 </div>
                                 <div class="action-button panel-footer">
-                                    <button id="frankaForceLeftCalibrate" type="button" onclick="triggerService('')" class="btn">
+
-                                        Calibrate 
-                                    </button>
                                 </div>
                             </div>
                         </div>
@@ -213,10 +211,10 @@
                             
                             <div class="panel-body text-center">
                                 <div class="button-area">
-                                    <button type="button" onclick="triggerService('/left/aescape/hardware/setTrainingEEF')" active class="btn btn-primary col-md-6">
+                                    <button type="button" onclick="setEEFConfig('left', 'teach')" active class="btn btn-primary col-md-6">
                                         Recording EEF
                                     </button>            
-                                    <button type="button" onclick="triggerService('/left/aescape/hardware/setExecutionEEF')" class="btn btn-primary col-md-6">
+                                    <button type="button" onclick="setEEFConfig('left', 'play')" class="btn btn-primary col-md-6">
                                         Massage EEF
                                     </button>
                                 </div>
@@ -229,7 +227,7 @@
                         </div>                        
                     </div>
                     <div class="control-container col-md-6">
-                        <div class="control-card-six control-card panel panel-default">
+                        <div id="left-operation-mode" class="control-card-six control-card panel panel-default">
                             <div class="panel-heading">
                                 Operation Mode
                                 <i class="fas fa-info-circle" tabindex="0" data-toggle="popover" data-trigger="focus" data-content="This selects the operation mode of the left arm."></i>
@@ -312,14 +310,11 @@
                             </div>
                             <div class="panel-body text-center">
                                 <div class="button-area">
-                                        <button type="button" onclick="triggerService('/left/aescape/mode/activateReadyController')" class="btn col-md-4">
+                                        <button id="recordingStartButton" type="button" onclick="startTeachRecoding()" class="btn btn-primary col-md-6">
-                                            Activate Ready Mode
+                                            Start Recording
                                         </button>
-                                        <button id="recordingStartButton" type="button" onclick="triggerService('/left/aescape/hardware/calibrateRobotiq'); triggerService('/left/aescape/mode/activateTeachingController'); triggerService('/aescape/bags/startTeachRecording')" class="btn btn-primary col-md-4">
+                                        <button id="recordingStopButton" type="button" onclick="stopTeachRecording()" class="btn btn-primary col-md-6">
-                                            Start Teach Recording
+                                            Stop Recording
-                                        </button>
-                                        <button id="recordingStopButton" type="button" onclick="triggerService('/aescape/bags/stopTeachRecording'); triggerService('/left/aescape/mode/activateReadyController'); displayTaggingOptions('teach')" class="btn btn-primary col-md-4">
-                                            Stop Teach Recording
                                         </button>
                                 </div>
                             </div>
@@ -354,16 +349,13 @@
                                 </div>
                                 <div class="panel-body text-center">
                                     <div class="button-area">
-                                        <button type="button" onclick="triggerService('/left/aescape/mode/activateReadyController')" class="btn col-md-3">
+                                        <button id="executionLoadButton" type="button" onclick="displayLoadingOptions()" class="btn btn-primary col-md-4">
-                                            Activate Ready Mode
-                                        </button>
-                                        <button id="executionLoadButton" type="button" onclick="displayLoadingOptions()" class="btn btn-primary col-md-3">
                                             Load Recording
                                         </button>
-                                        <button id="executionStartButton" type="button" onclick="triggerService('/left/aescape/mode/activateExecutionController'); triggerTopic('/left/run_trajectory')" class="btn btn-primary col-md-3">
+                                        <button id="executionStartButton" type="button" onclick="playLoadedRecording()" class="btn btn-primary col-md-4">
                                             Play Loaded Recording
                                         </button>
-                                        <button id="executionStopButton" type="button" onclick="triggerService('/aescape/bags/stopPlayingBag'); triggerService('/left/aescape/mode/activateReadyController'); displayTaggingOptions('play')" class="btn btn-primary col-md-3">
+                                        <button id="executionStopButton" type="button" onclick="stopPlayingRecording()" class="btn btn-primary col-md-4">
                                             Stop Playing Recording
                                         </button>
                                     </div>
@@ -407,7 +399,8 @@
                                             </div>
                                         </div>
                                         <div class="col-md-2">
-                                            <i class="fas fa-check-circle" onclick="hideLoadingOptions(true)"></i>
+                                            <!-- <i class="fas fa-check-circle" onclick="hideLoadingOptions(true)"></i> -->
+                                            <button class="primary" onclick="hideLoadingOptions(true)">Load Bag</button>
                                             <i class="fas fa-times-circle" onclick="hideLoadingOptions(false)"></i>
                                         </div>
                                     </div>
@@ -541,6 +534,57 @@
                                         </div>
                                     </div>
                                 </div>
+                                <div class="status-container">
+                                    <div class="franka-force force-bar panel panel-default panel-horizontal">
+                                        <div class="panel-heading">
+                                            Franka Force
+                                        </div>
+                                        <div class="panel-body text-center">
+                                                <div class="status-list">
+                                                    <div class="commanded">
+                                                        <div class="progress">
+                                                            <div class="progress-bar bg-success" role="progressbar" id="frankaForceCommandedRight" aria-valuenow="25" aria-valuemin="0" aria-valuemax="200">N/A</div>
+                                                        </div>
+                                                        <ul class="list-group list-group-horizontal text-center">
+                                                            <li class="list-group-item force-value">
+                                                                <h4>X: <span id="frankaForceRightCommandedX">0.0</span></h4>
+                                                            </li>
+                                                            <li class="list-group-item force-value">
+                                                                <h4>Y: <span id="frankaForceRightCommandedY">0.0</span></h4>
+                                                            </li>
+                                                            <li class="list-group-item force-value">
+                                                                <h4>Z: <span id="frankaForceRightCommandedZ">0.0</span></h4>
+                                                            </li>        
+                                                        </ul>
+                                                    </div>
+                                                    <div class="observed">
+                                                      <div class="progress">
+                                                          <div class="progress-bar bg-success" role="progressbar" id="frankaForceObservedRight" aria-valuenow="25" aria-valuemin="0" aria-valuemax="200">
+                                                              <span>N/A</span>
+                                                              <div>
+                                                                  <span>Observed</span>
+                                                              </div>
+                                                          </div>
+                                                      </div>
+                                                      <ul class="list-group list-group-horizontal text-center">
+                                                          <li class="list-group-item force-value">
+                                                              <h4>X: <span id="frankaForceRightObservedX">0.0</span></h4>
+                                                          </li>
+                                                          <li class="list-group-item force-value">
+                                                              <h4>Y: <span id="frankaForceRightObservedY">0.0</span></h4>
+                                                          </li>
+                                                          <li class="list-group-item force-value">
+                                                              <h4>Z: <span id="frankaForceRightObservedZ">0.0</span></h4>
+                                                          </li>        
+                                                      </ul>
+                                                  </div>
+                                                </div>
+                                        </div>
+                                        <div class="action-button panel-footer">
+        
+                                        </div>
+                                    </div>
+                                </div>
                             </div>
     
                             <div class="control-container col-md-6">
@@ -551,10 +595,10 @@
                                     </div>
                                     <div class="panel-body text-center">
                                         <div class="button-area">
-                                            <button type="button" onclick="triggerService('/right/aescape/hardware/setTrainingEEF')" class="btn btn-primary col-md-6">
+                                            <button type="button" onclick="setEEFConfig('right', 'teach')" class="btn btn-primary col-md-6">
                                                 Recording EEF
                                             </button>            
-                                            <button type="button" onclick="triggerService('/right/aescape/hardware/setExecutionEEF')" class="btn btn-primary col-md-6">
+                                            <button type="button" onclick="setEEFConfig('right', 'play')" class="btn btn-primary col-md-6">
                                                 Massage EEF
                                             </button>
                                         </div>
@@ -567,7 +611,7 @@
                                 </div>                        
                             </div>
                             <div class="control-container col-md-6">
-                                <div class="control-card-six control-card panel panel-default">
+                                <div id="right-operation-mode" class="control-card-six control-card panel panel-default">
                                     <div class="panel-heading">
                                         Operation Mode
                                         <i class="fas fa-info-circle" tabindex="0" data-toggle="popover" data-trigger="focus" data-placement="bottom" data-content="This selects the operation mode of the right arm."></i>

client/app/components/coordinator/js/ros_scripts.js

@@ -60,6 +60,26 @@ class ArmControls
     });
     this.safetyStatusTopic.subscribe(this.safetyStatus.bind(this));
 
+    this.frankaForceObservedTopic = new ROSLIB.Topic({
+      ros : ros,
+      name : '/'+this.namespace+'/monitor/base_wrench', 
+      messageType : 'geometry_msgs/WrenchStamped',
+      throttle_rate : 500 // 2Hz
+    });
+
+    this.frankaForceObservedTopic.subscribe(this.frankaForce.bind(this, 'observed'));
+
+    this.frankaForceCommandedTopic = new ROSLIB.Topic({
+      ros : ros,
+      name : '/'+this.namespace+'/robot_goal', 
+      messageType : 'geometry_msgs/WrenchStamped',
+      throttle_rate : 500 // 2Hz
+    });
+
+    this.frankaForceCommandedTopic.subscribe(this.frankaForce.bind(this, 'commanded'));
+
+    
+  
     this.lastActiveButton = {
       safetyMode : null, 
       frankaMode : null, 
@@ -180,6 +200,34 @@ class ArmControls
 
     this.lastActiveButton.safetyMode = message.data;
   }
+
+  frankaForce(forceTypeCode, message) {
+    if(this.namespace == 'left') {
+      var bindedArm = "Left";
+    }
+    if(this.namespace == 'right') {
+      var bindedArm = "Right";
+    }
+    if (forceTypeCode == 'observed') {
+      var forceType = "Observed";
+    }
+    if (forceTypeCode == 'commanded') {
+      var forceType = "Commanded";
+    }
+    var force = message.wrench.force
+    document.getElementById("frankaForce" + bindedArm + forceType + "X").innerHTML = force.x.toFixed(1)
+    document.getElementById("frankaForce" + bindedArm + forceType + "Y").innerHTML = force.y.toFixed(1)
+    document.getElementById("frankaForce" + bindedArm + forceType + "Z").innerHTML = force.z.toFixed(1)
+  
+    var totalForce = math.norm([force.x, force.y, force.z]).toFixed(1)
+    var totalForcePercent = totalForce/2
+  
+    var doc = document.getElementById("frankaForce" + forceType + bindedArm);
+    doc.style.width = totalForcePercent + "%";
+    doc.innerHTML = totalForce + " N";
+    doc.setAttribute('aria-valuenow', totalForce);
+  }
+
 }
 
 var leftArmControls = new ArmControls('left');
@@ -314,10 +362,9 @@ bagPlayingTopic.subscribe(function(message) {
 // Services
 ////////////////////////////////////////////////////////////////
 
-
 function triggerService(serviceName, toSend, callback) {
 
-  callback = callback || $.noop;
+  var callback = callback || $.noop;
 
   if(!toSend) {
     serviceType = 'std_srvs/demobot.Trigger'
@@ -328,7 +375,7 @@ function triggerService(serviceName, toSend, callback) {
   var service = new ROSLIB.Service({
     ros : ros,
     name : serviceName,
-    serviceType : serviceType
+    serviceType : serviceType, 
   });
 
   if(!toSend) {
@@ -338,6 +385,8 @@ function triggerService(serviceName, toSend, callback) {
     var request = new ROSLIB.ServiceRequest({message : toSend});  
   }
 
+  var hellobiatch = "hellobiatch"
+
 
   /*
   var service = new ROSLIB.Service({
@@ -354,13 +403,13 @@ function triggerService(serviceName, toSend, callback) {
   */
   
 
-  var request = new ROSLIB.ServiceRequest({message : toSend});  
+  //var request = new ROSLIB.ServiceRequest({message : toSend});  
 
 
   var serviceCallTimeStamp = Date.now();
   var serviceCallReturned = false;
   service.callService(request, function(result) {
-      //check for result.success == false
+      serviceCallReturned = true;
       if(result.success == true) {
         $(document).ready(function(){
           $.notify({
@@ -374,7 +423,9 @@ function triggerService(serviceName, toSend, callback) {
             delay: 1000 
           });
         });
-        callback({success : true});
+        console.log("callback from callService in triggerService");
+        console.log(callback);
+        return callback(null, {success : true});
       } else {
         $(document).ready(function(){
           $.notify({
@@ -388,11 +439,11 @@ function triggerService(serviceName, toSend, callback) {
             delay: 1000 
           });
         });
-        callback({success : false});
+        return callback("Something went wrong :( " + serviceName, {success : false});
       }
-      serviceCallReturned = true;
   }, function(error){
       $(document).ready(function(){
+        serviceCallReturned = true;
         $.notify({
           message: "Something went wrong :( " + serviceName
         }, {
@@ -404,8 +455,7 @@ function triggerService(serviceName, toSend, callback) {
           delay: 1000 
         });
       });
-      serviceCallReturned = true;
+      return callback("Something went wrong :( " + serviceName, {success : false})
-      callback({success : false})
   });
   var responseCheckInterval = setInterval(function() {
     var timeElapsedSinceServiceCall = Date.now() - serviceCallTimeStamp; // milliseconds elapsed since start
@@ -426,7 +476,6 @@ function triggerService(serviceName, toSend, callback) {
           });
         });
         clearInterval(responseCheckInterval);
-        callback({success : true})
         return;
     }
   }, 1000);
@@ -439,10 +488,97 @@ function invokeSafetyController(event)
    var keyID = event.keyCode;
    //8 == backspace, 46 == delete
    if(keyID == 46) {
-    triggerService('/aescape/safety_activate')
+    triggerService('/aescape/safety_activate');
    }
 }
 
+function setEEFConfig(arm, config) {
+  var operationModeSelector = '#' + arm + '-operation-mode';
+  document.querySelectorAll(operationModeSelector + ' button').forEach(function(element){
+    element.classList.add('disabled')
+  })
+  var serviceMap = {
+    teach : 'setTrainingEEF', 
+    play : 'setExecutionEEF'
+  }
+  var service = '/' + arm + '/aescape/hardware/' + serviceMap[config]
+  triggerService(service, false, function(error, result){
+    document.querySelectorAll(operationModeSelector + ' button').forEach(function(element){
+      element.classList.remove('disabled')
+    })
+  })
+}
+
+function startTeachRecoding() {
+  async.waterfall([
+      function(callback) {
+          triggerService('/left/aescape/hardware/calibrateRobotiq', false, function(){
+            callback();
+          });
+      },
+      function(response, callback) {
+          triggerService('/left/aescape/mode/activateTeachingController', false, callback);
+      },
+      function(response, callback) {
+          triggerService('/aescape/bags/startTeachRecording', false, callback)
+      }
+  ], function (err, result) {
+
+  });
+}
+
+function stopTeachRecording() {
+  async.waterfall([
+      function(callback) {
+          console.log("function 1");
+          triggerService('/aescape/bags/stopTeachRecording', false, callback);
+      },
+      function(response, callback) {
+          console.log("function 2");
+          triggerService('/left/aescape/mode/activateReadyController', false, callback);
+      },
+      function(response, callback) {
+          console.log("function 3");
+          console.log(callback)
+          displayTaggingOptions('teach');
+          callback();
+      }
+  ], function (err, result) {
+    console.log(err);
+  });
+}
+
+
+function playLoadedRecording() {
+  async.waterfall([
+      function(callback) {
+        triggerService('/left/aescape/mode/activateExecutionController', false, callback); 
+      },
+      function(response, callback) {
+        triggerTopic('/left/run_trajectory', false, callback)
+      }
+  ], function (err, result) {
+
+  });
+}
+
+function stopPlayingRecording() {
+  async.waterfall([
+      function(callback) {
+          triggerService('/aescape/bags/stopPlayingBag', false, callback);
+      },
+      function(response, callback) {
+          triggerService('/left/aescape/mode/activateReadyController', false, callback);
+      },
+      function(response, callback) {
+          displayTaggingOptions('play');
+          callback();
+      }
+  ], function (err, result) {
+      // result now equals 'done'
+  });
+}
+
 
 
 

client/app/components/coordinator/js/update_guis.js

@@ -88,7 +88,11 @@ function displayTaggingOptions(mode) {
     fileNameSelectize.addOption(results.map(function(element){
       return {name : element.filename}
     }));
+    if(results[0]) {
+      fileNameSelectize.setValue(results[0].filename);
+    }
   })
+  
 
   var $categoryNameSelect = $(categorySelector).selectize({
     valueField: 'name',
@@ -133,13 +137,14 @@ function hideTaggingOptions(mode, updateValues=false) {
   }
 
   var modeTag = '#' + mode + 'Tags';
+  var fileNameSelector = '#' + mode + 'BagFileName';
+  var categorySelector = '#' + mode + 'BagCategory';
+  var tagSelector = '#' + mode + 'BagTags';
+  var errorSelector = '#' + mode + 'TagError';
+
+
 
   if(updateValues) {
-    var fileNameSelector = '#' + mode + 'BagFileName';
-    var categorySelector = '#' + mode + 'BagCategory';
-    var tagSelector = '#' + mode + 'BagTags';
-    var errorSelector = '#' + mode + 'TagError';
-
     var filename = $(fileNameSelector).val();
     var category = $(categorySelector).val();
     var tags = $(tagSelector).val();
@@ -156,6 +161,10 @@ function hideTaggingOptions(mode, updateValues=false) {
           displayMessage(flase, updateError);
         } else {
           displayMessage("Bag data saved successfully!");
+          //clear selectize
+            $(fileNameSelector).selectize()[0].selectize.destroy();
+            $(categorySelector).selectize()[0].selectize.destroy();
+            $(tagSelector).selectize()[0].selectize.destroy();
         }
       });
     })
@@ -181,7 +190,7 @@ function updateBagMetadata(mode, bag_name, modified_bag_name, category, tags, ca
     tags : tags
   }
 
-  triggerService('/aescape/bags/updateBagMeta', toSend, function(result) {
+  triggerService('/aescape/bags/updateBagMeta', toSend, function(error, result) {
     if(!result) {
       return callback("Something went wrong in the backend while updating.");
     }
@@ -306,7 +315,7 @@ function loadSelectedBagFile(selectedBagName, singleArmMode, callback) {
     single_arm : singleArmMode
   }
 
-  triggerService('/aescape/bags/startPlayingRecording', toSend, function(result) {
+  triggerService('/aescape/bags/startPlayingRecording', toSend, function(error, result) {
     if(!result) {
       return callback("Something went wrong in the backend while updating.");
     }

client/app/main.css

@@ -467,4 +467,5 @@ html, body {
         height: 100% !important;
     }
     */
+
     

client/index.html

@@ -11,14 +11,14 @@
 
 
         <script type="text/javascript" src="include/js/eventemitter2.min.js"></script>
+        <script type="text/javascript" src="include/js/roslib.js"></script>
                 
         <script src="include/js/jquery.min.js"></script>
         <script src="include/js/bootstrap.min.js"></script>
         <script src="include/js/math.min.js"></script>
 
-        <script src="../include/js/roslib.js"></script>
-        <script type="text/javascript" src="include/js/roslib.js"></script>
 
+        <script src="include/bower_components/async/dist/async.js"></script>
         <script src="//cdnjs.cloudflare.com/ajax/libs/annyang/2.6.0/annyang.min.js"></script>
         <script>
         if (annyang) {