LiDARを用いた障害物検知

LiDARを用いた障害物検知を実装します．
Tags:
Categories:

目的

Turtlebot3の上部にはLiDAR(Light Detection And Ranging)と呼ばれるセンサが搭載されています． 本講義の前半では座学形式でLiDARの仕組みを学び，後半ではLiDARを用いた障害物検知プログラミングの方法について取り組みます．

Turtlebot3に搭載されているLiDAR

Turtlebot3の上部には搭載されているLDS-02は，レーザ光を照射し，対象物に反射した光を受けて計測して距離を測ります． 周囲360°を観測するために，レーザー光の投光及び受光ユニットが機会的に回転します． LiDARの基本スペックは下記の表の通りです． 詳細なスペックについてはこちらのページを参照してください．

LiDAR

LiDAR スペック
製品名 LDS-02
レーザー波長 793nm
安全等級 クラス1
測定距離 160 ~ 8,000mm
測定範囲 360°
測定精度(160 ~ 300mm) ±10mm
測定精度(300 ~ 6,000mm) ±3.0%
測定精度(6,000 ~ 8,000mm) ±5.0%
基本測定分解能 1°
サイズ 70(W) x 90(D) x 42(H)mm
重さ 131g

起動とトピックの確認

前回の講義と同様に，sshでTurtlebot3にログインし，ROS2で使用するためのノードを起動します．

1

＄ ros2 launch turtlebot3_bringup robot.launch.py

LiDARが配信しているトピックを可視化して見てみましょう．

1

＄ ros2 run rviz2 rviz2

rviz2が起動したらDisplay項目の中にあるGlobal OptionsのFixed Frameをbase linkに設定します． 次に左下のAddボタンをクリックします． 一覧からLaserScanを選択してOKをクリックしてください． DisplayにLaserScanが追加されました． LaserScanをダブルクリックして設定項目を表示します． Topicが表示されたらダブルクリックしてくさい． また，Topicにて\scanを選択します． Topicの中にあるReliability PolicyをReliableからSystem Defaultに変更します． 上記の設定でLiDARが配信しているトピックを可視化することができます． 可視化される点群が小さくて見づらい場合には，Size(m)の値を大きくしてください．

rviz

次に，LiDARに関連するノードとトピックを確認しましょう． まずは起動しているノードを確認します．

1
2
3
4
5

$ ros2 node list
/diff_drive_controller
/ld08_driver
/robot_state_publisher
/turtlebot3_node

LiDARを扱うノードは/ld08_driverです． どのようなトピックをやり取りしているかを調べます．

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20

$ ros2 node info /ld08_driver
/ld08_driver
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /scan: sensor_msgs/msg/LaserScan
  Service Servers:
    /ld08_driver/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /ld08_driver/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /ld08_driver/get_parameters: rcl_interfaces/srv/GetParameters
    /ld08_driver/list_parameters: rcl_interfaces/srv/ListParameters
    /ld08_driver/set_parameters: rcl_interfaces/srv/SetParameters
    /ld08_driver/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

上記の出力結果からLiDARは，/scanというトピック名で配信されていることが確認できました． また，メッセージとしてはsensor_msgs/msg/LaserScanを使用することが分かります． 配信されるデータの中身を調べてみましょう．

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

$ ros2 interface show sensor_msgs/msg/LaserScan
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

std_msgs/Header header # timestamp in the header is the acquisition time of
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id
                             # the first ray in the scan.
                             #
                             # in frame frame_id, angles are measured around
                             # the positive Z axis (counterclockwise, if Z is up)
                             # with zero angle being forward along the x axis
float32 angle_min            # start angle of the scan [rad]
float32 angle_max            # end angle of the scan [rad]
float32 angle_increment      # angular distance between measurements [rad]

float32 time_increment       # time between measurements [seconds] - if your scanner
                             # is moving, this will be used in interpolating position
                             # of 3d points
float32 scan_time            # time between scans [seconds]

float32 range_min            # minimum range value [m]
float32 range_max            # maximum range value [m]

float32[] ranges             # range data [m]
                             # (Note: values < range_min or > range_max should be discarded)
float32[] intensities        # intensity data [device-specific units].  If your
                             # device does not provide intensities, please leave
                             # the array empty.

配信しているデータの種類が多いので少し分かりにくくなっています． 関連するものだけを下記の表にまとめました．

変数名 型 説明
angle_min float32 測定範囲の下限
angle_max float32 測定範囲の上限
angle_increment float32 角度分解能
ranges float32の配列 測定した距離

LDS-02は周囲360°の距離を計測可能できますが，サンプリングレートが固定されているため角度分解能がスキャン周波数によって異なります． そのため，角度の測定範囲の開始が0[rad]ではなく，若干ずれたangle_min[rad]から始まります． 角度分解能はangle_increment[rad]として配信されます．

障害物検知プログラム

LiDARから配信されたトピックを購読し，前方の障害物までの距離を計測するプログラムを実装します． 下記のサンプルプログラムでは，LiDARで前方30[°]を観測し，障害物までの平均距離を表示します． これまでに学んだ方法にてワークスペースとパッケージを作成し， 下記のサンプルプログラムを入力してください．

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

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

class DetectObstacle(Node):
    def __init__(self):
        super().__init__('detect_obstacle_node')
        qos = rclpy.qos.QoSProfile(depth=10)
        qos.reliability = rclpy.qos.QoSReliabilityPolicy.BEST_EFFORT
        self.pose_sub = self.create_subscription(LaserScan, 'scan', self.scan_callback, qos)

    def scan_callback(self, scan):
        angle = scan.angle_min
        total_distance = 0.0
        cnt = 0
        for i in range(len(scan.ranges)):
            if(angle > ((23 * math.pi) / 12) or angle < (math.pi / 12)):
                if(scan.ranges[i] == scan.ranges[i]):
                    total_distance = total_distance + scan.ranges[i]
                    cnt = cnt + 1
            angle = angle + scan.angle_increment
        print(total_distance / cnt, "[m]")

def main(args=None):
    rclpy.init(args=args)
    node = DetectObstacle()
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

画像のような環境で実行すると下記のような出力結果が得られます．

1
2
3
4
5
6

$ ros2 run detect_obstacle detect_obstacle_node
0.5146363600817594 [m]
0.5147878805073824 [m]
0.5146875071376562 [m]
0.5159696981545651 [m]
0.5160294127464294 [m]

プログラムを実行した環境は，LiDARの中心から壁までの最短距離が50.0cm，最長距離が51.8cmでした． ターミナルに出力した結果は約51.5cmですので，障害物までのおおよその距離を計測できていることがわかります．

プログラムの内容

プログラムの内容を確認しましょう．

1
2
3
4

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

必要なモジュールを読み込んでいます． LiDARから配信されるトピックを購読するために，sensor_msgsパッケージのLaserScanをimportしています．

 6
 7
 8
 9
10
11

class DetectObstacle(Node):
    def __init__(self):
        super().__init__('detect_obstacle_node')
        qos = rclpy.qos.QoSProfile(depth=10)
        qos.reliability = rclpy.qos.QoSReliabilityPolicy.BEST_EFFORT
        self.pose_sub = self.create_subscription(LaserScan, 'scan', self.scan_callback, qos)

_init__関数内でトピック/scanを購読した際のコールバック関数を定義しています． これまで扱ってきたサンプルプログラムとは異なり， 今回はQoS(Quality of Service)を設定しています． QoSは通信品質に関わる設定です． 実環境で運用する場合，通信状況によってはセンサから得られるデータは確実に全てを購読できる訳ではありません． サンプルプログラムでは，過去10フレームのデータを保存し，ReliabilityをBEST_EFFORTとします．

13
14
15
16
17
18
19
20
21
22
23

   def scan_callback(self, scan):
        angle = scan.angle_min
        total_distance = 0.0
		cnt = 0
        for i in range(len(scan.ranges)):
            if(angle > ((23 * math.pi) / 12) or angle < (math.pi / 12)):
                if(scan.ranges[i] == scan.ranges[i]):
                    total_distance = total_distance + scan.ranges[i]
                    cnt = cnt + 1
            angle = angle + scan.angle_increment
        print(total_distance / cnt, "[m]")

LiDARから配信されるトピックを購読するとscan_callbackメソッドが実行されます． 先でも述べたように，LDS-02は周囲360°の距離を1°ずつ測定できますが， 実際にはスキャン周波数によってずれが生じます． 前方30°(23π/12 ~ π/12)までの距離のみを加算し，加算した回数で割ることでLiDARから障害物までの平均距離を計算しています． ただし，距離が計測できなかった場合にはscan.ranges[i]にinfが入っています． infまで加算すると平均距離を計算することができなくなるため，infであった場合には計算から除きます．

25
26
27
28
29
30
31
32
33
34

def main(args=None):
    rclpy.init(args=args)
    node = DetectObstacle()
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

main関数は以前と同じ内容ですので省略します．