New computation of safety QA

Now uses a fixed value for acceleration and has the sum of x and y linear velocities
rosin-project · May 21, 2020 · 86a23da · 86a23da
1 parent 55d861f
commit 86a23da
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 8 deletions.
diff --git a/src/rosgraph_monitor/observers/energy_quality_observer.py b/src/rosgraph_monitor/observers/energy_quality_observer.py
@@ -13,9 +13,9 @@ def __init__(self, name):
 
     def calculate_attr(self, msgs):
         status_msg = DiagnosticStatus()
-	
+
         attr =(msgs[0].data - 0.2)/(5.0-0.2) #normalized calue for energy
-        print("{0}".format(str(attr)))
+        print("normalized energy: {0}".format(str(attr)))
 
         status_msg = DiagnosticStatus()
         status_msg.level = DiagnosticStatus.OK

diff --git a/src/rosgraph_monitor/observers/safety_quality_observer.py b/src/rosgraph_monitor/observers/safety_quality_observer.py
@@ -4,6 +4,7 @@
 from diagnostic_msgs.msg import DiagnosticStatus, KeyValue
 from sensor_msgs.msg import Imu
 from nav_msgs.msg import Odometry
+from math import sqrt
 
 
 class SafetyQualityObserver(TopicObserver):
@@ -13,18 +14,28 @@ def __init__(self, name):
         super(SafetyQualityObserver, self).__init__(
             name, 10, topics)
 
+
     def calculate_attr(self, msgs):
         status_msg = DiagnosticStatus()
 
-        attr = msgs[0].twist.twist.linear.x
-        d_break= ((msgs[0].twist.twist.linear.x**2)/(2*msgs[1].linear_acceleration.x))
+        vel_x = msgs[0].twist.twist.linear.x
+        vel_y = msgs[0].twist.twist.linear.y
+
+        velocity = sqrt(vel_x ** 2 + vel_y ** 2)
+
+
+        ## We assume a max acceleration of 1 m/s^2
+        ## The minimum value
+        d_break = (velocity)/(0.4)
 
        # print (d_break)
         normalized_safety=1.0
-        if (d_break>msgs[2].data):
-            normalized_safety=msgs[2].data/d_break
-        print ("{0}".format(d_break))
-        print("{0}".format(normalized_safety))
+        if (d_break > msgs[2].data):
+            normalized_safety = msgs[2].data/d_break
+
+        #print ("d_break: {0}".format(d_break))
+        #print("disntace:{0}".format(msgs[2].data))
+        print("normalized safety:{0}".format(normalized_safety))
         status_msg = DiagnosticStatus()
         status_msg.level = DiagnosticStatus.OK
         status_msg.name = self._id