実際のところ
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 --- ...
このままだと値がバタついて使い物にならないので、実用の場合は何かしらのフィルタリング処理が必要でしょうね。