ROS講座105 toioをROSで操作する

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
SIEから[toio](https://toio.io/)というおもちゃが発売されました。これの最大の特徴はプレイマットの上ならtoioのコアキューブの位置と角度が取得できることです。
タイヤがついていて自走するおもちゃは最近多いですが、絶対自己位置が取れるものって珍しいですね。さらに[仕様](https://toio.github.io/toio-spec/)が公開されてscratchで操作できるようになっています。私はROSしか使えないのでROSから操作できるようにしましょう。

# Bluetoothについて（基礎編）
toioはbluetooth low energy (BLE)で通信します。通信のための基礎知識を軽く書いておきます。

まずbluetoothにはBluetooth Basic Rate/Enhanced Data Rate (BR/EDR) とver4から追加された。 Bluetooth Low Energy (LE)の2種類があります。この2種類は無線HWが共通であることを除き別物です。無線イヤホンなどの従来からある機器はほとんどがER/EDRのプロトコルです。一方toioのように比較的小さなデータを断続的に送る用途ではLEが使われます。

BLE(Bluetooth Low Energy)ではブロードキャストとコネクション2つのネットワーク構成があります。ブロードキャストはbeaconのように不特定多数の送信者と受信者がいるような通信に使われます。コネクションは1つのセントラルに複数のペリフェラルが紐づけられて通信を行います。
すべてのbluetooth機器は固有のHWアドレスを持っています。などでbluetoothドングルにもtoioにもついていて、これで特定のbluetooth機器を指定するのに使います。これは6Byteで"11:22:33:44:55:66"のように表記します。

BLEのペリフェラルはいくつかのサービスを持ち、サービスの中にはいくつかのキャラクタリスティックがあります。
例えば[toioのコアキューブ](https://toio.github.io/toio-spec/docs/ble_communication_overview.html)には「10B20100-5B3B-4571-9508-CF3EFCD7BBAE」というサービスUUIDののサービスが1つあります。このサービスの中で読み取りセンサは「10B20101-5B3B-4571-9508-CF3EFCD7BBAE」というキャラクタリスティックUUIDのキャラクタリスティックがあります。

BLEのデータアクセスの種類は3つしかありません

* writeはセントラルがCharacteristic UUIDを指定してペリフェラルにデータを転送します。
* readではセントラルがCharacteristic UUIDを指定してデータを要求してペリフェラルはデータを返します。
* notifyではセントラルがCharacteristic UUIDを指定してペリフェラルにnotify開始を要求します。するとペリフェラルはそれ以降一定周期でセントラルにデータを送信し続けます。

ここでのデータとはuint8の配列になります。

まとめ
というわけでubuntuとtoioが通信するためには

* ubuntuにさしたBLEのドングルがセントラルとなり、HWアドレスを指定してtoioと接続する。
* サービスUUIDを指定してtoioのサービスを取得する。
* 読み取りセンサのキャラクタリスティックUUIDを指定してreadをしてデータを得る

という手順になります。

# Bluetoothについて（実践編）
上記のことを実際にやってみましょう。

## bluetoothドングルの確認
今回はbluetoothドングルはこれを使いました。PC内蔵のものではBR/EDRは使えてもBLEは使えないことがありました。別売りのドングルを買うのが吉です。
`hciconfig`でbluetoothのデバイスリストが表示されます。ここで表示されるのはPCに刺さっているドングルや内臓のbluetoothレシーバーだけです。

```shell:bluetoothドングルの確認
$ hciconfig
hci0:	Type: BR/EDR  Bus: USB
	BD Address: XX:XX:XX:XX:XX:XX  ACL MTU: 8192:128  SCO MTU: 64:128
	UP RUNNING 
	RX bytes:539 acl:0 sco:0 events:28 errors:0
	TX bytes:860 acl:0 sco:0 commands:28 errors:0
```

## BLEデバイスのスキャン
このように近くになるbluetoothデバイスの一覧が表示されます。
toioの電源を入れて以下のコマンドを実行します。toioのHWアドレスをメモっておきましょう。
Ctrl+Cで止められます。

```shell:BLEデバイスのスキャン
$ sudo hcitool -i hci0 lescan
LE Scan ...
XX:XX:XX:XX:XX:XX (unknown)
XX:XX:XX:XX:XX:XX toio Core Cube
...
(以下続く)
```

ここで`Set scan parameters failed: Input/output error`と出る場合はそのデバイスはBLEには対応していません。
このようにデバイスのスキャンをするときのみroot権限が必要になります。


## toioとの接続の開始
gatttoolというBLEとデータのやり取りをするツールを使います。まずは以下のコマンドで開始します。XX:XX:XX:XX:XX:XXは上記のlescanで取得したtoioのHWアドレスを入れます。

```shell:toioとの接続の開始
$ gatttool -b XX:XX:XX:XX:XX:XX -t random -I
[XX:XX:XX:XX:XX:XX][LE]> 
```

## connect
実際の接続をします。正常ならtoioから接続音がします。

```shell:toioとの接続の開始
[XX:XX:XX:XX:XX:XX][LE]> connect
[XX:XX:XX:XX:XX:XX][LE]>            # <- 色が青くなる
```

## サービス・チャラクタリスティックの取得
toioの持っているキャラクタリスティックの一覧を取得します。どれが何に対応するかは[toioの仕様](https://toio.github.io/toio-spec/docs/ble_communication_overview.html)を見てください。

```shell:キャラクタリスティックの取得
[XX:XX:XX:XX:XX:XX][LE]> char-desc 0x0b000b
handle: 0x0001, uuid: 00002800-0000-1000-8000-00805f9b34fb
handle: 0x0002, uuid: 00002803-0000-1000-8000-00805f9b34fb
handle: 0x0003, uuid: 00002a00-0000-1000-8000-00805f9b34fb
handle: 0x0004, uuid: 00002803-0000-1000-8000-00805f9b34fb
handle: 0x0005, uuid: 00002a01-0000-1000-8000-00805f9b34fb
...
(以下続く)
```

## チャラクタリスティックのread
試しにボタンのON/OFFを見てみます。ランプのところのボタンを押したりしながら実行してみてください。

```shell:キャラクタリスティックのread
[XX:XX:XX:XX:XX:XX][LE]> char-read-uuid 10B20107-5B3B-4571-9508-CF3EFCD7BBAE
handle: 0x001e 	 value: 01 80 
```

押しているとvalueの2つ目が`80`、離すと`00`になります。

## キャラクタリスティックのwrite
キャラクタリスティックのwriteは以下のようにハンドラで指定します。上記の`char-desc 0x0b000b`で出てくるリストではランプの色の制御をする`10B20103-5B3B-4571-9508-CF3EFCD7BBAE`のhandleは`0x0014`であるので、これを使用します。
以下のコマンドでランプが白く光ります。

```shell:キャラクタリスティックのread
[XX:XX:XX:XX:XX:XX][LE]> char-write-cmd 0x0014 03000101808080
```

## toioとの切断
```shell:toioとの切断
[XX:XX:XX:XX:XX:XX][LE]> disconnect
```

# bluepyの使い方
今回はbluepyというpythonライブラリを使っていきます。

## インストール

```
sudo pip install bluepy
```

## 接続と読み書き
基本的な接続とデータの読み書きの方法です。

```python:toio_lecture/script/toio_basic1.py
#!/usr/bin/python
# -*- coding: utf-8 -*- 

from bluepy import btle
import sys

LAMP_HANDLE = 20
BATTERY_HANDLE = 34

if len(sys.argv) != 2:
    print("1 argument (mac address of toio) is nessesary")
    sys.exit( )

# connect
toio_peripheral = btle.Peripheral(sys.argv[1], btle.ADDR_TYPE_RANDOM, 1)

# read battery status
print("battery", ord(toio_peripheral.readCharacteristic(BATTERY_HANDLE)))

# write lamp
data = [3, 0, 1, 1, 200, 50, 50]
toio_peripheral.writeCharacteristic(LAMP_HANDLE, bytearray(data), True)
```

* `rosrun toio_lecture toio_basic1.py F0:5D:68:8E:A8:7C`と実行します。
* `btle.Peripheral()`で接続をします。第1引数はBLEデバイスのmac addressで、第2引数はアドレスのタイプです。toioならbtle.ADDR_TYPE_RANDOMと指定しないとつながりません。第3引数は接続するbluetoothドングルで、1だと`hci1`につながります。Noneもしくは第2引数まででは自動でどれかにつながります。
* `toio_peripheral.readCharacteristic(BATTERY_HANDLE)`で読み込みをします。読み込みの結果はバイト列になります。これを`ord()`で整数の配列に直します。例えば`('battery', 100)`と表示されるはずです（後ろはバッテリーの残量（%）です）。
* `toio_peripheral.writeCharacteristic(LAMP_HANDLE, bytearray(data), True)`で書き込みます。整数の数列でデータをセットして`bytearray()`でバイト列にして送り込むのがおすすめです。この命令でLEDが淡い赤で光ります。

![toio1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/8677abc3-d105-a39d-d8af-f307662f1490.png)


## notifyの使い方
上の例の読み込みではプログラム側のタイミングでデータを読みますが、デバイス内で値が更新されるたびにデバイスから値が投げられるのがnotifyです。

```python:toio_lecture/script/toio_basic2.py
#!/usr/bin/python
# -*- coding: utf-8 -*- 

from bluepy import btle
import sys

BATTERY_HANDLE = 34

class MyDelegate(btle.DefaultDelegate):
    def __init__(self):
        btle.DefaultDelegate.__init__(self)
        print("set delegate")

    def handleNotification(self, cHandle, data):
        print("callback")
        if(cHandle == BATTERY_HANDLE):
            print("battery", ord(data))

if len(sys.argv) != 2:
    print("1 argument (mac address of toio) is nessesary")
    sys.exit( )

# connect
toio_peripheral = btle.Peripheral(sys.argv[1], btle.ADDR_TYPE_RANDOM, 1)

# set delegate
toio_peripheral.withDelegate(MyDelegate())

# set notify
toio_peripheral.writeCharacteristic(BATTERY_HANDLE+1, b'\x01', True)

try:
    while True:
        TIMEOUT = 0.1
        toio_peripheral.waitForNotifications(TIMEOUT)
except KeyboardInterrupt:
    None
```

* `rosrun toio_lecture toio_basic2.py F0:5D:68:8E:A8:7C`のように実行します。

```text:結果
set delegate
callback
('battery', 90)
callback
('battery', 90)
callback
('battery', 90)
```

* `toio_peripheral.writeCharacteristic(BATTERY_HANDLE+1, b'\x01', True)`のようにhandleの値+1したhandleに1を書き込むとnotifyがスタートします。
* `btle.DefaultDelegate`クラスを継承したdelegateクラスを作りそれをbleの関数でセットします。`toio_peripheral.waitForNotifications()`で待っている間に何らかのnotifyが来ると`handleNotification()`がcallbackされます。

# rosから使う

## ソースコード
ソースコードを以下に置いておきます。
[toio_bridge.py](https://github.com/project-srs/ros_lecture/blob/master/toio_lecture/script/toio_bridge.py)

* twistで速度の入力を取るので、それを車輪の速度に変更してtoioに指令します。
* toioが位置センサの値を返しますが、これはメートル単位でないので変換します。マット表面の中心が原点になるようにしました。またXY座標の軸の方向もROSとは流儀が違うので変換します。
* bluepyはスレットセーフではなく、rospyはroscppと違ってコールバックはメインルーチンとは別スレッドを立てて実行します。そのために普通に実装すると例えばmainルーチンの`waitForNotifications()`とsubscriber中で呼び出す`writeCharacteristic()`が同時に呼ばれることがあります。この場合はbluepyは例外を出してしまいます。これを防止するために上記のプログラムではwriteしたい場合はいったんキューに入れて、mainルーチンの中でキューを見て書き込みを行っています。

## 実行

以下のlaunchの中でmac addressが指定されています。書き換えが必要です。

```
roslaunch toio_lecture with_kicker.launch
```

![toio3.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/d5bac703-2e92-3b01-efcc-c8e0602e8e7a.gif)


# 複数台接続のために

上記のlaunchの中でtoio_bridgeを複数台立ち上げると複数台のtoioに接続することができます。しかし、後から接続したtoioでnotifyが1病弱遅れて届くという現象が必ず発生します。今のところこれの対応策はtoioの数だけbluetoothドングルを用意して、各々のtoio_bridgeに別のbluetoothドングルを割り当てるということです。[dual.launch](https://github.com/project-srs/ros_lecture/blob/master/toio_lecture/launch/dual.launch)にサンプルコードがあります。

# 参考
[BLE入門](http://jellyware.jp/kurage/bluejelly/ble_guide.html)
[ble関連のコマンドの使い方](https://qiita.com/Peeeeepei/items/58cd6eae81784f17205f)
[bluepyの使いかた](https://www.ipride.co.jp/blog/2510)
[toioコアキューブ仕様書](https://toio.github.io/toio-spec/)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)