Skip to content
Snippets Groups Projects
Select Git revision
  • eb8f6f73f90dc1a8ee4eb9ea0bdc3a5f44f259ef
  • main default protected
  • archer
  • llvm13
4 results

typeart-output.patch

Blame
  • tof_pico_node.py 2.54 KiB
    import rclpy
    from rclpy.node import Node
    from sensor_msgs.msg import PointCloud2, PointField
    from std_msgs.msg import Header
    import sensor_msgs_py.point_cloud2 as pc2
    import serial
    import math
    import struct
    
    class VL53PicoPointCloud(Node):
        def __init__(self):
            super().__init__('vl53_pico_pointcloud')
            self.publisher = self.create_publisher(PointCloud2, 'vl53_pointcloud', 10)
            self.ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
            self.timer = self.create_timer(0.1, self.read_and_publish)
    
        def read_and_publish(self):
            line = self.ser.readline().decode().strip()
            try:
                values = list(map(float, line.split(',')))
                if len(values) != 64:
                    return
    
                # Convert to meters and filter invalid
                distances_m = [
                    d / 10.0 # eigentlich /1000 für m anstatt mm
                    for d in values
                ]
    
                points = []
                fov_deg = 60.0
                step_angle = math.radians(fov_deg / 8)  # 8 rows & columns
                row_offset = -math.radians(fov_deg / 2) + step_angle / 2
                col_offset = -math.radians(fov_deg / 2) + step_angle / 2
    
                for row in range(8):
                    for col in range(8):
                        idx = row * 8 + col
                        d = distances_m[idx]
                        if math.isnan(d):
                            continue
    
                        theta = col_offset + col * step_angle
                        phi = row_offset + row * step_angle
    
                        # Convert spherical to Cartesian (approximation for RViz)
                        x = d * math.cos(phi) * math.sin(theta)
                        y = d * math.sin(phi)
                        z = d * math.cos(phi) * math.cos(theta)
    
                        points.append((x, y, z))
    
                header = Header()
                header.stamp = self.get_clock().now().to_msg()
                header.frame_id = 'vl53_frame'
    
                fields = [
                    PointField(name='x', offset=0,  datatype=PointField.FLOAT32, count=1),
                    PointField(name='y', offset=4,  datatype=PointField.FLOAT32, count=1),
                    PointField(name='z', offset=8,  datatype=PointField.FLOAT32, count=1)
                ]
    
                cloud = pc2.create_cloud(header, fields, points)
                self.publisher.publish(cloud)
    
            except Exception as e:
                self.get_logger().warn(f"Failed to parse or publish: {e}")
    
    def main(args=None):
        rclpy.init(args=args)
        node = VL53PicoPointCloud()
        rclpy.spin(node)
        node.destroy_node()
        rclpy.shutdown()