リモートルンバを作ろう【ソフトウェア編】

#はじめに
　本記事はリモートルンバを作ろう【ハードウェア編】の続編となります。まだ前編をご覧になられていない方ははじめにそちらをご覧ください。

#目標
　今回は前回までに準備したルンバを実際に制御していきます。

#環境
　使用したプログラミング言語はPython3.7です。ラズパイOSには標準でインストールされているかと思います。一般的にルンバを制御する場合ROSを用いる方が多いようなのですが、当方ROSはあまり使ったことがなく慣れておりません。今後のルンバを発展させる上で柔軟性や理解し易さの観点から、本記事ではPythonのみを使用したルンバの制御となります。

#ソースコード
###1.制御
####1-1.実機準備
　基本的にはルンバを制御するコードはiRobotが公式にだしている仕様書iRobot Roomba 500 Open Interface (OI) Specificationに公開されており、それをシリアル信号でルンバに送ればそれ通りの動作を行ってくれます。（仕様書には500と記載されていますがどのシリーズでもコマンドに変更はありません）

　しかしながらこの仕様書を読んでいくと、細かな動作を行うにはそれなりにコードを上手く記述しなければなりません。そこでMartin Schaef氏が過去にPython用ルンバAPIを製作して公開してくださっているのでそちらを拝借いたします。

martinschaef/roomba
↑ソースコードはこちらです。

問題はこのコード自体は古く、Python2.7用にプログラムされている点です。したがって該当コードをコピペしただけでは全く動作しません。自らで多少修正する必要があります。

修正するコードはcreate.pyです。こちらが大元のルンバAPIとなります。

はじめに、このコードではシリアル用コマンドがすべてchr型で記述されています。

create.py

START = chr(128)    # already converted to bytes...
BAUD = chr(129)     # + 1 byte
CONTROL = chr(130)  # deprecated for Create
SAFE = chr(131)
FULL = chr(132)
POWER = chr(133)
SPOT = chr(134)     # Same for the Roomba and Create
CLEAN = chr(135)    # Clean button - Roomba
etc...

このコードをPython3で実行した場合、以下のようなエラーが返ってきます。

TypeError: unicode strings are not supported, please encode to bytes: '\x80'

意味合いは、シリアル通信にstr型は対応していませんという意味です。しかしこれがPython2.7ではしっかり動作します。これはいったい？その答えは Python 2 と Python 3 でchr型の扱いが変わったという事です。具体的には以下のように変更されています。

	Python2	Python3
int to bytes	chr(i)	bytes([i])

ここでわたしはドツボにはまってしまったのですが、よく見るとbytes型は**()の中に[]を用いて数字を記載する必要**があるという事です。皆様はお忘れなく記載ください。

　ところで単にchr型をbytesに変換するだけなら.encode()を付随させればいいのでは、と思われる方も多いと思います。たしかにそうするとプログラム自体のエラーは無くなるのですが、ルンバは全く動作しません。そこで.encode()を付随させた場合の文字列を確認してみると以下のように表記されます。

128 b'\xc2\x80'
129 b'\xc2\x81'
130 b'\xc2\x82'
131 b'\xc2\x83'
132 b'\xc2\x84'
133 b'\xc2\x85'

本来、16進数で数値を送信する場合は0x80または/x80と表記されるはずです。しかし結果では全ての値に/xc2という値が付随してしまっています。これは何かというとUTF-8であるという情報が加えられているという事です。たしかに思えば単純なint型を変換したのではなく、str型をバイトに変換しているのでその情報も付け加えられていたのです。したがってこの場合はU+0081という値が変換されていたことのなるわけで、この結果から単純に.encode()を付随させた場合ではルンバは動作しないわけであります。

　ですので皆様には大変お手数ですが、このコード内でchr(i)型に該当する箇所全てをbytes([i])に変換していただきたく存じます。

create.py

START = bytes([128]) # already converted to bytes...
BAUD = bytes([129]) # + 1 byte
CONTROL = bytes([130]) # deprecated for Create
SAFE = bytes([131])
FULL = bytes([132])
POWER = bytes([133])
SPOT = bytes([134]) # Same for the Roomba and Create
CLEAN = bytes([135]) # Clean button - Roomba
etc...

　また上記の修正で動作自体はするようになるのですが、本来記載されていたprintによるメッセージがpython2の文法で記述されているため、すべて無視されてしまっています。あまりにも味気なさすぎるので同様に書き直していただくとよいかと思います。

create.py

print 'Serial port did open, presumably to a roomba...'
                          ↓
print('Serial port did open, presumably to a roomba...')

ここまで来ましたら、実際にルンバと接続できているか試してみましょう。確認には以下のコードを用います。

test_roomba.py

import create
import time

ROOMBA_PORT="/dev/ttyAMA0"
robot = create.Create(ROOMBA_PORT)

robot.printSensors() 
robot.toSafeMode()
robot.go(0,10)

robot.close()

ラズパイとルンバを接続して上記のコードを実行すると、ルンバのセンサーの値を取得してコマンドラインに表示され、ルンバは多少回転するはずです。

もしエラーが出力されKeyError:とか表示された場合は、ルンバとラズパイが上手く接続されていない可能性があります。考えられる要因として1.ルンバが起動していない、2.ケーブルが断線している、という2つがあります。前者の場合はルンバ中央のCLEANボタンを軽く押して起動させ再度試してみてください。後者の場合はケーブルの導通をご確認ください。

また、センサーの値は取得できるがルンバが全く動かないという方は以下のコードを実行してみてください。

test_roomba.py

import serial
ser = serial.Serial('/dev/ttyAMA0', 115200)
ser.write(b'0x80')

上記のコードでルンバが充電ドックを探し始めた場合は、シリアル通信がRS232Cレベルで行われている可能性があります。TTLレベルに変換しなおして再度接続してください。

次にキーボードからの入力によるルンバ制御を行います。先ほどのMartin Schaef氏のソースコードmartinschaef/roombaからgame.pyを多少修正して使用します。

game.py

ROOMBA_PORT = "/dev/tty.usbserial-DA017V6X"
              ↓
ROOMBA_PORT = "/dev/ttyAMA0"

元のソースコードでは接続先が異なっているので、21行目を上記のように修正してください。

game.py

if event.key == pygame.K_x:
   robot.seekDock()
   time.sleep(2.0)
   pygame.quit()
   return

game.py

screen.blit(
  font.render("Clean Mode Roomba with c, press key x make Roomba back to the dock.",
                        1, (10, 10, 10)), (10, 580))

またリモート制御時に自動で充電ドックに戻る機能が欲しかったので、上記のコードを115行目と166行目にそれぞれ追加しました。

最後にこれらの実行ファイルがあるディレクトリに新たにimgフォルダを製作し、その中にroomba.pngを置いて実機の準備は完了です。

####1-2.ホストコンピュータ準備
　リモートでルンバを制御したいのでこちらも準備が必要です。VcXsrv（Xサーバー）を利用してラズパイに表示される制御画面をホストコンピュータに移します。

VcXsrv（Xサーバー）をWindowsにインストールしてLinuxのGUIをリモート操作する
↑この項目の詳細はこちらの方のページに大変詳しく、わかりやすくまとめられております。こちらをご参照ください。

####1-3.動作確認
　上記の項目がすべて完了すると実際にルンバを制御できるようになります。早速Python3で実行してみてください。詳しい制御方法は表示された画面に記載されてるとは思いますが、一応簡単にご説明いたしますと、wで前進、sで後退、a、dそれぞれで左右方向へ回転です。spaceで一時ポーズでescで終了となっております。その他各センサーのリアルタイムに取得された値が表示されていると思います。

###2.映像転送
　次にWebカメラで取得した映像データをホストコンピューターへ転送するプログラムを作っていこうと思います。はじめはopen_cvを用いてラズパイ上で映像を開き、そのウインドウをVcXsrvでホストコンピュータへ転送しようと思ったのですが、あまりうまくいかなかったのでsocket通信による映像転送の方法を取ることにしました。

ライブラリにopencv-pythonをpipでインストールしてください。尚、現在最新であるopencvのバージョン4.4.0.42をインストールしようとするとPython3.7の場合、永遠にビルトが終わらないことがあります。そういう場合は少しバージョンを下げて4.1.0.25あたりでインストールするとすんなりいくようです。

pip3 install opencv-python==4.1.0.25

以下がラズパイ上で実行するプログラムです。

video_server.py

import socketserver
import cv2
import sys

HOST = "192.168.XXX.XXX"#ここはラズパイのIPアドレス
PORT = 5569

class TCPHandler(socketserver.BaseRequestHandler):
    videoCap = ''

    def handle(self):
        self.data = self.request.recv(1024).strip()
        ret, frame = videoCap.read()
        encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 100]
        jpegsByte = cv2.imencode('.jpeg', frame, encode_param)[1].tostring()
        self.request.send(jpegsByte)

videoCap = cv2.VideoCapture(0)
socketserver.TCPServer.allow_reuse_address = True
server = socketserver.TCPServer((HOST, PORT), TCPHandler)

try:
    server.serve_forever()
except KeyboardInterrupt:
    server.shutdown()
    sys.exit()

以下がホストコンピュータで実行するプログラムです。

video_client.py

import socket
import numpy
import cv2

HOST = "192.168.XXX.XXX"#ここはラズパイのIPアドレス
PORT = 5569

def getimage():

    sock=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
    sock.connect((HOST,PORT))

    buf=b''
    recvlen=100
    while recvlen>0:
        receivedstr=sock.recv(1024*8)
        recvlen=len(receivedstr)
        buf += receivedstr
    sock.close()

    narray=numpy.fromstring(buf,dtype='uint8')
    return cv2.imdecode(narray,1)

while True:
    img = getimage()
    cv2.imshow('Capture',img)

これらのプログラムをserver.pyから起動させ、上手く接続されるとホストコンピュータ側に映像が転送されているはずです。

###3.自動起動
　最後にラズパイの電源を入れるとこれまでのプログラムが自動で起動するように設定します。Systemdに則ってサービスファイルを作成していきます。

　ラズパイにて/etc/systemd/system/に移動してroomba.serviceファイルを作成してください。内容は以下のものを記載してください。

[Unit]
Description = Roomba

[Service]
ExecStart=/bin/bash /home/pi/roomba.sh
Restart=always

[Install]
WantedBy=multi-user.target

ここでわざわざシェルスクリプトで各プログラムを起動しているのは、今後も変更が行われることを考慮して楽に編集できるようにするためです。

　したがって次に/home/pi/roomba.shファイルを作成してください。

#!/bin/sh
sudo python3 /home/pi/roomba/server.py &
sudo python3 /home/pi/roomba/game.py &

サービスを確認して有効にして完了です。

$ sudo systemctl enable roomba
$ sudo systemctl start roomba

#まとめ
　ここまででとりあえずホストコンピュータから映像と制御の2つが確認できるはずです。

↑こんな感じで表示されているかとおもいます。

実際操作してみると、カメラの映像だけでは大変難しく、また既存のコードであるgame.pyではカーブする際にかなりぎこちない動作をするようです。

ただ、わざわざすべての処理をPythonで行っているだけあって自由度は非常に高いはずです。自動運転のプログラムや画像処理による人物検知など、追加できそうなシステムはたくさんあるのでいろいろ発展させていこうと思います。皆さんも是非ためしてみてください。

最後までお付き合いいただきありがとうございました。

#参考文献
1.iRobot Roomba 500 Open Interface (OI) Specification
2.martinschaef/roomba
3.Python 2 と Python 3 のユニコード文字列、バイト列の違いメモ
4.PythonとOpenCVで動画送信