Python使ってUnity上の箱庭ロボットのカメラデータを取得してみよう

# はじめに
これ、Turtlebot3 の 3D モデルを Unity 上に取り込み、モーター、センサ系を再現したものです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/d5a32771-304d-7072-5c8d-0d800dc948ae.png)

この Unity 上のロボットを Python 使って制御する方法をご紹介します。

# 仕組み

このロボットをPythonでコントロールするためのプラットフォームとして、TOPPERS/箱庭を利用します。
最近ですと、以下の記事で EV3 というロボットをPython 使って強化学習した実績があるものです。

https://qiita.com/kanetugu2018/items/ee9b2bf0596a35e42b03

# ロボットの部品群

## モーター

差動モーターで駆動します。両輪それぞれにモーターが割り当てられており、モーターの回転速度差で直進、右曲がり、左曲がりができます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/36ca5c8c-ae07-4adc-5766-7ec48095e4db.png)

## レーザスキャナ

2D のレーザスキャナです。360度(1度単位)の障害物検出ができます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/aaf63fa9-5d8c-faf5-5ade-54f37f6eb49d.png)

## カメラセンサ

前方にカメラセンサを付けています。
640×480 のカメラデータが取れます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/8302ad55-c697-a82b-55d7-d10f2512f47a.png)


# ロボットの通信データ

ロボットの通信データは、ROS2のトピックデータ型で定義しています。
(cmd_vel 以外はすべてセンサデータです)

|ROSトピックデータ型|名前|サイズ[単位：バイト]|channel_id|
|---|---|---|---|
|geometry_msgs/Twist|cmd_vel|48|0|
|sensor_msgs/JointState|joint_states|440|1|
|sensor_msgs/Imu|imu|432|2|
|nav_msgs/Odometry|odom|944|3|
|tf2_msgs/TFMessage|tf|320|4|
|sensor_msgs/Image|image|1,024,280|5|
|sensor_msgs/CompressedImage|image/compressed|102,664|6|
|sensor_msgs/CameraInfo|camera_info|580|7|
|sensor_msgs/LaserScan|scan|3,044|8|

# Python側のプログラム

## 通信データ
Python側のプログラムでは、これらの通信データは、dictionary形式で読めます。

### モーターコントロール(cmd_vel)
linearのxが速度で、angularのzが回転方向です。
それ以外は無視されます。

```yaml
linear:
  x: 0
  y: 0
  z: 0
angular:
  x: 0
  y: 0
  z: 0
```

### カメラデータ(image/compressed)

data の中にjpeg形式のデータが格納されています。

```yaml
header:
  stamp:
    sec: 1678519485
    nanosec: 759772000
  frame_id: camera_link
format: jpeg
data: <生のバイナリデータ>
```

### レーザスキャナ(scan)

ranges の中にスキャンした結果が配列として360個格納されています。

```yaml
header:
  stamp:
    sec: 1678519496
    nanosec: 958531000
  frame_id: base_scan
angle_min: 0
angle_max: 6.2657318115234375
angle_increment: 0.01745329238474369
time_increment: 2.9880000511184335e-05
scan_time: 0
range_min: 0.11999999731779099
range_max: 3.5
ranges:
  - 3.5
  ：(全部で360個あります)
```

## 通信ライブラリ

Unity上のTurtlebot3を制御するためのプログラムは以下の通りです。

```python
class HakoRoboModelTb3:
    def __init__(self, hako):
        self.hako = hako
        self.hako.create_pdu_channel(0, 48, 'Twist')
        self.hako.subscribe_pdu_channel(1, 440, 'JointState')
        self.hako.subscribe_pdu_channel(2, 432, 'Imu')
        self.hako.subscribe_pdu_channel(3, 944, 'Odometry')
        self.hako.subscribe_pdu_channel(4, 320, 'TFMessage')
        self.hako.subscribe_pdu_channel(5, 1024280, 'Image')
        self.hako.subscribe_pdu_channel(6, 102664, 'CompressedImage')
        self.hako.subscribe_pdu_channel(7, 580, 'CameraInfo')
        self.hako.subscribe_pdu_channel(8, 3044, 'LaserScan')
        self.binary_data = bytearray(48)
        self.actions = { 0: binary_reader.binary_read(self.hako.offmap, 'Twist', self.binary_data) }

    def delta_usec(self):
        #20msec
        return 20000

    def camera_data(self, sensors):
        return (sensors[6]['data'])

    def laser_scan(self, sensors):
        return (sensors[8]['ranges'])

    def get_forward_distance(self, scan_datas):
        min1 = min(scan_datas[0:15])
        min2 = min(scan_datas[345:359])
        return min(min1, min2)
            
    def get_right_distance(self, scan_datas):
        return min(scan_datas[60:120])
    
    def foward(self, speed):
        self.actions[0]['linear']['x'] = speed
    
    def turn(self, speed):
        self.actions[0]['angular']['z'] = speed
            
    def stop(self):
        self.actions[0]['linear']['x'] = 0.0
        self.actions[0]['angular']['z'] = 0.0

    def step(self):
        for channel_id in self.actions:
            self.hako.write_pdu(channel_id, self.actions[channel_id])
```

上記ライブラリを使ったサンプルプログラムは以下の通りです。

```python
#!/usr/bin/python
# -*- coding: utf-8 -*-
import json
import sys
from hako_binary import offset_map
from hako_binary import binary_writer
from hako_binary import binary_reader
import hako_env
import hako
import time
import signal
import hako_robomodel_tb3 as tb3

def handler(signum, frame):
  print(f'SIGNAL(signum={signum})')
  sys.exit(0)
  
print("START TB3 TEST")

# signal.SIGALRMのシグナルハンドラを登録
signal.signal(signal.SIGINT, handler)

#create hakoniwa env
env = hako_env.make("base_practice_1", "tb3")
print("WAIT START:")
env.hako.wait_event(hako.HakoEvent['START'])
print("WAIT RUNNING:")
env.hako.wait_state(hako.HakoState['RUNNING'])
print("GO:")

#do simulation
robo = env.robo()
for episode in range(1):
  total_time = 0
  done = False
  state = 0
  total_reward = 0
  #100secs
  while not done and total_time < 4000:
    sensors = env.hako.execute()
      
    img = env.robo().camera_data(sensors)
    with open("camera.jpg" , 'bw') as f:
        f.write(img)
    
    scan_datas = env.robo().laser_scan(sensors)
    if env.robo().get_forward_distance(scan_datas) >= 0.2:
        env.robo().foward(0.5)
        env.robo().turn(0.0)
    else:
        env.robo().foward(0.0)
        env.robo().turn(-0.5)
    env.robo().step()  
    total_time = total_time + 1
  env.reset()
print("END")
env.reset()
sys.exit(0)
```

カメラデータは、上記プログラムのここで取得してファイルに保存しています。

```python
    img = env.robo().camera_data(sensors)
    with open("camera.jpg" , 'bw') as f:
        f.write(img)
```

実際に保存したカメラデータはこんな感じです。

![camera.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/dd053db4-d1cf-d18e-1d0e-ed4b1cac6a5e.jpeg)


# Unityのセットアップ手順

基本的なインストール手順は[前回](https://qiita.com/kanetugu2018/items/ee9b2bf0596a35e42b03)の記事の通りですが、[Unity環境のセットアップ](https://qiita.com/kanetugu2018/items/ee9b2bf0596a35e42b03#unity%E7%92%B0%E5%A2%83%E3%81%AE%E3%82%BB%E3%83%83%E3%83%88%E3%82%A2%E3%83%83%E3%83%97)のクローン対象リポジトリが異なります。

```
git clone -b unity-asset-for-tb3 --recursive https://github.com/toppers/hakoniwa-ros2sim.git
```

Unityエディタを起動後、プロジェクトビューから「Configurator」というシーンをダブルクリックします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/e8dedfd4-e716-ffb1-9fac-34b7f9cbd229.png)

起動すると、下図のようになります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/2627f7c7-bf58-a3eb-94ca-bd3f3465b943.png)

ヒエラルキービューを開き、「Robot」内にあるオブジェクトを「delete」キーで削除し、以下のTurtlebot3モデル(MiconTB3RoboModel)を配置します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/58e0170f-2063-e463-fb95-d6ffaa9c4e68.png)

`MiconTB3RoboModel`は、プロジェクトビューの Assets/Resources/Hakoniwa/Robots 配下にあります。

配置が終わったら、メニューから環境構成を外部出力します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/351d01bc-65f7-cbdb-b59a-8499b6c12084.png)

出力完了後、以下のコマンドで箱庭環境にインストールします。

```
bash hako-install.bash opt all
```

そして、Unityのシミュレーション用のシーンとして、「Toppers_Course」を起動します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/e5fd090c-7e35-2dab-dfa9-2cd2fcfb389f.png)

# Python側のセットアップ手順

Python 側のセットアップ手順は、[前回](https://qiita.com/kanetugu2018/items/ee9b2bf0596a35e42b03#python-%E5%AE%9F%E8%A1%8C%E7%92%B0%E5%A2%83%E3%81%AE%E3%82%A4%E3%83%B3%E3%82%B9%E3%83%88%E3%83%BC%E3%83%AB)の記事の通りです。

ただし、Turtlebot3を動かすには、以下のファイルを変更する必要があります。

* workspace/runtime/asset_def.txt 

変更前：dev/ai/ai_qtable.py
変更後：dev/ai/test_tb3-01.py:mqtt

test_tb3.py は、以下にあります。

https://github.com/toppers/hakoniwa-base/blob/ai/workspace/dev/ai/test_tb3-01.py

# シミュレーション実行方法

hakoniwa-base 上で、以下のように docker コンテナを起動します。

```
$ bash docker/run.bash runtime
ASSET_DEF=asset_def.txt
PYTHON_PROG=dev/ai/test_tb3.py
PDU_COMM=mqtt
INFO: ACTIVATING MOSQUITTO
[13703.343514]~DLT~   13~INFO     ~FIFO /tmp/dlt cannot be opened. Retrying later...
INFO: ACTIVATING HAKO-MASTER
OPEN RECIEVER UDP PORT=172.30.224.33:54001
OPEN SENDER UDP PORT=172.30.224.33:54002
mqtt_url=mqtt://172.30.224.33:1883
PUBLISHER Connecting to the MQTT server...
PUBLISHER CONNECTED to the MQTT server...
delta_msec = 20
max_delay_msec = 100
INFO: shmget() key=255 size=12160
Server Start: 172.30.224.33:50051
INFO: ACTIVATING :dev/ai/test_tb3.py
START TB3 TEST
create_channel: id=0 size=48
WAIT START:
```

この状態で、Unityのシミュレーションを開始します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/db5a3e38-6e4f-2883-12ad-f33384d40272.png)


この状態で、「開始」ボタンをクリックすると動き出します。

# デモ

上記の手順で実行したときのデモをお見せします。

カメラデータは、ローカルのファイルシステム上に保存されていますので、そのデータをWebブラウザ上で眺めています。

https://twitter.com/kanetugu2020/status/1637275140441645056