はじめに
これ、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ブラウザ上で眺めています。