Hakoniwa-drone-program
0.9K 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ライブラリの座標系 • NED座標系です（右図） • X︓前⽅⽅向（N︓前がプラス） • Y︓横⽅向（E︓右がプラス） • Z︓下⽅向（D︓下がプラス） X • 補⾜︓⾼度は上がプラス Y Z 右⼿系︓親指下、右回り 親指 ︓Z ⼈差し指 ︓X 中指 ︓Y 6
Unityの座標系 • 左⼿座標系です（右図） • X︓横⽅向（右がプラス） • Y︓上⽅向（上がプラス） • Z︓前⽅向（前がプラス） Z X Y 左⼿系︓親指上、左回り 親指 ︓Y ⼈差し指 ︓Z 中指 ︓X 7
参考︓ROSの座標系 • ENU座標系です（右図） • X︓前⽅⽅向（E︓前がプラス） • Y︓横⽅向（N︓左がプラス） • Z︓上⽅向（U︓上がプラス） X Y Z 右⼿系︓親指上、右回り 親指 ︓Z ⼈差し指 ︓X 中指 ︓Y 8
UC︓離陸する • API名︓takeoff • 引数で指定した⾼さ（単位︓メートル）まで浮上します。 離陸する • 以下のプログラムを実⾏して、ドローンが離陸することを確 認してみましょう。 # 3m浮上 client.takeoff(3) 9
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)}") 10
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程度で移動します。 • 現時点では、⽬標速度での移動は未サポートです。 11
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) 12
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のみサポートしています。 13
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度です。 14
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
15
UC︓着陸する • API名︓land 着陸する • ドローンを着陸させます • 以下のプログラムを実⾏して、ドローンを着陸させましょう。 # 3m浮上 client.takeoff(3) # 着陸 client.land() 16