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