ロボットシステムにおけるセンシング・アクチュエーション・通信③

複数のセンサを組み合わせてよりかしこくロボットを動かしてみよう

Learn

LiDARのスキャンデータを使って，障害物を回避してみよう

次に，LiDARでスキャンしたデータを使って，障害物を回避するようなプログラムを作ってみましょう．

LiDARスキャンのメッセージ（/scan）の中身を見てみよう

LiDARは，Light Detection And Rangingの略で，レーザ光を使って離れた場所にある物体形状や距離を測定するためのセンサです． 近年では，自動車の自動運転にも用いられることの多いセンサの一つです．

roombaに搭載されたLiDARセンサ（rplidar）の値は，/scanのトピックに流れていて，rostopic echo /scanをしてみるとメッセージとしてどんな情報が流れているかわかります．

大きなデータなので今回はテキストに掲載するのは省略しますが，rostopic type /scanをしてみると，メッセージとして，sensor_msgs/LaserScan型が使われていることがわかります．

root@dynamics:~/roomba_hack# rostopic type /scan
sensor_msgs/LaserScan

sensor_msgs/LaserScan型の定義を確認してみましょう． メッセージ型の定義は，ドキュメントのほか，rosmsg info sensor_msgs/LaserScanすることでもコマンドから確認できます．

root@dynamics:~/roomba_hack# rosmsg info sensor_msgs/LaserScan
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

angle_minにはスキャンの開始角度，angle_maxにはスキャンの終了角度がラジアンで記録されています． angle_incrementは，計測した間隔がラジアンで記録されています． range_maxにはスキャンの間で検出された最大の距離，range_minには最小の距離がメートルで記録されています．

rvizでLiDARスキャンの値を可視化してみよう

rvizでLiDARのスキャン結果を可視化してみましょう．

LaserScanをAddして，topicに/scanを設定すると，以下のように，ロボットを中心にLiDARによって計測された障害物が赤く表示されます．

LiDARを使って障害物を回避しよう

それでは，LiDARスキャン/scanの情報を使った制御の実装の例としてnavigation_tutorialパッケージの中のavoidance.pyをみてみましょう．

#!/usr/bin/env python3
import numpy as np

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

class Avoidance:
    def __init__(self):
        rospy.init_node('avoidance', anonymous=True)
        
        # Publisher
        self.cmd_vel_pub = rospy.Publisher('/planner/cmd_vel', Twist, queue_size=10)

        # Subscriber
        scan_sub = rospy.Subscriber('/scan', LaserScan, self.callback_scan)

        self.min_range = None

    def callback_scan(self, data):
        fov = np.deg2rad(60)
        min_range = data.range_max
        min_idx = -1
        angle = data.angle_min
        for idx, r in enumerate(data.ranges):
            angle += data.angle_increment
            if -fov<angle<fov:
                if r<min_range:
                    min_range = r
                    min_idx = idx
        if min_idx < len(data.ranges)/2.0:
            self.direction = "RIGHT"
        else:
            self.direction = "LEFT"
        self.min_range = min_range

    def process(self):
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            vel = Twist()
            if self.min_range is not None:
                if self.min_range >= 0.4:
                    vel.linear.x = 0.2
                    vel.angular.z = 0.0
                else:
                    vel.linear.x = 0.0
                    if self.direction == "RIGHT":
                        vel.angular.z = 0.5
                    elif self.direction == "LEFT":
                        vel.angular.z = -0.5
            self.cmd_vel_pub.publish(vel)
            r.sleep()

if __name__=='__main__':
    avoidance = Avoidance()
    try:
        avoidance.process()
    except rospy.ROSInitException:
        pass

このプログラムでは，LiDARを使って進行方向に存在する障害物を見つけ，それを回避しながら進むようにロボットを制御しています．具体的には，

• ロボットの進行方向に物体がなかったら直進
• ロボットの右側に障害物があったら左回転
• ロボットの左側に障害物があったら右回転

することで障害物を回避（ぶつかる前に方向転換）しています．

では，プログラムの中身を見ていきます．

/odomを使った制御の場合と同様に，ノードを定義する際に，コマンドを送るパブリッシャと，LiDARスキャンのデータを読み取るサブスクライバを作成します．

class Avoidance:
    def __init__(self):
        rospy.init_node('avoidance', anonymous=True)
        
        # Publisher
        self.cmd_vel_pub = rospy.Publisher('/planner/cmd_vel', Twist, queue_size=10)

        # Subscriber
        scan_sub = rospy.Subscriber('/scan', LaserScan, self.callback_scan)

        self.min_range = None

/scanのコールバックは，

    def callback_scan(self, data):
        fov = np.deg2rad(60)
        min_range = data.range_max
        min_idx = -1
        angle = data.angle_min
        for idx, r in enumerate(data.ranges):
            angle += data.angle_increment
            if -fov<angle<fov:
                if r<min_range:
                    min_range = r
                    min_idx = idx
        if min_idx < len(data.ranges)/2.0:
            self.direction = "RIGHT"
        else:
            self.direction = "LEFT"
        self.min_range = min_range

となっており，正面から左右60度の範囲内で最も短い距離をself.min_rangeに格納し，それが右側にあるのか左側にあるのかをself.directionに格納しています．．

このプログラムを実行するとprocessメソッドが（0.1秒おきに）常に実行されます．

    def process(self):
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            vel = Twist()
            if self.min_range is not None:
                if self.min_range >= 0.4:
                    vel.linear.x = 0.2
                    vel.angular.z = 0.0
                else:
                    vel.linear.x = 0.0
                    if self.direction == "RIGHT":
                        vel.angular.z = 0.5
                    elif self.direction == "LEFT":
                        vel.angular.z = -0.5
            self.cmd_vel_pub.publish(vel)
            r.sleep()

processメソッド内部では，格納されたself.min_rangeが0.4（メートル）より大きい場合は，ロボットの前に何もないと判断して直進，小さい場合は，self.directionの値を見て，RIGHTであれば右に障害物があると判断して左回転，LEFTであれば左に障害物があると判断して右回転するようなプログラムになっています．

それでは，実際にLiDARを使って障害物を回避するプログラムを実行してみましょう．

演習

(開発PC)(docker):~/roomba_hack# rostopic type /scan

(開発PC)(docker):~/roomba_hack# rostopic echo /scan

(開発PC)(docker):~/roomba_hack# roslaunch navigation_tutorial avoidance.launch