toio 入門 (4) - ROSによるキューブの操作

npaka

2022年1月16日 21:22

「Unity Robotics Hub」を使って、ROSでtoioキューブを操作する方法をまとめました。

・Unity 2020.3
・toio SDK for Unity v1.4.0
・noetic

前回

1. ROS

「ROS」（Robot Operating System）は、オープンソースのロボット開発プラットフォームです。ロボットソフトウェアの共同開発を世界規模で推進することを目的として開発されました。

今回は「cmd_vel」メッセージをtoio側でサブスクライブして、ROS側からtoioを操作できるようにします。

2. Unity Robotics Hub

「Unity Robotics Hub」は、Unityでのロボットシミュレーション用のツール、チュートリアル、リソース、およびドキュメントの情報をまとめたリポジトリです。

ROS-Unity間の通信は、Unityの「ROS-TCP-Connector」パッケージと、ROSの「ROS-TCP-Endpoint」パッケージとを介して行います。

3. Unity側の準備

「toio 入門 (1) - 事始め」のキューブを回転させるコードを、ROSによるキューブの操作を行うコードに変更します。

(1) Hierarchyウィンドウで「RotateCube」を削除。
(2) Unityの「Package Manager」の「＋ → Add package from git URL」で、「ROS TCP Connector」をインストール。

https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector

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

・RosCube.cs

using UnityEngine;
using UnityEngine.UI;
using toio;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Geometry;

// ROSによるキューブの操作
public class RosCube : MonoBehaviour
{
    public ConnectType connectType; // 接続種別

    CubeManager cm; // キューブマネージャ

    ROSConnection ros; // ROSコネクション
    int translate = 0; // 前進量 (-115+Abs(rotate)/2 ~ 115-Abs(rotate)/2)
    int rotate = 0; // 回転量 (-115+Abs(rotate)/2 ~ 115-Abs(rotate)/2)
    float ROSTimeout = 0.5f;
    float lastCmdReceived = 0f;

    // スタート時に呼ばれる
    async void Start()
    {
        // ROSコネクションの準備
        ros = ROSConnection.GetOrCreateInstance();
        ros.Subscribe<TwistMsg>("cmd_vel", ReceiveROSCmd);

        // キューブの接続
        cm = new CubeManager(connectType);
        await cm.MultiConnect(1);
    }

    // フレーム毎に呼ばれる
    void Update()
    {
        // ROSタイムアウト
        if (Time.time - lastCmdReceived > ROSTimeout)
        {
            translate = 0;
            rotate = 0;
        }

        // キューブの操作
        foreach (var handle in cm.syncHandles)
        {
            handle.Move(translate, rotate, 500, false);
        }
    }

    // cmd_velのサブスクライブ
    void ReceiveROSCmd(TwistMsg cmdVel)
    {
        translate = (int)(cmdVel.linear.x * 100);
        rotate = (int)(cmdVel.angular.z * 50);
        lastCmdReceived = Time.time;
    }
}

4. ROS側の準備

◎ ROSのDockerイメージの起動
(1) Dockerのインストール。
(2) ポート番号「10000」「5005」を追加して、ROSのDockerイメージを起動。

$ docker run -v ~/ros1_ws:/home/ubuntu/catkin_ws:cached -p 6080:80 -p 10000:10000 -p 5005:5005 --shm-size=1024m tiryoh/ros-desktop-vnc:noetic

(3) ブラウザで「http://127.0.0.1:6080/」を開く。

◎ ワークスペースのセットアップ
(1) ワークスペースのフォルダを作成し移動。

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws

(2) ワークスペースのビルド。

$ catkin build

(3) ワークスペースのセットアップ。
ターミナルを開くたびに必要になるため、「~/.bashrc」にも追加します。

$ source ~/catkin_ws/devel/setup.bash

◎ ROSパッケージのインストール
(1) 「ROS-TCP-Endpoint」パッケージのインストール。
Unity-ROS間の通信を行うROSパッケージです。

$ cd ~/catkin_ws/src
$ git clone https://github.com/Unity-Technologies/ROS-TCP-Endpoint
$ cd ~/catkin_ws
$ catkin build
$ source ~/catkin_ws/devel/setup.bash

(2) 「teleop-twist-keyboard」のインストール。
キー操作でcmd_velをパブリッシュするROSパッケージです。

$ sudo apt update
$ sudo apt install ros-noetic-teleop-twist-keyboard

5. 実行

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

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

$ roscore

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

$ rosparam set ROS_IP 0.0.0.0
$ rosrun ros_tcp_endpoint default_server_endpoint.py

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

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py _repeat_rate:=２

◎ Unity側の実行
(1) UnityのPlayボタンで実行。

「teleop-twist-keyboard」のターミナルを選択した状態で以下のキーを押すことで、toioを操作できます。

・U / I / O : ↖ ↑ ↗
・J / K / L : ← ・ →
・M / , / . : ↙ ↓ ↘