diff --git a/obstacle_detector.py b/obstacle_detector.py new file mode 100755 index 0000000..5b5fc58 --- /dev/null +++ b/obstacle_detector.py @@ -0,0 +1,108 @@ +#!/usr/bin/python3 +""" +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Monash Nova Rover Team + +A proximity sensor that takes in positional data and calculates +the average "forward" value of points within certain 3D bounds. +Specifically, a forward range of 0.3 - 3.0 and a vertical +and horizontal range of -0.15 - 0.15. +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +NODE: obstacle_detector +TOPICS: + - /D435/depth/color/points [sensor_msgs.msg.PointCloud2] + - /obstacle_proximity [Float32] +SERVICES: + - +ACTIONS: None +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +PACKAGE: task +AUTHOR(S): kelly +CREATION: 27/09/2021 +EDITED: 30/09/2021 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +TODO: + - +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +""" + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +from sensor_msgs.msg import PointCloud2 +from std_msgs.msg import Float32 + +import struct +from bitarray import bitarray + +#---- Subscribes to the topic /D435... and publishes the average Z value +class SubscriberNode(Node): + def __init__(self): + super().__init__('obstacle_detector') + self.subscription = self.create_subscription( + PointCloud2, + '/D435/depth/color/points', + self.sub_callback, + 10) + self.subscription # Avoid unused variable warning + self.publisher = self.create_publisher( + Float32, + '/obstacle_proximity', + 10) + + def sub_callback(self, msg): + """ + Parses positional data, calculates the average value and publishes + it to the topic /obstacle_proximity. + """ + data = msg.data + y_offset = 4 + z_offset = 8 + z_sum = 0.0 + z_count = 0.0 + factor = 5 + # Interates through the dataset and takes the x,y,z values from every 100th subarray. + for i in range(0,len(data),factor*20): + # Deriving the elements corresponding to the x value and converting them into a flaoat. + x_byte = data[slice(i,i+4)] + x_float = floatify_bytes(x_byte) + + y_byte = data[slice(i+y_offset,i+4+y_offset)] + y_float = floatify_bytes(y_byte) + + z_byte = data[slice(i+z_offset,i+4+z_offset)] + z_float = floatify_bytes(z_byte) + # Summing the z values of the corresponding x/y/z values that satisfy the conditions. + if -0.15f", arr)[0] + +#---- Set up parameters and nodes and start the ROS class +def main(args = None): + rclpy.init(args = args) + subscriber = SubscriberNode() + rclpy.spin(subscriber) + + subscriber.destroy_node() + rclpy.shutdown + +if __name__ == '__main__': + main() + +