Go to Qiita Advent Calendar 2024 Top
LoginSignup
13
7

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@srs(project_srs)

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

Last updated at Posted at 2019-11-19

環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

SIEからtoioというおもちゃが発売されました。これの最大の特徴はプレイマットの上ならtoioのコアキューブの位置と角度が取得できることです。
タイヤがついていて自走するおもちゃは最近多いですが、絶対自己位置が取れるものって珍しいですね。さらに仕様が公開されて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のコアキューブには「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レシーバーだけです。

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で止められます。

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アドレスを入れます。

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

connect

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

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

サービス・チャラクタリスティックの取得

toioの持っているキャラクタリスティックの一覧を取得します。どれが何に対応するかはtoioの仕様を見てください。

キャラクタリスティックの取得
[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を見てみます。ランプのところのボタンを押したりしながら実行してみてください。

キャラクタリスティックの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であるので、これを使用します。
以下のコマンドでランプが白く光ります。

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

toioとの切断

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

bluepyの使い方

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

インストール

sudo pip install bluepy

接続と読み書き

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

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

notifyの使い方

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

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のように実行します。
結果
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

  • 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

複数台接続のために

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

参考

BLE入門
ble関連のコマンドの使い方
bluepyの使いかた
toioコアキューブ仕様書

目次ページへのリンク

ROS講座の目次へのリンク

13
7
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
13
7

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?