added node for tof reading

my_robot_package/tof_pico_node.py

+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()

setup.py

@@ -24,6 +24,7 @@ setup(
     entry_points={
         'console_scripts': [
         'joint_reader = my_robot_package.joint_reader:main',
+        'tof_pico_node = my_robot_package.tof_pico_node:main',
         ],
     },
 )