自作ノードを実行する（Python）¶

この章ではシミュレーション上のドローンを操作するROSノードをPythonで作成します。

ROSパッケージを作る¶

はじめに、自作ノードを格納するためのROSパッケージを作ります。パッケージ名はpx4_sim_pkgとします。また、rospy、geometry_msgs、mavros_msgsパッケージを使用するので依存パッケージに追加しておきます。

cd ~/catkin_ws/src
catkin_create_pkg px4_sim_pkg rospy geometry_msgs mavros_msgs

ノードを書く¶

それでは実際にノードを書いてみましょう。今回は、ドローンを2m上空に離陸させるノードを書きます。

このコードを offboard_sample.py という名前で、 px4_sim_pkg/script に保存してください。

offboard_sample.py¶

#!/usr/bin/env python

import rospy

from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest
from mavros_msgs.srv import SetMode, SetModeRequest

current_state = State()
def state_cb(msg):
    global current_state
    current_state = msg

def offboard_node():

    rospy.init_node("offb_node")
    r = rospy.Rate(20)

    rospy.Subscriber("mavros/state", State, state_cb)
    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local",
                                     PoseStamped,
                                     queue_size=10)
    arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
    set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)

    while not rospy.is_shutdown() and not current_state.connected:
        r.sleep()

    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 2

    for i in range(100):
        local_pos_pub.publish(pose)
        r.sleep()

        if rospy.is_shutdown():
            break

    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = "OFFBOARD"

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    last_request = rospy.Time.now()

    while not rospy.is_shutdown():
        if current_state.mode != "OFFBOARD" \
              and (rospy.Time.now() - last_request > rospy.Duration(5)):

            try:
                offb_set_mode_resp = set_mode_client(offb_set_mode)
                if offb_set_mode_resp.mode_sent:
                    rospy.loginfo("Offboard enabled")
            except rospy.ServiceException as e:
                rospy.logwarn(e)

            last_request = rospy.Time.now()

        else:
          if not current_state.armed \
                and (rospy.Time.now() - last_request > rospy.Duration(5)):

            try:
                arm_cmd_resp = arming_client(arm_cmd)
                if arm_cmd_resp.success:
                    rospy.loginfo("Vehicle armed")
            except rospy.ServiceException as e:
                rospy.logwarn(e)

            last_request = rospy.Time.now()

        local_pos_pub.publish(pose)
        r.sleep()

if __name__ == "__main__":
    try:
        offboard_node()
    except rospy.ROSInterruptException:
        pass

コードの解説¶

#!/usr/bin/env python

ROSノードとして実行するPythonプログラムには一行目にこの宣言が必要です。詳細は以下のリンクを参照してください。

import rospy

from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest
from mavros_msgs.srv import SetMode, SetModeRequest

mavrosパッケージで提供されるトピックやサービスを使うためのメッセージは mavros_msgs パッケージで定義されています。

rospy.init_node("offb_node")

ROSノードを offb_node という名前で初期化しています。

r = rospy.Rate(20)

Offboardコマンドのタイムアウト時間は500msなので、2Hz以上の周波数で機体にコマンドを送る必要があります。また、Offboardコマンドがタイムアウトした場合は、その直前の飛行モードに切り替わるので、PositionモードからOffboardモードに切り替えるようにしておけば、Offboardモードへの移行が失敗してもその場にとどまってくれます。

rospy.Subscriber("mavros/state", State, state_cb)

mavros/state トピックにパブリッシュされる、 State 型のメッセージをサブスクライブするためのサブスクライバを初期化しています。第二引数は受け取ったメッセージを保持しておくキューのサイズです。第三引数の state_cb は受け取ったメッセージを処理するためのコールバック関数です。 state_cb は次のように定義されています。

State 型のメッセージには、機体の接続状況やモータの動作状況などが格納されています。

current_state = State()
def state_cb(msg):
    global current_state
    current_state = msg

state_cb は State 型の変数を受け取り、それをグローバル変数 current_state に格納する関数です。受け取ったメッセージをローカル変数ではなく、グローバル変数に格納するために global キーワードを使っています。

local_pos_pub = rospy.Publisher("mavros/setpoint_position/local",
                                 PoseStamped,
                                 queue_size=1000)

mavros/setpoint_position/local トピックに PoseStamped 型のメッセージをパブリッシュするパブリッシャを初期化しています。第三引数は受け取ったメッセージを保持しておくキューのサイズです。

arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)

モータの起動に用いる mavros/cmd/arming とモードの変更に用いる mavros/set_mode サービスのクライアントを初期化しています。それぞれ、 CommandBool 型と SetMode 型のメッセージを使います。

while not rospy.is_shutdown() and not current_state.connected:
    r.sleep()

FCUと接続するまで待ちます。 current_state.connected はFCUと接続されているときに true になるので、接続されるとwhileループから抜けます。また、 rate.sleep() は、このノードが20Hzで動作するように必要な時間だけノードを停止します。

pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 2

for i in range(100):
    local_pos_pub.publish(pose)
    r.sleep()

    if rospy.is_shutdown():
        break

あらかじめ移動先の位置をパブリッシュしておきます。移動先の位置をパブリッシュしておかないとOffboardモードへの移行がリジェクトされます。ここでは、移動先の位置は上方向に2mとなっています。 PoseStamped の各フィールドについては geometry_msgs/PoseStamped Message を見てください。

offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"

SetMode メッセージの custom_mode フィールドを OFFBOARD にします。このメッセージをサービスサーバに送信することでモードの切り替えをおこないます。 SetMode メッセージの定義は mavros_msgs/SetMode Service から見れます。

arm_cmd = CommandBoolRequest()
arm_cmd.value = True

/mavros/cmd/arming サービスへ送信するためのメッセージを作成します。 CommandBool メッセージの定義は mavros_msgs/CommandBool Service にあります。 mavrosパッケージで使用するメッセージの定義は、 mavrosのROS wikiページから見ることができます。

last_request = rospy.Time.now()

while not rospy.is_shutdown():
    if current_state.mode != "OFFBOARD" \
          and (rospy.Time.now() - last_request > rospy.Duration(5)):

        try:
            offb_set_mode_resp = set_mode_client(offb_set_mode)
            if offb_set_mode_resp.mode_sent:
                rospy.loginfo("Offboard enabled")
        except rospy.ServiceException as e:
            rospy.logwarn(e)

        last_request = rospy.Time.now()

    else:
      if not current_state.armed \
            and (rospy.Time.now() - last_request > rospy.Duration(5)):

        try:
            arm_cmd_resp = arming_client(arm_cmd)
            if arm_cmd_resp.success:
                rospy.loginfo("Vehicle armed")
        except rospy.ServiceException as e:
            rospy.logwarn(e)

        last_request = rospy.Time.now()

    local_pos_pub.publish(pose)
    r.sleep()ate.sleep();
}

最初のif節は現在のモードが OFFBOARD かつ、モード切替が行われたのが5秒以上前であるときに実行されます。最後のモード切替からの経過時間は last_request 変数を使って判断しています。

set_mode_client(offb_set_mode) で、 offb_set_mode 変数のメッセージを使って mavros/set_mode サービスをコールしています。サービスにはリクエストのフィールド（サービスコールの際に必要な情報を与えるために使う）と、レスポンスのフィールド（処理結果等を格納するのに使う）があり、サービスコールが成功するとレスポンスが帰ってきます。今回はレスポンスの mode_sent を調べることでモード切り替えの命令がきちんと認識され、送信されたかを判断しています。

サービスコールが失敗した場合には例外が送出されるので、try-except節で例外をキャッチする必要があります。

モードが OFFBOARD の時は機体にモータを起動するように命令を送っています。この部分の処理はOffboardコマンドを送るところとほぼ同じです。

最後に、ループ毎に目標位置をパブリッシュしています。

ビルドする¶

offboard_sample.py が使用するメッセージが確実に生成されているようにするためにビルドします。

cd ~/catkin_ws
catkin_make

実行権限の付与¶

PythonプログラムをROSノードとして実行するには実行権限を与える必要があります。

chmod +x offboard_sample.py

ノードの起動¶

はじめにシミュレータを起動します。 ROSノードとの通信を行うためにはmavrosが必要なので、 mavros_posix_sitl.launch を使います。

roslaunch px4 mavros_posix_sitl.launch

次に先ほど作成したノードを起動します。

rosrun px4_sim_pkg offboard_sample.py

launchファイルを書く¶

シミュレータとOffboardノードを別々に起動するのは面倒なので、launchファイルにまとめて一つのコマンドで起動できるようにしましょう。

px4_sim_pkg/launch 以下に py_offb_sample.launch という名前で以下の内容を保存してください。

<launch>

    <include file="$(find px4)/launch/mavros_posix_sitl.launch" />
    <node name="offb_node" pkg="px4_sim_pkg" type="offboard_sample.py" />

</launch>

以下のコマンドを使ってこのlaunchファイルからシミュレータとOffboardノードを起動できます。

roslaunch px4_sim_pkg py_offb_sample.launch

参考¶

mavros - ROS Wiki: mavrosのROS Wikiページ
Pythonでのグローバル（global）変数の宣言方法: グローバル変数について
シンプルな配信者(Publisher)と購読者(Subscriber)を書く(Python): ROS Wikiのパブリッシャとサブスクライバを書くチュートリアル
シンプルなサービスとクライアントを書く (Python): ROS Wikiのサービスとクライアントを書くチュートリアル
rospy/Overview/Services: Overview of rospy Service
Class ServiceProxy: Service Proxy API Reference