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

皆様[ルンバ](https://www.irobot-jp.com/)を使っていますか。自動的に掃除をしてくれる便利なヤツで、一度使うと手放せなくなりますよね。「ルンバ900シリーズ」以降はカメラを用いた[SLAM](https://ja.wikipedia.org/wiki/SLAM)（visual slam）を実装して[効率的に掃除する](https://www.irobot-jp.com/product/900series/smart.html#iadapt)ようになっており、最新のルンバアプリではSLAMでマッピングした地図と掃除履歴等が確認できます。

![Screenshot_20191208-153722.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37648/dccf303e-9e6a-9e2b-c1f6-03d935d4bd74.png)　![Screenshot_20191208-153705.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37648/472c0f93-bc71-d1fa-db57-670c15fa5dca.png)

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

ということで、ルンバの低レベルな操作が可能なオープンインタフェース [Roomba Open Interface](https://www.irobot.lv/uploaded_files/File/iRobot_Roomba_500_Open_Interface_Spec.pdf) を[ROS](http://wiki.ros.org/)から利用し、ルンバの移動軌跡をOSSのIoTプラットフォームである[FIWARE](https://www.fiware.org/)に蓄積して可視化してみましょう。

# 検証環境
* Roomba Open Interfaceにシリアル-USB変換アダプタをつけたルンバ
    * [iRobot Roomba 780](https://www.irobot-jp.com/storeproduct/700series/)
    * [FTDI USBシリアル変換アダプター](https://www.amazon.co.jp/exec/obidos/ASIN/B009AME9R4/karaage0703-22/)
* ROSが動作するRaspberryPi
    * [Raspberry Pi 3 model B+](https://www.raspberrypi.org/products/raspberry-pi-3-model-b-plus/) (Revision: a020d3)
    * [Ubuntu Mate](https://ubuntu-mate.org/) 16.04.6 LTS (4.14.52-v7+ #1123 SMP Wed Jun 27 17:35:49 BST 2018 armv7l)
    * ROS [Kinetic](http://wiki.ros.org/kinetic)
* クラウド上で動作するFIWARE
    * [Microsoft Azure AKS](https://azure.microsoft.com/ja-jp/services/kubernetes-service/) ([kubernetes 1.15.5](https://github.com/kubernetes/kubernetes/releases/tag/v1.15.5))
    * FIWARE ([Release 7.7.1](https://github.com/FIWARE/catalogue/releases/tag/FIWARE_7.7.1))
        * [FIWARE orion](https://fiware-orion.readthedocs.io/en/master/) ([2.2.0](https://github.com/telefonicaid/fiware-orion/releases/tag/2.2.0))
        * [FIWARE iotagent-json](https://fiware-iotagent-json.readthedocs.io/en/latest/) ([1.10.1](https://github.com/telefonicaid/iotagent-json/releases/tag/1.10.0))
        * [FIWARE cygnus](https://fiware-cygnus.readthedocs.io/en/latest/) ([1.15.0](https://github.com/telefonicaid/fiware-cygnus/releases/tag/1.15.0))
        * [FIWARE STH Comet](https://fiware-sth-comet.readthedocs.io/en/latest/) ([2.5.0](https://github.com/telefonicaid/fiware-sth-comet/releases/tag/2.5.0))
    * [Rabbit MQ](https://www.rabbitmq.com/) ([3.7.16](https://github.com/rabbitmq/rabbitmq-server/releases/tag/v3.7.16))
    * [Mongo DB](https://www.mongodb.com/) ([4.1.13](https://github.com/mongodb/mongo/releases/tag/r4.1.13))
* 作業用PC
    * iMac (Retina 5K, 27-inch, 2017) macOS Mojave 10.14.6
    * [kubectl](https://kubernetes.io/docs/reference/kubectl/overview/) v1.16.3
    * [curl](https://curl.haxx.se/) 7.54.0
    * [jq](https://stedolan.github.io/jq/) jq-1.6
    * [mosquitto_sub](https://mosquitto.org/) 1.6.7

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

## Roomba Open Interfaceにシリアル-USB変換アダプタを接続
ルンバ780のRoomba Open Interfaceにシリアル-USB変換アダプタを接続するためのハードウェア的な手順は、[ルンバをRaspberry Piを使ってROSでコントロールできるように改造してみた（半田付け無しでOK）](https://karaage.hatenadiary.jp/entry/2017/05/12/073000)を参考に実施しました。
> ありがとうございます、 [からあげ (id:karaage)](https://karaage.hatenadiary.jp/about)さん！

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

![DSC_1850.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37648/da494a78-1595-b946-59d8-bd57bef49448.jpeg)
![DSC_1851.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37648/87f3a828-4d6b-4205-afbd-8aa5f7ee7261.jpeg)


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

### ROS Workspaceの構築
`AutonomyLab/create_autonomy` が[catkin tools](https://catkin-tools.readthedocs.io/en/latest/)でのビルドを要求しているため、まずはcatkin toolsをインストールします。

<details><summary>catkin toolsのインストールコマンド</summary><div>

```bash
ros@ros-desktop:~$ sudo apt-get install python-rosdep python-catkin-tools
```
</div></details>

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

<details><summary>ROS Workspace作成＆初期化コマンド</summary><div>

```bash
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
```
</div></details>

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

<details><summary>AutonomyLab/create_autonomyのgit cloneコマンド</summary><div>

```bash
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/
```
</div></details>

残念ながら[AutonomyLab/create_autonomy](https://github.com/AutonomyLab/create_autonomy)にはRoomba Open Interfaceが規定する4つのモード（off, passive, safe, full）を動的に変更する機能が実装されておらず、ROS Nodeとして起動すると自動的にfull mode（ルンバを完全遠隔制御するモードで、CLEANボタンやスケジューラが機能しなくなる）に変更してしまいます。
> 起動後にモード変更を可能にする[Issue](https://github.com/AutonomyLab/create_autonomy/issues/13)は上がっており[実装](https://github.com/AutonomyLab/create_autonomy/tree/set-modes)も存在しますが、mergeされず3年以上放置されているので期待薄かと。。。

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

```diff
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` のままで大丈夫です。

<details><summary>AutonomyLab/create_autonomyの設定</summary><div>

```bash: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"
```
</div></details>

### AutonomyLab/create_autonomyのビルド
では、 `catkin tools` を用いてをビルドしましょう。

<details><summary>AutonomyLab/create_autonomyのビルドコマンド</summary><div>

```bash
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
```
</div></details>

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

## ルンバの位置と姿勢が取得できることを確認
それでは、ルンバの情報を取得してみましょう。

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

<details><summary>ca_driverの起動コマンド</summary><div>

```bash
ros@ros-desktop:~/catkin_ws$ source devel/setup.bash
ros@ros-desktop:~/catkin_ws$ roslaunch ca_driver create_2.launch
```
</div></details>

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

<details><summary>ca_driverの起動ログ</summary><div>

```text
[ 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.
```
</div></details>

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

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

<details><summary>ルンバのodometryの取得コマンド</summary><div>

```bash
ros@ros-desktop:~$ cd catkin_ws/
ros@ros-desktop:~/catkin_ws$ source devel/setup.bash
ros@ros-desktop:~/catkin_ws$ rostopic echo /odom
```
</div></details>

以下のようなメッセージがひたすら表示され続けると思います。これはルンバの現在のodometry（タイヤの回転角の累積から現在位置と姿勢を推定した情報）です。 `pose` の **`position`** がルンバの現在の位置（x座標, y座標, z座標） **`orientation`** が[quaternion](https://ja.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B0)で表したルンバの現在の姿勢（x軸、y軸、z軸周りの回転角度）です。`ca_driver` を起動した位置と姿勢が原点となり座標軸が設定されるので、最初は座標も回転角度も全部0です。

<details><summary>ca_driver起動直後のルンバのodometry</summary><div>

```yaml
---
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]
```
</div></details>

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

<details><summary>移動中のルンバのodometry</summary><div>

```yaml
---
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]
---
```
</div></details>

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

<details><summary>`/base_footprint`は？</summary><div>

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

# クラウド上にIoTプラットフォーム（FIWARE）を構築
ではルンバの位置や姿勢の情報を収集するプラットフォームとして、弊社がOSSとして公開しているロボットプラットフォームの[RoboticBase](https://github.com/RoboticBase/core)を用いてFIWARE環境をAzure AKS上に起動しましょう。

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

|<a href="https://www.slideshare.net/nbykmatsui/fiware-20190913" alt="fiware study"><img src="https://image.slidesharecdn.com/20190913fiwarestudy-191210025050/95/slide-1-1024.jpg" width="480px" alt="fiware study" style="border: solid #808080"></a>|
|:--:|

[FIWARE勉強会 20190913 @ slideshare](https://www.slideshare.net/nbykmatsui/fiware-20190913)

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

## FIWARE環境をAzure AKS上へ構築
### リポジトリの取得と環境固有のパラメータを設定
[RoboticBase/core](https://github.com/RoboticBase/core)をcloneしてください。同梱されている環境構築手順書は、`${HOME}`直下にcloneした前提で書かれています。

cloneしたcoreディレクトリに移動し、[docs/environments/azure_aks/env.template](https://github.com/RoboticBase/core/tree/master/docs/environments/azure_aks/env.template) を `env` にrenameして、 `TENANT` や `DOMAIN` 、 `EMAIL` や `MQTT__iotagent` を適切に書き換えてください。

### 環境構築
環境構築手順書は [jupyter-notebook](https://jupyter.org/) で書かれていますので、 ブラウザから実行できます。以下の手順を順に実行してください。

<details><summary>Jupyter Notebookの起動コマンド</summary><div>

```bash
nmatsui@:core$ cd docs/en-jupyter_notebook/
nmatsui@:en-jupyter_notebook$ ./setup_jupyter_notebook.sh
nmatsui@:en-jupyter_notebook$ ./start_jupyter_notebook.sh
```
</div></details>

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

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

* [azure_aks/01_prepare_aks.ipynb](https://github.com/RoboticBase/core/blob/master/docs/en-jupyter_notebook/azure_aks/01_prepare_aks.ipynb)
* [azure_aks/02_start_base_pods.ipynb](https://github.com/RoboticBase/core/blob/master/docs/en-jupyter_notebook/azure_aks/02_start_base_pods.ipynb)
* [azure_aks/03_start_auth_pods.ipynb](https://github.com/RoboticBase/core/blob/master/docs/en-jupyter_notebook/azure_aks/03_start_auth_pods.ipynb)
* [azure_aks/04_start_fiware_pods.ipynb](https://github.com/RoboticBase/core/blob/master/docs/en-jupyter_notebook/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が立ち上がっています。

<details><summary>RoboticBase/coreインストール後のPod状態</summary><div>

```bash
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
```
</div></details>

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

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

<details><summary>${CORE_ROOT} 環境変数を設定するコマンド</summary><div>

```bash
nmatsui@:~$ export CORE_ROOT="${HOME}/core"
```
</div></details>

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

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

<details><summary>envを読み込みするコマンド</summary><div>

```bash
nmatsui@:~$ source ${CORE_ROOT}/docs/environments/azure_aks/env
```
</div></details>

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

<details><summary>Bearer Token取得コマンド</summary><div>

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

またFIWAREは、[マルチテナンシー](https://fiware-orion.readthedocs.io/en/master/user/multitenancy/index.html)と[階層型のスコープ分割](https://fiware-orion.readthedocs.io/en/master/user/service_path/index.html)をサポートしています。今回は検証用のFIWAREなのでテナントやスコープを分割する必要は無いのですが、FIWAREのAPIが必要とするため定義しておきます。
さらに、FIWAREにルンバを登録する際に必要となるため、 `ROBOT_TYPE` と `ROBOT_ID` も定義しておきます。

```bash
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リクエストを実行してください。

```bash
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` が返ってくれば成功です。

<details><summary>iotagent-json service登録コマンドの実行結果</summary><div>

```bash
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

{}
```
</div></details>

### IoTデバイスの登録
では、ルンバを `device` として登録します。以下はルンバの位置（x座標, y座標)と姿勢（z軸回りの回転角 θ）を収集する設定です。
> 今回は収集していませんが、Roomba Open Interfaceからはルンバのバッテリー電圧（ある一定以下まで電圧降下するとバッテリー切れ）等も取れます。収集してグラフ化すると面白いかもしれません。

```bash
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` が返ってくることを確認してください。

<details><summary>iotagent-json device登録コマンドの実行結果</summary><div>

```bash
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

{}
```
</div></details>

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

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

```bash
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` が返ってくることを確認してください。

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

```bash
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
```
</div></details>

# 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](https://github.com/nmatsui/roomba_ros_bridge)を参照してください。

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

<details><summary>nmatsui/roomba_ros_bridgeのgit cloneコマンド</summary><div>

```bash
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 ..
```
</div></details>

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

```python: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ライブラリも自動でインストールされます。

<details><summary>nmatsui/roomba_ros_bridgeのビルドコマンド</summary><div>

```bash
ros@ros-desktop:~/catkin_ws$ rosdep install --from-paths src -i
ros@ros-desktop:~/catkin_ws$ catkin build
```
</div></details>

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

<details><summary>nmatsui/roomba_ros_bridgeのビルドコマンドの実行結果</summary><div>

```text
Finished  <<< roomba_ros_bridge                [ 11.2 seconds ]
[build] Summary: All 5 packages succeeded!
```
</div></details>

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

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

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

<details><summary>roomba_ros_bridgeの起動コマンド</summary><div>

```bash
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
```
</div></details>

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

<details><summary>roomba_ros_bridgeの起動ログ</summary><div>

```
[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
```
</div></details>

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

<details><summary>ca_driverの起動コマンド</summary><div>

```bash
ros@ros-desktop:~/catkin_ws$ source devel/setup.bash
ros@ros-desktop:~/catkin_ws$ roslaunch ca_driver create_2.launch
```
</div></details>

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

<details><summary>mosquitto_subで全topicをsubscribeするコマンド</summary><div>

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

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

```text
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に蓄積された履歴データを参照することができます。

<details><summary>FIWARE STH Cometから履歴データを取得するコマンド</summary><div>

```bash
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
```
</div></details>

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

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

```json
{
  "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"
      }
    }
  ]
}
```
```json
{
  "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"
      }
    }
  ]
}
```
```json
{
  "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"
      }
    }
  ]
}
```
```json
{
  "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"
      }
    }
  ]
}
```
</div></details>

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

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

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

<details><summary>ランダム文字列を生成するエイリアスの設定</summary><div>

```bash
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
```
</div></details>

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

```bash
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
```
```bash
nmatsui@:~$ kubectl delete secret auth-tokens
nmatsui@:~$ kubectl create secret generic auth-tokens --from-file=${CORE_ROOT}/secrets/auth-tokens.json
```
</div></details>

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

<details><summary>自動生成されたパスワードを表示するコマンド</summary><div>

```bash
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
```
</div></details>

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

<details><summary>可視化アプリをK8Sにデプロイするyamlファイル</summary><div>

```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
```
```yaml: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
```
</div></details>

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

<details><summary>可視化アプリのAレコードを登録するコマンド</summary><div>

```bash
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}"
```
</div></details>

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

<details><summary>接続確認するコマンドとその結果</summary><div>

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

```text
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">
...
```
</div></details>

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

* 12/08 15:15 - 16:15
![スクリーンショット 2019-12-09 11.41.46.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37648/a6b444ec-8e14-db58-6215-5018f8c9b249.png)

* 12/09 10:00 - 11:30
![スクリーンショット 2019-12-09 11.39.59.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/37648/901f0036-3389-f938-8387-a6395a222dc9.png)

・・・あれれ〜？おかしいぞ〜？掃除した部屋は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してみようかしら？

# 参考サイト
* [ルンバをRaspberry Piを使ってROSでコントロールできるように改造してみた（半田付け無しでOK） - からあげ (id:karaage)](https://karaage.hatenadiary.jp/entry/2017/05/12/073000)
* [RaspberryPi 3B+にROSを導入する - Isao HARA](http://hara.jpn.com/_default/ja/Topics/RaspPi3BPlusRos.html)