From 09f12fd0b19f4b9aad1308cb6fbb79bc8abb98b7 Mon Sep 17 00:00:00 2001
From: kelvin <kelvin.valani@gmail.com>
Date: Thu, 30 Sep 2021 02:05:44 +1000
Subject: [PATCH] induction task subscriber added

---
 obstacle_detector.py | 65 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 65 insertions(+)
 create mode 100755 obstacle_detector.py

diff --git a/obstacle_detector.py b/obstacle_detector.py
new file mode 100755
index 0000000..7113e8e
--- /dev/null
+++ b/obstacle_detector.py
@@ -0,0 +1,65 @@
+
+
+#!/usr/bin/env python3
+
+'''
+Name : kelvin valani
+Nova rover induction task
+'''
+
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from std_msgs.msg import Float32
+from std_msgs.msg import String
+import struct
+class Subscriber (Node):
+    
+    def __init__(self):
+        '''
+        Intialising the subscriber and the publisher  that will publish immeditely after receving data and 
+        perofrming necessary calculations
+        '''
+        super().__init__('obstacle_detector')
+        self.subscription = self.create_subscription(PointCloud2, '/D435/depth/color/points', self.callback_func, 10)
+        self.publisher = self.create_publisher(Float32,'obstacle_proximity',10)
+    def callback_func (self, msg):
+        '''
+	receives the data field from the point cloud and calcualtes the average distance (zvalue average)
+        from the byte arrays that are given.
+        '''
+        sumZvals = 0
+        nZvals = 0
+        avg =  Float32()
+        for i in range(0,len(msg.data),20):
+                x = floatify_byte(msg.data[i:i+4])
+                y = floatify_byte(msg.data[i+4:i+8])
+                z = floatify_byte(msg.data[i+8:i+12])
+              
+                if((-0.15<x<0.15) and (-0.15<y<0.15) and (0.3<z<3.0)):
+                        nZvals = nZvals + 1
+                        sumZvals = sumZvals + z
+        avg.data = sumZvals/nZvals
+        output ="current distance from obstacle is " +  str(avg.data)
+        self.publisher.publish(avg)
+        self.get_logger().info(output)
+
+                
+
+def floatify_byte(arr):
+        '''
+        converts the array of values into a byte array and then converts it into a small elidian form float
+        '''
+	
+        return struct.unpack('<f', bytearray(arr))[0]
+
+
+
+def main ():
+    rclpy.init()
+    subscriber = Subscriber()
+    rclpy.spin(subscriber)
+
+if __name__ == "__main__":
+    main()