Go to Qiita Advent Calendar Top
LoginSignup
3
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@dandelion1124

toioのROS 2対応パッケージを作る

Last updated at Posted at 2025-12-17

はじめに

CyberAgent AI Lab Advent Calendar 2025 - Adventar 18日目の記事です。

最近、個人開発でtoioをROS 2対応したパッケージをOSSとして公開しました。

この記事では開発したパッケージの機能だけでなく、どんな流れでROS 2対応していったかの開発過程を知見として共有していきます。

toioとは

toioは、ソニー・インタラクティブエンタテイメント（SIE）によって開発されたキューブ型ロボットです。

高レベル（例：指定された座標に移動する）のAPIだけでなく、低レベル（例：左右のモーターを制御する）のAPIも提供されているという特徴があります。これはROS 2対応するしかないですね！

開発したパッケージ

今回開発したパッケージは以下の通りです。

パッケージ パッケージ概要 リポジトリURL
toio_description robot_description（3Dモデル、フレーム）配信 https://github.com/atinfinity/toio_description
toio_ros2 toioのROS 2ラッパー https://github.com/atinfinity/toio_ros2

toio_descriptionでできること

  • robot_description（3Dモデル、フレーム）配信

RViz2でrobot_descriptionを可視化している例です。

image.png

toio_ros2でできること

  • toio位置姿勢の配信（トピック、tf）
  • cmd_velトピックによる車両制御
  • goal_poseトピックによる車両制御
  • バッテリー残量情報の配信

toioの位置姿勢をRViz2で可視化している例です。

image.png

また、goal_poseトピックを使ってtoioを移動させることもできます。

以降、各パッケージの開発過程について述べていきます。

toio_descriptionパッケージ

ここではtoio_descriptionパッケージの開発過程をまとめます。

toioの寸法

toioコアキューブの寸法はhttps://toio.github.io/toio-spec/docs/hardware_shape/に記載されています。今回、主に以下の点を確認しました。

  • 駆動輪の位置
    • フレーム定義に用いる
  • 駆動輪半径：6.25mm
    • cmd_velトピックによる車両制御に用いる
  • トレッド幅：26.6mm
    • cmd_velトピックによる車両制御に用いる

toioの3Dモデル

https://toio.github.io/toio-spec/docs/hardware_shape/で3Dモデルが公開されています。このページにて

キューブの 3D データはクリエイティブ・コモンズ 表示-改変禁止 4.0 国際 パブリック・ライセンスでライセンスされます。詳しくはこちらをご確認ください。

とあるので、こちらのライセンスに則って使用することにします。

URDF作成

オリジナルの3DモデルをそのままURDFで使おうとすると、ROS 2とスケール、座標軸の向きが異なるため、URDF（toio.urdf.xacro）では以下のように記述しています。

<link name="center">
    <visual>
        <origin xyz="0 0 0" rpy="3.14159 0 0"/>
        <geometry>
            <mesh filename="package://toio_description/meshes/toio/toiocorecube_v001.stl" scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="body">
            <color rgba="1.0 1.0 1.0 1.0" />
        </material>
    </visual>
</link>

ポイントは以下の通りです。

  • 3Dモデルの単位はmmなので、メートル単位にするためscaleを0.001にする
  • ROS 2と座標軸の向きを合わせるため、rollを3.14159ラジアン回転させる

toio_ros2パッケージ

ここではtoio_ros2パッケージの開発過程をまとめます。

toio APIをROS 2ノードで呼べるようにする

今回、toioの制御には公式のtoio.pyを用いることにしました。また、詳細な制御をするために低レベルAPIを持つ非同期API (ToioCoreCube API)を使っています。

まずはtoio APIの雰囲気を掴んでもらうため、toio.py 入門 - クイックスタート｜npakaにあるサンプルコードを以下に引用します。

import asyncio
from toio import *

async def hello():
    async with ToioCoreCube() as cube:
        # 回転
        await cube.api.motor.motor_control(30, -30)
        await asyncio.sleep(2)

        # 停止
        await cube.api.motor.motor_control(0, 0)

    return 0

if __name__ == "__main__":
    asyncio.run(hello())

toioをROS 2対応するにはROS 2ノード内でこれらの非同期APIを呼び出すことになります。そこでtoio_ros2パッケージではROS 2ノード初期化時に以下のようにしています。

  • イベントループを作成する
  • コルーチン内でtoioの非同期APIを呼び出す
class ToioNode(Node):
    def __init__(self) -> None:

        ##### 途中のコード省略

        # イベントループ作成
        self.loop = asyncio.new_event_loop()
        self.thread = threading.Thread(
            target=self.start_loop, daemon=True)
        self.thread.start()
        # コルーチンを実行
        asyncio.run_coroutine_threadsafe(
            self.connect_toio(),
            self.loop)

async def connect_toio(self) -> None:
    self.cube = ToioCoreCube()

    # ここで非同期APIを呼ぶ
    await self.cube.scan()
    await self.cube.connect()
    self.is_connected = True
    self.get_logger().info('toio is connected.')

toio位置姿勢取得

toioをtoio開発用A4プレイマット、toio開発用A3プレイマットなどのPosition IDが印字されたマット上に置かれている場合、toio APIでtoioの現在位置と姿勢を取得することができます。今回、https://toio.github.io/toio-spec/docs/ble_id#position-idを参考にし、Position ID情報のうち、以下の値を読み出すことにしました。

また、https://support.toio.io/article/programmingに以下の説明があります。

Q.キューブで位置や座標を指定する際の距離の単位は何ですか？
キューブの座標や距離の単位は独自のスケールになっておりmm等の単位で割り切れない値になっています。 そのため固定値での変換係数を使用すると使用するサイズによって誤差が蓄積してしまうため公表していません。 座標や距離のmmやcm等への変換が必要な場合は、 誤算が蓄積しないようにするために実際に使う最大の距離とその時の座標での距離を実測していただいて変換式を割り出して使用していただくようお願いします。

（toioの座標をmmへ変換する係数＝実測の距離[mm]/toioの座標での距離）

ということで、toio APIで得られるX座標値、Y座標値が実寸の値ではない点に注意が必要です。そのため、toio APIで得られるtoio位置姿勢をROS 2で使うためには以下の手順が必要になります。

  • 実寸に直す
  • ROS 2と回転の単位、向きを合わせる

実寸に直す

今回、toioコアキューブ（単体）に付属のtoio開発用A4プレイマットを用いました。このマットはA4サイズと寸法が既知のため、こちらの情報を用いて実スケールに変換しています。また、このAPIで得られるX座標値、Y座標値はtoioキューブの中心（center）基準の座標になるので、必要に応じて、base_link、base_footprint基準の座標にします。

ROS 2と回転の単位、向きを合わせる

前述の通り、toio APIで得られる姿勢は以下の通りです。

一方、REP 103 -- Standard Units of Measure and Coordinate Conventions (ROS.org)には以下の記述があります。

Quantity Unit
angle radian

By the right hand rule, the yaw component of orientation increases as the child frame rotates counter-clockwise, and for geographic poses, yaw is zero when pointing east.

要約すると以下の通りです。

  • 角度の単位はラジアン
  • 反時計回り

そのため、REPに合わせて変換を行います。

cmd_velトピックによる車両制御

ROS 2のナビゲーションでよく用いられるcmd_velトピックを購読し、toioの制御を行えるようにします。ROS 2 Humbleでも使えるようにcmd_velトピックはgeometry_msgs/msg/Twistにします。

おおまかには以下の流れでtoioのモーター制御を行っています。

  • cmd_velトピックを購読する
  • geometry_msgs/msg/Twistのうち、linear、angularを取り出す
  • 左右のモーターの回転方向、回転速度（RPM）を計算する
  • toio APIのモーター制御に用いるための値に変換する
  • toio APIを呼び出し、左右のモーターを制御する

toio APIでモーター制御を行うAPIの呼び出し例を以下に示します。

await cube.api.motor.motor_control(30, -30)

ここで引数に与えている値はどういう意味でしょうか？APIドキュメントを読むと第一引数が「左の駆動輪のモーター」、第二引数が「右の駆動輪のモーター」を制御するためのinputとなっています。

class toio.cube.api.motor.MotorControl(left: int, right: int, duration_ms: int | None)

前述のモーター制御のinputについてはhttps://support.toio.io/article/programmingに以下の記述があります。

Q.キューブの速さを指定できるブロックで指定可能な速度の範囲は？
仕様上、キューブの動作可能な速度の範囲は8～115（および-8～-115）となっています。
1～7（および-1～-7）を入力しても速度が0の扱いになるため動きません。
また115（および-115）を超えた数値を入力してもそれを超える速度にはなりません。
詳細は「toio コア キューブ技術仕様」をご覧ください。

そのため、cube.api.motor.motor_controlに与える値はモーターのRPM値でなく、上記のルールに沿って与える必要があります。

goal_poseトピックによる車両制御

今回、goal_poseトピックによる車両制御も実装しました。toio APIにはtoioを指定した座標に移動させるためのAPIが提供されています。
https://toio.github.io/toio.py/latest/toio.cube.api.motor.html#toio.cube.api.motor.TargetPosition

そのため、goal_poseトピックによる車両制御を行うために以下のような処理を行います。

  • goal_poseトピックを購読する
  • ROS 2の位置姿勢情報からtoioが扱う位置姿勢情報に変換する
  • toio.cube.api.motor.TargetPositionで指定した座標に移動させる

goal_poseトピックを使ってtoioを移動させた動画を再掲します。

バッテリー情報取得

toio APIでtoioのバッテリー情報を取得することができます。
https://toio.github.io/toio-spec/docs/ble_batteryに以下の説明があります。

バッテリー残量は 0 から 100 までの範囲を 10 刻みで取得可能です。単位はパーセントです。

1パーセント刻みでないことに注意が必要です。また、rviz_2d_overlay_pluginsで可視化したかったのでstd_msgs/msg/Float32のトピックを配信しています。

おまけ

今回開発したパッケージでcmd_vel対応とtf対応をしていたのでodomフレームなしでNav2を動かす #ROS - Qiitaを参考にしてtoioをnav2で動かしてみました。

ハマったこと

今回、toio.pyをいろんなプラットフォーム上で動かしてみてハマった点を残しています。

パッケージ問題（Windows 11）

Windows環境で以下のようにしてtoio.pyをインストールしました。

pip install toio.py==1.1.0

その後、サンプルコードを実行したのですが以下のエラーが出てしまいました。

Exception in callback BleakScannerWinRT._received_handler(<winrt._winrt...0015EAD0D2C10>, <winrt._winrt...0015EAD0D2B90>)
handle: <Handle BleakScannerWinRT._received_handler(<winrt._winrt...0015EAD0D2C10>, <winrt._winrt...0015EAD0D2B90>)>
Traceback (most recent call last):
  File "C:\Users\toio\AppData\Local\Programs\Python\Python312\Lib\asyncio\events.py", line 88, in _run
    self._context.run(self._callback, *self._args)
  File "C:\Users\toio\work\toio-venv\Lib\site-packages\bleak\backends\winrt\scanner.py", line 218, in _received_handler
    self.call_detection_callbacks(device, advertisement_data)
  File "C:\Users\toio\work\toio-venv\Lib\site-packages\bleak\backends\scanner.py", line 239, in call_detection_callbacks
    callback(device, advertisement_data)
  File "C:\Users\toio\work\toio-venv\Lib\site-packages\toio\device_interface\ble.py", line 209, in check_condition
    interface=BleCube(device),
              ^^^^^^^^^^^^^^^
  File "C:\Users\toio\work\toio-venv\Lib\site-packages\toio\device_interface\ble.py", line 70, in __init__
    from bleak.backends.winrt.scanner import _RawAdvData
ImportError: cannot import name '_RawAdvData' from 'bleak.backends.winrt.scanner' (C:\Users\toio\work\toio-venv\Lib\site-packages\bleak\backends\winrt\scanner.py

pip install toio.py==1.1.0

を実行すると、依存パッケージであるbleak v1.1.1がインストールされますが、bleakのバージョンアップに伴ってAPIの破壊的変更が入っているようで、toio APIが正常に実行できませんでした。そのため、bleak v0.22.3をインストールすることで対処しました。

pip install bleak==0.22.3

パッケージ問題（macOS対応）

今回、macOS上ではPixiを使ってROS 2の環境を構築（https://pixi.sh/dev/tutorials/ros2/）し、toio.pyをインストールしました。

pixi add toio.py

この記事執筆時点でtoio.pyの最新バージョンはv1.1.0なのですが、pixi addでインストールするとtoio.py v1.0.2がインストールされます。toio.pyは、v1.1.0になった時点でAPIが大幅に変更されており、かつ、使いやすくなっているため、以下のコマンドを実行し、toio.py v1.1.0をインストールしました。

pixi add --pypi toio.py

おわりに

本記事ではどんな流れでtoioをROS 2対応していったかの過程を解説しました。今回作ったROS 2パッケージについてぜひ使ってみてフィードバック、PRください。

3
0
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
3
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?