(cache)ラズパイ x Arduino x Unity x HMDで簡易的なテレイグジスタンス体験をする

今回は夏休み中に趣味兼研究目的で作っていた簡易的なテレイグジスタンス体験ができる装置の紹介をしたいと思います。

テレイグジスタンスとは?
システム構成
実行環境
作業
まとめ
エラーなど
- C#でのUdpClientについて
- Python2系と3系でのpyserialの違い
参考

テレイグジスタンスとは?

Wikipediaによると以下のように書かれています。

テレイグジスタンス（英: Telexistence、遠隔臨場感、遠隔存在感)とは、バーチャルリアリティの一分野であり、遠隔地にある物（あるいは人）があたかも近くにあるかのように感じながら、操作などをリアルタイムに行う環境を構築する技術およびその体系のこと

テレイグジスタンス - Wikipedia

端的に言えば、遠隔地に存在するロボットなどの装置にユーザーがなんらかの方法で乗り移って、まるで自分がその場にいるかのように振る舞うことができるようになる技術のことです。この技術により、例えば、危険な地域(有毒ガス地帯、放射線量が高い地域、宇宙空間等)に実際に人間が赴くことなく、ロボットから送られてくる五感情報を通してスムーズに作業ができるようになったり、なんらかの理由により家から出ることができない方がロボットを通して社会で活躍したりイベントに参加したりできるようになるのです。

システム構成

システム構成は以下のようにしました。

f:id:yubeshineko:20191030172512p:plain

Unity
- Raspberry Piが配信しているカメラの映像をHMDに表示
- HMDの回転情報をサーボモーターで扱えるように変換してRaspberry Piに送信
Raspberry Pi
- 接続されているカメラの映像を配信
- Unityから送られてきたHMDの回転情報をArduinoに送信
Arduino
- Raspberry Piから送られてきたHMDの回転情報を利用しサーボを制御

Unity, Raspberry Pi間はネットワークを通じて、Raspberry Pi, Arduino間はUSBケーブルで繋がっています。

実行環境

使用ハードウェア
- HTC Vive
- Raspberry Pi Model B+
- Arduino Uno R3
- Windowsマシン
ソフトウェア
- Windows 10
- Raspbian GNU/Linux 10 (buster)
- Unity 2019.2.8f1
- Python 3.7.3
パーツ

作業

ステップ1: ラズパイのカメラモジュールの映像を配信する

ラズパイを使ってカメラの映像を配信するとなったら、一般的にはmjpg-streamerが使われることが多いです。今回のプロジェクトでもこちらを使います。

カメラモジュールの設定についてはこちらが分かりやすいかと思います。

www.pc-koubou.jp

次にmjpg-streamerをインストールします。今回はラズパイのカメラモジュールに対応している派生版のmjpg-streamerを使用します。

github.com

# 準備
$ sudo apt-get install cmake libjpeg8-dev
# もしgcc, g++が入ってなければ
$ sudo apt-get install gcc g++

# インストール
$ git clone https://github.com/jacksonliam/mjpg-streamer.git
$ cd mjpg-streamer-experimental
$ make
$ sudo make install

これでmjpg-streamerを利用できるようになりました。カメラモジュールが接続されていることを確認して利用してみます。

# カメラモジュールが接続されているか確認
$ vcgencmd get_camera
# "supported=1 detected=1" と表示されたら、カメラが認識されている

$ pwd
/[mjpg-streamerをインストールしたフォルダまでのパス]/mjpg-streamer/mjpg-streamer-experimental

$ export LD_LIBRARY_PATH=.
$ ./mjpg_streamer -o "output_http.so -w ./www" -i "input_raspicam.so"

これでカメラの映像が配信されます。ラズパイと同じネットワークに接続されているデバイスでブラウザを開いてhttp://[ラズパイのIPアドレス]:8080/にアクセスするとmjpg-streamerのデモページが開き、左側のメニューからStreamを開くと、現在のカメラの映像が見れます。

しかし、毎回長い実行コマンドを打つのも大変なので、実行する際のシェルスクリプトを書きます。お好きなエディタで以下のファイルを作成し、mjpg-streamer-experimentalフォルダ以下に保存します。

mjpg_streamer_boot.sh

#!/bin/sh
 
# mjpg-streamer start script
 
# Path to mjpg_streamer and libraries
export LD_LIBRARY_PATH="/[mjpg-streamerをインストールしたフォルダまでのパス]/mjpg-streamer/mjpg-streamer-experimental"
STREAMER="$LD_LIBRARY_PATH/mjpg_streamer"
 
# Pi camera configurations
XRES="640"
YRES="480"
FPS="30"
 
# Web configurations
WWWDOC="$LD_LIBRARY_PATH/www"
PORT="8080"
 
# Start streaming
$STREAMER -i "input_raspicam.so -x $XRES -y $YRES -fps $FPS" \
          -o "output_http.so -w $WWWDOC -p $PORT" \
      -b

$ ./mjpg_streamer_boot.shで配信を開始することができます。なお、オプションでバックグラウンドで動作するようにしています。これは後にpythonコードを走らせるためです。

プロセスをkillする際は以下のようにします。

$ pgrep mjpg
# mjpg-streamerのプロセスIDが表示される
$ kill -9 [mjpg-streamerのプロセスID]

オプションについての説明は省略します。色々あるので調べてみてください。

ステップ2: ラズパイが配信している映像をUnityで受信する

次に、ラズパイで配信している映像を受信するためのUnityでの処理が必要になります。ネットで何かしらの方法がないものかと調べていたところ、ぴったりのものを見つけました。

hammmm.hatenablog.com

こちらの方が作成されたアセットを利用させていただきます。なお、このアセットはAndroid、Windows 32/64bitでしか動作が確認されておらず、私がMacでやったときは動作しませんでした。

アセットをダウンロード

上記のページからダウンロードページに飛んでダウンロードします。
ダウンロードしたunityパッケージをProjectにインポートする
シーンにオブジェクトを配置(Quadなど)
配置したオブジェクトにMJStreamingPlayerスクリプトをアタッチする
MJStreamingPlayerコンポーネントのServerUrlフィールドにサーバーのURLを書く。

http://[ラズパイのIPアドレス]:8080/?action=streamとなります。
マテリアルを作成し、ShaderをUnlit/Textureに変更。このマテリアルを先ほど追加したオブジェクトにD&D。

これで準備はできました。ラズパイでmjpg-streamerを起動しておいてからUnityで実行してみます。Unityまで画像が来ていることが確認できたら成功です。

ステップ3: HMDの角度をサーボモーターで扱える値に変換してラズパイに送信する。

ラズパイに繋がったカメラの映像をUnityで見ることができました。次はUnityでのHMDの傾きをラズパイに送る処理を実装します。HMDはHTC Viveを使いました。

まずはHMDを利用できるようにします。

Project Settings > Player Settings > XR Settingsで Virtual Reality Supportedにチェックを入れてVirtual Reality SDKsにOpenVRを追加します。これでカメラオブジェクトの映像がHMDに映るようになります。

あとはカメラオブジェクトに自身の回転情報を取得するスクリプトを書いてあげてそれをラズパイに送ればそれでOK!

...というわけにはいきません。なぜなら、サーボモーターでの回転角度と、Unity内での回転の角度が一致していないためです。そのため、ラズパイに回転角度を送る前にUnityで角度の変換をする必要があります。また、サーボモーターは180度回転のサーボを利用しています。

実際には以下のような違いがあります。

f:id:yubeshineko:20191031013819j:plain

transform.localEulerAnglesをサーボの回転角度に変えていきます。

 
 using UnityEngine;
 
 public class HeadRotation : MonoBehaviour
 
 {
 
     public string GetServoAngle ()
 
     {
 
         return $"{ConvertPitch(transform.localEulerAngles.x)},{ConvertYaw(transform.localEulerAngles.y)}";
 
     }
 
     int ConvertPitch (float unityAngleX)
 
     {
 
         var servoPitch = 0f;
 
         if (270f <= unityAngleX && unityAngleX < 360f)
 
         {
 
            servoPitch = unityAngleX - 270f;
 
         }
 
         else if (0f <= unityAngleX && unityAngleX <= 90f)
 
         {
 
             servoPitch = unityAngleX + 90f;
 
         }
 
         return (int)servoPitch;
 
     }
 
     int ConvertYaw (float unityAngleY)
 
     {
 
         var servoYaw = 0f;
 
         if (0f <= unityAngleY && unityAngleY < 90f)
 
         {
 
             servoYaw = 90f - unityAngleY;
 
         }
 
         else if (90f <= unityAngleY && unityAngleY < 180f)
 
         {
 
             servoYaw = 0f;
 
         }
 
         else if (180f <= unityAngleY && unityAngleY < 270f)
 
         {
 
             servoYaw = 180f;
 
         }
 
         else
 
         {
 
             servoYaw = 270f - (unityAngleY - 180f);
 
         }
 
         return (int)servoYaw;
 
     }
 
 }

view raw HeadRotation.cs hosted with ❤ by GitHub

Unityの角度 -> サーボの角度の変換コード。

ヨーに関してですが、HMDのY軸角度がサーボで回ることができない90度 ~ 270度の範囲にある際は、90~180の間の時は0度に、180~270の間の時は180度に制限しています。最初は360度回るサーボモーターでやってみたのですが、360度サーボをArudinoのサーボモーターライブラリで動かすことができなかったので今回は断念しました。

次に、ラズパイに変換した角度データを送信するスクリプトを書きます。通信にはUDPを用います。

 
 using UnityEngine;
 
 using System.Net.Sockets;
 
 public class UdpSender
 
 {
 
     string _remoteHost = "";
 
     int _remotePort = 60000;
 
     UdpClient _udpClient;
 
     public UdpSender(string remoteHost, int remotePort)
 
     {
 
         _remoteHost = remoteHost;
 
         _remotePort = remotePort;
 
         _udpClient = new UdpClient();
 
     }
 
     public void SendData (string data)
 
     {
 
         byte[] sendBytes = System.Text.Encoding.ASCII.GetBytes(data);
 
         try
 
         {
 
             _udpClient.Send(sendBytes, sendBytes.Length, _remoteHost, _remotePort);
 
         }
 
         catch (SocketException se)
 
         {
 
             Debug.LogError(se.ToString());
 
             Debug.LogError($"Error Code : {se.ErrorCode}");
 
         }
 
     }
 
     public void UdpClientClose()
 
     {
 
         _udpClient.Close();
 
         Debug.Log("udp was closed.");
 
     }
 
 }

view raw UdpSender.cs hosted with ❤ by GitHub

Udp通信をするためのスクリプト。

そして、この2つのクラスを利用するスクリプトを書いてあげます。

UserDataManager.cs

using UnityEngine;

public class UserDataManager : MonoBehaviour
{
    public GameObject cameraObject;
    public string remoteHost = "";
    public int remotePort = 60000;
    UdpSender udpSender;
    HeadRotation headRotation;

    void Start()
    {
        udpSender = new UdpSender(remoteHost, remotePort);
        headRotation = cameraObject.GetComponent<HeadRotation>();
    }

    void Update()
    {
       udpSender.SendData(headRotation.GetServoAngle()); 
    }

    void OnApplicationQuit()
    {
        udpSender.UdpClientClose();
    }
}

3つのスクリプトが書けたら、カメラオブジェクトにHeadRotationスクリプト、UserDataManagerスクリプトをアタッチします。アタッチしたら、UserDataManagerコンポーネントのフィールドを適切に設定していきます。

CameraObjectにはカメラオブジェクトをD&D、RemoteHostはラズパイのIPアドレスとします。RemotePortは今回は60000にしておきます。

準備ができたら、ラズパイで以下のコマンドを打ってパケットのキャプチャをしてみます。

# tcpdumpがなければ
$ sudo apt-get install tcpdump
$ sudo tcpdump -A -n udp port 60000

Unityにてシーンを再生して、正しくパケットがキャプチャされていれば成功です。

最後に、スクリーンとなるオブジェクトをVRカメラの子にしてHMDの正面に張り付くようにしてあげましょう。

ステップ4: Unityから送られてきたサーボモーターの角度データを受信してArduinoに送信する

ラズパイでUnityから送られてきたデータを受信することができましたので、次は送られてきたデータを受け取って、Arduinoに渡すためのPython スクリプトを書きます。

ArduinoとラズパイはUSBケーブルで繋がっています。そのため、シリアル通信用のライブラリが必要になります。今回はpyserialというライブラリを使用することにします。pyserialはpipを使ってインストールできます。

# pipがなければ
$ sudo apt-get install python-pip
$ pip3 install pyserial

また、ラズパイのシリアル通信を有効にしていないのであれば有効にしてあげる必要があります。

$ sudo raspi-config

pyserialがインストールできたらコードを書いていきます。

 
 #coding:utf-8
 
 import threading
 
 import time
 
 import signal
 
 import sys
 
 import serial
 
 from socket import socket, AF_INET, SOCK_DGRAM
 
 HOST = ''   
 
 PORT = 60000
 
 is_running = True
 
 angle_data = bytes(b'')
 
 # UDP通信部
 
 sock = socket(AF_INET, SOCK_DGRAM)
 
 def udp_communication ():
 
 	global is_running
 
 	global sock
 
 	global angle_data
 
 	sock.bind((HOST, PORT))
 
 	try:
 
 		while is_running:
 
 			msg, address = sock.recvfrom(64) # 64はバッファのサイズ
 
 			print(f"message: {msg}\nfrom: {address}")
 
 			angle_data = msg
 
 	except KeyboardInterrupt:
 
 		print ("Keyboard Interrupt Exception")
 
 # シリアル通信部
 
 ser = serial.Serial('/dev/ttyACM0', 115200, timeout=None)
 
 def serial_communication ():
 
 	global is_running
 
 	global ser
 
 	global angle_data
 
 	interbal = 0.03
 
 	try:
 
 		while is_running:
 
 			ser.write(angle_data)
 
 			time.sleep(interbal)
 
 	except KeyboardInterrupt:
 
 		print ("Keyboard Interrupt Exception")
 
 def interuppt_handler(signum, frame):
 
 	global is_running
 
 	global sock
 
 	global ser
 
 	print ("Signal handler!!!")
 
 	is_running = False
 
 	ser.close()
 
 	sock.close()
 
 	sys.exit(-1)
 
 signal.signal(signal.SIGINT, interuppt_handler)
 
 if __name__ == "__main__":
 
 	threads = [threading.Thread(target=serial_communication),
 
 				threading.Thread(target=udp_communication)]
 
 	for thread in threads:
 
 		thread.start()
 
 	for thread in threads:
 
 		thread.join()

view raw telexistenceApp.py hosted with ❤ by GitHub

Unityから送られてくる角度データ受信し、Arduinoに送信するスクリプト。

スレッドを利用してUDP通信部とシリアル通信部を同時に実行しています。ちなみに、32行目の/dev/ttyACM0ついて、基本的にはこれで問題ないのですが、この部分は人によっては違うことがあるかもしれません。その際はこの部分を書き換えてあげる必要があります。

ステップ5: ラズパイから送られてきたデータを処理してサーボを制御する

いよいよサーボを制御する段階まできました。というわけで、早速スクリプトです。

 
 #include <Servo.h>
 
 // 受信文字列
 
 String received_data;
 
 // 受信した文字列を変換する
 
 String angles[2];
 
 // ピッチ, ヨー
 
 int angle_pitch;
 
 int angle_yaw;
 
 // サーボオブジェクト
 
 Servo servo_pitch;
 
 Servo servo_yaw;
 
 // 文字列分割関数
 
 // https://algorithm.joho.info/arduino/string-split-delimiter/
 
 int split(String data, char delimiter, String *dst){
 
     int index = 0;
 
     int arraySize = (sizeof(data)/sizeof((data)[0]));  
 
     int datalength = data.length();
 
     for (int i = 0; i < datalength; i++) {
 
         char tmp = data.charAt(i);
 
         if ( tmp == delimiter ) {
 
             index++;
 
             if ( index > (arraySize - 1)) return -1;
 
         }
 
         else dst[index] += tmp;
 
     }
 
     return (index + 1);
 
 }
 
 void setup()
 
 {
 
   // Serial Setting
 
   Serial.begin(115200);
 
   Serial.setTimeout(20);
 
   servo_pitch.attach(9);
 
   servo_yaw.attach(10);
 
   servo_pitch.write(90);
 
   servo_yaw.write(90);
 
 }
 
 void loop()
 
 {
 
 }
 
 void serialEvent()
 
 {
 
   received_data = Serial.readStringUntil('\n');
 
   if (received_data == "Init")
 
   {
 
     servo_pitch.write(90);
 
     servo_yaw.write(90);
 
     return;
 
   }
 
   split(received_data, ',', angles);
 
   angle_pitch = angles[0].toInt();
 
   angle_yaw = angles[1].toInt();
 
   servo_pitch.write(angle_pitch);
 
   servo_yaw.write(angle_yaw);
 
   angles[0] = "";
 
   angles[1] = "";
 
 }

view raw TelexistenceApp.ino hosted with ❤ by GitHub

ラズパイから送られてきたサーボ角度のデータを処理してサーボを制御するコード

38行目でSerial.setTimeout(20)としてタイムアウトの時間を短くしています。デフォルトの値は1000(ms)となっており、今回のような速さが求められるプロジェクトにおいて1秒では大きすぎるので20msとしてあります。

また、送られてくるデータは{ピッチの角度},{ヨーの角度}となっているため、それぞれの角度を取り出さなくてはなりません。どうやら、Arduinoには文字列を区切り文字で分割するsplit関数は用意されていないとのことなので、以下の方が作成されたsplit関数を利用させていただきます。

algorithm.joho.info

コードが書けたら回路も作ります。

f:id:yubeshineko:20191031134241p:plain

サーボモーターの電源は外部から取ります。私の場合、12V1AのACアダプターを利用しました。また、サーボモーターの定格電圧は4.8V~5Vとなっているので、5Vの三端子レギュレーターによって5Vを作り出してサーボモーターに給電しています。

カメラマウントの組み立てについてはこちらをご覧ください。

studio.beatnix.co.jp

ヨー制御用のサーボとカメラマウントの間に微小のスペースが空いていて少しぐらつくので、スペーサーを挟んで接着剤で固定したりするといいかもしれないです。カメラの設置には両面テープなどを使用します。また、サーボを動かす際はカメラマウントは固定しないとかなり動きますので、何かしらの方法で固定する必要があります。

ステップ6: 全体テスト

最終チェックをします。

mjpg-streamerを起動する
Arduinoにコードを書き込む
ArduinoとラズパイをUSBケーブルで接続する
ラズパイでPythonコードを実行する

$ python3 telexistenceApp.py
Unityでシーンを再生する

シーンを再生して、HMDとサーボモーターの角度が一致していることが確認できたら成功です!

ちなみに、Viveでやった後にOculus Questでも動作することを確認しました!

まとめ

今回は遠隔のカメラ映像をHMDを通して見て、HMDの傾きを遠隔のカメラにも適応させることで簡易的なテレイグジスタンス体験を味わうことをやってみました。しかし、ただ遠隔地のカメラ映像を見るだけではテレイグジスタンスとしては不十分です。そこで、次はロボットアームを導入したりして遠隔地の対象とのインタラクションができるようにしてみたいですね。また、今はまだ同一のネットワークでしか利用できないので、外部ネットワークからのアクセスをできるようにもしてみたいです。他にも、サーボモーターの制御にて、writeMicroseconds()関数を利用することで角度を指定するよりも細かくサーボの角度を制御できるので、これについてもやってみたいです。

エラーなど

C#でのUdpClientについて

UdpClientでのデータの送信方法としては

UdpClient udp = new UdpClient ();
udp.Send (sendBytes, sendBytes.Length, remoteHost, remotePort);

とする方法と

UdpClient udp = new UdpClient (remoteHost, remotePort);
udp.Send (sendBytes, sendBytes.Length);

とする方法があるのだが、後者のほうでやろうとすると、最初の1度はデータが送られるのに次からはデータが送られずにエラーを吐く。SocketExcepthonをキャッチしてエラーコードを確認したところ、コードは10061であった。接続対象から接続が拒否されているらしいが、原因は全く持って不明。

https://support.microsoft.com/ja-jp/help/819124/windows-sockets-error-codes-values-and-meanings

Python2系と3系でのpyserialの違い

pyserialにてPython2系と3系で微妙な違いがあった。シリアル通信の際に 2系では

ser.write("Hoge")

とすればすぐにできるが、3系では

ser.write(str.encode(“Hoge”))

としないとできないとのこと。

stackoverflow.com

参考

mjpg-streamer

https://blue-black.ink/?page_id=5298

https://blue-black.ink/?page_id=2245

PythonでのUDP通信

https://www.shujima.work/entry/2018/07/13/195100

C#でのUDP通信

https://dobon.net/vb/dotnet/internet/udpclient.html

シリアル通信について

https://www.arduino.cc/reference/en/language/functions/communication/serial/readstring/

https://karaage.hatenadiary.jp/entry/2015/06/10/080000

https://novicengineering.com/シリアルモニターの使い方【arduino】/

Pythonでのスレッドの止め方

https://codeday.me/jp/qa/20190711/1225730.html

ゆべねこの足跡

主にVR、Unity、Arduinoについて書いていきます。

ラズパイ x Arduino x Unity x HMDで簡易的なテレイグジスタンス体験をする