ROS course 105 Operate toio with ROS

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

The data here is an array of uint8.

Summary So for ubuntu and toio to communicate

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)

toio1.png

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

result


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

Use from ros

Source code

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

Run

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

roslaunch toio_lecture with_kicker.launch

toio3.gif

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

Recommended Posts

ROS course 105 Operate toio with ROS
ROS course 105 Operate toio with ROS
ROS Lecture 108 Using Database (mongo) with ROS
ROS Lecture 113 Perform tasks with smach
Operate Kinesis with Python
Operate Blender with Python
Operate Excel with Python (1)
Operate Excel with Python (2)
Operate Excel with Python openpyxl
Operate Db2 container with Go
[Note] Operate MongoDB with Python
Operate your website with Python_Webbrowser
[Python] [SQLite3] Operate SQLite with Python (Basic)
Operate Nutanix with REST API Part 2
Operate a receipt printer with python
ROS Lecture 108 Using Database (mongo) with ROS
ROS Lecture 113 Perform tasks with smach
Try to operate Facebook with Python
Operate ECHONET Lite appliances with Python
Operate Maya with OSC from vvvv
Operate Linux Network Namespace with Go