ROSでLidarセンサーを使う時、ロボットの車体など一定角度の範囲に常に障害物があり、これを無視したい時があると思いますが、その方法を紹介します。
以下は、得られた/scan_rawトピックのデータの一定角度の範囲を無視して新しい/scanトピックを発行する例を紹介します。
※Ubuntu 18.04 ROS melodicにて動作確認済みです。
#!/usr/bin/python # -*- coding: utf-8 -*- import rospy import numpy as np from sensor_msgs.msg import LaserScan scan_publisher = None def convert_scan(data): global scan_publisher ignore_angle_min = -0.523599 ignore_angle_max = 0.523599 # 新しいLaserScanメッセージを作成します。 filtered_scan = LaserScan() filtered_scan.header = data.header filtered_scan.angle_min = data.angle_min filtered_scan.angle_max = data.angle_max filtered_scan.angle_increment = data.angle_increment filtered_scan.time_increment = data.time_increment filtered_scan.scan_time = data.scan_time filtered_scan.range_min = data.range_min filtered_scan.range_max = data.range_max # 無視する範囲のデータを除外します。 for i, angle in enumerate(np.arange(data.angle_min, data.angle_max, data.angle_increment)): if angle > ignore_angle_min and angle < ignore_angle_max: filtered_scan.ranges.append(float('nan')) else: filtered_scan.ranges.append(data.ranges[i]) # フィルタリング後のデータをパブリッシュします。 scan_publisher.publish(filtered_scan) if __name__ == '__main__': rospy.init_node('ignore_back_scan', log_level=rospy.DEBUG) rospy.Subscriber('/scan_raw', LaserScan, convert_scan) scan_publisher = rospy.Publisher('/scan', LaserScan, queue_size=10) r = rospy.Rate(10) while not rospy.is_shutdown(): r.sleep()
上記の例では、-30度から30度の範囲を除外するようにフィルタリングしています。無視する角度の範囲は、必要に応じて変更することができます。また、Subscribe/Publishするトピック名ももちろん変更することが可能です。