Gazebo シミュレート上でLIDARをPythonで処理する(ROS2-Foxy)

はじめに
前提条件
Gazebo上のLIDARのデータを取得する
おわりに

はじめに

前回の記事では、cmd_vel と IMU のデータを取得・送信しました。

今回の記事では、LIDAR を Python で取得していきます。

前提条件

前提条件は、以下の通りです。

前回までの記事が完了している

Gazebo上のLIDARのデータを取得する

前回までの記事が完了していれば、Pythonプログラムを作成するだけです。

cd ~/ros_ws/src/ros2_first_test/ros2_first_test
touch get_lidar.py

今作成した get_lidar.py に、以下のコードを記述してください。

import rclpy 
from rclpy.node import Node 
from sensor_msgs.msg import LaserScan

class getLidar(Node):
    def __init__(self):
        # wake up node name
        super().__init__("lidar_subscriber")
        self.subscription = self.create_subscription(LaserScan, "/scan", self.get_scan_values, 10)
        self.subscription

    def get_scan_values(self, scan_data):
        print(scan_data)

def main(args=None):
    rclpy.init(args=args)
    gl = getLidar()
    rclpy.spin_once(gl)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

プログラムの内容は、前回の記事で説明したので省略します。

次に、setup.py にPythonプログラムを追加します。

'console_scripts': [
    'translate_cmdvel=ros2_first_test.translate_cmdvel:main',
    'get_imu=ros2_first_test.get_imu:main',
    'get_lidar=ros2_first_test.get_lidar:main',    ← これを追加
],

ここまで出来たら、ビルドを実行し、シミュレータを起動します。

cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.py

シミュレータが起動したら、以下の画像のように箱をgazebo 上に設置します。

この状態で、別のターミナルを開き、以下のコマンドを実行します。

ros2 run ros2_first_test get_lidar

すると、ターミナルに以下の数値が表示されます。

sensor_msgs.msg.LaserScan(
  header=std_msgs.msg.Header(
    stamp=builtin_interfaces.msg.Time(sec=67, nanosec=590000000), 
    frame_id='lidar_link'
  ), 
  angle_min=0.0, 
  angle_max=3.140000104904175, 
  angle_increment=0.008746517822146416, 
  time_increment=0.0, 
  scan_time=0.0, 
  range_min=0.15000000596046448, 
  range_max=6.0, 
  ranges=[inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 3.1079890727996826, 3.1089258193969727, 3.1121914386749268, 3.112844944000244, 3.0926382541656494, 3.1010019779205322, 3.0826656818389893, 3.095937967300415, 3.082425832748413, 3.080289363861084, 3.0816826820373535, 3.088649272918701, 3.0784549713134766, 3.07792592048645, 3.061919689178467, 3.055903196334839, 3.061077356338501, 3.059711217880249, 3.058401584625244, 3.070952892303467, 3.066819667816162, 3.065356969833374, 3.057581663131714, 3.0550143718719482, 3.0587191581726074, 3.073047637939453, 3.0550243854522705, 3.043236017227173, 3.051419496536255, 3.0651824474334717, 3.068891763687134, 3.057870388031006, 3.0892016887664795, 3.0878193378448486, 3.0551300048828125, 3.0699400901794434, 3.0737242698669434, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf], 
  intensities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

LIDARの受信データの詳細については以前の記事で説明しました。
公式サイトの説明はこちらにあります。

今回はレーザーの強度を取得するオプションを設定していないので、intensities は全て 0 です。
ranges に障害物の位置と距離が格納されています。

データをよりわかりやすくするために、get_lidar.py の get_scan_values を編集します。

def get_scan_values(self, scan_data):
    ranges = scan_data.ranges
    ranges = np.array(ranges)
    length = len(ranges)
    inf = float('inf')
    ranges[ranges == inf] = 0.0
    ranges_args = ranges.nonzero()
    scan_value = ranges[ranges_args]
    print("length={}, \nranges_args={}, \nscan_value={}".format(length, ranges_args, scan_value))

ビルドして get_lidar を実行します。

cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/bashrc
ros2 run ros2_first_test get_lidar

出力は以下の通りです。

length=360,
ranges_args=(array([154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166,
       167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179,
       180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192,
       193, 194, 195, 196, 197, 198]),),
scan_value=[2.5432742 2.5297503 2.5515137 2.5337625 2.51462   2.5241334 2.519027
 2.4925215 2.4989135 2.494576  2.4987764 2.4967434 2.4946518 2.465911
 2.4933004 2.4965827 2.4842067 2.4813433 2.4806154 2.4651732 2.4824047
 2.4775321 2.4687994 2.476408  2.472752  2.4631271 2.4753494 2.457275
 2.4878418 2.4712384 2.4547858 2.4587235 2.4736233 2.4698026 2.4759266
 2.4691157 2.46807   2.4671834 2.470698  2.4616892 2.461269  2.477951
 2.4782434 2.5058408 2.5037754]

今回は 0° ~ 180° の間で 0.5° 毎にレーザーを拾うように設定してあります。

まず、ranges_args ですが、今回は0.5°毎に取得しているので、全ての数値を 1/2 にした数値が、そのまま障害物のある角度となります。
ranges_args = ranges_args[0] / 2 と追加すれば角度が出力されることになります。

scan_value は、障害物までの距離(m)を示します。今回は 2.46m ~ 2.54m の位置に障害物があります。