4. Extrait de Code & Intégration (Nœud ROS2 Laser Télémétrie (rclpy))
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
import serial
class TofLidarNode(Node):
"""Nœud ROS2 publiant la distance laser ToF sur /sensors/laser_distance."""
def __init__(self):
super().__init__('tof_lidar_node')
self.pub = self.create_publisher(Range, '/sensors/laser_distance', 10)
self.ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.1)
self.timer = self.create_timer(0.01, self.read_distance) # 100 Hz
def read_distance(self):
if self.ser.read(1) == b'\x59' and self.ser.read(1) == b'\x59':
data = self.ser.read(7)
dist_cm = data[0] + (data[1] << 8)
msg = Range()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'lidar_link'
msg.range = float(dist_cm) / 100.0 # Mètres
self.pub.publish(msg)
def main():
rclpy.init()
node = TofLidarNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()