Hakoniwa-drone-program
5.1K Views
May 18, 24
スライド概要
この資料では、箱庭ドローンを操作するPythonプログラムの作成方法を解説します。
- 箱庭ドローン・Python API 操作のユースケース説明
- ユースケース毎のAPI実装方法説明
- 機体を離陸する
- 機体の現在位置を取得する
- 機体を目的地へ移動する
- 機体で荷物を運ぶ
- 機体のカメラで撮影する
- 機体のレーザセンサ(LiDAR)で周辺の障害物を検出する
- 機体を着陸する
TOPPERS／箱庭WG活動でUnityやらAthrillやらmROSやら触ってます。 最近は仕事の関係でWeb系の技術に注力しつつ、箱庭への転用を模索しています。 2023年8月1日：合同会社箱庭ラボに移動しました
関連スライド
各ページのテキスト
箱庭(Hakoniwa) Drone Program 合同会社箱庭ラボ 森崇
はじめに • この資料では、箱庭ドローンを操作するPythonプログラムの作成方法を解 説します。 • 箱庭ドローン・Python API 操作のユースケース説明 • ユースケース毎のAPI実装方法説明 • 機体を離陸する • 機体の現在位置を取得する • 機体を目的地へ移動する • 機体で荷物を運ぶ • 機体のカメラで撮影する • 機体のレーザセンサ(LiDAR)で 周辺の障害物を検出する • 機体を着陸する 詳細を把握されたい場合は、 こちらを参照ください 解説範囲 Python アプリ Python API ライブラリ ワールド・機体 カスタマイズ編で詳説 詳細を把握されたい場合は、 こちらを参照ください 箱庭ドローン シミュレータ Unity ワールド/機体 箱庭コア機能 箱庭PDUデータ （共有メモリ：MMAP） 詳細を把握されたい場合は、 こちらを参照ください 2
Python アプリ&API ライブラリの位置付け Pythonアプリ Python APIライブラリ 箱庭ドローン ドローンを操作し、 様々なミッションを コントロール 箱庭ドローンを 操作するため のAPIセット Unityの3Dモデル で作成された ドローン 3
箱庭ドローン・Python API 操作のユースケース 箱庭ドローンを操作するAPIセットは全部で７個あります。 現在位置を 取得する 着陸する 離陸する カメラ撮影する 障害物を検出する 目的地へ 移動する 荷物を運ぶ 4
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())
5
Python APIライブラリの座標系 • ENU座標系です（右図） X • X：前方方向（E：前がプラス） • Y：横方向（N：左がプラス） • Z：上方向（U：上がプラス） 参考：ROSの座標系と同じです。 Y Z 右手系：親指上、右回り 親指 ：Z 人差し指 ：X 中指 ：Y 6
Unityの座標系 • 左手座標系です（右図） Z • X：横方向（右がプラス） • Y：上方向（上がプラス） • Z：前方向（前がプラス） X Y 左手系：親指上、左回り 親指 ：Y 人差し指 ：Z 中指 ：X 7
UC：離陸する • API名：takeoff • 引数で指定した高さ（単位：メートル）まで浮上します。 離陸する • 以下のプログラムを実行して、ドローンが離陸することを確 認してみましょう。 # 3m浮上 client.takeoff(3) 8
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)}") 9
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程度で移動します。 • 現時点では、目標速度での移動は未サポートです。 10
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) 11
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のみサポートしています。 12
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度です。 13
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
14
UC：着陸する • API名：land 着陸する • ドローンを着陸させます • 以下のプログラムを実行して、ドローンを着陸させましょう。 # 3m浮上 client.takeoff(3) # 着陸 client.land() 15