More than 3 years have passed since last update.

TISAdvent Calendar 2019Day 10

ルンバの掃除状況を可視化しよう〜 ROSとFIWAREを添えて〜

Last updated at 2019-12-10Posted at 2019-12-10

皆様ルンバを使っていますか。自動的に掃除をしてくれる便利なヤツで、一度使うと手放せなくなりますよね。「ルンバ900シリーズ」以降はカメラを用いたSLAM（visual slam）を実装して効率的に掃除するようになっており、最新のルンバアプリではSLAMでマッピングした地図と掃除履歴等が確認できます。

しかしそれ以前のルンバは、観察していないと実際にどこを掃除したのかイマイチよくわかりませんでした。もしかして観測されてないときはサボってるかも？

ということで、ルンバの低レベルな操作が可能なオープンインタフェース Roomba Open Interface をROSから利用し、ルンバの移動軌跡をOSSのIoTプラットフォームであるFIWAREに蓄積して可視化してみましょう。

検証環境

Roomba Open Interfaceにシリアル-USB変換アダプタをつけたルンバ
- iRobot Roomba 780
- FTDI USBシリアル変換アダプター
ROSが動作するRaspberryPi
- Raspberry Pi 3 model B+ (Revision: a020d3)
- Ubuntu Mate 16.04.6 LTS (4.14.52-v7+ #1123 SMP Wed Jun 27 17:35:49 BST 2018 armv7l)
- ROS Kinetic
クラウド上で動作するFIWARE
- Microsoft Azure AKS (kubernetes 1.15.5)
- FIWARE (Release 7.7.1)
  - FIWARE orion (2.2.0)
  - FIWARE iotagent-json (1.10.1)
  - FIWARE cygnus (1.15.0)
  - FIWARE STH Comet (2.5.0)
- Rabbit MQ (3.7.16)
- Mongo DB (4.1.13)
作業用PC
- iMac (Retina 5K, 27-inch, 2017) macOS Mojave 10.14.6
- kubectl v1.16.3
- curl 7.54.0
- jq jq-1.6
- mosquitto_sub 1.6.7

ルンバのROS化

まずはルンバ780をROS化して、掃除している際の自己位置をROSから取れるようにします。
※ コネクタにジャンパを刺す作業をする際は、ルンバの電源が落ちていることを確認してください

Roomba Open Interfaceにシリアル-USB変換アダプタを接続

ルンバ780のRoomba Open Interfaceにシリアル-USB変換アダプタを接続するためのハードウェア的な手順は、ルンバをRaspberry Piを使ってROSでコントロールできるように改造してみた（半田付け無しでOK）を参考に実施しました。

ありがとうございます、からあげ (id:karaage)さん！

ただし Raspberry Pi 3 model B+ は core_freq=250 はすでに設定済みですので、 /boot/config.txt には enable_uart=1 のみ追記すればOKです。
またルンバ780のRoomba Open Interfaceは取っ手の下に設置されているため、天板を外す必要はありません。

実は天板を外して、「コネクタが無い！」とかやってます。

ルンバの状態を取得するROSパッケージをインストール

Raspberry PiのUSBポートにシリアル-USB変換アダプタを接続すれば、Roomba Open Interface経由でルンバを操作できるようになりますが、インタフェース仕様を元に自力で実装するのは大変です。そのため既存のROSパッケージを活用しましょう。今回はAutonomyLab/create_autonomyを使いました。

ROS Workspaceの構築

AutonomyLab/create_autonomy がcatkin toolsでのビルドを要求しているため、まずはcatkin toolsをインストールします。

catkin toolsのインストールコマンド

ros@ros-desktop:~$ sudo apt-get install python-rosdep python-catkin-tools

続いてインストールした catkin tools を用いてROS Workspaceを初期化します。

ROS Workspace作成＆初期化コマンド

ros@ros-desktop:~$ mkdir -p ~/catkin_ws/src
ros@ros-desktop:~$ cd ~/catkin_ws/
ros@ros-desktop:~/catkin_ws$ catkin init
ros@ros-desktop:~/catkin_ws$ catkin build

AutonomyLab/create_autonomyの修正

ROS Workspaceのsrc以下に、 AutonomyLab/create_autonomy をcloneします。

AutonomyLab/create_autonomyのgit cloneコマンド

ros@ros-desktop:~/catkin_ws$ cd src/
ros@ros-desktop:~/catkin_ws/src$ git clone https://github.com/AutonomyLab/create_autonomy.git
ros@ros-desktop:~/catkin_ws/src$ cd create_autonomy/

残念ながらAutonomyLab/create_autonomyにはRoomba Open Interfaceが規定する4つのモード（off, passive, safe, full）を動的に変更する機能が実装されておらず、ROS Nodeとして起動すると自動的にfull mode（ルンバを完全遠隔制御するモードで、CLEANボタンやスケジューラが機能しなくなる）に変更してしまいます。

起動後にモード変更を可能にするIssueは上がっており実装も存在しますが、mergeされず3年以上放置されているので期待薄かと。。。

そのためpassive mode（ルンバの状態を取得するだけのモード）として起動するようにソースを１行改造し、ルンバのスケジューラを有効にしておきます。

ros@ros-desktop:~/catkin_ws/src/create_autonomy$ git diff
diff --git a/ca_driver/src/create_driver.cpp b/ca_driver/src/create_driver.cpp
index 8f723e4..345ee81 100644
--- a/ca_driver/src/create_driver.cpp
+++ b/ca_driver/src/create_driver.cpp
@@ -82,7 +82,7 @@ CreateDriver::CreateDriver(ros::NodeHandle& nh)
   ROS_INFO("[CREATE] Connection established.");

   // Start in full control mode
-  robot_->setMode(create::MODE_FULL);
+  robot_->setMode(create::MODE_PASSIVE);

   // Show robot's battery level
   ROS_INFO("[CREATE] Battery level %.2f %%", (robot_->getBatteryCharge() / robot_->getBatteryCapacity()) * 100.0);

なお今回はシリアルインタフェースから直接ではなくUSBからルンバの情報を取得するため、データ取得元デバイスファイルの設定は /dev/ttyUSB0 のままで大丈夫です。

AutonomyLab/create_autonomyの設定

ca_driver/config/default.yaml

ros@ros-desktop:~/catkin_ws/src/create_autonomy$ grep "dev" ca_driver/config/default.yaml
# The device path for the robot
dev: "/dev/ttyUSB0"

AutonomyLab/create_autonomyのビルド

では、 catkin tools を用いてをビルドしましょう。

AutonomyLab/create_autonomyのビルドコマンド

ros@ros-desktop:~/catkin_ws/src/create_autonomy$ cd ../..
ros@ros-desktop:~/catkin_ws$ rosdep update
ros@ros-desktop:~/catkin_ws$ rosdep install --from-paths src -i
ros@ros-desktop:~/catkin_ws$ catkin build

ビルドに成功すると、 [build] Summary: All 4 packages succeeded! と表示されます。

ルンバの位置と姿勢が取得できることを確認

それでは、ルンバの情報を取得してみましょう。

AutonomyLab/create_autonomyを起動

ルンバの電源を入れ、インストールした AutonomyLab/create_autonomy の ca_driver を起動します。

ca_driverの起動コマンド

ros@ros-desktop:~/catkin_ws$ source devel/setup.bash
ros@ros-desktop:~/catkin_ws$ roslaunch ca_driver create_2.launch

Roomba Open Interfaceと正しく接続され、 create_autonomy が正しくインストールされていれば、「ピポッ」という電子音が鳴り、以下のようなログが表示されます。

ca_driverの起動ログ

[ INFO] [1575765308.248248198]: [CREATE] "CREATE_2" selected
[ INFO] [1575765309.397929018]: [CREATE] Connection established.
[ INFO] [1575765309.398282194]: [CREATE] Battery level 100.00 %
[ INFO] [1575765309.810264514]: [CREATE] Ready.

ルンバの位置と姿勢の取得

別のコンソールからRaspberry PiにSSHし、 /odom ROS topicからルンバのodometryを取得してみましょう。

ルンバのodometryの取得コマンド

ros@ros-desktop:~$ cd catkin_ws/
ros@ros-desktop:~/catkin_ws$ source devel/setup.bash
ros@ros-desktop:~/catkin_ws$ rostopic echo /odom

以下のようなメッセージがひたすら表示され続けると思います。これはルンバの現在のodometry（タイヤの回転角の累積から現在位置と姿勢を推定した情報）です。 pose の position がルンバの現在の位置（x座標, y座標, z座標） orientation がquaternionで表したルンバの現在の姿勢（x軸、y軸、z軸周りの回転角度）です。ca_driver を起動した位置と姿勢が原点となり座標軸が設定されるので、最初は座標も回転角度も全部0です。

ca_driver起動直後のルンバのodometry

---
header:
  seq: 3064
  stamp:
    secs: 1575765616
    nsecs: 210830228
  frame_id: "odom"
child_frame_id: "base_footprint"
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

スケジューラに従ってルンバが掃除を開始すると、ルンバの位置と姿勢が変化しますので、その情報がodometryから得られます。例えば以下のような情報が得られた場合、ルンバはホーム位置から見て x軸方向に -1.73m、 y軸方向に -0.51m 程度移動しており、z軸周りに -57.6度程度回転していることがわかります。ルンバは床の上を移動するので、z座標やx軸回転角、y軸回転角は常に0です。

移動中のルンバのodometry

---
header:
  seq: 6597
  stamp:
    secs: 1575765969
    nsecs: 510832893
  frame_id: "odom"
child_frame_id: "base_footprint"
pose:
  pose:
    position:
      x: -1.7331609726
      y: -0.512128353119
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.482031389874
      w: 0.87615394719
  covariance: [160.9838409423828, 18.37824249267578, 0.0, 0.0, 0.0, 154.49525451660156, 18.378467559814453, 149.55435180664062, 0.0, 0.0, 0.0, -58.26539993286133, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 154.49525451660156, -58.26539993286133, 0.0, 0.0, 0.0, 242.8682861328125]
twist:
  twist:
    linear:
      x: 6.65016174316
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 2.97879505157
  covariance: [0.0006561162881553173, -0.00103395851328969, 0.0, 0.0, 0.0, 0.0008680145256221294, -0.00103395851328969, 0.001632406609132886, 0.0, 0.0, 0.0, -0.000663519836962223, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0008680140599608421, -0.000663519836962223, 0.0, 0.0, 0.0, 0.1656971126794815]
---

ここまでで、ルンバの現在の位置と現在の姿勢をリアルタイムに得ることができました。しかしこのままでは、取得したルンバの位置と姿勢の情報は闇に消えていくだけです。そこで取得した情報をIoTプラットフォームであるFIWAREに送りつけ、履歴データして蓄積してもらいましょう。

`/base_footprint`は？

SLAMで自己位置推定をする場合、通常 /map → /odom → /base_footprint と座標変換をします。 ca_driver でも tfで /base_footprint を発行するようにオプションを設定できますが、位置と姿勢の値自体は /odom と同じなので今回は省略しています。

クラウド上にIoTプラットフォーム（FIWARE）を構築

ではルンバの位置や姿勢の情報を収集するプラットフォームとして、弊社がOSSとして公開しているロボットプラットフォームのRoboticBaseを用いてFIWARE環境をAzure AKS上に起動しましょう。

FIWAREやRoboticBaseについては、次の資料を参考にしてください。

FIWARE勉強会 20190913 @ slideshare

現時点のRoboticBase/core 0.4.4 には、ローカルPC上のminikubeかAzure AKSで環境構築する手順が整備されています。しかし基本的にクラウド固有の機能は使っていないため、Persistent Volume ClaimのAnnotationを変更するなど、少し手を加えればAWS EKS等でも動作すると思います。

FIWARE環境をAzure AKS上へ構築

リポジトリの取得と環境固有のパラメータを設定

RoboticBase/coreをcloneしてください。同梱されている環境構築手順書は、${HOME}直下にcloneした前提で書かれています。

cloneしたcoreディレクトリに移動し、docs/environments/azure_aks/env.template を env にrenameして、 TENANT や DOMAIN 、 EMAIL や MQTT__iotagent を適切に書き換えてください。

環境構築

環境構築手順書は jupyter-notebook で書かれていますので、ブラウザから実行できます。以下の手順を順に実行してください。

Jupyter Notebookの起動コマンド

nmatsui@:core$ cd docs/en-jupyter_notebook/
nmatsui@:en-jupyter_notebook$ ./setup_jupyter_notebook.sh
nmatsui@:en-jupyter_notebook$ ./start_jupyter_notebook.sh

jupyter notebookから以下の手順を順に実行してください。

注意 core のインストールディレクトリが ${HOME}/core ではない場合、各手順の冒頭で実行している export CORE_ROOT="${HOME}/core"をインストールしたpathに書き換えてください。

azure_aks/01_prepare_aks.ipynb
azure_aks/02_start_base_pods.ipynb
azure_aks/03_start_auth_pods.ipynb
azure_aks/04_start_fiware_pods.ipynb
- 今回は iotagent-ul は使いませんので、 iotagent-json だけ起動すれば大丈夫です。

今回は検証用のため、リソース監視やログ監視を導入する手順 azure_aks/05_start_monitoring_and_logging.ipynb 、及びmongodbのバックアップジョブを準備する手順 azure_aks/06_backup_mongodb.ipynb は飛ばしています。

最後まで手順を実施すると、以下のようなpodが立ち上がっています。

RoboticBase/coreインストール後のPod状態

nmatsui@:~$ kubectl get pods
NAME                             READY   STATUS    RESTARTS   AGE
ambassador-5bd46978fc-h5lfm      1/1     Running   0          20h
ambassador-5bd46978fc-qbzcf      1/1     Running   0          20h
ambassador-5bd46978fc-zmdd4      1/1     Running   0          20h
auth-78d5889f49-kfhg9            1/1     Running   0          20h
auth-78d5889f49-m7zf2            1/1     Running   0          20h
auth-78d5889f49-wlmjs            1/1     Running   0          20h
comet-85df776594-bphnc           1/1     Running   0          20h
comet-85df776594-m4mrn           1/1     Running   0          20h
comet-85df776594-nw8ms           1/1     Running   0          20h
cygnus-mongo-66fb56b9f-gbnh5     1/1     Running   0          20h
cygnus-mongo-66fb56b9f-ssmrk     1/1     Running   0          20h
cygnus-mongo-66fb56b9f-tgmjb     1/1     Running   0          20h
iotagent-json-7b9c6985b5-j84n6   1/1     Running   0          20h
iotagent-json-7b9c6985b5-nn6dw   1/1     Running   0          20h
iotagent-json-7b9c6985b5-vd2x7   1/1     Running   0          20h
mongodb-0                        1/1     Running   0          20h
mongodb-1                        1/1     Running   0          20h
mongodb-2                        1/1     Running   0          20h
orion-7b5bdd6dbf-m9qkd           1/1     Running   0          20h
orion-7b5bdd6dbf-nvprh           1/1     Running   0          20h
orion-7b5bdd6dbf-xncp7           1/1     Running   0          20h
rabbitmq-0                       1/1     Running   0          21h
rabbitmq-1                       1/1     Running   0          21h
rabbitmq-2                       1/1     Running   0          21h

ルンバをIoTデバイスとして登録する

FIWARE環境が立ち上がったので、ルンバをIoTデバイスとしてFIWAREへ登録します。

環境変数の設定

以降の手順で必要となるため、まずは ${CORE_ROOT} 環境変数を設定します。

${CORE_ROOT} 環境変数を設定するコマンド

nmatsui@:~$ export CORE_ROOT="${HOME}/core"

注意 core のインストールディレクトリが ${HOME}/core ではない場合、インストールしたpathに置き換えてください。

RoboticBaseの環境構築時に設定した env を読み込んでおきます。

envを読み込みするコマンド

nmatsui@:~$ source ${CORE_ROOT}/docs/environments/azure_aks/env

RoboticBaseの環境構築手順に沿ってインストールした場合、FIWAREのREST APIにはBearer Tokenによる認証がかかっています。そのため、インストール時に生成した認証認可設定ファイルからBearer Tokenを取得し、環境変数にセットしておきます。

Bearer Token取得コマンド

nmatsui@:~$ export TOKEN=$(cat ${CORE_ROOT}/secrets/auth-tokens.json | jq '.[0].settings.bearer_tokens[0].token' -r)

またFIWAREは、マルチテナンシーと階層型のスコープ分割をサポートしています。今回は検証用のFIWAREなのでテナントやスコープを分割する必要は無いのですが、FIWAREのAPIが必要とするため定義しておきます。
さらに、FIWAREにルンバを登録する際に必要となるため、 ROBOT_TYPE と ROBOT_ID も定義しておきます。

nmatsui@:~$ export FIWARE_SERVICE="roomba"
nmatsui@:~$ export ROBOT_SERVICEPATH="/roomba"
nmatsui@:~$ export ROBOT_TYPE="roomba780"
nmatsui@:~$ export ROBOT_ID="my_roomba"

IoTサービスの登録

FIWAREの iotagent-json は、 service と device という概念でIoTデバイスを管理します。以下のPOSTリクエストを実行してください。

nmatsui@:~$ curl -i -H "Authorization: bearer ${TOKEN}" -H "Fiware-Service: ${FIWARE_SERVICE}" -H "Fiware-ServicePath: ${ROBOT_SERVICEPATH}" -H "Content-Type: application/json" https://api.${DOMAIN}/idas/json/manage/iot/services/ -X POST -d @- <<__EOS__
{
  "services": [
    {
      "apikey": "${ROBOT_TYPE}",
      "cbroker": "http://orion:1026",
      "resource": "/iot/json",
      "entity_type": "${ROBOT_TYPE}"
    }
  ]
}
__EOS__

201 Created が返ってくれば成功です。

iotagent-json service登録コマンドの実行結果

HTTP/1.1 201 Created
x-powered-by: Express
fiware-correlator: 37d85aff-99dd-477c-befc-004dd2a6616d
content-type: application/json; charset=utf-8
content-length: 2
etag: W/"2-vyGp6PvFo4RvsFtPoIWeCReyIC8"
date: Sun, 08 Dec 2019 02:49:51 GMT
x-envoy-upstream-service-time: 47
server: envoy

{}

IoTデバイスの登録

では、ルンバを device として登録します。以下はルンバの位置（x座標, y座標)と姿勢（z軸回りの回転角 θ）を収集する設定です。

今回は収集していませんが、Roomba Open Interfaceからはルンバのバッテリー電圧（ある一定以下まで電圧降下するとバッテリー切れ）等も取れます。収集してグラフ化すると面白いかもしれません。

nmatsui@:~$ curl -i -H "Authorization: bearer ${TOKEN}" -H "Fiware-Service: ${FIWARE_SERVICE}" -H "Fiware-ServicePath: ${ROBOT_SERVICEPATH}" -H "Content-Type: application/json" https://api.${DOMAIN}/idas/json/manage/iot/devices/ -X POST -d @- <<__EOS__
{
  "devices": [
    {
      "device_id": "${ROBOT_ID}",
      "entity_name": "${ROBOT_ID}",
      "entity_type": "${ROBOT_TYPE}",
      "timezone": "Asia/Tokyo",
      "protocol": "json",
      "attributes": [
        {
          "name": "time",
          "type": "ISO8601"
        },
        {
          "name": "x",
          "type": "number"
        },
        {
          "name": "y",
          "type": "number"
        },
        {
          "name": "theta",
          "type": "number"
        }
      ],
      "commands": [
      ],
      "transport": "AMQP"
    }
  ]
}
__EOS__

201 Created が返ってくることを確認してください。

iotagent-json device登録コマンドの実行結果

HTTP/1.1 201 Created
x-powered-by: Express
fiware-correlator: 67903552-5402-4128-9cc1-5f77bec49994
content-type: application/json; charset=utf-8
content-length: 2
etag: W/"2-vyGp6PvFo4RvsFtPoIWeCReyIC8"
date: Sun, 08 Dec 2019 03:03:50 GMT
x-envoy-upstream-service-time: 516
server: envoy

{}

履歴データ蓄積の設定

FIWAREはマイクロサービスアーキテクチャを採用しており、各コンポーネントは単一のシンプルな役割だけを果たします。上述の iotagent-json はIoTデバイスの管理が責務のため、収集したデータを履歴として永続化するためには別のコンポーネントが必要となります。FIWAREには FIWARE cygnus や FIWARE draco 、 FIWARE QuantumLeap と履歴データの永続化のために利用できるコンポーネントがいくつかありますが、今回は FIWARE cygnus を用います。

次のRESTリクエストを実行し、ルンバの状態が変わったら更新されたデータをcygnusに記録させるように設定します。

nmatsui@:~$ curl -i -H "Authorization: bearer ${TOKEN}" -H "Fiware-Service: ${FIWARE_SERVICE}" -H "Fiware-ServicePath: ${ROBOT_SERVICEPATH}" -H "Content-Type: application/json" https://api.${DOMAIN}/orion/v2/subscriptions/ -X POST -d @- <<__EOS__
{
  "subject": {
    "entities": [{
      "idPattern": "${ROBOT_ID}.*",
      "type": "${ROBOT_TYPE}"
    }],
    "condition": {
      "attrs": ["time", "x", "y", "theta"]
    }
  },
  "notification": {
    "http": {
      "url": "http://cygnus-mongo:5050/notify"
    },
    "attrs": ["time", "x", "y", "theta"],
    "attrsFormat": "legacy"
  }
}
__EOS__

同様に 201 Created が返ってくることを確認してください。

FIWARE orionへcygnusを購読者として登録するコマンドの実行結果

HTTP/1.1 201 Created
content-length: 0
location: /v2/subscriptions/5dec6b83ee5dac24959ec950
fiware-correlator: 66b0b09c-1969-11ea-b3cc-ae28e78999dd
date: Sun, 08 Dec 2019 03:18:27 GMT
x-envoy-upstream-service-time: 87
server: envoy

Roomba → Raspberry Pi → FIWARE へ位置と姿勢を送信

Raspberry PiとFIWAREの設定が完了したので、Roombaのodometryから位置（x, y)と角度（θ）を取り出してFIWAREへ転送してみましょう。

ROSとFIWAREを中継するROSパッケージをインストール

/odom ROS topicからRoombaのodometryを取り出し、MQTTSでFIWAREに転送するROS packageをインストールします。ロボットのアプリケーションは本来C++で書くことが多いのですが、MQTTSでの接続をC++で書くのは案外手間なので、今回はお手軽にpython2で書きました。詳細はgithubのリポジトリ nmatsui/roomba_ros_bridgeを参照してください。

nmatsui/roomba_ros_bridgeの取得

ROS Workspaceのsrc以下に、 nmatsui/roomba_ros_bridge をcloneします。

nmatsui/roomba_ros_bridgeのgit cloneコマンド

ros@ros-desktop:~/catkin_ws$ cd src
ros@ros-desktop:~/catkin_ws/src$ git clone https://github.com/nmatsui/roomba_ros_bridge.git
ros@ros-desktop:~/catkin_ws/src$ cd ..

処理の内容はシンプルで、/odom ROS topicにメッセージが到着すると、一定の時間間隔（今回は1000msとして設定）が経過していれば、ルンバのodometryのx座標、y座標、z軸方向の回転角を指定された名前のMQTTトピックにpublishする、という感じです。

src/roomba_ros_bridge/attrs_bridge.py(抜粋)

class AttrsBridge(object):
    def __init__(self):
        self.__mqtt_attrs_topic = '/{}/{}/attrs'.format(entity_type, entity_id)
        rospy.Subscriber('/odom', Odometry, self._on_odom_receive, queue_size=1)

    def _on_odom_receive(self, odom):
        now = datetime.datetime.now(self.__tz)
        if now >= self.__prev_ms + datetime.timedelta(milliseconds=self.__send_delta_ms) and self.__lock.acquire(False):
            self.__prev_ms = now

            pos = odom.pose.pose.position
            qt = odom.pose.pose.orientation
            theta = math.degrees(euler_from_quaternion([qt.x, qt.y, qt.z, qt.w])[2])

            message = {
                'time': now.isoformat(),
                'x': pos.x,
                'y': pos.y,
                'theta': theta,
            }

            if self.__client:
                self.__client.publish(self.__mqtt_attrs_topic, json.dumps(message))
            self.__lock.release()

nmatsui/roomba_ros_bridgeのビルド

AutonomyLab/create_autonomy と同様に、 nmatsui/roomba_ros_bridge も catkin tools を用いてをビルドします。 rosdep install しておくことで、 paho-mqtt 等の依存pythonライブラリも自動でインストールされます。

nmatsui/roomba_ros_bridgeのビルドコマンド

ros@ros-desktop:~/catkin_ws$ rosdep install --from-paths src -i
ros@ros-desktop:~/catkin_ws$ catkin build

roomba_ros_bridge もビルドしますので、一つ増えて全部で5つのパッケージのビルドが成功していることを確認してください。

nmatsui/roomba_ros_bridgeのビルドコマンドの実行結果

Finished  <<< roomba_ros_bridge                [ 11.2 seconds ]
[build] Summary: All 5 packages succeeded!

ルンバの位置と姿勢がFIWAREに送信されることを確認

それでは、ルンバの位置と姿勢がFIWAREに送信されることを確認しましょう。

roomba_ros_bridgeを起動

roomba_ros_bridge は、接続すべきMQTT Brokerのホスト名やユーザー名/パスワード等を環境変数から読みます。環境変数を設定してから、 roomba_ros_bridge を起動してください。

注意自分の設定に従って、 MQTT_HOST や MQTT_PASSWORD を変更してください。

roomba_ros_bridgeの起動コマンド

ros@ros-desktop:~/catkin_ws$ source devel/setup.bash
ros@ros-desktop:~/catkin_ws$ export MQTT_HOST="mqtt.example.com"
ros@ros-desktop:~/catkin_ws$ export MQTT_PORT="8883"
ros@ros-desktop:~/catkin_ws$ export MQTT_USERNAME="iotagent"
ros@ros-desktop:~/catkin_ws$ export MQTT_PASSWORD="password_of_iotagent"
ros@ros-desktop:~/catkin_ws$ export MQTT_USE_CA="true"
ros@ros-desktop:~/catkin_ws$ export ENTITY_TYPE="roomba780"
ros@ros-desktop:~/catkin_ws$ export ENTITY_ID="my_roomba"
ros@ros-desktop:~/catkin_ws$ roslaunch roomba_ros_bridge roomba_ros_bridge.launch

パラメータが正しく設定されていれば、以下のようなログが出力されます。 status=0 であることを確認してください。

roomba_ros_bridgeの起動ログ

[INFO] [1575780791.545769]: [roomba_ros_bridge.attrs_bridge:AttrsBridge.connect] try to Connect mqtt broker, host=mqtt.example.com
[INFO] [1575780791.760914]: [roomba_ros_bridge.attrs_bridge:AttrsBridge.start] StateBridge start
[INFO] [1575780791.796936]: [roomba_ros_bridge.attrs_bridge:AttrsBridge._on_connect] connected to mqtt broker, status=0

AutonomyLab/create_autonomyを起動

別のコンソールから、先程と同様に AutonomyLab/create_autonomy の ca_driver を起動します。

ca_driverの起動コマンド

ros@ros-desktop:~/catkin_ws$ source devel/setup.bash
ros@ros-desktop:~/catkin_ws$ roslaunch ca_driver create_2.launch

MQTTメッセージの確認

mosquitto_sub 等のMQTT Clientを用いて、送信されているMQTT メッセージを確認してみましょう。

mosquitto_subで全topicをsubscribeするコマンド

nmatsui@:~$ mosquitto_sub -h mqtt.${DOMAIN} -p 8883 --cafile ${CORE_ROOT}/secrets/DST_Root_CA_X3.pem -d -u iotagent -P ${MQTT__iotagent} -t /#

1秒に1回程度の頻度で、ルンバの位置と姿勢の情報がMQTTSで送信されていることが確認できます。

Client mosq-klK0QVqajbc8jFD5nz received PUBLISH (d0, q0, r0, m0, '/roomba780/my_roomba/attrs', ... (123 bytes))
{"y": 0.7616984844207764, "x": -1.2141335010528564, "theta": 83.25262254796264, "time": "2019-12-08T14:48:58.916712+09:00"}
Client mosq-klK0QVqajbc8jFD5nz received PUBLISH (d0, q0, r0, m0, '/roomba780/my_roomba/attrs', ... (123 bytes))
{"y": 0.9965053796768188, "x": -1.1818443536758423, "theta": 45.91221399702931, "time": "2019-12-08T14:49:00.016734+09:00"}
Client mosq-klK0QVqajbc8jFD5nz received PUBLISH (d0, q0, r0, m0, '/roomba780/my_roomba/attrs', ... (123 bytes))
{"y": 0.9886845350265503, "x": -1.190068006515503, "theta": 108.02877176215789, "time": "2019-12-08T14:49:01.016932+09:00"}

FIWARE STH Cometから履歴データを取得

FIWARE STH Comet のREST APIを用いると、時間を指定してFIWAREに蓄積された履歴データを参照することができます。

FIWARE STH Cometから履歴データを取得するコマンド

nmatsui@:~$ for a in "time" "x" "y" "theta"; do
  curl -sS -H "Authorization: bearer ${TOKEN}" -H "Fiware-Service: ${FIWARE_SERVICE}" -H "Fiware-ServicePath: ${ROBOT_SERVICEPATH}" "https://api.${DOMAIN}/comet/STH/v1/contextEntities/type/${ROBOT_TYPE}/id/${ROBOT_ID}/attributes/${a}?lastN=10&dateFrom=2019-12-08T05:48:58.000Z&dateTo=2019-12-08T05:49:01.999Z" | jq .
done

上記のMQTTメッセージとして送られたルンバの位置と姿勢の情報が、履歴として蓄積されていることが確認できました。

FIWARE STH Cometから履歴データを取得するコマンドの実行結果

{
  "contextResponses": [
    {
      "contextElement": {
        "attributes": [
          {
            "name": "time",
            "values": [
              {
                "recvTime": "2019-12-08T05:48:58.940Z",
                "attrType": "ISO8601",
                "attrValue": "2019-12-08T14:48:58.916712+09:00"
              },
              {
                "recvTime": "2019-12-08T05:49:00.034Z",
                "attrType": "ISO8601",
                "attrValue": "2019-12-08T14:49:00.016734+09:00"
              },
              {
                "recvTime": "2019-12-08T05:49:01.037Z",
                "attrType": "ISO8601",
                "attrValue": "2019-12-08T14:49:01.016932+09:00"
              }
            ]
          }
        ],
        "id": "my_roomba",
        "isPattern": false,
        "type": "roomba780"
      },
      "statusCode": {
        "code": "200",
        "reasonPhrase": "OK"
      }
    }
  ]
}

{
  "contextResponses": [
    {
      "contextElement": {
        "attributes": [
          {
            "name": "x",
            "values": [
              {
                "recvTime": "2019-12-08T05:48:58.940Z",
                "attrType": "number",
                "attrValue": "-1.2141335010528564"
              },
              {
                "recvTime": "2019-12-08T05:49:00.034Z",
                "attrType": "number",
                "attrValue": "-1.1818443536758423"
              },
              {
                "recvTime": "2019-12-08T05:49:01.037Z",
                "attrType": "number",
                "attrValue": "-1.190068006515503"
              }
            ]
          }
        ],
        "id": "my_roomba",
        "isPattern": false,
        "type": "roomba780"
      },
      "statusCode": {
        "code": "200",
        "reasonPhrase": "OK"
      }
    }
  ]
}

{
  "contextResponses": [
    {
      "contextElement": {
        "attributes": [
          {
            "name": "y",
            "values": [
              {
                "recvTime": "2019-12-08T05:48:58.940Z",
                "attrType": "number",
                "attrValue": "0.7616984844207764"
              },
              {
                "recvTime": "2019-12-08T05:49:00.034Z",
                "attrType": "number",
                "attrValue": "0.9965053796768188"
              },
              {
                "recvTime": "2019-12-08T05:49:01.037Z",
                "attrType": "number",
                "attrValue": "0.9886845350265503"
              }
            ]
          }
        ],
        "id": "my_roomba",
        "isPattern": false,
        "type": "roomba780"
      },
      "statusCode": {
        "code": "200",
        "reasonPhrase": "OK"
      }
    }
  ]
}

{
  "contextResponses": [
    {
      "contextElement": {
        "attributes": [
          {
            "name": "theta",
            "values": [
              {
                "recvTime": "2019-12-08T05:48:58.940Z",
                "attrType": "number",
                "attrValue": "83.25262254796264"
              },
              {
                "recvTime": "2019-12-08T05:49:00.034Z",
                "attrType": "number",
                "attrValue": "45.91221399702931"
              },
              {
                "recvTime": "2019-12-08T05:49:01.037Z",
                "attrType": "number",
                "attrValue": "108.02877176215789"
              }
            ]
          }
        ],
        "id": "my_roomba",
        "isPattern": false,
        "type": "roomba780"
      },
      "statusCode": {
        "code": "200",
        "reasonPhrase": "OK"
      }
    }
  ]
}

ルンバの位置を可視化する

では最後に、FIWAREに収集されたルンバの位置情報を可視化してみましょう。

可視化用アプリをデプロイ

FIWAREにもwirecloudというエンドユーザーコンピューティング的な可視化ツールがあるのですが、インストールが面倒なわりには融通がきかず結局JSでコードを書く必要があり微妙に使いづらいので、 D3.jsで可視化アプリをゴリゴリかいてしまいます。
今回使ったアプリnmatsui/fiware-robot-visualizationはgithubに公開していますので、興味がある方は確認してみてください。

可視化アプリ用の認証認可情報の追加

まずはRoboticBaseの認証認可コンポーネントに、可視化アプリ用のBasic認証設定を追加します。ユーザー名は odom-viewer 、パスワードはランダム生成文字列とします。

ランダム文字列を生成するエイリアスの設定

nmatsui@:~$  if [ "$(uname)" == 'Darwin' ]; then
  alias randomstr8='cat /dev/urandom | LC_CTYPE=C tr -dc 'a-zA-Z0-9' | head -c 8'
elif [ "$(expr substr $(uname -s) 1 5)" == 'Linux' ]; then
  alias randomstr8='cat /dev/urandom 2>/dev/null | head -n 40 | tr -cd 'a-zA-Z0-9' | head -c 8'
else
  echo "Your platform ($(uname -a)) is not supported."
  exit 1
fi

RoboticBaseの認証認可コンポーネントにサブドメイン指定でBasic認証を追加するコマンド

nmatsui@:~$  cat ${CORE_ROOT}/secrets/auth-tokens.json | jq '.|=.+[{
  "host": "odom-viewer\\..+$",
  "settings": {
    "bearer_tokens": [
    ],
    "basic_auths": [
      {
        "username": "odom-viewer",
        "password": "'$(randomstr8)'",
        "allowed_paths": ["/locus/", "/positions/"]
      }
    ],
    "no_auths": {
      "allowed_paths": ["^.*/static/.*$"]
    }
  }
}]' | tee /tmp/auth-tokens.json
mv ${CORE_ROOT}/secrets/auth-tokens.json ${CORE_ROOT}/secrets/auth-tokens.json.back
mv /tmp/auth-tokens.json ${CORE_ROOT}/secrets/auth-tokens.json

nmatsui@:~$ kubectl delete secret auth-tokens
nmatsui@:~$ kubectl create secret generic auth-tokens --from-file=${CORE_ROOT}/secrets/auth-tokens.json

以下のコマンドで自動生成されたパスワードを環境変数に設定し、その値を表示することができます。

自動生成されたパスワードを表示するコマンド

nmatsui@:~$ PASSWORD=$(cat ${CORE_ROOT}/secrets/auth-tokens.json | jq '.[]|select(.host == "odom-viewer\\..+$")|.settings.basic_auths|map(select(.allowed_paths[]|contains("/locus")))|.[0].password' -r);echo $PASSWORD

nmatsui/fiware-robot-visualization をデプロイ

下記のyamlを用いて、nmatsui/fiware-robot-visualization をAzure AKSにデプロイしてください。（コンテナイメージはnmatsui/fiware-robot-visualization:0.3.1.roombaとしてDockerHubに登録済みですので、自力ビルドする必要はありません。）

可視化アプリをK8Sにデプロイするyamlファイル

odom-viewer-service.yaml

apiVersion: v1
kind: Service
metadata:
  name: odom-viewer
  labels:
    app: odom-viewer
  annotations:
    getambassador.io/config: |
      ---
      apiVersion: ambassador/v0
      kind:  Mapping
      name:  odom-viewer
      prefix: /
      host: "^odom-viewer\\..+$"
      host_regex: true
      service: http://odom-viewer:8888
      timeout_ms: 60000
spec:
  type: ClusterIP
  selector:
    app: odom-viewer
  ports:
  - name: odom-viewer
    port: 8888
    targetPort: 8888

odom-viewer-deployment.yaml

apiVersion: apps/v1
kind: Deployment
metadata:
  name: odom-viewer
spec:
  replicas: 3
  selector:
    matchLabels:
      app: odom-viewer
  template:
    metadata:
      labels:
        app: odom-viewer
    spec:
      containers:
      - name: odom-viewer
        image: nmatsui/fiware-robot-visualization:0.3.1.roomba
        imagePullPolicy: Always
        env:
        - name: LISTEN_PORT
          value: "8888"
        - name: LOG_LEVEL
          value: "DEBUG"
        - name: FETCH_LIMIT
          value: "100"
        - name: COMET_ENDPOINT
          value: "http://comet:8666"
        - name: FIWARE_SERVICE
          value: "${FIWARE_SERVICE}"
        - name: FIWARE_SERVICEPATH
          value: "${ROBOT_SERVICEPATH}"
        - name: ENTITY_TYPE
          value: "${ROBOT_TYPE}"
        - name: ENTITY_ID
          value: "${ROBOT_ID}"
        ports:
          - name: odom-viewer
            containerPort: 8888

またインターネットから可視化アプリが使えるように、デプロイ後に可視化アプリ用のAレコードをAzure DNSに登録してください。

可視化アプリのAレコードを登録するコマンド

nmatsui@:~$ HTTPS_IPADDR=$(kubectl get services -l app=ambassador -o json | jq '.items[0].status.loadBalancer.ingress[0].ip' -r)
nmatsui@:~$ az network dns record-set a add-record --resource-group ${DNS_ZONE_RG} --zone-name "${DOMAIN}" --record-set-name "odom-viewer" --ipv4-address "${HTTPS_IPADDR}"

Aレコード登録後、curlで接続確認をして 200 OK が返ってくればOKです。

接続確認するコマンドとその結果

nmatsui@:~$ curl -i https://odom-viewer:${PASSWORD}@odom-viewer.${DOMAIN}/locus/

HTTP/1.1 200 OK
server: envoy
date: Sun, 08 Dec 2019 08:21:14 GMT
content-type: text/html; charset=utf-8
content-length: 5101
x-envoy-upstream-service-time: 14

<!Doctype html>
<html lang="ja">
...

ルンバの掃除軌跡の可視化と考察

無事に可視化アプリがインターネットから使えるようになりましたので、早速ルンバの掃除軌跡を表示してみましょう。2回分のデータを可視化してみました。

12/08 15:15 - 16:15
12/09 10:00 - 11:30

・・・あれれ〜？おかしいぞ〜？掃除した部屋は15mや20mも広くないし、掃除完了後にホームベースで停止しているハズのルンバの座標と姿勢が原点近くになってない！

12/08 16:15:00.867 は (x, y, θ) = (-10.25, 20.33, -22.35)
12/09 11:30:00.752 は (x, y, θ) = (-4.66, 7.37, 147.86)

これはルンバ自身が認識している位置と姿勢が、現実とはズレてしまっていることを意味します。今回はルンバのodometry（タイヤの回転角の累積から現在位置と姿勢を推定した情報）を可視化しましたが、現実のタイヤはスリップします。特にルンバは頻繁に回転しますが、回転するたびに実際の回転角とodometryとは顕著にズレが発生し、その誤差はどんどん蓄積されていきます。そのため、時間が経つにつれ彼はありもしない場所を走行している気分になっていくのです。

この問題を解決し、部屋内での正しい位置と姿勢を認識するためには、何らかのセンサーを用いて外部の環境を観測し、自身の位置と姿勢を常に補正し続ける必要があります。そのためによく用いられるのがSLAMと呼ばれる技術であり、まさに「ルンバ900シリーズ」以降で実装されたものです。SLAMが実装されたからこそ、最新のルンバは冒頭で示した「地図」が表示できるようになったのですね。

おわりに

ということで、ROSとFIWAREを用いてルンバの掃除軌跡を可視化してみました。しかし正しい座標と姿勢を推定し、掃除した部屋の「地図」として可視化するためには、ルンバ780にも外部環境を認識するセンサーを備え付け、odometryだけでなくその値も用いて自己位置推定する必要がありそうです。次はRGBDカメラ等を付けてでVSLAMしてみようかしら？

参考サイト

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up

ルンバの掃除状況を可視化しよう 〜 ROSとFIWAREを添えて 〜

検証環境

ルンバのROS化

Roomba Open Interfaceにシリアル-USB変換アダプタを接続

ルンバの状態を取得するROSパッケージをインストール

ROS Workspaceの構築

AutonomyLab/create_autonomyの修正

AutonomyLab/create_autonomyのビルド

ルンバの位置と姿勢が取得できることを確認

AutonomyLab/create_autonomyを起動

ルンバの位置と姿勢の取得

クラウド上にIoTプラットフォーム（FIWARE）を構築

FIWARE環境をAzure AKS上へ構築

リポジトリの取得と環境固有のパラメータを設定

環境構築

ルンバをIoTデバイスとして登録する

環境変数の設定

IoTサービスの登録

IoTデバイスの登録

履歴データ蓄積の設定

Roomba → Raspberry Pi → FIWARE へ位置と姿勢を送信

ROSとFIWAREを中継するROSパッケージをインストール

nmatsui/roomba_ros_bridgeの取得

nmatsui/roomba_ros_bridgeのビルド

ルンバの位置と姿勢がFIWAREに送信されることを確認

roomba_ros_bridgeを起動

AutonomyLab/create_autonomyを起動

MQTTメッセージの確認

FIWARE STH Cometから履歴データを取得

ルンバの位置を可視化する

可視化用アプリをデプロイ

可視化アプリ用の認証認可情報の追加

nmatsui/fiware-robot-visualization をデプロイ

ルンバの掃除軌跡の可視化と考察

おわりに

参考サイト

ルンバの掃除状況を可視化しよう〜 ROSとFIWAREを添えて〜