hakoniwa-drone-multi

箱庭ドローン・デモシリーズ 複数機体シミュレーション編 合同会社箱庭ラボ CTO 森崇

アジェンダ • 本資料の位置付け • 対応しているフライトコントローラ • 複数機体構成のアーキテクチャと通信ポート • 箱庭Python APIのアーキテクチャと内部設計 • セットアップ手順 • シミュレーション手順 2

本資料の位置づけ • 箱庭ドローンにおける複数機体シミュレーション機能の構成と利用方法を整理したものです。 • 箱庭ドローンでは、単一機体だけでなく、複数のドローンを同時に制御・観測できるように設計されています。 • 本資料では、そのアーキテクチャ、通信ポート構成、制御APIの設計方針、および実行手順を体系的に示します。 • これにより、以下を実現できるようになります。 • 複数機体構成の通信構造と動作原理を理解する • 各機体を個別または協調的に制御するための Python API を利用できる • PX4・ArduPilot・独自制御プログラムを混在させた複合シミュレーションを再現・実行できる ※用語： 箱庭ドローンとは、無償版の箱庭ドローンシミュレータ（hakoniwa-drone-core）の省略名です。 箱庭ドローンPROは、有償版の箱庭ドローンシミュレータ(hakoniwa-drone-pro)を指します。 3

対応しているフライトコントローラ • 本資料で説明する構成は、箱庭ドローンで連携できる全てのフライトコントローラに対応しています。 • PX4、ArduPilot、controller(箱庭ドローン内部で利用できる制御プログラム) PX4 Ardupilot comm physics mavlink service logger config controller aircraft aircraft service Game World! drone service hakoniwa Hakoniwa World! 4

複数機体のアーキテクチャ(全体像) • 箱庭ドローンシミュレータは、１つのプロセスで複数機体を管理しています。 • 機体毎に、コンフィグパラメータがあり、コンフィグディレクトリに連番でコンフィグファイルを配置します。 • ArduPilot/PX4 • Mavlink Python API Ardupilot/PX4 App<0> FC<0> 箱庭ドローンシミュレータ(Engine) drone config directory プロセス：機体毎 シミュレーション接続：MAVLink通信 • シミュレーション通信周期：3msec • アプリ接続： MAVLink通信 • MAVLink Protocol mavlink <0> MAVLink Protocol aircraft <0> drone_config_0.json service <0> controller <0> App<1> mavlink <1> FC<1> MAVLink Protocol MAVLink Protocol aircraft <1> drone_config_1.json service <1> controller <1> • 独自制御 • Python API プロセス：箱庭ドローンシミュレータ内部 • controller<index> シミュレーション接続：関数コール • アプリ接続： PDU通信(共有メモリ) App<0> Hakoniwa PDU • hakoniwa App<1> Hakoniwa PDU 5

PX4の通信ポート 14540/UDP 4560/TCP App<0> 箱庭ドローンシミュレータ (Engine) PX4<0> 14541/UDP 4561/TCP App<1> PX4<1> 18570/UDP 18571/UDP QGC 6

ArduPilotの通信ポート 9002/UDP 14550/UDP App<0> ArduPilot<0> 9003/UDP 9012/UDP 14560/UDP App<1> 箱庭ドローンシミュレータ (Engine) ArduPilot<1> 14550/UDP Mission Planner <0> 9013/UDP 14560/UDP Mission Planner <1> 7

アジェンダ • 本資料の位置付け • 対応しているフライトコントローラ • 複数機体構成のアーキテクチャと通信ポート • 箱庭Python APIのアーキテクチャと内部設計 • セットアップ手順 • シミュレーション手順 8

箱庭Python APIのアーキテクチャと内部設計 • 箱庭Python APIとは、箱庭ドローンを操作するPythonプログラムです。 • ユースケースは、全部で７種類です。 • ただし、利用するゲームエンジンによって、機能差分があります。 ユースケース Unity Unreal Engine 機体を離陸する ○ ○ 機体の現在位置を取得する ○ ○ 機体を目的地へ移動する ○ ○ 機体で荷物を運ぶ ○ X 機体のカメラで撮影する ○ ○ 機体のレーザセンサ(LiDAR)で 周辺の障害物を検出する ○ X 機体を着陸する ○ ○ ※2025/10/14時点のサポート状況 9

Python アプリ&API ライブラリの位置付け Pythonアプリ Python APIライブラリ 箱庭ドローン ドローンを操作し、 様々なミッションを コントロール 箱庭ドローンを 操作するため のAPIセット Unityの3Dモデル で作成された ドローン 10

箱庭ドローン・Python API 操作のユースケース 箱庭ドローンを操作するAPIセットは全部で７個あります。 現在位置を 取得する 着陸する 離陸する カメラ撮影する 障害物を検出する 目的地へ 移動する 荷物を運ぶ 11

Python APIの利用手順
• 事前準備
• 箱庭ドローンシミュレータのインスール
• Python(バージョン3.12)のインストール
• custom.jsonの生成
• Unityエディタ上で生成するコンフィグファイルです。

• Python APIライブラリ詳細はこちら

• Python APIを呼び出すための

最小限のプログラムは右のものです。

import sys
#箱庭のライブラリのインポート
import hakosim
def main():
if len(sys.argv) != 2:
print(f"Usage: {sys.argv[0]} <config_path>")
return 1
#箱庭ドローンを操作するためのクライアントとオブジェクトを取得します
#この際、custom.jsonを引数で渡してください
client = hakosim.MultirotorClient(sys.argv[1])
# クライアントオブジェクトの初期化処理を実行します（以下の３行）
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
#ここに自分のプログラムを書きます

• このテンプレートプログラムをベースにして、

自分のプログラムを追加していきましょう。

return 0
if __name__ == "__main__":
sys.exit(main())

12

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#readme

Python APIライブラリの座標系 • ENU座標系です（右図） X • X：前方方向（E：前がプラス） • Y：横方向（N：左がプラス） • Z：上方向（U：上がプラス） 参考：ROSの座標系と同じです。 Y Z 右手系：親指上、右回り 親指 ：Z 人差し指 ：X 中指 ：Y 13

Unityの座標系 • 左手座標系です（右図） Z • X：横方向（右がプラス） • Y：上方向（上がプラス） • Z：前方向（前がプラス） X Y https://docs.unity3d.com/ja/2023.2/Manual/Q uaternionAndEulerRotationsInUnity.html 左手系：親指上、左回り 親指 ：Y 人差し指 ：Z 中指 ：X 14

• https://docs.unity3d.com/ja/2023.2/Manual/QuaternionAndEulerRotationsInUnity.html

Unrealの座標系 • 左手座標系です（右図） X • X：前方向（前がプラス） • Y：右方向（右がプラス） • Z：上方向（上がプラス） Y Z https://dev.epicgames.com/documentation/ja-jp/unrealengine/coordinate-system-and-spaces-in-unreal-engine 左手系：親指上、左回り 親指 ：Z 人差し指 ：X 中指 ：Y 15

• https://dev.epicgames.com/documentation/ja-jp/unreal-engine/coordinate-system-and-spaces-in-unreal-engine

UC：離陸する • API名：takeoff • 引数で指定した高さ（単位：メートル）まで浮上します。 離陸する • 以下のプログラムを実行して、ドローンが離陸することを確 認してみましょう。 # 3m浮上 client.takeoff(3) 16

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#takeoffheight-vehicle_namenone

UC：現在位置を取得する • API名：simGetVehiclePose • ドローンの現在位置を姿勢を取得できます。 現在位置を 取得する • 以下のプログラムを実行して、ドローンの現在位 置と姿勢を表示しましょう。 # ドローンの現在位置と姿勢を取得 pose = client.simGetVehiclePose() # 現在位置をデバッグ出力 print(f"POS : {pose.position.x_val} {pose.position.y_val} {pose.position.z_val}") # 姿勢情報は、クォータニオンであるため、オイラー角(ラジアン)に変換します roll, pitch, yaw = hakosim.hakosim_types.Quaternionr.quaternion_to_euler(pose.orientation) # オイラー角を度に変換して、デバッグ出力 print(f"ANGLE: {math.degrees(roll)} {math.degrees(pitch)} {math.degrees(yaw)}") 17

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#simgetvehicleposevehicle_namenone

UC：目的地へ移動する • API名：moveToPosition • 目的地へ移動させます。 目的地へ 移動する • 以下のプログラムを実行して、ドローンを目的地 まで移動させましょう。 # ドローを目的地へ移動させます # X軸の位置：10m # Y軸の位置：0m # 高度：3m # 機首の方向：0度 client.moveToPosition(x=10, y=0, z=3, speed=3, yaw_deg=0) • 補足： • speedは最大で3m/sec程度で移動します。 • 現時点では、目標速度での移動は未サポートです。 18

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#movetopositionx-y-z-speed-yaw_degnone-vehicle_namenone

UC：荷物を運ぶ • API名：grab_baggage • スイッチングマグネットホルダーを使って、荷物の吸引およ びその解除を操作します • 以下のプログラムを実行して、荷物の吸引、目的地移 動、荷物を下ろす操作をしてみましょう。 # 荷物配置場所(baggage_pos)へ移動します client.moveToPosition(baggage_pos['x'], baggage_pos['y'], 3, 0, -90) client.moveToPosition(baggage_pos['x'], baggage_pos['y'], 3, 5) client.moveToPosition(baggage_pos['x'], baggage_pos['y'], 3, 5, 0) # 荷物を吸引できる位置まで高度まを下げます client.moveToPosition(baggage_pos['x'], baggage_pos['y'], 0.7, 0.01, 0) # 荷物を吸引し、高度を上げます client.grab_baggage(True) client.moveToPosition(baggage_pos['x'], baggage_pos['y'], 3, 0.01) 荷物を運ぶ • 補足： • 荷物を吸引する条件は、ド ローン直下1m以下に存在 するすべての荷物が対象とな ります # 荷物を下ろす場所(transfer_pos)へ移動します client.moveToPosition(transfer_pos['x'], transfer_pos['y'], 3, 0.1) client.moveToPosition(transfer_pos['x'], transfer_pos['y'], transfer_pos['z'], 0.01) # 荷物を下ろします client.grab_baggage(False) 19

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#grab_baggagegrab-vehicle_namenone
• https://www.youtube.com/watch?v=CzGmWsxNnO0

UC：カメラ撮影する(1/2) • API名：simGetImage • カメラ設置位置(デフォルト：前方)からカメラ撮影します。 • 以下のプログラムを実行して、カメラ撮影し、ファイル保存 しましょう。 カメラ撮影する # 引数は必ず以下値としてください # id = “0” # image_type = hakosim.ImageType.Scene png_image = client.simGetImage("0", hakosim.ImageType.Scene) # ファイル名をscene.pngとしてファイル保存します。ファイルは、Pythonプログラム実行場所に出力されます。 if png_image: with open("scene.png", "wb") as f: f.write(png_image) • 補足： • カメラは複数配置できませんので、常にidには”0”を指定してください。 • image_typeは、ImageType.sceneのみサポートしています。 20

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#simgetimageid-image_type-vehicle_namenone

UC：カメラ撮影する(2/2) • API名：simSetCameraOrientation • カメラのピッチ角を変更します。 • 以下のプログラムを実行して、カメラを下向きにしてかカメ ラ撮影し、ファイル保存しましょう。 カメラ撮影する # 引数は必ず以下値としてください # id = “0” client.simSetCameraOrientation("0", degree=-90) png_image = client.simGetImage("0", hakosim.ImageType.Scene) # ファイル名をscene.pngとしてファイル保存します。ファイルは、Pythonプログラム実行場所に出力されます。 if png_image: with open("scene.png", "wb") as f: f.write(png_image) • 補足：degreeは下向きがマイナスです。上向きはプラスです。 • 前方真正面は０度、下向きの最大角度は-90度、上向きは+15度です。 21

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#simsetcameraorientationid-degree-vehicle_namenone

UC：障害物を検出する
• API名：getLidarData
• レーザ照射し、障害物の位置を取得します。本APIは、AirSimのLiDAR

のAPI仕様に準拠していますので、詳細はこちらを参照ください。
• 以下のプログラムを実行して、障害物を検出してみましょう。

障害物を検出する

# LiDARデータを取得します
lidarData = client.getLidarData()
if (len(lidarData.point_cloud) < 3):
print("\tNo points received from Lidar data")
else:
print(f"len: {len(lidarData.point_cloud)}")
# 障害物情報はポイントクラウドというデータで返されますので、parse_lidarData()で整形します。
points = parse_lidarData(lidarData)
# 障害物の情報をデバッグ出力します。pointsに位置情報が入っていますので、このデータを走査すれば障害物の位置を特定できます
print("\tReading: time_stamp: %d number_of_points: %d" % (lidarData.time_stamp, len(points)))
# LiDARの取り付け位置と姿勢情報も参照できます
print("\t\tlidar position: %s" % (pprint.pformat(lidarData.pose.position)))
print("\t\tlidar orientation: %s" % (pprint.pformat(lidarData.pose.orientation)))

def parse_lidarData(data):
# reshape array of floats to array of [X,Y,Z]
points = numpy.array(data.point_cloud, dtype=numpy.dtype('f4’))
points = numpy.reshape(points, (int(points.shape[0]/3), 3))
return points

22

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#getlidardatavehicle_namenone
• https://microsoft.github.io/AirSim/lidar/

UC：着陸する • API名：land 着陸する • ドローンを着陸させます • 以下のプログラムを実行して、ドローンを着陸させましょう。 # 3m浮上 client.takeoff(3) # 着陸 client.land() 23

• https://github.com/toppers/hakoniwa-px4sim/tree/main/drone_api/libs#landvehicle_namenone

ArduPilot/PX4連携方法説明 • 本APIは、当初、箱庭PDUを前提としたものでした。 • しかし、本API実装を MAVLink通信に差し替えることで、 • ArduPilot/PX4と連携可能にしました。以下がその設計概要です。 従来の箱庭Python APIセット sensor_client MultirotorClient 箱庭依存のセンサは 従来のAPIを利用 (LiDAR/Camera) フライトコントローラに依存しない 共通MAVLink通信手順を扱う MultirotorClientと同じAPIセットで、 機体の移動等はMAVLink通信実装 Mavlink MultirotorClient ArduPilot/PX4に応じて、MAVLink通信手順が変 わるもの(ARM等)を抽象化して実装を隠蔽化 AbstractFlightController MavlinkDrone ArduPilotController Hakoniwa MAVLink (pymavlink) PX4Controller 24

ArduPilot/PX4連携APIの初期化方法 • 最初に、箱庭 Python APIを作成し、 • MAVLink通信向け箱庭 Python APIを作成します。 # 箱庭 Python API作成 client_ = hakosim.MultirotorClient(pdu_config_path, "Drone") # MAVLink 通信向け箱庭 Python API作成 client = MavlinkMultirotorClient('Drone', client_) client.add_vehicle("Drone", "udp:127.0.0.1:14550") # 以下、従来と同じ初期化 client.confirmConnection() client.enableApiControl(True) client.armDisarm(True) 25

アジェンダ • 本資料の位置付け • 対応しているフライトコントローラ • 複数機体構成のアーキテクチャと通信ポート • 箱庭Python APIのアーキテクチャと内部設計 • セットアップ手順 • シミュレーション手順 26

セットアップ手順 • 基本的なセットアップ手順は１台構成の場合と同じです。 • ArduPilot/PX4のclone/buildを機体数分だけ実施してください。 機体数分(※)だけ clone/build (※)例：AruduPilotを2機体構成で動かす場合、c1/ c2 ディレクトリを作成します。 27

ディレクトリ構成 • hakoniwa • hakoniwa-drone-core • hakoniwa-webserver • {ardupilot/px4}-controllers/ • c1 • ardupilot • c2 • ardupilot 28

アジェンダ • 本資料の位置付け • 対応しているフライトコントローラ • 複数機体構成のアーキテクチャと通信ポート • 箱庭Python APIのアーキテクチャと内部設計 • セットアップ手順 • シミュレーション手順 29

シミュレーション手順(PX4) 1) PX4（WSL2） # 機体0 bash -x hakoniwa-drone-core/tools/px4/run.bash \ px4-controllers/c1/PX4-Autopilot 0 # 機体1 bash -x hakoniwa-drone-core/tools/px4/run.bash \ px4-controllers/c2/ PX4-Autopilot 1 2) 箱庭（Docker） bash hakoniwa-drone-core/docker/tools/run-hako.bash px4 \ hakoniwa-drone-core/config/pdudef/webavatar-2.json 2 3) Python スクリプト（Docker 内） cd hakoniwa-drone-core/drone_api/pymavlink # 機体0 python3 -m hakosim_mavlink --name Drone --connection udp:127.0.0.1:14540 --type px4 # 機体1 python3 -m hakosim_mavlink --name Drone1 --connection udp:127.0.0.1:14541 --type px4 30

シミュレーション手順(PX4) https://www.youtube.com/watch?v=UpaDkUVhyoc 31

• https://www.youtube.com/watch?v=UpaDkUVhyoc

シミュレーション手順(ArduPilot) 1) Ardupilot（WSL2） # 機体0 bash -x hakoniwa-drone-core/tools/ardupilot/run.bash \ ardupilot-controllers/c1/ardupilot/ <HOST_IP> 0 # 機体1 bash -x hakoniwa-drone-core/tools/ardupilot/run.bash \ ardupilot-controllers/c2/ardupilot/ <HOST_IP> 1 2) 箱庭（Docker） bash hakoniwa-drone-core/docker/tools/run-hako.bash ardupilot \ hakoniwa-drone-core/config/pdudef/webavatar-2.json 2 3) Python スクリプト（Docker 内） cd hakoniwa-drone-core/drone_api/pymavlink # 機体0 python3 -m hakosim_mavlink --name Drone --connection udp:127.0.0.1:14550 --type ardupilot # 機体1 python3 -m hakosim_mavlink --name Drone1 --connection udp:127.0.0.1:14560 --type ardupilot 32

シミュレーション手順(ArduPilot) https://www.youtube.com/watch?v=LM9fwNWIJGY 33

• https://www.youtube.com/watch?v=LM9fwNWIJGY