ROS入門 (17) - toioを操作するROSアプリケーションの作成
「toio」を操作するROSアプリケーションの作成方法をまとめました。
前回
1. ロボットのサブシステム
ロボットアプリケーションのシステムは非常に複雑になります。しかし、1つ1つのサブシステムに分けて考えることで、設計・解析が容易になります。
ロボットで共通して使われる主要なサブシステムは、次の3つです。
ROSの開発では慣例的に、はじめにスタンドアロンのライブラリを作り(または準備し)、その後に他のROSモジュールとメッセージ送受信するためのラップレイヤーを実装します。これによって、ROS以外でのサブシステムの再利用が容易になるだけでなく、単体テストも容易になります。
2. toioのサブシステム
今回は練習として、以下のようなtoioのサブシステムを作成し、シミュレータで実行してみます。
3. toio SDK for Unityのインストール
「toio SDK for Unity」は、「Unity」でtoioコアキューブを制御するための開発環境です。Unityエディタ上でのシミュレーションおよび、実環境での動作が可能です。
「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」でインポート。
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秒停止を繰り返します。
次回
この記事が気に入ったらサポートをしてみませんか?