Bye Bye Moore

PoCソルジャーな零細事業主が作業メモを残すブログ

YDLiDAR X4の一番近い距離についてROS2 humbleのtopicとして流す

実際のところ

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float32

class ClosestDistancePublisher(Node):
    def __init__(self):
        super().__init__('closest_distance_publisher')
        qos_profile = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)
        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.scan_callback,
            qos_profile)
        self.publisher = self.create_publisher(Float32, 'closest_distance', qos_profile)

    def scan_callback(self, msg):
        # 最も近い距離が0ではない範囲を見つける
        closest_distance = min([distance for distance in msg.ranges if distance > 0.0])
        self.publish_closest_distance(closest_distance)

    def publish_closest_distance(self, closest_distance):
        distance_msg = Float32()
        distance_msg.data = closest_distance
        self.publisher.publish(distance_msg)
        self.get_logger().info('Published Closest Distance: "%s"' % closest_distance)

def main(args=None):
    rclpy.init(args=args)
    node = ClosestDistancePublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
$ python3 testlidar.py 
[INFO] [1699789825.058294669] [closest_distance_publisher]: Published Closest Distance: "0.2939999997615814"
[INFO] [1699789825.092901897] [closest_distance_publisher]: Published Closest Distance: "0.2669999897480011"
...
$ ros2 topic echo /closest_distance
data: 0.3540000021457672
---
data: 0.31299999356269836
---
data: 0.367000013589859
---
...

このままだと値がバタついて使い物にならないので、実用の場合は何かしらのフィルタリング処理が必要でしょうね。