From a6a034315404fa9bd3f31671d8cfb4480c5f9f96 Mon Sep 17 00:00:00 2001
From: David Walsh <david@aescape.co>
Date: Fri, 19 Apr 2019 18:29:16 -0400
Subject: [PATCH] Add safety controller status

---
 coordinator/coordinator.html  | 48 ++++++++++++++++++++++++-----------
 coordinator/js/ros_scripts.js | 22 ++++++++++++++++
 2 files changed, 55 insertions(+), 15 deletions(-)

diff --git a/coordinator/coordinator.html b/coordinator/coordinator.html
index e56c72e..7a14021 100644
--- a/coordinator/coordinator.html
+++ b/coordinator/coordinator.html
@@ -4,22 +4,40 @@
             Select Operation Mode
         </div>
         <div class="panel-body text-center">
-            <button id="teachingModeButton" type="button" onclick="triggerService('/aescape/mode/activateTeachingController')" class="btn btn-primary">
-                Teaching Mode
-            </button>
-            <button id="executionModeButton" type="button" onclick="triggerService('/aescape/mode/activateExecutionController')" class="btn btn-primary">
-                Massage Mode
-            </button>
-            <button id="readyModeButton" type="button" onclick="triggerService('/aescape/mode/activateReadyController')" class="btn btn-primary">
-                Ready Mode
-            </button>
-            <button id="standbyModeButton" type="button" onclick="triggerService('/aescape/mode/activateStandbyController')" class="btn btn-primary">
-                Standby Mode
-            </button>            
-            <button id="stoppedModeButton" type="button" onclick="triggerService('/aescape/mode/stopControllers')" class="btn btn-primary">
-                Stopped Mode
-            </button>
+            <div class="col-md-6">
+                <button id="teachingModeButton" type="button" onclick="triggerService('/aescape/mode/activateTeachingController')" class="btn btn-primary">
+                    Teaching Mode
+                </button>
+                <button id="executionModeButton" type="button" onclick="triggerService('/aescape/mode/activateExecutionController')" class="btn btn-primary">
+                    Massage Mode
+                </button>
+                <button id="readyModeButton" type="button" onclick="triggerService('/aescape/mode/activateReadyController')" class="btn btn-primary">
+                    Ready Mode
+                </button>
+                <button id="standbyModeButton" type="button" onclick="triggerService('/aescape/mode/activateStandbyController')" class="btn btn-primary">
+                    Standby Mode
+                </button>            
+                <button id="stoppedModeButton" type="button" onclick="triggerService('/aescape/mode/stopControllers')" class="btn btn-primary">
+                    Stopped Mode
+                </button>
+            </div>
+            <div class="col-md-6">
+                Safety Monitor Status:
+                <div class="row-md-3">
+                    <h4><span id="safetyRunning" class="label label-default">Running</span></h4>
+                </div>
+                <div class="row-md-3">
+                    <h4><span id="safetyStopped" class="label label-default">Stopped</span></h4>
+                </div>
+                <div class="row-md-3">
+                    <button type="button" onclick="triggerService('/aescape/mode/activateSafetyController')" class="btn btn-sm btn-default">
+                        Activate Safety Monitor
+                    </button>
+                </div>
+            </div>
         </div>
+
+
     </div>
     <div class="panel panel-default">
         <div class="panel-heading">
diff --git a/coordinator/js/ros_scripts.js b/coordinator/js/ros_scripts.js
index 8a8a335..ebd53bc 100644
--- a/coordinator/js/ros_scripts.js
+++ b/coordinator/js/ros_scripts.js
@@ -60,6 +60,28 @@ modeStatus.subscribe(function(message) {
   }
 });
 
+// Safety Status
+var safetyStatusTopic = new ROSLIB.Topic({
+  ros : ros,
+  name : '/aescape/mode/safety_status',
+  messageType : 'std_msgs/String',
+  throttle_rate : 500 // 2Hz
+});
+
+safetyStatusTopic.subscribe(function(message) {
+  document.getElementById("safetyRunning").className = 'label-default';
+  document.getElementById("safetyStopped").className = 'label-default';
+  
+  if (message.data == "stopped") 
+  {
+    document.getElementById("safetyStopped").className = 'label-danger';
+  } 
+  else if (message.data == "running")
+  {
+    document.getElementById("safetyRunning").className = 'label-success';
+  }
+});
+
 
 // Recording Bag
 var recordingBagTopic = new ROSLIB.Topic({