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

はじめに

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

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

仕組み

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

ロボットの部品群

モーター

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

レーザスキャナ

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

カメラセンサ

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

ロボットの通信データ

ロボットの通信データは、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が回転方向です。
それ以外は無視されます。

linear:
  x: 0
  y: 0
  z: 0
angular:
  x: 0
  y: 0
  z: 0

カメラデータ(image/compressed)

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

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

レーザスキャナ(scan)

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

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を制御するためのプログラムは以下の通りです。

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])

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

#!/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)

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

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

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

Unityのセットアップ手順

基本的なインストール手順は前回の記事の通りですが、Unity環境のセットアップのクローン対象リポジトリが異なります。

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

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

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

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

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

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

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

bash hako-install.bash opt all

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

Python側のセットアップ手順

Python 側のセットアップ手順は、前回の記事の通りです。

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

• workspace/runtime/asset_def.txt

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

test_tb3.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のシミュレーションを開始します。

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

デモ

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

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