Raspberry Pi 3B + Webカメラを使って機械学習のためのロボット自動走行用データセット(画像＋ラベル)を用意しよう。

概要

さて、最近話題の機械学習ですが（多分）、画像認識や自然言語処理等、まだまだできることに限りがあるように思われます。すごい人、がんばれ！

背景としまして、（長いので読まなくていいです）元々はFPGAにPcamを刺して画像収集をして手動でラベル付けを済ませて学習させたのですが、その学習済みモデルはなんと学習で使われた角度における白線の角度ではうまくいった（あくまで正答率での話）のですが、高速で推論させるためにラスベリーパイにエッジTPUを刺して、ラズパイをホストにして計算させるためにラズパイにWebカメラをさして演算させようと思ったところ、カメラの位置や角度が大きく変わってしまったためか、学習モデルが全く役に立ちませんでした。なので再度データセットを集めて学習させたいのですが、再び手動でラベル付けをするのは面倒です。なので自動でラベル付けをしてくれるスクリプトを生成しましょう！

ということで、題名の通り機械学習を用いてロボットが自動走行をするためのデータセットを自動で作ってもらえるようにしましょう。

環境

ハードウェア:
ラズパイ　: Raspberry Pi 3 Model B
Webカメラ: Logicool C615

OS: Ubuntu 16.04

python:
python3.5.2 （コントロール用）
python2.7.12(ROS用)

ROS: kinetic

使ったロボット:
TurtleBot3 Burger

ROSを使ってロボットを操作しよう

Robot Operationg System、通称ROSというオープンソース・ロボティクス財団が提供しているパッケージを使うと、ハードウェアに疎い私のような人でも簡単にロボットを制御できるようになります。すごい！その代わりすごく処理が重たいですが。

ROSパッケージのインストール等は、本家サイトをご覧ください。http://wiki.ros.org/ja
ここではROSパッケージをラズベリーパイにインストールできている前提で進めます。ちなみに私はkinetic版のROSを使用しています。

さて、ざっくりとROSを使ってロボットを制御する仕組みを説明しておきます。なお素人の説明なので、間違っている、なんか変等あるかもしれないので、ご了承ください。

ROSを用いてあるデバイスを制御する方法は、大きく分けて２つあります。
１つ目は、制御するロボット、通信する相手を特定して同期的にやりとりする(ROS　Service)
２つ目は、制御するロボットや相手をを特定せず非同期でやり取りをする（ROS　Topic）

１つ目のROS　Serviceを使ったやりとりは、主に一度しか呼ばれない処理（デバイスの初期化など）かつ成功したかどうかの判断をしたいときに使われるようです。
今回は別にそこまで処理時間に致命的ではない上に、何度も通信することになるので、２つ目のROS　Topicを説明していきます。

ROS Topicでは、ある型のデータを誰でも受け取ることができるように空間全体に送信する Publisher と、Publisherによって送信された、ある型のデータを受け取る Subscriberに分かれて通信します。TCP通信ではsenderとreceiverですね、多分。
Publisherがあるデータを送信しつづけ、Subscriberはデータを非同期に受け取りつづけます。これらを別々のプログラムに書いてやり、データをやり取りすることができます。

では、ちょっとだけサンプルコード的なものを見てみましょう。

　ROS環境設定

環境変数の設定

その前に、ROSを使うためには、クライアント、サーバーのように通信を制御するものが必要です。ROSの世界では、プロセスを制御するものをマスターと呼び、制御されるプロセスをノードと呼びます。
ノードでやり取りするためには、必ずマスターを立ち上げる必要があります。同じデバイスの中でマスターとホストを同時に起動することもできますし、別々のデバイスで起動させることもできます。
マスターを起動させて、ホスト同士（異なるホストでも、同一ホストでも可）が通信する、みたいなイメージでいいと思います。

私はなるべくラズベリーパイにかかる処理を減らしたいので、適当なROSをインストールしたPCをマスターにしています。ラズベリーパイがホストです。

マスターを起動するためには、roscore　というコマンドを実行する必要があります。これを実行するとターミナルが１つ専有されるので、もしホスト用で何かを実行したいのであれば別なターミナルやタブを開きましょう。

roscoreを実行する時に、.bashrc　や.bash_profile などの ROS_MASTER_URIとROS_HOSTNAMEを実行するデバイスのものとして設定しておきましょう。

.bashrc(PC)

export ROS_MASTER_URI=http://192.168.1.249:11311
export ROS_HOSTNAME=192.168.1.249
export TURTLEBOT3_MODEL=%{TB3_MODEL}

ROS_MASTER_URIのほうの:11311はどのデバイスをマスターにするときもつける必要があります。IPアドレスの代わりにlocalhostだと、うまく通信できないことがあります。

ついでにラズパイの方にも同じようにしておきましょう。

.bashrc(RaspberryPie)

export ROS_MASTER_URI=http://192.168.1.249:11311 #さっきのホストPCのIPアドレス                                                                                                                      
export ROS_HOSTNAME=192.168.1.223                #このラズパイのIPアドレス
export TURTLEBOT3_MODEL=burger

同一ラズベリーパイをマスターかつホストとして動かしたいなら、どちらもラズパイのIPアドレスになりますね！
回線を変えるたびに設定し直さないといけないのは面倒ですが、仕方ないです

作業用ワークスペース、パッケージの雛形の作成

ROSのプログラムを実行するには、専用の作業用ディレクトリを作成する必要があります。ただmkdirでディレクトリを作成するだけではROSプログラムを実行できません。まずは以下のコマンドをターミナルに入力し、実行してください。

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ~/catkin_ws
catkin_make

ディレクトリ名は別になんでもいいと思いますが、catkin_wsというワークスペースを作るのが一般的らしいです。ディレクトリ作成後はビルドしてください。ROSパッケージをしっかりインストールできていたら、catkin_makeが通るはずです。

ビルド成功後、~/catkin_ws/devel/setup.bash　というファイルが作成されていると思います。これはROSプログラムを実行する場合、必ず実行する必要があります（PC起動後一回でいいです）。面倒なら.bashrc等に書いておいたほうがいいです。今のうちに追加しておきましょう。

.bashrc

source catkin_ws/devel/setup.bash

もしも似たものが追加されていたら（source /opt/ros/.../setup.bash 等　...にはROSのバージョンが入ります）削除しておきましょう！

これでワークスペースが完成しました。これからROSプログラムを書いていくわけですが、それらのコードは作成したワークスペースのsrcディレクトリ配下に新しいディレクトリを作成し、そのディレクトリ内で１つの昨日を持ったプログラムの塊として扱っていきます。これはパッケージと呼ばれます。

簡単にまとめると、~/catkin_ws/src/ 以下に新しいパッケージ用のディレクトリを作成し、そこに書いたコードを突っ込んでおけばOKです。

cd ~/catkin_ws/src
catkin_create_package hogehoge rospy roscpp std_msg  

catkin_create_package でパッケージの雛形を作成できます。作成するパッケージ名はなんでもいいですが、それ以降は依存しているパッケージなので、ここはその通りにしたほうがいいと思います。

パッケージ作成後は、再度ビルドする必要があります。以下のコマンドを実行しましょう。

cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

これでhogehogeというディレクトリが作成され、ここにROSコードを配置することで、実行できるようになります。サンプルコードを１つだけ載せておきます。

なお、cd ~/catkin_ws/src/hogehoge の代わりに、roscd hogehoge で一発でディレクトリにアクセスできます。便利！

ROS サンプルコード

簡単なものを１つだけ。マスターノードがないみたいなエラーがでたら、roscoreを忘れているので、別のターミナルを立ち上げて、実行してください。

publisher.py

#!/usr/bin/python2

import rospy
from std_msgs.msg import String

rospy.init_node('HelloWorld_sender')
pub = rospy.Publisher('HelloWorld', String, queue_size=10)
rate = rospy.Rate(1)

while not rospy.is_shutdown():
    s = String()
    s.data = "Hello World!"
    pub.publish(s)
    rate.sleep()

subscriber.py

#!/usr/bin/python2

import rospy
from std_msgs.msg import String

def callback(s):
    rospy.loginfo(s.data)

rospy.init_node('HelloWorld_receiver')
sub = rospy.Subscriber('HelloWorld', String, callback)
rospy.spin()

この２つをそれぞれ別のターミナルで実行すると、subscriber.pyを実行した方のスクリーンに、いろいろ出てきたら成功です。

成功例：

[INFO] [1589588641.545402]: Hello World!
[INFO] [1589588642.545695]: Hello World!
[INFO] [1589588643.545670]: Hello World!
[INFO] [1589588644.545670]: Hello World!
[INFO] [1589588645.545677]: Hello World!
[INFO] [1589588646.545695]: Hello World!
[INFO] [1589588647.545665]: Hello World!
[INFO] [1589588648.545742]: Hello World!
[INFO] [1589588649.545762]: Hello World!

Webカメラの準備

それではまずはカメラがしっかり動いているかを確認。とりあえずUSBにぶっさして、fswebcam コマンドを使って画像をキャプチャします。入ってなければインストールしましょう。

fswebcam -r 640*480 test.jpg

ここで注意点が２つあります。拡張子がpngだと私の環境では画像データが壊れていました。設定すればpngファイルにすることもできると思いますが、とりあえずjpg形式にしておきました。

そしてふたつ目、こっちは結構大事です。どうやらWebカメラを起動し、一番最初に画像をキャプチャした際、や、単純に一度fswebcamコマンドを実行しただけだと、真っ黒な画像が保存されてしまいます。理由はよくわかりませんが、とりあえず２回実行すれば２枚目の画像はしっかり保存されるので、カメラの動作確認をする際はシェルか何かに二回上記コマンドを連続して書いてみてください。しっかり保存できているはずです。

このコマンドをpythonで操作することで、Webカメラを動作させましょう。

　自動走行用データセット作成

さて、それでは自動走行のデータセットを作成しましょう。あるキャプチャした画像を入力をし、その際ロボットが操作されていた際の角速度、そして速度をラベルとして保存します。一時停止など他のラベルを加えたくなったら適宜追加しましょう。

ロボットを操作(キーボード入力)するコード(Publisher)

ロボットを操作するプログラムを用意しましょう。とりあえず単純に、キーボードから入力されたキーに対応した角速度、速度を publishし続け、さらにそのラベルデータをファイルに入力するようにしましょう。なお、今回使用したロボット、TurtleBot3では、公式サイト(http://emanual.robotis.com/docs/en/platform/turtlebot3/overview/)通りに設定すれば /cmd_vel　型のデータを subscribe し、自動的に動いてくれるので(roslaunch turtlebot3_bringup turtlebot3_robot.launchを忘れずに),
今回のケースでは subscriber のコードを用意する必要はありません。もしも自前のロボットを操作したければ、適宜subscriberを作成し、モーター等を直接操作してください。

vel_publisher.py

#!/usr/bin/python2

import rospy
from geometry_msgs.msg import Twist
import cv2
import datetime
import time

today = datetime.datetime.now()

def init_vel():
    vel.linear.x = 0.
    vel.linear.y = 0.
    vel.linear.z = 0.
    vel.angular.x = 0.
    vel.angular.y = 0.
    vel.angular.z = 0.

def save_data(vel):
    with open("velocity.txt", "w") as f:
        f.write(str(vel.linear.x) + " " + str(vel.angular.z))

    with open("velocity.txt", "r") as f:
        data = f.read()
        print(data)
        x, z = data.split(" ")
        print(x, z)

rospy.init_node('vel_publisher')
#pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

vel = Twist()

# 0.1 sec
#rate = rospy.Rate(1)

while not rospy.is_shutdown():
    direction = raw_input('f: forward, b: backfard, r: right, l: left >')

    if 'f' in direction:
        vel.linear.x = 0.025
        vel.angular.z = 0.0

    elif 'b' in direction:
        vel.linear.x = -0.025
        vel.angular.z = 0.0

    elif 'l' in direction:
        vel.angular.z = vel.angular.z + 0.1
        #vel.angular.z = 0.2

    elif 'r' in direction:
        vel.angular.z = vel.angular.z - 0.1
        #vel.angular.z = -0.2

    elif 's' in direction:
        #vel.linear.z = vel.linear.z - 0.1
        vel.linear.x = 0.
        vel.angular.z = 0.


    elif 'q' in direction:
        break

    else:
        continue

    print vel
    pub.publish(vel)


    # label
    save_data(vel)

この保存したラベルデータ(キーボードで入力されている速度、加速度)は、画像をキャプチャするコードの方でラベル用のファイルに追加していきましょう。

ロボットの操作(画像キャプチャ)

キャプチャした画像は適当なディレクトリ配下に番号を振って保存します。その際publisherによってファイルに保存されているラベルデータも同時にラベルとして別ファイルに保存します。ラベルデータは同じファイルに追加していく形にします。

　image_capture.py

#!/usr/bin/python3

import subprocess as sp
import time
import sys
import datetime
import shutil

today = datetime.datetime.now()
dir_path = str(today.month) + "_" + str(today.day) + "_images/"
image_path = "capture.jpg"

def init():

    #print(exists)
    # Directory exists
    while True:
        try:
            exists = sp.check_output(["mkdir", dir_path])

            if not exists == b"":
                print("Something wrong.")

            break

        except:
            print("Directory {} exists! Please rename the old one." .format(dir_path))


            while True:
                ans = input("Would you like to rename the directory and continue?[Y/n] >")

                if ans == 'Y':
                    ans = input("Type the name of old directory >")
                    shutil.move(dir_path, ans)
                    print("{} was succsessfully renamed. Continue the program.")
                    break

                elif ans == 'n':
                    ans = input("Would you like to remove the old directory and continue?[Y/n] >")
                    if ans == 'Y':
                        shutil.rmtree(dir_path)
                        print("{} was succsessfully removed. Continue the program.")
                        break

                    elif ans == 'n':
                        print("Then, please re-start.")
                        sys.exit()
                    else:
                        print("Invalid input.")
                        continue
                else:
                    print("Invalid input.")
                    continue


init()
i = 0

# Execute getting images
while True:
    try:
        # Return 0 if success
        #check = sp.check_output(["fswebcam", "-r", "640*480", "-D", "1", image_path])

        #check = sp.run(["fswebcam", "-r", "640*480", "-D", "1", image_path], stdout = sp.PIPE, stderr = sp.PIPE)
        check = sp.run(["fswebcam", "-r", "640*480", image_path], stdout = sp.PIPE, stderr = sp.PIPE)

        if "Processing captured image" in str(check.stderr):
            print("Success to capture image.")

        else:
            print("\n************* Captured error.**************")
            print(check.stderr)
            print("\n*************This is a captured error end.**************")

            # Solve the following error by re-binding USB
            # Check the Bus id and Port id by "lsusb" and "lsusb -t"
            if "Unable to find a compatible palette format." in str(check.stderr):
                # Bus id - Port id. child port id
                # You can see /sys/bus/usb/drivers/usb/
                sp.run(["sudo", "echo", "-n", "1-1.5", "|", "sudo", "/sys/bus/usb/drivers/usb/unbind"])
                time.sleep(1)
                sp.run(["sudo", "echo", "-n", "1-1.5", "|", "sudo", "/sys/bus/usb/drivers/usb/bind"])
                time.sleep(3)
                print("USB re-binded!")

            elif "No such file or directory" in str(check.stderr):
                # Need to wait a little bit more
                time.sleep(3)
        if True:
            shutil.move(image_path, dir_path + "{}.jpg" .format(i))

            with open("velocity.txt", "r") as vel_file:
                data = vel_file.read()
                x, z = data.split(" ")

            with open(str(today.month) + "_" + str(today.day) + "_labels.txt", "a") as f:
                f.write(str(x) + " " + str(z) + "\n")
                print("Data successfully saved. x: {}, z: {}" .format(x,z))

        i += 1
        time.sleep(0.5)

    except Exception as e:
        print(e)
        continue

無限ループで画像をキャプチャし続け、その度にその時点で最も新しく発行されたデータをキャプチャした画像のラベルデータとして保存します。

今回使ったWebカメラとラズベリーパイは相性が良くなかったみたいで、連続してキャプチャし続けると

Unable to find a compatible palette format.

というエラーが発生して、画像がそれ以上キャプチャできなくなってしまいます。

デバイスによっては、fswebcamコマンドに -D 1 などと追加してやることで、キャプチャに遅延をいれることができこのエラーがでにくくなるという情報もありますが、私の環境ではUSBを抜き差しすることでしかこのエラーを解決できなかったです。(openCV などで提供されている画像のキャプチャでもうまくいかないらしいです)

最も簡単な解決策は、相性の良いWebカメラを探して購入することのようですが、面倒な上に財力もないので、このエラーが起こるたびにコマンドでUSBを抜き差しすることでエラーを回避しました。

抜き差し（アンバインド、バインド）するには、lsusb -t　等でUSB接続しているデバイスのバスID,ポートIDを確認して、直接コマンドに入力してやる必要があります。今回はバスIDが1, ポートIDが1、その配下のポートIDが1だったので、1-1.5　って感じです。

lsusb -t
/:  Bus 01.Port 1: Dev 1, Class=root_hub, Driver=dwc_otg/1p, 480M
    |__ Port 1: Dev 2, If 0, Class=Hub, Driver=hub/5p, 480M
        |__ Port 1: Dev 3, If 0, Class=Vendor Specific Class, Driver=smsc95xx, 480M
        |__ Port 3: Dev 4, If 0, Class=Communications, Driver=cdc_acm, 12M
        |__ Port 3: Dev 4, If 1, Class=CDC Data, Driver=cdc_acm, 12M
        |__ Port 5: Dev 60, If 0, Class=Audio, Driver=snd-usb-audio, 480M
        |__ Port 5: Dev 60, If 1, Class=Audio, Driver=snd-usb-audio, 480M
        |__ Port 5: Dev 60, If 2, Class=Video, Driver=uvcvideo, 480M
        |__ Port 5: Dev 60, If 3, Class=Video, Driver=uvcvideo, 480M

今回抜き差ししたいWebカメラは、どうやら Bus01-Port1-Port5　って感じの位置に接続されているようです。なお-tをつけずにlsusbをするとデバイス名がわかってみやすいです。

lsusb
Bus 001 Device 060: ID 046d:082c Logitech, Inc. 
Bus 001 Device 004: ID 0483:5740 STMicroelectronics STM32F407
Bus 001 Device 003: ID 0424:ec00 Standard Microsystems Corp. SMSC9512/9514 Fast Ethernet Adapter
Bus 001 Device 002: ID 0424:9514 Standard Microsystems Corp. SMC9514 Hub
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub

なお、OSやラズパイの種類によって微妙に構成が違うこともあるので、/sys/bus/usb/drivers/usb/ に抜き差ししたいIDのデバイスがあるかを確認しましょう。

ls /sys/bus/usb/drivers/usb/
1-1  1-1.1  1-1.3  1-1.5  bind  uevent  unbind  usb1

しっかりとlsusb -t で表示されたものと一致していますね。

Webカメラを再バインド後、すぐにキャプチャすると違うエラーが出て怒られるので、sleepを適当に入れてやります。

また、画像キャプチャを無限ループにいれ実行すると、OS panic　が発生したりフリーズが頻繁に発生する場合は、不要なUSB(マウスとかキーボードとか)を全てはずして実行するようにしてください。USBカメラとマウス等入力機器は相性が悪いみたいな話を聞いたことがあります。調べてませんが,,

こっちのコードではROSを使わないので、subprocess.runを使いたいのもあってpython3でかいてあります。これを使うとshellコマンドをpythonで実行でき、その際の戻り値や標準出力を取得することができるので便利です。python3.5以降にしか入ってないらしいので注意してください。

これでキーボードでロボットを操作しつつ、データセットを準備できました！

　おまけ(Wiiリモコンでロボットを操作)

さて、PCのキーボードのキー入力によりロボットを操作していましたが、流石に数千枚画像を撮ろうと思ったら重すぎて疲れます。せっかくPCやラズパイにはBlue Toothがついてるんだから、もっと軽いリモコンで操作してやりましょう。

面倒だったのでPCとWiiリモコンで接続しました。キーボード入力だったコードを少し修正して、Wiiリモコンの十字キーで操作できるように変更します。こちらの方の記事を参考にしました(ぱくった)　https://qiita.com/t4t5u0/items/605ad611cc02716e3b98。

なお、gnureadlineがインストールされている場合、十字キーが異なる入力として扱われるかもしれないので、もしうまくいかないならアンインストールしたほうが良いと思います。

python2.7 -m pip uninstall gnureadline

wiicontroller_vel_publisher.py

#!/usr/bin/python2

import rospy
from geometry_msgs.msg import Twist
import cv2
import datetime
import time

import sys
import tty
import termios
def getch():

    fd = sys.stdin.fileno()
    old = termios.tcgetattr(fd)
    try:
        tty.setraw(fd)
        return sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old)



EOT = 3
TAB = 9
ESC = 27

today = datetime.datetime.now()

def init_vel():
    vel.linear.x = 0.
    vel.linear.y = 0.
    vel.linear.z = 0.
    vel.angular.x = 0.
    vel.angular.y = 0.
    vel.angular.z = 0.

def save_data(vel):
    with open("velocity.txt", "w") as f:
        f.write(str(vel.linear.x) + " " + str(vel.angular.z))

    with open("velocity.txt", "r") as f:
        data = f.read()
        print(data)
        x, z = data.split(" ")
        print(x, z)

rospy.init_node('vel_publisher')
#pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

vel = Twist()

# 0.1 sec
#rate = rospy.Rate(1)

while not rospy.is_shutdown():
    key = ord(getch())
    if key == EOT:
        break
    elif key == TAB:
        print('keydown TAB')
    elif key == ESC:
        key = ord(getch())
        if key == ord('['):
            key = ord(getch())
            if key == ord('A'):
                # Left
                vel.angular.z = vel.angular.z + 0.1
                #vel.angular.z = 0.2

            elif key == ord('B'):
                # Right
                vel.angular.z = vel.angular.z - 0.1
                #vel.angular.z = -0.2

            elif key == ord('C'):
                # Up
                vel.linear.x += 0.025
                #vel.angular.z = 0.0

            elif key == ord('D'):
                # Down
                vel.linear.x -= 0.025

    else:
        continue

    print vel
    pub.publish(vel)

    # label
    save_data(vel)

横持ちしたときの十字キー入力なので注意してください。
これでのんびりデータセットが集められます！適当に1000枚くらい集めようかなぁ

あとがき

せっかくWiiリモコンつなげたんだから、他のボタンも使って機能を増やしても面白いかもしれませんね。

収集したデータセットを学習させ、結果が良ければそれについて記事を書くかもしれません。