Select Git revision
typeart-output.patch
-
Simon Schwitanski authoredSimon Schwitanski authored
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()