ROS入門 (17) - toioを操作するROSアプリケーションの作成

「toio」を操作するROSアプリケーションの作成方法をまとめました。

・Melodic
・Unity 2020.3
・ROS-TCP-Connector 0.6.0
・ROS-TCP-Endpoint 0.5.0
・toio SDK for Unity 1.3.1

前回

1. ロボットのサブシステム

ロボットアプリケーションのシステムは非常に複雑になります。しかし、１つ１つのサブシステムに分けて考えることで、設計・解析が容易になります。

ロボットで共通して使われる主要なサブシステムは、次の3つです。

・アクチュエーション : サーボやモーターなど、ロボットを動作させるサブシステム。
・センシング : カメラや加速度センサーなど、周囲の情報を収集するサブシステム
・コンピューティング : アクチュエーションとセンシングをつなぎ、ロボットに何らかのタスクを実行させるサブシステム。

ROSの開発では慣例的に、はじめにスタンドアロンのライブラリを作り（または準備し）、その後に他のROSモジュールとメッセージ送受信するためのラップレイヤーを実装します。これによって、ROS以外でのサブシステムの再利用が容易になるだけでなく、単体テストも容易になります。

2. toioのサブシステム

今回は練習として、以下のようなtoioのサブシステムを作成し、シミュレータで実行してみます。

・toio_cube : toioのモーターの制御 (アクチュエーション)
・toio_control : 「1秒回転→1秒停止」を繰り返す (コンピューティング)

3. toio SDK for Unityのインストール

「toio SDK for Unity」は、「Unity」でtoioコアキューブを制御するための開発環境です。Unityエディタ上でのシミュレーションおよび、実環境での動作が可能です。

GitHub - morikatron/toio-sdk-for-unity: Unityでtoio™コア キューブを制御するための開発環境

「toio SDK for Unity」のインストール手順は、次のとおりです。

(1) Unity 2020.2以降で、Unityプロジェクトを3Dテンプレートで作成。
(2) Unityのメニュー「Window → Package Manager」で「Package Manager」を開き、「＋ → Add package from git URL....」で以下のURLを入力し、「Add」ボタンを押す。
「UniTask」がインストールされます。

https://github.com/Cysharp/UniTask.git?path=src/UniTask/Assets/Plugins/UniTask

(3) 以下のサイトから、「toio-sdk-for-unity.unitypackage」をダウンロードし、メニュー「Assets → Import Package → Custom Package」でインポート。

Release toio SDK for Unity v1.3.1 · morikatron/toio-sdk-for-unity

4. スタンドアロンのtoioアプリケーションの作成

スタンドアロンのtoioアプリケーションの作成手順は、次のとおりです。

(1) Hierarchyウィンドウの「Main Camera」と「Directional Light」を削除。
(2) Projectウィンドウの「/Assets/toio-sdk/Scripts/Simulator/Resources」の「Cubeプレハブ」と「Stageプレハブ」を、Hierarchyウィンドウにドラッグ&ドロップ。

(3) Hierarchyウィンドウで、「＋ → Create → Empty Object」で空のオブジェクトを生成し、「ToioCube」という名前を指定。
(4) 「ToioCube」に「Add Component」で新規スクリプト「ToioCube」を生成し、以下のように編集。

using UnityEngine;
using toio;

// キューブの制御
public class ToioCube : MonoBehaviour
{
    float elapsedTime = 0; // 経過時間
    Cube cube; // キューブ

    // スタート時に呼ばれる
    async void Start()
    {
      	// Bluetoothデバイスを検索
        var peripheral = await new NearestScanner().Scan();

       	// デバイスに接続してキューブを取得
        cube = await new CubeConnecter().Connect(peripheral);
    }

    // フレーム毎に呼ばれる
    void Update()
    {
        // Cube変数が取得できるまで無処理
        if (null == cube) return;

        // 50ミリ秒毎の処理
        elapsedTime += Time.deltaTime;
        if (0.05f > elapsedTime) return;
        elapsedTime = 0.0f;

     	// 左モーター速度に50、右モーター速度に-50、制御時間に200ミリ秒を指定
        cube.Move(50, -50, 200);
    }
}

(5) UnityエディタのPlayボタンで実行。
キューブが中央で回転し続けます。

5. toio_controlの作成

「toio_control」の作成手順は、次のとおりです。

(1)  「toio_control」の準備。
メッセージとスクリプトは、次のとおりです。

・ToioControl.msg

int32 left
int32 right

・toio_control.py

#!/usr/bin/env python
# coding: UTF-8
import rospy
from hello.msg import *

# メイン
def main():
    # パブリッシャーの生成
    pub = rospy.Publisher("toio_cube", ToioControl, queue_size=10)

    # ノードの初期化
    rospy.init_node("toio_control", anonymous=True)

    r = rospy.Rate(1)
    move = True
    while not rospy.is_shutdown():
        # メッセージの生成
        if move:
            msg = ToioControl(-50, 50)
        else:
            msg = ToioControl(0, 0)
        move = not move

        # ログ出力
        rospy.loginfo("Publish " + str(msg.left) + ","+ str(msg.right))

        # パブリッシュの送信
        pub.publish(msg)

        # スリープ
        r.sleep()

if __name__ == "__main__":
    main()

ビルド方法は、「ROS入門 (10) - ROS1のトピックによる通信」と同様です。

(2) RPS-TCP-Endpointの準備。
「ROS入門 (16) - ROS-Unity間通信」と同様です。

6. toio_cubeの作成

「toio_cube」の作成手順は、次のとおりです。

(1) ROS-TCP-Connectorのインストール。
「ROS入門 (16) - ROS-Unity間通信」と同様です。

(2) ROSの設定の確認。
「ROS入門 (16) - ROS-Unity間通信」と同様です。

(3) メッセージのインポート。
「ROS入門 (16) - ROS-Unity間通信」の手順で「ToioControl.msg」をインポートします。

(4) スクリプト「ToioCube」を以下のように編集。

using UnityEngine;
using toio;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using ToioControlMsg = RosMessageTypes.Hello.ToioControlMsg;

// キューブの制御
public class ToioCube : MonoBehaviour
{
    Cube cube; // キューブ

    // スタート時に呼ばれる
    async void Start()
    {
      	// Bluetoothデバイスを検索
        var peripheral = await new NearestScanner().Scan();

      	// デバイスに接続してキューブを取得
        cube = await new CubeConnecter().Connect(peripheral);

        // ROSコネクションへのサブスクライバーの登録
        ROSConnection.instance.Subscribe<ToioControlMsg>("toio_cube", ToioControlCallback);
    }

    // メッセージの受信時に呼ばれる
    void ToioControlCallback(ToioControlMsg msg){
        // Cube変数が取得できるまで無処理
        if (null == cube) return;

      	// モーターの動作
        cube.Move(msg.left, msg.right, 1000);
    }
}

7. 実行

実行手順は、次のとおりです。

◎ ROS側の実行
(1) ターミナルを開き、「roscore」を実行。

$ roscore

(2) もう1つのターミナルを開き、ROSのIPとポートを指定して、「ROS-TCP-Endpoint」を実行。

$ rosparam set ROS_IP 0.0.0.0
$ rosparam set ROS_TCP_PORT 10000
$ rosrun ros_tcp_endpoint default_server_endpoint.py

(3) もう1つのターミナルを開き、「toio_controller」を実行。

$ rosrun hello toio_controller.py

◎ Unity側の実行
(1) Playボタンで実行。
1秒前進→1秒停止を繰り返します。

次回

Unityではじめる ROS・人工知能 ロボットプログラミング実践入門