From 71cc38813c2587c5bc77f3381202e0f545fd866d Mon Sep 17 00:00:00 2001
From: David Walsh <david@aescape.co>
Date: Thu, 28 Mar 2019 13:30:56 -0400
Subject: [PATCH] Robotiq data subscription

---
 coordinator/coordinator.html  | 22 ++++++++++++++++++----
 coordinator/js/ros_scripts.js | 22 ++++++++++++++++++++++
 2 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/coordinator/coordinator.html b/coordinator/coordinator.html
index a105b88..2f88272 100644
--- a/coordinator/coordinator.html
+++ b/coordinator/coordinator.html
@@ -57,12 +57,26 @@
     </div>
     <div class="panel panel-default">
         <div class="panel-heading">
-            Hardware Commands
+            Robotiq
         </div>
         <div class="panel-body text-center">
-            <button id="calibrateButton" type="button" onclick="triggerService('/aescape/hardware/calibrateRobotiq')" class="btn btn-primary">
-                Calibrate Robotiq
-            </button>
+            <div class="col-md-4">
+                Current Robotiq Values:
+                <div class="row-md-3">
+                    <h4>X: <span id="robotiqX" class="label label-default">0.0</span></h4>
+                </div>
+                <div class="row-md-3">
+                    <h4>Y: <span id="robotiqY" class="label label-default">0.0</span></h4>
+                </div>
+                <div class="row-md-3">
+                    <h4>Z: <span id="robotiqZ" class="label label-default">0.0</span></h4>
+                </div>
+            </div>
+            <div class="row-md-3">
+                <button id="calibrateButton" type="button" onclick="triggerService('/aescape/hardware/calibrateRobotiq')" class="btn btn-primary">
+                    Calibrate Robotiq
+                </button>
+            </div>
         </div>
     </div>
     <div class="panel panel-default">
diff --git a/coordinator/js/ros_scripts.js b/coordinator/js/ros_scripts.js
index 3702390..28a0215 100644
--- a/coordinator/js/ros_scripts.js
+++ b/coordinator/js/ros_scripts.js
@@ -110,6 +110,28 @@ frankaStatus.subscribe(function(message) {
   }
 });
 
+
+// Robotiq Data
+var robotiqDataTopic = new ROSLIB.Topic({
+  ros : ros,
+  name : '/robotiq_ft_wrench',
+  messageType : 'geometry_msgs/WrenchStamped'
+});
+
+robotiqDataTopic.subscribe(function(message) {
+  var force = message.wrench.force
+  document.getElementById("robotiqX").innerHTML = force.x.toFixed(1)
+  document.getElementById("robotiqY").innerHTML = force.y.toFixed(1)
+  document.getElementById("robotiqZ").innerHTML = force.z.toFixed(1)
+  
+  if ((Math.abs(force.x) > 0.5) || (Math.abs(force.y) > 0.5) || (Math.abs(force.z) > 0.5))
+  {
+    document.getElementById("calibrateButton").className = "btn btn-danger"
+  } else {
+    document.getElementById("calibrateButton").className = "btn btn-default"
+  }
+});
+
 ////////////////////////////////////////////////////////////////
 // Services
 ////////////////////////////////////////////////////////////////