environment

This article works in the following environment.

item	value
CPU	Core i5-8250U
Ubuntu	16.04
ROS	Kinetic

For installation, refer to ROS Course 02 Installation. Also, the program in this article has been uploaded to github. See ROS Course 11 git repository.

Overview

A toy called toio has been released by SIE. The biggest feature of this is that you can get the position and angle of toio's core cube on the playmat. There are many toys that have tires and run on their own these days, but it is rare that they can take their own position. In addition, Specifications has been published so that it can be operated with scratch. I can only use ROS, so let's make it possible to operate from ROS.

About Bluetooth (Basic)

toio communicates with bluetooth low energy (BLE). I will briefly write down the basic knowledge for communication.

First, bluetooth was added from Bluetooth Basic Rate / Enhanced Data Rate (BR / EDR) and ver4. There are two types: Bluetooth Low Energy (LE). These two types are different except that the wireless HW is common. Most traditional devices such as wireless earphones use the ER / EDR protocol. On the other hand, LE is used for applications such as toio that send relatively small data intermittently.

BLE (Bluetooth Low Energy) has two network configurations, broadcast and connection. Broadcasts are used for communications such as beacons that have an unspecified number of senders and receivers. A connection communicates with multiple peripherals linked to one central. All bluetooth devices have a unique HW address. The bluetooth dongle is also attached to toio, and it is used to specify a specific bluetooth device. This is 6 bytes and is written as "11:22:33:44:55:66".

BLE peripherals have several services, some of which are characteristic. For example, toio's core cube has one service UUID called "10B20100-5B3B-4571-9508-CF3EFCD7BBAE". There is. In this service, the reading sensor has a characteristic UUID characteristic called "10B20101-5B3B-4571-9508-CF3EFCD7BBAE".

There are only 3 types of BLE data access

write transfers data to the peripheral with Central specifying the Characteristic UUID.
In read, the central requests the data with the Characteristic UUID and the peripheral returns the data.
In notify, Central requests the peripheral to start notify by specifying the Characteristic UUID. The peripheral will then continue to send data to Central at regular intervals thereafter.

The data here is an array of uint8.

Summary So for ubuntu and toio to communicate

The BLE dongle attached to ubuntu becomes central and connects to toio by specifying the HW address.
Get the toio service by specifying the service UUID.
Specify the characteristic UUID of the reading sensor and read to obtain the data.

It becomes the procedure.

About Bluetooth (Practice)

Let's actually do the above.

Confirmation of bluetooth dongle

This time the bluetooth dongle used this. In some cases, BR / EDR can be used but BLE cannot be used with the one built into the PC. It is good to buy an optional dongle. hciconfig will display the bluetooth device list. Only the dongle stuck in the PC and the built-in bluetooth receiver are displayed here.

`Confirmation of bluetooth dongle`


$ 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 device scan

You will see a list of nearby bluetooth devices like this. Turn on toio and execute the following command. Make a note of toio's HW address. You can stop it with Ctrl + C.

`BLE device scan`


$ sudo hcitool -i hci0 lescan
LE Scan ...
XX:XX:XX:XX:XX:XX (unknown)
XX:XX:XX:XX:XX:XX toio Core Cube
...
(Continued below)

If you get Set scan parameters failed: Input / output error here, the device does not support BLE. You only need root privileges to scan your device in this way.

Start connecting with toio

We use gatttool, a tool that exchanges data with BLE. First, start with the following command. For XX: XX: XX: XX: XX: XX, enter the HW address of toio obtained by lescan above.

`Start connecting with toio`


$ gatttool -b XX:XX:XX:XX:XX:XX -t random -I
[XX:XX:XX:XX:XX:XX][LE]>

connect Make the actual connection. If it is normal, you will hear a connection sound from toio.

`Start connecting with toio`


[XX:XX:XX:XX:XX:XX][LE]> connect
[XX:XX:XX:XX:XX:XX][LE]>            # <-The color turns blue

Acquisition of service charactristic

Get a list of the characteristics that toio has. See toio specifications to see which corresponds to what.

`Get characteristic`


[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
...
(Continued below)

Charactualistic read

Let's see the button ON / OFF as a trial. Try it while pressing the button at the lamp.

`Characteristic read`


[XX:XX:XX:XX:XX:XX][LE]> char-read-uuid 10B20107-5B3B-4571-9508-CF3EFCD7BBAE
handle: 0x001e 	 value: 01 80

When pressed, the second value becomes 80, and when released, it becomes 00.

Characteristic write

Characteristic write is specified in the handler as follows. In the list that appears with char-desc 0x0b000b above, the handle of 10B20103-5B3B-4571-9508-CF3EFCD7BBAE that controls the color of the lamp is 0x0014, so use this. The lamp glows white with the following command.

`Characteristic read`


[XX:XX:XX:XX:XX:XX][LE]> char-write-cmd 0x0014 03000101808080

Disconnection from toio

`Disconnection from toio`


[XX:XX:XX:XX:XX:XX][LE]> disconnect

How to use bluepy

This time we will use a python library called bluepy.

Installation

sudo pip install bluepy

Connection and read / write

Basic connection and data read / write method.

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

Run rosrun toio_lecture toio_basic1.py F0: 5D: 68: 8E: A8: 7C.
Connect with btle.Peripheral (). The first argument is the mac address of the BLE device and the second argument is the type of address. If it is toio, it will not connect unless you specify btle.ADDR_TYPE_RANDOM. The third argument is the bluetooth dongle to connect, with 1 leading to hci1. None or up to the second argument will automatically connect to either.
Read with toio_peripheral.readCharacteristic (BATTERY_HANDLE). The result of reading is a string of bytes. Convert this to an array of integers with ʻord () . For example, you should see ('battery', 100)` (behind is the battery level (%)).
Write with toio_peripheral.writeCharacteristic (LAMP_HANDLE, bytearray (data), True). It is recommended to set the data with a sequence of integers and send it as a byte sequence with bytearray (). This command causes the LED to glow pale red.

How to use notify

In the reading of the above example, the data is read at the timing of the program side, but notify is that the value is thrown from the device every time the value is updated in the device.

`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

Execute like rosrun toio_lecture toio_basic2.py F0: 5D: 68: 8E: A8: 7C.

`result`


set delegate
callback
('battery', 90)
callback
('battery', 90)
callback
('battery', 90)

Write 1 to handle value +1 like toio_peripheral.writeCharacteristic (BATTERY_HANDLE + 1, b'\ x01', True) to start notify.
Create a delegate class that inherits the btle.DefaultDelegate class and set it with the ble function. If any notify comes while waiting at toio_peripheral.waitForNotifications (), handleNotification () will be called back.

Use from ros

Source code

I'll put the source code below. toio_bridge.py

Twist to take the speed input, change it to the wheel speed and command toio.
toio returns the value of the position sensor, but since this is not in meters, convert it. The center of the mat surface is the origin. Also, the direction of the axis of the XY coordinates is different from that of ROS, so convert it.
bluepy is not threat-safe, and unlike roscpp, callbacks are executed in a separate thread from the main routine. Therefore, if implemented normally, for example, waitForNotifications () of the main routine and writeCharacteristic () called in the subscriber may be called at the same time. In this case bluepy will throw an exception. To prevent this, in the above program, if you want to write, put it in the queue once, and in the main routine, look at the queue and write.

Run

The mac address is specified in the launch below. It needs to be rewritten.

roslaunch toio_lecture with_kicker.launch

For connecting multiple units

You can connect to multiple toio by launching multiple toio_bridges in the above launch. However, the phenomenon that notify arrives with a delay of a little less than one disease always occurs in toio connected later. For now, the workaround for this is to have as many bluetooth dongles as there are toio and assign different bluetooth dongles to each toio_bridge. Sample code can be found at dual.launch.

reference

Introduction to BLE How to use ble related commands How to use bluepy toio Core Cube Specification

Link to table of contents page

Link to ROS course table of contents

ROS course 105 Operate toio with ROS

environment

Overview

About Bluetooth (Basic)

About Bluetooth (Practice)

Confirmation of bluetooth dongle

Confirmation of bluetooth dongle

BLE device scan

BLE device scan

Start connecting with toio

Start connecting with toio

Start connecting with toio

Acquisition of service charactristic

Get characteristic

Charactualistic read

Characteristic read

Characteristic write

Characteristic read

Disconnection from toio

Disconnection from toio

How to use bluepy

Installation

Connection and read / write

toio_lecture/script/toio_basic1.py

How to use notify

toio_lecture/script/toio_basic2.py

result

Use from ros

Source code

Run

For connecting multiple units

reference

Link to table of contents page

`Confirmation of bluetooth dongle`

`BLE device scan`

`Start connecting with toio`

`Start connecting with toio`

`Get characteristic`

`Characteristic read`

`Characteristic read`

`Disconnection from toio`

`toio_lecture/script/toio_basic1.py`

`toio_lecture/script/toio_basic2.py`

`result`