この記事は、TIS Advent Calendar 2018の24日目の記事です。

はじめに

とうとうロボットのプログラムがクラウド上で書ける時代にまでなりましたが、皆様ロボットはお好きでしょうか？

先日公開したAWS RoboMaker入門 ~デモ起動からコード修正、実機デプロイまで~という記事では、クラウド上で書いたロボットアプリケーションをクラウド上でシミュレートし、その後インターネット越しに実機にデプロイする、という流れを説明しました。

しかしこの記事で動かしたロボットアプリケーションは、起動するとロボットがひたすらグルグル回り続けるだけで、外界とは何も連携しないものでした。これでは全然楽しくないので、今回はAWS IoT Coreからロボットへ動作を命令できるようにしたいと思います。

今回の記事で作成したROSアプリケーションのリポジトリ

今回の記事で作成したROSアプリケーションは、githubのリポジトリで公開しています。記事にあわせてご確認ください。

シミュレーション環境を準備する

前回の記事を参考に、AWS RoboMakerのシミュレーション環境を立ち上げます。

注意点は、RoboMakerシミュレーション環境をインターネットゲートウェイを持つマルチAZなVPC内で動作させることです。

RoboMakerのシミュレーション環境は、VPCを指定しないと外界と接続しないクローズドな仮想ネットワーク上で起動します。今回動作させるロボットアプリケーションは、インターネット経由でAWS IoT Coreに接続しますので、シミュレーション環境でもインターネットに接続できなければなりません。そのため、インターネットゲートウェイを持つVPCを明示的に指定し、その上でシミュレーション環境を起動させたいと思います。¹

SubnetとSecurity Groupを作成する

ローカルのアドレス以外はインターネットへルーティングするSubnetを、us-east-1cとus-east-1dに作ります。

また、Inboundは全部Denyで、Outboundは全部AllowするSecurity Groupも一つ作っておきます。

RoboMakerシミュレーション環境を起動する

前回の記事の "Hello World デモを動かしてみよう" と同様に、まずはVPC外でHello worldデモのサンプルシミュレーション環境を起動します。シミュレーション環境が正しく起動したことを確認したら、そのシミュレーション環境で "Clone" をクリックします。

"Step 1: Condigure simulation" の "Edit" をクリックします。

先ほど作成したVPCを選択し、Subnetを二つとも指定します。また先ほど作成したSecurity Groupも指定し、 "Next" をクリックします。

"Step 2: Specify robot application." はそのままにしておき、 "Step 3: Specify simulation application." の "Edit" をクリックします。

"TURTLEBOT3_MODEL" 環境変数の値として "waffle" を指定することで、シミュレータ上に出現するロボットを "Turtlebot3 Waffle" に変更しておきます。

上記のように設定を変更し、 "Create" すると、新たにシミュレーション環境が起動します。指定したSubnetとSecurity Group内で起動していること、Simulation applicationの環境変数 "TURTLEBOT3_MODEL" の値が "waffle" として設定されていることを確認してください。
（VPC外で起動させた最初のシミュレーション環境はもう使わないので、 "Cancel" してしまってかまいません。）

では、起動したシミュレーション環境がインターネットに接続できることを確認しましょう。シミュレーション環境の "Terminal" を起動し、次のコマンドを実行してください²。

$ python -c "import urllib;print(urllib.urlopen('https://www.google.com').read())"

正しく設定されていれば、 www.google.com のHTMLが表示されるはずです。

AWS IoT Coreを準備する

シミュレーション環境が起動したので、次はAWS IoT Coreを準備します。

Thingを登録し、証明書をダウンロードする

AWS IoTのコンソールを用いて、ロボットに相当するモノ（Thing）を登録します。

"Manage > Things" から、 "Register a Thing" をクリックします。

"Create a single Thing" をクリックし、名前として "turtlebot3_waffle" を入力した後に "Next" をクリックします（"Thing Type" や "Thing Group" はデフォルトのままで大丈夫です）。

"Create certificate" をクリックし、AWS IoT Coreと接続するためのクライアント証明書と公開鍵、秘密鍵のペアを生成します。

生成されたクライアント証明書と秘密鍵をダウンロードします。
また、"root CA for AWS IoT" のリンク先から、AWS IoT Coreのroot証明書をダウンロードします（Amazon Root CA 1で大丈夫です）。

ダウンロードした後に "Done" をクリックすれば、 "turtlebot3_waffle" というThingが登録されます（Policyはまだ作っていないので、後からアタッチします）。

証明書にPolicyを設定する

次に、このThingに許可するPolicyを設定しましょう。今回のROSアプリケーションでは、AWS IoT Coreへ次の操作をする権限が必要です。

AWS IoT CoreへMQTT Clientとして接続する
/hello_world_robot/#というMQTT Topicをsubscribeする

またAWS IoT Core経由でロボットへ動作命令を出すためには、AWS IoT CoreへMQTTメッセージを送りつける権限が必要です。別の役割なので本来は別のThingとして扱ったほうが良いのですが、今回は横着して同じThingを使い回すことにします。そのため、次の権限も合わせて付与します。

/hello_world_robot/subというMQTT Topicへメッセージをpublishする

では、実際にコンソールからPolicyを作成しましょう。 "Secure > Policies" から、 "Create a policy" をクリックします。

今回はjson形式でPolicyを設定するので、 "Advanced mode" をクリックします。名前として "robomaker_iot_policy" を入力し、PolicyのStatementとして以下のjsonを入力して "Create" します（MQTTの記法としては、トピックフィルタのワイルドカードは+と#ですが、IAMの記法に合わせて *を用いることに注意してください）。

注意： 999999999999は自身のアカウントIDに置き換えてください

Policy

{
  "Version": "2012-10-17",
  "Statement": [
    {
      "Effect": "Allow",
      "Action": [
        "iot:Connect"
      ],
      "Resource": "*"
    },
    {
      "Effect": "Allow",
      "Action": [
        "iot:Subscribe"
      ],
      "Resource": [
        "arn:aws:iot:us-east-1:999999999999:topicfilter//hello_world_robot/*"
      ]
    },
    {
      "Effect": "Allow",
      "Action": [
        "iot:Receive",
        "iot:Publish"
      ],
      "Resource": [
        "arn:aws:iot:us-east-1:999999999999:topic//hello_world_robot/sub"
      ]
    }
  ]
}

無事にPolicyが登録できたら、先ほど作成した証明書にアタッチします。
"Secure > certificates" から、生成済みの証明書の "Attach policy" をクリックします。

作成した "robomaker_iot_policy" を選択し、 "Attach" します。

最後に、証明書を "Activate" します。これにより、この証明書が使えるようになります。

AWS IoT Coreのエンドポイントを確認する

登録したThingが接続するAWS IoT Coreのエンドポイントは、 "Settings" から確認できます。後で必要になりますので、メモしておきましょう。

Macから接続を確認する

では、AWS IoTへ正しくThingが登録できたか、ダウンロードした証明書や秘密鍵を用いてMacから接続してみます。事前にMQTT Client（mosquitto_subとmosquitto_pub）をインストールしておいてください。

なお注意点ですが、QoS=1を明示的に指定してください³。
mosquitto_subもmosquitto_pubも、デフォルト（-qオプションを指定しない）ではQoS=0でMQTT Brokerへ接続します。ローカルのMQTT Brokerに接続する場合はQoS=0でも問題ありませんが、AWS IoT Coreはインターネットの向こう側にあるため、QoS=0だとsubscriberまでメッセージが届かない場合があります。

subscriberを起動

ダウンロードしたクライアント証明書と秘密鍵、AWS IoT Coreのroot証明書を用いて、先ほど確認したエンドポイントの8883ポートへ接続し、QoS=1で/hello_world_robot/# をsubscribeします。

$ mosquitto_sub -d \
--cafile <<<root証明書のpath>>> \
--cert <<<クライアント証明書のpath>>> \
--key <<<秘密鍵のpath>>> \
-h <<<AWS IoT Coreのエンドポイント>>> -p 8883 \
-t /hello_world_robot/# \
-q 1

AWS IoT Coreが正しく設定されてれば、subsciribeに成功するはずです。

次に、別のTerminalから /hello_world_robot/sub へメッセージを送ってみます。

mosquitto_pub -d \
--cafile <<<root証明書のpath>>> \
--cert <<<クライアント証明書のpath>>> \
--key <<<秘密鍵のpath>>> \
-h <<<AWS IoT Coreのエンドポイント>>> -p 8883 \
-t /hello_world_robot/sub \
-q 1 \
-m "{\"message\": \"robomaker iot\"}"

最初のTerminalへメッセージが届けば、AWS IoT Coreの準備は完了です。

AWS IoT Coreに接続するROSアプリケーションを書く

諸々準備が整いましたので、RoboMakerの開発環境を立ち上げて、AWS IoT Coreから命令を受け取るROSアプリケーションを書きましょう。

RoboMakerの開発環境を起動する

前回の記事のHello Worldデモのコードを修正するを参考に、1. RoboMakerのROS開発環境を起動 2. Hello worldデモのソースコードの取り込み 3. ROSワークスペースの初期化まで実行します（このRoboMakerの開発環境は、シミュレーション環境用に作ったVPCに同居させてかまいません）。

開発環境へ証明書をコピーする

HelloWorld/robot_ws/src/hello_world_robot直下にcertsディレクトリを作成し、ダウンロードしたクライアント証明書と秘密鍵、及びAWS IoT Coreのroot証明書をドラッグ＆ドロップして開発環境へコピーします。

証明書をバンドル対象に指定する

RoboMaker開発環境にディレクトリを作成してファイルを配置しただけでは、シミュレーション環境や本番環境へデプロイするバンドルファイルに取り込まれません。そのため次のように、CMakeLists.txtの install(DIRECTORY ...) 命令に、certs ディレクトリを追加してください。ビルドやバンドルする際には、colcon（が起動するcatkin）がこのCMakeLists.txtを参照し、イイカンジに処理してくれます。

CMakeLists.txt

   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

-install(DIRECTORY launch
+install(DIRECTORY launch certs
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

PythonのMQTTクライアントライブラリをインストールする

ROSではrosdepというツールを用いて、ROSアプリケーションが依存するOSパッケージやPythonライブラリ等をインストールすることができます。/etc/ros/rosdep/sources.list.d/以下の.listファイル（が参照しているYAMLファイル）を確認すれば、rosdepによってインストールできるライブラリが探せます⁴。

RoboMakerで用いているROS（Kinetic）では、Python用のMQTTクライアントライブラリであるpaho-mqttがpython-paho-mqtt-pipという名前でリストアップされていますので、今回はこれを使うことにします。（デフォルトでリストアップされているPythonライブラリの詳細は、python.yamlを確認してください。）

では、python-paho-mqtt-pipに依存することを宣言しましょう。次のように、package.xmlにpython-paho-mqtt-pipを追加してください。

package.xml

   <build_export_depend>message_runtime</build_export_depend>
   <exec_depend>message_runtime</exec_depend>
   <exec_depend>turtlebot3_bringup</exec_depend>
+  <depend>python-paho-mqtt-pip</depend>
 </package>

package.xmlを修正した後に下記のコマンドを再実行すると、rosdepが "paho-mqtt" をインストールします。

$ cd $HOME/environment/HelloWorld/robot_ws/
$ rosdep install --from-paths src --ignore-src -r -y

Successfully installed paho-mqtt-<<バージョン番号>>というログが出力されることを確認してください。

AWS IoT Coreに接続するソースコードを書く

それでは、AWS IoT Coreに接続するソースコードを書きましょう。

今回のROSアプリケーションはある程度複雑になりますので、AWS IoT Coreから命令を受け取ってロボットを操作するクラスを作ることにします。src/hello_world_robot以下⁵にawsiot.pyを作成し、AWSIoTクラスを実装しましょう。

src/hello_world_robot/awsiot.py

# -*- coding: utf-8 -*-
import ssl
import json
import time

import rospy
from geometry_msgs.msg import Twist

import paho.mqtt.client as mqtt


class AWSIoT(object):
    QOS = 1
    HZ = 10

    def __init__(self):
        rospy.loginfo("AWSIot#__init__")
        self.is_connected = False
        self.__client = mqtt.Client(protocol=mqtt.MQTTv311)
        self.__client.on_connect = self._on_connect
        self.__client.on_message = self._on_message
        rospy.on_shutdown(self._on_shutdown)
        self.__params = rospy.get_param("~awsiot") # get parameters from ros parameter server

    def run(self):
        rospy.loginfo("AWSIoT#run")

        # set certification files
        self.__client.tls_set(
            ca_certs=self.__params["certs"]["rootCA"],
            certfile=self.__params["certs"]["certificate"],
            keyfile=self.__params["certs"]["private"],
            tls_version=ssl.PROTOCOL_TLSv1_2)

        # connect to AWS IoT Core
        self.__client.connect(
            self.__params["endpoint"]["host"],
            self.__params["endpoint"]["port"],
            keepalive=120)
        self.__client.loop_start()

    # this method is called when connected to AWS IoT Core successfully
    def _on_connect(self, client, userdata, flags, response_code):
        rospy.loginfo("AWSIoT#_on_connect response={}".format(response_code))
        # subscribe '/hello_world_robot/sub' mqtt topic
        client.subscribe(self.__params["mqtt"]["topic"]["sub"], qos=AWSIoT.QOS)
        self.is_connected = True
        # create a ROS publisher to publish a Twist message to '/cmd_vel' ROS topic
        self.__cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)

    # this method is called when received a message from AWS IoT Core
    def _on_message(self, client, userdata, data):
        topic = data.topic
        payload = str(data.payload)
        rospy.loginfo("AWSIoT#_on_message payload={}".format(payload))
        twist = Twist()
        try:
            params = json.loads(payload)
            if "x" in params and "z" in params and "sec" in params:
                start_time = time.time()
                d = float(params["sec"])
                r = rospy.Rate(AWSIoT.HZ)
                # publish Twist message to '/cmd_vel' ROS topic in order to operate Turtlebot3
                while time.time() - start_time < d:
                    twist.linear.x = float(params["x"])
                    twist.angular.z = float(params["z"])
                    self.__cmd_pub.publish(twist)
                    r.sleep()
        except (TypeError, ValueError):
            pass

        twist.linear.x = 0.0
        twist.angular.z = 0.0
        self.__cmd_pub.publish(twist)

    # this method is called when terminated ROS node
    def _on_shutdown(self):
        logmsg = "AWSIoT#_on_shutdown is_connected={}".format(self.is_connected)
        rospy.loginfo(logmsg)
        if self.is_connected:
            self.__client.loop_stop()
            self.__client.disconnect()

このAWSIoTクラスは、次のような処理を行います。

runメソッドが呼び出されると、バンドルされている証明書を用いてAWS IoT CoreにQoS=1で接続します。証明書のパスやAWS IoT Coreのエンドポイントは、ROSのParameter Server（後述）から取得します。
接続に成功すれば_on_connectメソッドがコールバックされ、Parameter Serverから取得したMQTTトピックをsubscribeします。またTurtlebot3を操作するために、ROSの/cmd_velトピックへメッセージをpublishするpublisherも作成しておきます。
subscribeしているMQTTトピックへメッセージが到着すると、_on_messageメソッドがコールバックされ、そのメッセージが配信されてきます。今回の実装では、メッセージを受信すると、次のような処理を行っています。
1. メッセージのjsonが "x", "z", "sec" という数値型の属性を持っているjsonの場合、次のようなTwistメッセージを "sec" 秒経過するまで0.1秒ごとにROSの/cmd_velトピックへpublishする。
```
{
    linear:  {
        x: <<受信した"x"の数値>>, 
        y: 0.0,
        z: 0.0
    },
    angular: {
        x: 0.0,
        y: 0.0,
        z: <<受信した"z"の数値>>
    }
}
```
2. 最後に、linearとangularのx, y, zが全て0.0のTwistメッセージを１回だけROSの/cmd_velトピックへpublishする（直進速度と回転速度が0なので、Turtlebot3がその場で停止することになる）。

ROSノードの起動スクリプトを修正する

次に、ただグルグル回るだけだったnodes/rotateを修正し、AWSIoTインスタンスへ実際の処理を委譲するように書き換えます。

nodes/rotate

#!/usr/bin/env python

# TODO fix to set appropriate PYTHONPATH by configurations
import sys
import os
sys.path.append(os.path.join(os.path.dirname(__file__), 
"../../../../../usr/local/lib/python2.7/dist-packages"))

import rospy
from hello_world_robot.awsiot import AWSIoT


def main():
    rospy.init_node('awsiot')
    try:
        rospy.loginfo("main start")
        AWSIoT().run() # call AWSIoT#run method
        rospy.spin() # keep python from exiting until this node is stopped
        rospy.loginfo("main end")
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()

rospy.spin()を入れ忘れると、このROSアプリケーションは起動直後に終了してしまいますので、ご注意ください。

注意:
Hello worldデモを改造して作った今回のROSアプリケーションは、colconがバンドルしたファイルをシミュレータや実機にデプロイした際、なぜか rosdepがインストールしcolconによってバンドルされたPythonライブラリへのパスを通してくれません。colcon-coreやcolcon-bundleまわりの設定がどこかにあるのだと思いますが、いまいちよくわかりません。
仕方ないので、nodes/rotate内でrosdepのインストール先を直接ライブラリパスに追加しちゃってます。美しくないので、正しいやり方をご存知の方は、こっそり私に教えてください（笑

launchファイルにROSパラメータを設定する

rotate.launchに<param>タグを追加し、今回のROSアプリケーションで必要となるパラメータを設定します。これらのパラメータは、ROSアプリケーション起動時にROSのParameter Serverに設定され、以降ROSプログラムから読み書きできるようになります。

launch/rotate.launch

   <!-- Rotate the robot on launch -->
-  <node pkg="hello_world_robot" type="rotate" name="rotate" output="screen"/>
+  <node pkg="hello_world_robot" type="rotate" name="rotate" output="screen">
+    <param name="awsiot/endpoint/host" value="<<AWS IoT Coreのエンドポイント"/>
+    <param name="awsiot/endpoint/port" value="8883"/>
+    <param name="awsiot/certs/rootCA" value="$(find hello_world_robot)/certs/AmazonRootCA1.pem"/>
+    <param name="awsiot/certs/certificate" value="$(find hello_world_robot)/certs/<<クライアント証明書のファイル名>>"/>
+    <param name="awsiot/certs/private" value="$(find hello_world_robot)/certs/<<秘密鍵のファイル名>>"/>
+    <param name="awsiot/mqtt/topic/sub" value="/hello_world_robot/sub"/>
+  </node>
 </launch>

Hello Worldデモのコードを修正するを参考に、 "HelloWorld Robot" と "HelloWorld Simulation" をビルドしてバンドルし、それらを用いてシミュレーション環境を再起動してください。

ROSアプリケーションが起動していることを確認する

シミュレーション環境が再起動したら、シミュレータの "Terminal" を開き、rosnode listコマンドで起動しているros nodeの一覧を表示します。/rotatenodeが起動していることを確認してください。

うまく動かなかった場合には

何らかのミスがあり、ROSアプリケーションが上手く起動できなかった場合、シミュレータの "Terminal" から次のコマンドを叩けば、シミュレーション環境のdockerコンテナ上でROSアプリケーションの起動を試みることができます。

$ source $HOME/workspace/robot-application/bundle/opt/install/local_setup.bash
$ roslaunch hello_world_robot rotate.launch

そもそも起動に失敗したROSアプリケーションですから、やっぱりエラーが発生して落ちるでしょう。が、その際に、rospy.loginfo()やprint()で出力したログメッセージや、エラー発生箇所のスタックトレースが "Terminal" 上に表示されます。それを頼りにデバッグすると良いでしょう。

シミュレータ上のロボットをAWS IoT Coreから操作する

それでは、デプロイしたROSアプリケーションの動作を確認してみましょう。
Macのターミナルから、AWS Iot Coreへ次のようなメッセージをpublishします。ロボットがこのメッセージを受信したら、並進速度0.1m/s、回転速度0.5rad/sで、6.28秒間だけ動くはずです。

$ mosquitto_pub -d \
--cafile <<<root証明書のpath>>> \
--cert <<<クライアント証明書のpath>>> \
--key <<<秘密鍵のpath>>> \
-h <<<AWS IoT Coreのエンドポイント>>> -p 8883 \
-t /hello_world_robot/sub \
-q 1 \
-m "{\"message\": \"start node\", \"x\": 0.1, \"z\": 0.5, \"sec\": 6.28}"

無事に動作しましたね！
（後半でシミュレータのロボットがガクガクしているのは、Macのネットワークの調子が良くなかったせいです・・・）

実機のロボットをAWS IoT Coreから操作する

では最後に、このROSアプリケーションをTurtlebot3の実機へデプロイして、動作を確認してみましょう。
シミュレータと同様に、{"message": "start node", "x": 0.1, "z": 0.5, "sec": 6.28}というメッセージをAWS IoT Coreにpublishします。

MQTTメッセージを受け取ると、ロボットが命令に従って動作しました！

まとめ

AWS RoboMakerを使ってロボットをAWS IoT Coreに接続することにより、外部からロボットに動作を命令することができました。またロボット本体で特に作業をせずとも、外部のPythonライブラリを利用するROSアプリケーションをインターネット越しにロボットへデプロイすることもできました。

加えて、AWS RoboMakerとAWS IoTの認証認可機構をうまく組み合わせることで、昨今問題になっているIoTデバイスのセキュリティ問題へも一貫した手順で対策が可能となっています（証明書がロボット側に同梱されてしまうため、ロボットが物理的に盗難された場合への対応は、別途考える必要がありますが）。

今回はROSアプリケーションをあまり複雑にしたくなかったため、ロボットの動作仕様に即したメッセージ（"x", "z", "sec"）をAWS IoT Coreから送信する形で実装しました。しかし送信するメッセージをより抽象的な命令セットにし、それらの命令セットをロボットの仕様に合わせて翻訳するトランスレータをROSアプリケーションとして実装する形にすれば、別機種のロボットへ入れ替えても動作を命令する側は変更する必要が無くなり、より柔軟なシステムを組み立てることができるようになると思います。

まだ始まったばかりのAWS RoboMakerですが、ぜひ試してみていただければと思います。

検証なのにわざわざマルチAZにしているのは、AWS RoboMakerがシングルAZのVPCを受け付けてくれないためです。 ↩
シミュレーション環境のdockerコンテナは、pingやnslookup等のネットワーク関連のコマンドが入っていません。またsudoもできないため、rootになって "iputils-ping" や "net-tools" をインストールすることもできません。Python2.7は動作するため、結局このようなワンライナーを使うことにしました。 ↩
QoS 0 (At most once) ：メッセージは最大で1回送信される（まったく送信されないこともある）。メッセージが届くことは保証されない。
QoS 1 (At least once) ：メッセージは最低1回送信される。受信側は同じメッセージを複数回受け取る場合がある。
QoS 2 (Exactly once) ：メッセージは常に、正確に1回送信される。 ↩
デフォルトで対応していないライブラリをインストールしたい場合は、別途YAMLファイルを書いてrosdepに認識させる必要があります。 ↩
RoboMakerのHello worldデモは、setup.pyの設定とCMakeLists.txtのcatkin_python_setup()命令により、src/hello_world_robot以下がPythonのライブラリパスに含まれるように設定されています。またsrc/hello_world_robot/__init__.pyも最初から作成されています。 ↩