From ce93585dba45834ba74a40dcfa3439081d18f91c Mon Sep 17 00:00:00 2001
From: oliverkania <oliver.kania@rwth-aachen.de>
Date: Tue, 29 Apr 2025 18:43:33 +0200
Subject: [PATCH] added node for tof reading
---
my_robot_package/tof_pico_node.py | 74 +++++++++++++++++++++++++++++++
setup.py | 1 +
2 files changed, 75 insertions(+)
create mode 100644 my_robot_package/tof_pico_node.py
diff --git a/my_robot_package/tof_pico_node.py b/my_robot_package/tof_pico_node.py
new file mode 100644
index 0000000..651c9e3
--- /dev/null
+++ b/my_robot_package/tof_pico_node.py
@@ -0,0 +1,74 @@
+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()
diff --git a/setup.py b/setup.py
index 3183c99..8f8fdb0 100644
--- a/setup.py
+++ b/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',
],
},
)
--
GitLab