Go to Qiita Advent Calendar 2024 Top
LoginSignup
5
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@hsgucci

ドローンを動かす(SITL編)

Last updated at Posted at 2019-04-26

#はじめに
このページは，

ドローン操作システムを作ろう

の１ページです．
全体を見たい場合は上記ページへお戻りください．

#概要

ここでは，キーボード入力でdronekit-sitlのドローンを操作してみます．

#準備するもの

・Ubuntu Linuxの入ったIntel系ＰＣ
　（dronekit-sitlの記事で使ったもの）

#ベースとなるプログラム
これまでの記事を応用し，

・コマンドラインでホームポイント(緯度/経度/高度)を指定してSITL起動
・キー入力でフライトモード変更

ができるベースプログラムを用意しました．
※前回の記事「dronekit-python を使ってみる 実機編(キーボード入力を取る方法)」の中でも紹介した kbhit.py を同じフォルダに置いておいてください．

以下をコピーするか，
あるいは　ここ　を右クリックして[名前を付けて保存]してください．
ファイル名はskelton_sitl.pyとしています．

skelton_sitl.py
#!usr/bin/env python
# -*- coding: utf-8 -*-
print( "dronekitスタート" )    # 開始メッセージ

# 必要なライブラリをインポート
from kbhit import *                 # kbhitを使うために必要(同じフォルダにkbhit.pyを置くこと)
from subprocess import Popen        # subprocessの中から、Popenをインポート
from signal import signal, SIGINT   # Ctrl+C(SIGINT)の送出のために必要 
from dronekit import connect        # connectを使いたいのでインポート
from dronekit import VehicleMode    # VehicleModeも使いたいのでインポート
import time                         # ウェイト関数time.sleepを使うために必要

# kbhit()を使うための「おまじない」を最初に２つ書く
atexit.register(set_normal_term)
set_curses_term()

# dronekit SITL の起動情報
# example: 'dronekit-sitl copter --home=35.079624,136.905453,50.0,3.0 --instance 0'
sitl_frame          = 'copter'          # rover, plane, copterなどのビークルタイプ
sitl_home_latitude  = '35.894087'       # 緯度(度)  柏の葉キャンパス駅前ロータリー
sitl_home_longitude = '139.952447'      # 経度(度)
sitl_home_altitude  = '17.0'            # 高度(m)
sitl_home_direction = '0.0'             # 機首方位(度)
sitl_instance_num   = 0                 # 0〜

# コマンドライン入力したい文字列をリスト形式で作成
sitl_boot_list = ['dronekit-sitl',sitl_frame,
                  '--home=%s,%s,%s,%s' % (sitl_home_latitude,sitl_home_longitude,sitl_home_altitude,sitl_home_direction),
                  '--instance=%s'%(sitl_instance_num)]

print '# sitl command: ', sitl_boot_list        # 文字列を表示
p = Popen(sitl_boot_list)   # サブプロセスの起動
time.sleep(1)   # 起動完了のために1秒待つ

#connection_stringの生成
connection_string = 'tcp:localhost:' + str(5760 + int(sitl_instance_num) * 10 ) # インスタンスが増えるとポート番号が10増える

# フライトコントローラ(FC)へ接続
print( "FCへ接続: %s" % (connection_string) )    # 接続設定文字列を表示
vehicle = connect(connection_string, wait_ready=True)    # 接続

#Ctrl+cが押されるまでループ
try:
    while True:
        if kbhit():     # 何かキーが押されるのを待つ
            key = getch()   # 1文字取得

            # keyの中身に応じて分岐
            if key=='g':                # guided
                vehicle.mode = VehicleMode( 'GUIDED' )
            elif key=='l':              # land
                vehicle.mode = VehicleMode( 'LAND' )


        # ここはif文と同じインデントなので，キーに関係なく1秒に1回実行される
        # 現在の状態を表示
        print("--------------------------" )
        print(" System status: %s" % vehicle.system_status.state)
        print(" Is Armable?: %s" % vehicle.is_armable)
        print(" Armed: %s" % vehicle.armed) 
        print(" Mode: %s" % vehicle.mode.name )
        print(" Global Location: %s" % vehicle.location.global_frame)
        time.sleep(1)

except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
    print( "SIGINTを検知" )

# フライトコントローラとの接続を閉じる
vehicle.close()

 # サブプロセスにもSIGINT送信
p.send_signal(SIGINT)
p.communicate()
time.sleep(1)   # 終了完了のために1秒待つ

print("終了．")    # 終了メッセージ

フライトモードはGUIDED(ウェイポイント移動)とLAND(着陸)があれば十分です．

実行はいつも通りです．終了はCtrl+cです．

$python skelton_sitl.py

実行結果

以下の項目を１秒に１回表示しています．
・システムステータス
・PreArmチェックの状態
・ARM状態
・フライトモード
・現在の緯度・経度・高度

--------------------------
 System status: STANDBY
 Is Armable?: True
 Armed: False
 Mode: STABILIZE
 Global Location: LocationGlobal:lat=35.894087,lon=139.952447,alt=17.03

#キー入力でARM/DISARM
まずは，skelton_sitl.pyにARMとDISARMの機能を追加しましょう．
ARMなので'a'キー，DISARMなので'd'キーで良いでしょう．

キー入力のif-elseif文の下に追加します．
Pythonなのでインデントの深さを揃える必要があります．

```python:skelton_sitl.pyにaと`d`のキーを追加する
# keyの中身に応じて分岐
if key=='g': # guided
vehicle.mode = VehicleMode( 'GUIDED' )
elif key=='l': # land
vehicle.mode = VehicleMode( 'LAND' )
elif key=='a': # arm
vehicle.armed = True
elif key=='d': # disarm
vehicle.armed = False


実行して試してみましょう．

キーを押して試してみると分かりますが，
`Is Armable?`がTrueにならないとARMできません．
これは実機と同様です．PreArmのチェックが全て終わって準備OKになる必要があります．

'a'キーを押してから実際にARMが完了するまでは数秒のタイムラグがあります．
ARM時にも裏で様々な処理が行われる（ホーム地点の設定や，ロギングの開始）ので，しばらく時間がかかるのです．

また，**ARMした後10秒以内に離陸しないと，勝手にDISARMしてモータが止まります**．
これも実機と同じ仕様です．

-----
dronekit-pythonのサンプルの１つである**simple_goto.py**では，
このように書いてあります．

```python:simple_goto.pyの一部
    print("Basic pre-arm checks")
    # Don't try to arm until autopilot is ready
    while not vehicle.is_armable:     # Armableになるまで待ち続ける
        print(" Waiting for vehicle to initialise...")
        time.sleep(1)

    print("Arming motors")
    # Copter should arm in GUIDED mode
    vehicle.mode = VehicleMode("GUIDED")
    vehicle.armed = True

    # Confirm vehicle armed before attempting to take off
    while not vehicle.armed:     # ARM完了するまで待ち続ける
        print(" Waiting for arming...")
        time.sleep(1)

while文を使って，
・PreArmチェックが完了するまで１秒待ちのループ
・ARMが完了するまで1秒待ちのループ
待っていることがわかります．

本来はこのように書いたほうが良いでしょう．

実は，vehicleクラスには，vehicle.arm()やvehicle.disarm()といったメンバ関数も用意されています．
しかし，これらはブロッキング関数なので，１秒毎の表示処理が止まってしまいます．
飛行中にはDISARMできないので，vehicle.disarm()を間違って飛行中に呼ぶと，永久に操作が効かなくなります（泣

したがって，vehicle.armed = True vehicle.armed = Falseと書くほうが安全です．

#キー入力で離陸/着陸
次は離陸と着陸をskelton_sitl.pyに追加します．
離陸はTakeoffなので't'キーで良いでしょう．

skelton_sitl.pyの変更
            # keyの中身に応じて分岐
            if key=='g':                # guided
                vehicle.mode = VehicleMode( 'GUIDED' )
            elif key=='l':              # land
                vehicle.mode = VehicleMode( 'LAND' )
            elif key=='a':              # arm
                vehicle.armed = True
            elif key=='d':              # disarm
                vehicle.armed = False
            elif key=='t':              # takeoff
                vehicle.simple_takeoff(alt=10)

離陸はvehicleクラスのメンバ関数であるsimple_takeoffを使いました．
離陸高度は10mにしています．
着陸用の関数は用意されていないので，'LAND'にすることで着陸させます．

実行して試してみましょう．
離陸後，altの値が離陸地点の海抜17mから，+10mの27mまで増加することを確認します．
LANDすると17mまで降りてきます．

以下のような特徴を試しておきましょう．

・フライトモードがSTABILIZEのままだと，離陸できない
　　->モードをGUIDEDに変えてから('g'キー)，離陸させましょう．

・飛行中(ステータスACTIVE)の時に'd'キーを押しても，DISARMしない．
　　->いきなりモータが止まったら墜落するので，セーフティが働いている．

・フライトモードLANDで着陸すると，勝手にDISARMされモータが止まる．
　　->ARMのまま待ち続けることはない．

・LANDで着陸後，再離陸するにはGUIDEDに戻す必要がある．

#キー入力で移動

位置の移動には，LocationGlobal，LocationGlobalRelativeというクラスを使いたいので，skelton_sitl.pyの冒頭部分，色々なimport文が固まっている箇所に，新たにインポートを追加します．

skelton_sitl.pyにインポート文を追加
from dronekit import LocationGlobal, LocationGlobalRelative

移動は'1'，'2'の数字キーを，ゴーホームするために'r'キーを使いましょう．

```python:1と`2`キーで目的地へ移動，`r`キーでゴーホーム
elif key=='1': # simple_goto
# 柏の葉キャンパス交番上空30mへ
point = LocationGlobalRelative( 35.893246, 139.954909 , 30 )
vehicle.simple_goto(point)
elif key=='2': # simple_goto
# 三井ガーデンホテル上空50mへ
point = LocationGlobalRelative( 35.895236, 139.952468 , 50 )
vehicle.simple_goto(point)
elif key=='r': # RTL
vehicle.mode = VehicleMode( 'RTL' )

改変内容全体をまとめたプログラムを　[key_ctrl.py](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter04/key_ctrl_sitl.py)　として置いておきます．

**解説**
`LocationGlobal`と`LocationGlobalRelative`の２種類のクラスをインポートしました．
両者の違いは**絶対高度を使うか，相対高度を使うか**だけの違いです．
高度の絶対/相対なので注意してください．**緯度経度は常に絶対値**です．

離陸地点が海抜ゼロメートルならばどちらを使っても同じになりますが，
今回は海抜17メートルを離陸地点，と設定しました．
`LocationGlobalRelative`で30mや50mの高度を指定していますから，
実際は海抜47mと67mに移動することになります．



実行して試してみましょう．


現実世界と同じシミュレーションなので，目標地点に到達するまでは結構時間がかかります．
RTLすると，離陸地点まで戻って勝手に着陸します．

#おわりに

今回は，キーボード入力でsitlのシミュレーションドローンを動かしてみました．
これができれば，実機ドローンでも同じことが試せます．

次回は，プログラムからちょっと離れて，実機ドローンのシステム構成について書きたいと思います．
5
3
16

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?