#はじめに
本記事は、DragonBoard＋IoTアプリコンテスト向けに作成する作品「ルンバとお掃除」の制作内容をまとめたものです。

#目指したもの

昨今、お掃除ロボットは様々なメーカーから発売されており、それなりの知名度はあるかと思います。
一方、その普及率は5%程度といわれています。

私も興味本位で購入して使ってみましたが、その印象は「おバカさん」でした。お安いロボット掃除機は特にランダム的に動作する（高価なものはカメラ等でのSLAM機能を持っており、もう少しお利口だとは思います）ので、あそこのゴミを掃除してほしいと思ってもそのように動いてはくれません。ある程度のスペースをほったらかしで掃除させておくというのが本来の使い方でしょうから、少々「おバカさん」（ランダム動作）しても結果的に部屋がきれいになるので、製品としては問題はないです。

そこで、このお掃除ロボットを自分好みにお利口にするため、DragonBoardを使って、自分が掃除しているときに少しでもお手伝いしてくれるようにしてみようと思いました。

具体的には、カメラを使い、追跡する物体を認識させて、それが動く方向にお掃除ロボットを追従させようと思います。
そして、最終的には、私が細かい周辺を掃除していると、その手前をお掃除ロボットが掃除してくれるという、お掃除ロボットとの共同作業を実現できればと思います。

実際は、箒の先に目印となるもの（例えば赤いボール）をつけて、それを追随するようにしてみようと思います。

#ルンバの制御
ルンバはシリアルで制御するためのインターフェース仕様が公開されています。（900シリーズのWiFiからの制御インタフェースは未公開とのこと）
ここにある資料は、Create2というお掃除機能を取り除いたロボット教育用の機種のインタフェース仕様ですが、Create2ではない一般売りの600シリーズでも同じように使用できます。

さらに、このインタフェースを使って、ルンバを制御するためのROS (Robot Operating System)のパッケージもいくつか開発されています。今回は、create_autonomyを使用して、ルンバを制御します。

ラズベリーパイでの例は以下のサイトに詳しく書かれています。今回、私もいろいろと参考にさせていただきました。

ルンバをRaspberry Piを使ってROSでコントロールできるように改造してみた（半田付け無しでOK）

#ルンバへの指令

ルンバへの指令の方法として、以下の２つが実現できるといいのではないかと考えました。

カメラによるトラッキング
音声による指示

今回は、１つめのカメラによるトラッキングについてチャレンジしてみました。トラッキングを実現するために、OpenCV3のTraking APIを活用します。

#用意するもの（ハード編）

今回の試作に用意したハードウェアの一覧を以下に列記します。

品名	型番等
お掃除ロボット	ルンバ622
制御ボード	Dragonboard 410C
制御ボード用電源	cheero Canvas + 昇圧機能内蔵変換ケーブル
コネクター	ミニDIN 8Pプラグを使って自作
USBシリアル変換アダプター	スイッチサイエンス製 FTDI USBシリアル変換アダプター(5V/3.3V切り替え機能付き)
Webカメラ	Logicool C270
SDカード	適当なもの。OSインストールに使ったものを再利用すればよい。

Raspberry piで実現されている例はいくつかみうけますが、今回、Dragonboardを使用するにあたっては以下の点で気をつけないといけません。

GPIOの動作電圧がルンバは5vに対してDragonboardは1.8v
ロジックレベル変換は面倒なので、接続をUSBシリアル接続にすることで回避
Dragonboardの動作電圧が12vのため、一般的なモバイルバッテリーが使えない
ノートPCでの使用に対応したモバイルバッテリー（サンワサプライ 700-BTL012BK）を試してみたが、低電流のためにオートパワーオフが動作するのかしばらくすると電源が切れてしまう事象が発生する。aitendoの昇圧機能内蔵変換ケーブルを使ってcheero Canvasに接続したところ、いまのところ問題なく使用できている。ただし、ケーブルの仕様としては最大出力電流：500mA（使用されているDC-DCコンバータチップのSD6271はデータシートによるとAdjustable Over Current Protection:0.5A~2.5Aとなっている）なので、高負荷時にどうなるかは不明なので自己責任で使用のこと。

すべてのハードを接続すると以下のようになります。

#用意するもの（ソフトウェア編）

今回の試作で使用したソフトウェアの一覧を以下に列記します。

分類	概要	バージョン等
OS	Linux OS	Debian 8
ROS	ロボット制御のためのミドルウェア	Kinetic Kame
create_autonomy	ルンバを制御するためのROSパッケージ	master
OpenCV	画像処理ミドルウェア	V3.2.0 with contrib

ソフトウェアインストール編

必要なミドルウェア一式をインストールする方法を確立するのに、非常に多くの時間を取られてしまいました。この辺り、情報量が少ないDragonboardで一番苦労する点ではないでしょうか。

以下に、順番にその手順を記載します。

1. OSのインストール

まずはOSですが、ROSをインストールするためにDebian8を選定します。現時点での最新の17.x系はDebian9に移行してしまっているので、16.x系の最新のものである、16.09をインストールしました。
※17.x系にROSをインストールされているかたもいらっしゃるので、できないわけではないと思います

ここではインストールの方法は割愛します。

2. ROSのインストール

次に、ROSをイントールします。Kinetic KameからDebian8 + arm64に正式対応したようです。
以下の公式Wikiの手順でaptを使ってインストール可能です。

Debian install of ROS Kinetic

3. create_autonomyのインストール

GitHubからソースを取得してインストールします。

GitHub - AutonomyLab/create_autonomy

GitHubのREADME.mdでは、python-catkin-toolsを使ったイントール方法がのっていますが、私はcatkin_makeを使った方法でインストールしました。

4. ROSの動作確認

ここでいったん、ROSでルンバが動作するかを確認してみます。
ルンバとDragonboardとを接続し、Dragonboardにログインします。

インストールしたcreate_autonomyが使える環境に切り替えます。

~$ cd catkin_ws/install
~/catkin_ws/install$ source setup.bash

次に、ルンバの電源を入れて、create_autonomyを起動します。今回使用するルンバは600シリーズなので、create_2.launchを指定します。

~/catkin_ws/install$ roslaunch ca_driver create_2.launch
... logging to /home/linaro/.ros/log/8d015aa8-6ac0-11e7-97aa-0200bddf2e58/roslaunch-leafa-4402.log
Checking log directory for disk usage. This may take awhile.
　　　　　　　　　　　　・・・
[ INFO] [1500276851.229953569]: [CREATE] "CREATE_2" selected
[ INFO] [1500276852.346942925]: [CREATE] Connection established.
[ INFO] [1500276852.347676985]: [CREATE] Battery level 87.95 %
[ INFO] [1500276852.980051191]: [CREATE] Ready.

正しく起動できると、上記のようにルンバのバッテリー残量が表示されます。

次に、別のコンソールを開いて、ルンバに動作指令を送ります。

~$ source /opt/ros/kinetic/setup.bash 
~$ rostopic pub /cmd_vel geometry_msgs/Twist -- '[0,0,0]' '[0,0,1]'
publishing and latching message. Press ctrl-C to terminate

最後の引数が動作量で['linear', 'angular']、つまり最初の３つでX,Y,Zの移動量、後の３つで回転量Rx,Ry,Rzを指定できます。上記のコマンドでは、z軸回転に1(radian/sec)を指定しており、ルンバは時計回りと逆(counter-clockwise）に回転します。移動量ですが、1でも相当素早く動くので0.1あたりで試すのがよいと思います。

5. OpenCV3.2のインストール

標準のaptパッケージでインストールするとv2.4がインストールされます。また、v3.xのパッケージは用意されていないようです。そこで、v3.2をソースからコンパイルする必要があります。

この場合に注意する点が２つあります。１つ目は、コンパイル中にメモリが不足するためにSDカード上にswapを作成しておく必要があること、２つ目はgitからv3.2のソースを取得してそのままコンパイルすると、必要となるTrakingのAPIのMakeに失敗してインストールされないこと。特に後者は、OpenCVのMake自体は最終的に成功してmake installできるのですが、実際にAPIを使用しようとすると、そんなものはないと言われてしまいます。

基本的な手順は以下のWikiに従います。SDにswapを作成する方法へのリンクも記載されています。

projects/home_surveillance/part-2 at master · 96boards/projects · GitHub

注意点は、「2.3 OpenCV Installation」でgithubから3.2.0をcheckoutした後、Tracking APIが正しくMakeされるためにいくつかソースを修正する必要があります。

原因は、V3.2.0のTracker APIのCMakeLists.txtでdnnがオプションになっていないが、標準ではdnnが有効になっていないのでTracker APIのMakeが失敗するためです。その為、最新のバージョン(v3.3.0-rc)ではdnnがオプションになっています。
この修正を3.2.0のソースに取り込む必要があります。

tracking: make opencv_dnn dependancy optional #1038

この修正したのちに、cmakeおよびmake、make installをすれば、Tracker APIが組み込まれます。

以上で基本となるソフトウェアの構築は完了です。ここから、実際にTrackingの結果からルンバを制御するソフトウェアの制作に入っていきます。

#ルンバ制御のソフトウェア
##1. 対象物をトラッキングして移動量を算出する。

以下のように箒の先につけたボールをトラッキングするプログラムを作成します。

Tracker APIのサンプルのtracker.pyを改良しました。

trackerRoomba.py

import numpy as np
import cv2
import sys
import time

cv2.namedWindow("tracking")
cv2.moveWindow('tracking',0, 0)
camera = cv2.VideoCapture(0)

while True:
    ok, image=camera.read()
    image = image[:,::-1]
    image = cv2.resize(image, (800,600))

    cv2.imshow('select target', image)
    cv2.moveWindow('select target',0, 0)

    k = cv2.waitKey(1)
    if k == 27:
        camera.release()
        cv2.destroyAllWindows()
        exit()


    if k == 115: # press 's'
        cv2.destroyWindow('select target')
        bbox = cv2.selectROI("tracking", image)
        break

#tracker = cv2.Tracker_create('MIL')
tracker = cv2.Tracker_create('KCF')
ok = tracker.init(image, bbox)

while camera.isOpened():
    ok, image=camera.read()
    if not ok:
        print('no image to read')
        break

    image = image[:,::-1]
    image = cv2.resize(image, (800,600))

    ok, newbox = tracker.update(image)

    if ok:
        p1 = (int(newbox[0]), int(newbox[1]))
        p2 = (int(newbox[0] + newbox[2]), int(newbox[1] + newbox[3]))
        cv2.rectangle(image, p1, p2, (200,0,0))

    cv2.imshow("tracking", image)
    k = cv2.waitKey(1) & 0xff
    if k == 27 : break # esc pressed

    time.sleep(0.5)

サンプルではTrackingのアルゴリズムにMILを使用していますが、KCFの方が追従性能がよかったのでそちらを採用しています。

アプリの動作には画面が必要なため、DragonboardにVNC　Server（x11vnc）をインストールしてアクセスします。

起動すると、カメラのキャプチャー画面が表示されます。

's'キーを押すと、トレース領域の選択画面になりますので、トレースの対象物を選択します。

その後、SPACEキーを押すとトレースを開始します。

Tracker APIのupdate()の返値としてトレース領域の座標が返るので、

　　　　ok, newbox = tracker.update(image)

この値の前回からの差分量を使って、ルンバの移動量を算出してROSのメッセージを送ります。

実際に動作させてみると、VNC経由での動作の影響もあってか、素早い動作の追従は難しい状態です。トレースが外れてしまうことがありました。

このあたりはUIなしで動作するように改良していく必要があるかと思います。アルゴリズムにも工夫が必要かもしれません。

##2. ROSのメッセージで移動指示する。

rospyを使って、create_autonomyに移動メッセージを送信する方法は以下のようになるかと思います。
※こちらのソースを参考にさせてもらっています。

moveAround.py

import rospy
from geometry_msgs.msg import Twist
from math import pi

class moveAround():
    def __init__(self):
        rospy.init_node('moveAround', anonymous=False)

        rospy.on_shutdown(self.shutdown)

        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        rate = 50
        r = rospy.Rate(rate)

        angular_speed = 1.0
        goal_angle = pi/4
        angular_duration = goal_angle / angular_speed

        move_cmd = Twist()
        move_cmd.angular.z = angular_speed

        ticks = int(goal_angle * rate)
        for t in range(ticks):
            self.cmd_vel.publish(move_cmd)
            r.sleep()

        self.cmd_vel.publish(Twist())

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

if __name__ == '__main__':
    try:
        moveAround()
    except:
        rospy.loginfo("Out-and-Back node terminated.")

移動量は固定になっていますが、

        angular_speed = 1.0
        goal_angle = pi/4

この値を変えることで回転方向と角度を変更できます。

#ということで

すいません、現状できているのはここまでです。

引き続き完成に向けて、プログラムの作成と検証をすすめていこうと思います。

以上です。